#include "Sonar.hh"
#include <iostream.h>

Sonar::Sonar()
{
  lock = new Semaphore(1);
  for ( int i = 0 ; i < MAX_SONAR ; i++ )
    {
      sonar_ptr[i] = 0;
      buffer_ptr[i] = 0;
      for ( int j = 0 ; j < BUF_SIZE ; j++ )
	{
	  sonar_value[i][j] = 0;
	  sonar_buffer[i][j] = 0;
	}
    }
}

Sonar::~Sonar()
{
  delete lock;
}

int
Sonar::setSonar( int sonar_num, int sonar_val )
{
  if ( sonar_num <= MAX_SONAR )
    {
      sonar_val = (int)(sonar_val * RANGE_CONV);
      /*
      if ( sonar_num == 6 )
	cout << "New Sonar Value: " << sonar_val <<endl;            // DEBUG
	*/
      /*
       * First, check to see if the new sonar reading is within a certain
       * threshold of the current buffer values.
       *
       * If it is, then add it and clear the buffer. Otherwise place it
       * into the buffer. Check the buffer for how large it is. If the
       * buffer has similar values, copy the buffer over to the real
       * value, afterward clearing the buffer.
       */
      /*
      if ( sonar_num == 6 )
	{
	  int next_sonar_ptr = sonar_ptr[sonar_num];
	  cout << "Sonar Values: ";
	  for ( int i = 0 ; i < BUF_SIZE ; i ++ )
	    cout << sonar_value[sonar_num][(next_sonar_ptr + i )% BUF_SIZE] << "   ";
	  cout << endl;
	  cout << "Diff: " << getSonar( sonar_num ) << " - " << sonar_val
	       << " = " 
	       << ( getSonar( sonar_num ) - sonar_val ) << endl;  // DEBUG
	}
	*/
      if ( ( getSonar( sonar_num ) - sonar_val ) < THRESHOLD )
	{
	  lock->bdown();

	  // Add the sonar value to the buffer
	  int next = sonar_ptr[sonar_num];
	  sonar_value[sonar_num][next] = sonar_val;
	  sonar_ptr[sonar_num] = (next+1) % BUF_SIZE;

	  // clear the buffer
	  buffer_ptr[sonar_num] = 0;
	  for ( int i = 0 ; i < MAX_SONAR ; i++ )
	    sonar_buffer[sonar_num][i] = 0;

	  lock->up();

	}
      else
	{
	  // Add to the buffer

	  lock->bdown();

	  int buf_next = buffer_ptr[sonar_num];
	  sonar_buffer[sonar_num][buf_next] = sonar_val;
	  buffer_ptr[sonar_num] = (buf_next+1) % BUF_SIZE;

	  // now check to see if the buffer is large enough and
	  // similar enough to affect the data values

	  // either buf next or buf next + 1????
	  double deviation = stdev( sonar_buffer[sonar_num], buf_next+1 );
	  /*
	  if (sonar_num == 6)                    // DEBUG
	    {
	      cout << "Added " << sonar_val << " to Buffer" <<endl;
	      for (int i = 0; i < (buf_next+1); i++)
		cout << "Buffer " << i << "  " << sonar_buffer[sonar_num][i] 
		     << endl;
	      cout << "Stdev " << deviation <<endl;
	    }
	    */
	  if ( (deviation < DEV_THRESHOLD) || ((buf_next+1) == BUF_SIZE) )
	    {
	      for (int i = 0; i < (buf_next+1); i++)
		{
		   int next = sonar_ptr[sonar_num];
		   sonar_value[sonar_num][next] = sonar_buffer[sonar_num][i];
		   sonar_ptr[sonar_num] = (next+1) % BUF_SIZE;
		}

	      // clear the buffer
	      buffer_ptr[sonar_num] = 0;
	      for ( int i = 0 ; i < MAX_SONAR ; i++ )
		sonar_buffer[sonar_num][i] = 0;	      
	    }

	  lock->up();
	}
    }
  else
    return 0;
  return 1;
}

double
Sonar::getSonar( int sonar_num )
{
  int last = sonar_ptr[sonar_num] - 1;
  if (last == -1)
    last = BUF_SIZE-1;
  double reading = sonar_value[sonar_num][last];
  return reading;
}

double
Sonar::stdev( double* array, int size )
{
  double s = 0.0, s2 = 0.0;
  int count = size;

  while ( count-- )
    {
      s += *array;
      s2 += (*array) * (*array);
      array++;
    }
  return sqrt( (s2 - s*s/size)/size );
}