/*
	** Given a file and bite index, find the bite in the file that
	** most resembles it.
	*/

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

#define	MAX_DATA	100000
#define	MAX_BITES	1000

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

{
FILE	*fpt;
double	acc_x,acc_y,acc_z,yaw,pitch,roll,scale;
double	*data[6],zero[3];
int	i,j,a,b;
int	total_data;
double	smoothed_data;
int	total_bites,bite_index[MAX_BITES];
char	parsing[320],gt_filename[320];
int	gt_time,gt_index;
char	gt_hand[80],gt_utensil[80],gt_container[80],gt_food[80];
int	match_bite;
double	match_score[MAX_BITES];


if (argc != 3)
  {
  printf("findsim [filename] [bite#]\n");
  exit(0);
  }
if ((fpt=fopen(argv[1],"r")) == NULL)
  {
  printf("Unable to open %s for reading\n",argv[1]);
  exit(0);
  }

for (j=0; j<6; j++)
  data[j]=(double *)calloc(MAX_DATA,sizeof(double));

	/* file format is acc_x acc_y acc_z yaw pitch roll scale */
	/* data is assumed to be line by line at 15 Hz */
total_data=0;
while (1)
  {
  i=fscanf(fpt,"%lf %lf %lf %lf %lf %lf %lf",
	&acc_x,&acc_y,&acc_z,&yaw,&pitch,&roll,&scale);
  if (i != 7)
    break;
  data[0][total_data]=acc_x;
  data[1][total_data]=acc_y;
  data[2][total_data]=acc_z;
  data[3][total_data]=yaw;
  data[4][total_data]=pitch;
  data[5][total_data]=roll;
  total_data++;
  if (total_data >= MAX_DATA)
    {
    printf("MAX DATA (%d) exceeded\n",MAX_DATA);
    fclose(fpt);
    exit(0);
    }
  }
fclose(fpt);
if (0)
  printf("%d total data\n",total_data);

	/* find zero points for gyros */
for (j=0; j<3; j++)
  zero[j]=0.0;
for (i=0; i<total_data; i++)
  {
  for (j=0; j<3; j++)
    zero[j]+=data[j+3][i];
  }
for (j=0; j<3; j++)
  zero[j]/=((double)total_data);
if (0)
  printf("zeros %.3lf %.3lf %.3lf\n",zero[0],zero[1],zero[2]);

	/* convert data voltages to deg/sec (gyros) and gravities (accels) */
	/* gyro=LPY410al, 2.5mv per deg/sec */
	/* accel=LIS344alh, Vdd=3.3v, 5/3.3=1.515 gravities per volt */
for (i=0; i<total_data; i++)
  {
	/* 3.3v supply so 1/2(3.3)=1.65 reference */
	/* 15/3.3=4.5454 instead, if chip wired to +-6g */
  for (j=0; j<3; j++)
    data[j][i]=(data[j][i]-1.65)*(5.0/3.3);
	/* reference voltage calculated as average ADC value for while file */
  for (j=3; j<6; j++)
    data[j][i]=(data[j][i]-zero[j-3])*400.0;
  }

	/* read GT file to get bite index times */
strcpy(parsing,argv[1]);
i=strlen(parsing)-1;
while (parsing[i] != '/')
  i--;
parsing[i]=0;
sprintf(gt_filename,"%s/gt_union.txt",parsing);
if ((fpt=fopen(gt_filename,"r")) == NULL)
  {
  printf("Unable to open %s for reading\n",gt_filename);
  exit(0);
  }
total_bites=0;
while (1)
  {
  i=fscanf(fpt,"%d %d %s %s %s %s",&gt_index,&gt_time,
		gt_hand,gt_utensil,gt_container,gt_food);
  if (i != 6)
    break;
  bite_index[total_bites]=gt_time;
  total_bites++;
  }
fclose(fpt);
if (0)
  printf("%d bites\n",total_bites);

match_bite=atoi(argv[2])-1;	/* use from-zero index */
if (match_bite < 0  ||  match_bite >= total_bites)
  {
  printf("bite to match must be between 1 and %d\n",total_bites);
  exit(0);
  }

	/* calculate matching score for all bites */
for (b=0; b<total_bites; b++)
  {
  match_score[b]=0.0;
  a=bite_index[match_bite]-45;	/* start of bite to match */
  for (i=bite_index[b]-45; i<=bite_index[b]+45; i++,a++)
    {
    for (j=5; j<6; j++)
      match_score[b]+=(fabs(data[j][i]-data[j][a]));
    }
  }

	/* print out all bite scores */
for (b=0; b<total_bites; b++)
  printf("bite %d score %lf\n",b+1,match_score[b]);

for (j=0; j<6; j++)
  free(data[j]);
}