/*
	** Read an acc/gyro file and calculate periods of rest/motion.
	** Rest defined by thresholds on gyro and accel.
	** During each motion period, calculate the total angle rotated
	** around each axis, and total distance moved along each axis.
	** All distances are obviously corrupted by Earth's gravity
	** unless the orientation is perfect in which case only Z corrupted.
	*/

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

#define	OUTPUT_RANGES	1	/* 0=>state at each t, 1=>periods of motion */
#define	SQR(x)		((x)*(x))
#define MAX_DATA	30000
#define	SMOOTH_WINDOW	15	/* 1 second window for smoothing */
#define	MIN_REST_DUR	15	/* 1 sec window min for rest period */
#define	MIN_MOTION_DUR	15	/* 1 sec window minimum for non-rest period */
#define ACCEL_THRESH	0.008	/* sum(acc) (G) max for stdev rest */
#define GYRO_THRESH	0.04	/* sum(gyro) (rad/sec) max for stdev rest */


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

{
FILE	*fpt;
char	header[80];
int	i,j,k,TotalData;
double	*timestamp;
double	*gyro[3];
double	*raw_acc[3],*acc[3];
int	*motion_state;
double	average,zero[3];
double	mean[6],stddev[6];
double	distance[3],angle[3],prev_velocity[3],curr_velocity[3],avg_velocity;
double	scale;


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

	/* allocate memory */
timestamp=(double *)calloc(MAX_DATA,sizeof(double));
motion_state=(int *)calloc(MAX_DATA,sizeof(int));
for (j=0; j<3; j++)
  {
  gyro[j]=(double *)calloc(MAX_DATA,sizeof(double));
  raw_acc[j]=(double *)calloc(MAX_DATA,sizeof(double));
  acc[j]=(double *)calloc(MAX_DATA,sizeof(double));
  }

	/* read header */
if (0)	/* no header in Cafeteria data set */
  for (i=0; i<7; i++)
    fscanf(fpt,"%s",header);

	/* read data */
TotalData=0;
while (1)
  {
  i=fscanf(fpt,"%lf",&(raw_acc[0][TotalData]));
  if (i != 1)
    break;
  fscanf(fpt,"%lf %lf",
	&(raw_acc[1][TotalData]),&(raw_acc[2][TotalData]) );
  fscanf(fpt,"%lf %lf %lf",&(gyro[0][TotalData]),
	&(gyro[1][TotalData]),&(gyro[2][TotalData]) );
  fscanf(fpt,"%lf",&scale);
  TotalData++;
  if (TotalData >= MAX_DATA)
    {
    printf("MAX_DATA (%d) exceeded\n",MAX_DATA);
    exit(0);
    }
  }
if (0)
  printf("%d total data read\n",TotalData);

	/* convert voltages to G and deg/sec */
for (j=0; j<3; j++)
  zero[j]=0.0;	/* zero defines reference voltage for gyro */
for (i=0; i<TotalData; i++)
  for (j=0; j<3; j++)
    zero[j]+=gyro[j][i];
for (j=0; j<3; j++)
  zero[j]/=(double)TotalData;
for (i=0; i<TotalData; i++)
  {
	/* convert data voltages to deg/sec (gyros) and
	** gravities (accelerometers) */
	/* gyro=LPY410al, 2.5mv per deg/sec */
	/* accel=LIS344alh, Vdd=3.3v, 5/3.3=1.515 gravities per volt */
  for (j=0; j<3; j++)	/* 3.3v supply so 1/2(3.3)=1.65 reference */
    raw_acc[j][i]=(raw_acc[j][i]-1.65)*(5.0/3.3);
  for (j=0; j<3; j++)	/* reference voltage assumed 1.245 V */
    gyro[j][i]=(gyro[j][i]-zero[j])*400.0;
  }

	/* smooth acceleration data */
for (i=0; i<TotalData; i++)
  {
  if (i < SMOOTH_WINDOW/2  ||  i >= TotalData-SMOOTH_WINDOW/2)
    {
    for (j=0; j<3; j++)
      acc[j][i]=raw_acc[j][i];
    continue;
    }
  for (j=0; j<3; j++)
    {
    average=0.0;
    for (k=i-SMOOTH_WINDOW/2; k<=i+SMOOTH_WINDOW/2; k++)
      average+=raw_acc[j][k];
    acc[j][i]=average/(double)SMOOTH_WINDOW;
    }
  }

if (0) 	/* print out raw and smoothed acceleration for comparison */
  {
  for (i=0; i<TotalData; i++)
    printf("%.3lf %.3lf %.3lf => %.3lf %.3lf %.3lf (%.3lf)\n",
	raw_acc[0][i],raw_acc[1][i],raw_acc[2][i],
	acc[0][i],acc[1][i],acc[2][i],
	sqrt(SQR(acc[0][i])+SQR(acc[1][i])+SQR(acc[2][i])) );
  exit(0);
  }

	/* determine rest state of each index */
for (i=0; i<TotalData; i++)
  motion_state[i]=1;	/* moving */
for (i=0; i<TotalData; i++)
  {
  if (i < MIN_REST_DUR/2  ||  i >= TotalData-MIN_REST_DUR/2)
    {
    motion_state[i]=2;	/* undefined */
    continue;
    }
	/* calculate stddevs of raw accels in MIN_REST_DUR */
  for (j=0; j<3; j++)
    {
    mean[j]=0.0;
    for (k=i-MIN_REST_DUR/2; k<=i+MIN_REST_DUR/2; k++)
      mean[j]+=acc[j][k];
    mean[j]=mean[j]/(double)MIN_REST_DUR;
    stddev[j]=0.0;
    for (k=i-MIN_REST_DUR/2; k<=i+MIN_REST_DUR/2; k++)
      stddev[j]+=SQR(mean[j]-acc[j][k]);
    stddev[j]=sqrt(stddev[j]/(double)MIN_REST_DUR);
    }
	/* calculate stddevs of gyros in MIN_REST_DUR */
  for (j=3; j<6; j++)
    {
    mean[j]=0.0;
    for (k=i-MIN_REST_DUR/2; k<=i+MIN_REST_DUR/2; k++)
      mean[j]+=gyro[j-3][k];
    mean[j]=mean[j]/(double)MIN_REST_DUR;
    stddev[j]=0.0;
    for (k=i-MIN_REST_DUR/2; k<=i+MIN_REST_DUR/2; k++)
      stddev[j]+=SQR(mean[j]-gyro[j-3][k]);
    stddev[j]=sqrt(stddev[j]/(double)MIN_REST_DUR);
    }
  if (!OUTPUT_RANGES)	/* print stddevs to look at rest state at each time */
    {
    printf("%d\t%lf\t%lf\t%lf\n",i,stddev[0]+stddev[1]+stddev[2],
		stddev[3]+stddev[4]+stddev[5],
		fabs(mean[3])+fabs(mean[4])+fabs(mean[5]));
    }
	/* motion test */
  if ((stddev[0]+stddev[1]+stddev[2]) < ACCEL_THRESH  &&
	(stddev[3]+stddev[4]+stddev[5]) < GYRO_THRESH*180.0/M_PI)
    {
    for (k=i-MIN_REST_DUR/2; k<=i+MIN_REST_DUR/2; k++)
      motion_state[k]=0;	/* rest */
    }
  }

	/* any timestamps thought to be at motion that are not part
	** of a sequence of minimum length are switched to rest */
if (1)
for (i=0; i<TotalData; i++)
  {
  if (motion_state[i] != 1)
    continue;
  j=i+1;
  while (motion_state[j] == motion_state[i])
    j++;
  if (j-i < MIN_MOTION_DUR)
    {
    while (i<j)
      {
      motion_state[i]=0;
      i++;
      }
    }
  else	/* move ahead to check next span */
    i=j;
  }

	/* print out periods of rest */
if (1)
for (i=0; i<TotalData; i++)
  {
  if (motion_state[i] != 0)
    continue;
  printf("rest\t%d\t",i);
  while (motion_state[i] == 0  &&  i < TotalData)
    i++;
  printf("%d\n",i-1);
  }

	/* calculate distance moved and angles rotated during motion periods */
if (0)
for (i=0; i<TotalData; i++)
  {
  if (motion_state[i] == 1)
    {
    if (OUTPUT_RANGES)
      printf("%d...",i);
    for (j=0; j<3; j++)
      distance[j]=angle[j]=prev_velocity[j]=curr_velocity[j]=0.0;
    while (motion_state[i] == 1  &&  i < TotalData)
      {
      for (j=0; j<3; j++)
        {
        angle[j]+=(gyro[j][i]*(timestamp[i]-timestamp[i-1]) );
	prev_velocity[j]=curr_velocity[j];
	curr_velocity[j]+=acc[j][i];
        avg_velocity=(prev_velocity[j]+curr_velocity[j])/2.0;
        distance[j]+=(avg_velocity*(timestamp[i]-timestamp[i-1]) );
	}
      i++;
      }
    if (OUTPUT_RANGES)
      printf("%d %.1lf %.1lf %.1lf  %.1lf %.1lf %.1lf\n",i-1,
		distance[0],distance[1],distance[2],
		angle[0]*180.0/M_PI,angle[1]*180.0/M_PI,angle[2]*180.0/M_PI);
    }
  }

}