/* 
   Solution to LSU EE 4770 (RTS) 1997 Homework 5, Problem 3

   */

#include <stdio.h>
#include <time.h>
#include <stdlib.h>
long random(void);  /* This should be in the include file but isn't. */

#define WINDOW 100  /* Number of samples to compare. */
#define DIST   1.0  /* Distance between sensors, in meters. */
#define SIZE  1000  /* Number of samples to store. */

/* 
   Routine below is core of solution. 
*/

int best_match(int *samp_A, int *samp_B,int size)
{
 int delta         =  0;
 double lowest_err =  0; /* Initialize only to eliminate compiler warnings. */
 int best_delta    = -1; 

 for( delta=0; delta<size-WINDOW; delta++ ){
  double this_err = 0;   /* Error for this delta. */
  int i;

  /* Compute error by summing absolute value of differences. */
  for( i=delta; i<delta+WINDOW; i++ ) 
   this_err += abs( samp_A[i] - samp_B[i-delta] );

  /* See if error for this delta is smaller than all previous. */
  if( best_delta == -1 || this_err < lowest_err ) {
   lowest_err = this_err;
   best_delta = delta;
  }}
 
 return best_delta;
}

/* 
   Routines below are stubs for interface and timing functions. 
   (That is, they allow the program to be run without having to
   read real sensors or to sleep.)

*/

double get_current_time()
{
 struct timespec tp;
 /* clock_gettime is part of posix library. Can substitute time or, since
    the return value isn't really used, can comment out completely. */
 /* clock_gettime(CLOCK_REALTIME, &tp); 
 return ((double)tp.tv_sec) + tp.tv_nsec * 1e9; */
 return 0.0;
}


static int ri_sample_history[SIZE];
static int ri_next_sample = -1;
static int ri_delta = SIZE / 2;

/* 
   Generate a random sample stream "for" sensor A. 
   Samples have a uniform distribution over [0,2147483647]
*/

int readInterfaceA() 
{
 if( ri_next_sample == -1 ){
  int i;
  for(i=0;i<SIZE;i++)ri_sample_history[i]=0;
  ri_next_sample = 0;
 } else {
  ri_next_sample++;
 }
 if( ri_next_sample == SIZE ) ri_next_sample = 0;

 return (ri_sample_history[ri_next_sample] = random() & 0x3FFF);
}

/*
   Sample stream "for" sensor B is A's stream delayed by constant ri_delta,
   with some non-trivial noise added.
*/

int readInterfaceB() {
 int index =  ri_next_sample - ri_delta;
 if( index < 0 ) index += SIZE;
 return ri_sample_history[ index ] + ( random() & 0x3FFF ) - 0x1FFF;
}

/* 
  The sleep_until routine does nothing since we're not reading from
  real systems anyway.  If this were an actual RTS, we would need
  to call a precision sleep routine. (The sleep routine in the C library
  is too coarse.
*/

void sleep_until(double wake_time){return;}

/* 
  This just writes to stdout if the count is in the proper range.  The
  count is used to only print out values of interest. (In a real system
  we would display every value.)
  */

void display(double value){
 static count=0;
 count++;
 if( count > 700 )printf("Flow velocity %f meters / second.\n",value); 
 if( count == 720 )exit(0); 
}

/* 
  This routine contains the main loop.
  */

void cross_cor()
{
 int samp_A[SIZE];   /* Hold values read from sensor A. */
 int samp_B[SIZE];   /* Hold values read from sensor B. */
 double next_sample_time;      /* Time at which sensors will be read. */
 double sample_interval = 0.1; /* Second, time between reading sensors. */
 
 int i;

 for(i=0;i<SIZE;i++)samp_A[i]=samp_B[i]=0.0;   /* Inititalize arrays. */

 next_sample_time = get_current_time();	       /* Initialize to now. */

 while(1){
  int a = readInterfaceA();	/* Read sesnor A. */
  int b = readInterfaceB();	/* Read sesnor B. */
  double flow_velocity;		
  int delta;

  next_sample_time += sample_interval;  /* Compute when sensors next read. */

  /* Make room for new samples. (There are faster ways of doing this.) */
  for(i=SIZE-1; i>0; i--){
   samp_A[i] = samp_A[i-1]; samp_B[i] = samp_B[i-1];
  }
  /* Store new samples. */
  samp_A[0]=a;
  samp_B[0]=b;

  /* Find number of sample times separating flow. */
  delta = best_match( &samp_A[0], &samp_B[0], SIZE );

  /* Compute flow velocity. */
  flow_velocity = DIST / ( sample_interval * delta );

  /* Display flow velocity. */
  display(flow_velocity);

  /* Function will return at next_sample_time. */
  sleep_until( next_sample_time );

 }
}

int main(int argv, char **argc)
{
 cross_cor(); /* Never returns. */
 return 0;
}