#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>

	/*
	** This program implements Ryan Mattfeld's algorithm to detect bite
	** events on the cafeteria dataset.
	** It first segments the data into stable and unstable periods,
	** then classifies each stable period as one of four possibilities:
	** no bite (0), single bite (1), food mass bite (2), drink bite (3).
	** It also reports the weight of detected bites and the time span
	** of the detected pattern.
	*/

#define VERBOSE		0	/* turn on/off extra output */

#define	MAX_DATA	100000	/* max number of raw data */
#define	MAX_SEGMENTS	1000	/* max number of stable periods */
#define	MAX_BITES	1000	/* max number of bites detected */

#define	WINDOW		15	/* samples, window size for stability */
#define	SIGMA_NOISE	0.29	/* grams, scale noise */

#define T1		30.0	/* max food bite weight */
#define	T2		100.0	/* min food mass weight removed from scale */
#define	T3		300.0	/* max food mass weight removed from scale */
#define	T4		80.0	/* min drink weight removed from scale */
#define	T5		550.0	/* max drink weight removed from scale */
#define	T6		80.0	/* max drink bite weight */

int main(int argc, char *argv[])

{
FILE		*fpt;
int		i,j,k;
int		TotalData;
double		ax,ay,az,yaw,pitch,roll,scale;
double		RawData[MAX_DATA];
double		mean;
int		Stability[MAX_DATA];	/* 0=>unstable, 1=>stable */
int		TotalSegments;
int		SegmentStart[MAX_SEGMENTS],SegmentEnd[MAX_SEGMENTS];
double		SegmentWeight[MAX_SEGMENTS];
int		TotalBites;
int		BiteClass[MAX_BITES];
double		BiteWeight[MAX_BITES];
int		BiteStart[MAX_BITES],BiteEnd[MAX_BITES];


if (argc != 2)
  {
  printf("Usage:  scale-detect [filename]\n");
  exit(0);
  }
if ((fpt=fopen(argv[1],"r")) == NULL)
  {
  printf("Unable to open %s for reading\n",argv[1]);
  exit(0);
  }

	/* Read data file.  7 columns separated by spaces.  15 Hz.
	** 7th column is scale, other 6 are ignored. */
TotalData=0;
while (1)
  {
  i=fscanf(fpt,"%lf %lf %lf  %lf %lf %lf  %lf",
	&ax,&ay,&az,&yaw,&pitch,&roll,&scale);
  if (i != 7)
    break;
  RawData[TotalData]=scale;
  TotalData++;
  if (TotalData >= MAX_DATA)
    {
    printf("MAX_DATA (%d) exceeded in this file\n",MAX_DATA);
    exit(0);
    }
  }
fclose(fpt);

	/* Because the scale data was oversampled, it sometimes contains
	** a value of 0.0.  This is erroneous.  Search for and replace
	** these with the average of surrounding values. */

for (i=0; i<TotalData; i++)
  if (RawData[i] < 1.0)		/* will be zero, but use < 1.0 for rounding */
    {
    for (j=i; j>=0; j--)
      if (RawData[j] >= 1.0)
        break;
    for (k=i; k<TotalData; k++)
      if (RawData[k] >= 1.0)
        break;
    if (j >= 0  &&  k < TotalData)
      RawData[i]=(RawData[j]+RawData[k])/2.0;
    else if (j >= 0)
      RawData[i]=RawData[j];
    else
      RawData[i]=RawData[k];
    }

	/* Identify stable points.  A stable point is any point within
	** a window of size WINDOW in which no datum is
	** more than 3 sigma(noise) = 0.87 g from the mean.
	** All points within that window are considered stable.
	*/

for (i=0; i<TotalData; i++)
  Stability[i]=0;
for (i=0; i<TotalData; i++)
  {
  mean=0.0;
  for (j=0; j<WINDOW; j++)
    mean+=RawData[i+j];
  mean/=(double)(WINDOW);
  for (j=0; j<WINDOW; j++)
    if (fabs(RawData[i+j]-mean) > 3.0*SIGMA_NOISE)
      break;
  if (j == WINDOW)
    for (j=0; j<WINDOW; j++)
      Stability[i+j]=1;
  }

	/* Segment data to find periods of stability. */
TotalSegments=0;
for (i=0; i<TotalData; i++)
  {
  if (Stability[i] == 0  ||  i == TotalData-1)	/* period at end of file */
    continue;
  for (j=i; j<TotalData; j++)
    if (Stability[j] == 0)
      break;
  SegmentStart[TotalSegments]=i;
  SegmentEnd[TotalSegments]=j-1;
  mean=0.0;
  for (k=i; k<j; k++)
    mean+=RawData[k];
  mean/=(double)(j-i);
  SegmentWeight[TotalSegments]=mean;
  TotalSegments++;
  i=j;
  }
if (VERBOSE)
  {
  printf("%d total stable segments\n",TotalSegments);
  for (i=0; i<TotalSegments; i++)
    printf("%d<->%d  %.1lf\n",SegmentStart[i],SegmentEnd[i],SegmentWeight[i]);
  }

	/* Classify stable segments.  First and last segment have to be
	** non-eating events because there is no previous/suceeding period
	** to use to evaluate. */
TotalBites=0;
for (i=1; i<TotalSegments-1; i++)
  {
  if (SegmentWeight[i-1]-SegmentWeight[i] >= 3.0*SIGMA_NOISE  &&
	SegmentWeight[i-1]-SegmentWeight[i] <= T1  &&
	SegmentWeight[i+1]-SegmentWeight[i] <= 3.0*SIGMA_NOISE)
    {
    BiteClass[TotalBites]=1;
    BiteWeight[TotalBites]=SegmentWeight[i-1]-SegmentWeight[i];
    BiteStart[TotalBites]=SegmentEnd[i-1]+1;
    BiteEnd[TotalBites]=SegmentEnd[i];
    TotalBites++;
    }
  else if (SegmentWeight[i-1]-SegmentWeight[i] >= T2  &&
	SegmentWeight[i-1]-SegmentWeight[i] <= T3  &&
	SegmentWeight[i-1]-SegmentWeight[i+1] >= 3.0*SIGMA_NOISE  &&
	SegmentWeight[i-1]-SegmentWeight[i+1] <= T1)
    {
    BiteClass[TotalBites]=2;
    BiteWeight[TotalBites]=SegmentWeight[i-1]-SegmentWeight[i+1];
    BiteStart[TotalBites]=SegmentEnd[i-1]+1;
    BiteEnd[TotalBites]=SegmentStart[i+1]-1;
    TotalBites++;
    }
  else if (SegmentWeight[i-1]-SegmentWeight[i] >= T4  &&
	SegmentWeight[i-1]-SegmentWeight[i] <= T5  &&
	SegmentWeight[i-1]-SegmentWeight[i+1] >= 3.0*SIGMA_NOISE  &&
	SegmentWeight[i-1]-SegmentWeight[i+1] <= T6)
    {
    BiteClass[TotalBites]=3;
    BiteWeight[TotalBites]=SegmentWeight[i-1]-SegmentWeight[i+1];
    BiteStart[TotalBites]=SegmentEnd[i-1]+1;
    BiteEnd[TotalBites]=SegmentStart[i+1]-1;
    TotalBites++;
    }
  }
if (VERBOSE)
  {
  printf("%d total bites detected\n",TotalBites);
  for (i=0; i<TotalBites; i++)
    printf("%d<->%d  %d(%s)  %.1lf\n",BiteStart[i],BiteEnd[i],BiteClass[i],
	(BiteClass[i] == 1 ? "food" : (BiteClass[i] == 2 ? "mass" : "drink")),
	BiteWeight[i]);
  }
else
  {
  for (i=0; i<TotalBites; i++)
    printf("%d %d %.1lf\n",BiteStart[i],BiteEnd[i],BiteWeight[i]);
    //printf("bite %d %d\n",BiteStart[i],BiteEnd[i]);
  }

}