/*
	** This program parses one of Yujie's data files and prints out
	** only the raw deg or deg/sec data for yaw pitch roll.
	** Yujie's files contained intermediate results and other data
	** he used in MATLAB for evaluation.
	*/

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

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

{
int	i,milliseconds,seconds,bite_count,index;
double	yaw,pitch,roll;
double	yaw_smooth,pitch_smooth,roll_smooth;
FILE	*fpt;
int	total_data;
double	avg_pitch,avg_roll,pitch_volts,roll_volts;

if (argc != 2)
  {
  printf("Usage:  raw-only [filename]\n");
  exit(0);
  }
if ((fpt=fopen(argv[1],"r")) == NULL)
  {
  printf("Unable to open %s for reading\n",argv[1]);
  exit(0);
  }
total_data=0;
avg_pitch=avg_roll=0.0;
while (1)
  {
  i=fscanf(fpt,"%d %d %d %d %lf %lf %lf %lf %lf %lf",
	&milliseconds,	/* ms since beginning of recording */
	&seconds,	/* s since beginning of recording */
	&bite_count,	/* bite count detected by algorithm up to this point */
	&index,		/* data index */
	&yaw,&pitch,&roll,	/* raw data */
	&yaw_smooth,&pitch_smooth,&roll_smooth);	/* smooth data */
  if (i != 10)
    break;
  pitch_volts=pitch*0.00333+1.23;
  roll_volts=roll*0.00333+1.23;
  avg_pitch+=pitch_volts;
  avg_roll+=roll_volts;
  if (total_data%4 == 0)  /* every 4th data to get 15Hz from 60Hz */
    {
    if (strstr(argv[1],"_Wi") != NULL)
      {
      pitch=(pitch_volts-1.286)*300.0;
      roll=(roll_volts-1.252)*300.0;
      printf("%.2lf %.2lf\n",pitch,roll);
      }
    else
      {
      printf("%.2lf %.2lf %.2lf\n",yaw,pitch,roll);
      }
    }
  total_data++;
  }
fclose(fpt);
if (0)
  printf("%.4lf %.4lf\n",avg_pitch/(double)total_data,
	avg_roll/(double)total_data);
}