#ifndef SONAR_HH
#define SONAR_HH

#include "Semaphore.hh"
#include <math.h>

#ifndef MAX_SONAR
#define MAX_SONAR 7
#endif

#ifndef AVG_SIZE
#define AVG_SIZE 8
#endif

#ifndef BUF_SIZE
#define BUF_SIZE 8
#endif

#ifndef THRESHOLD
#define THRESHOLD 25
#endif

#ifndef DEV_THRESHOLD     // The standard deviation threshold for the buffer
#define DEV_THRESHOLD 20
#endif

#ifndef RANGE_CONV
#define RANGE_CONV 0.1734
#endif

/*
 * Sonar information
 *
 *  SONAR     ANGLE
 * Sonar-0     90
 * Sonar-1     15
 * Sonar-2    7.5
 * Sonar-3     -0
 * Sonar-4   -7.5
 * Sonar-5    -15
 * Sonar-6    -90
 */

class Sonar
{
public:
  Sonar();
  ~Sonar();

  int setSonar( int sonar_num, int sonar_value );
  double getSonar( int sonar_num );

private:
  double stdev( double* array, int size );
  Semaphore *lock;

  double sonar_value[MAX_SONAR][BUF_SIZE]; // storage array 
  int sonar_ptr[MAX_SONAR];                // pointer to next spot

  double sonar_buffer[MAX_SONAR][BUF_SIZE];
  int buffer_ptr[MAX_SONAR];
};


#endif // SONAR_HH