#include <iostream>

#include "util/util.h"

#define SAMPLE 10

using namespace std;

// Pressure Sensor Class
class Psens{
private:
	double sigma;
public:
  // Constructor, set standard deviation
	Psens(double s = 0.0){ sigma = s; }; 
  // Measurement, return true value + noise
  double measure(double true_p){ return true_p + rand_regularized(0,sigma); };
  void set_sigma(double s){ sigma = s; };
};


// 3 Orifice pitot tube class
class Pitot_Tube{
private:
  Psens psens[3];                   // Pressure sensors
	double C_v, C_theta, C_23, rho;   // Pitot Tube Parameters
  double angle_offset;               // Pitot_tube offset angle
	double aspd, ang;				          // Measured Values incl. noise
public:
	Pitot_Tube(double Cv, double Ctheta, double C23, double adens, double offset, double s1, double s2, double s3);
	void measure( double true_aspd, double true_angle);
  double airspeed(void){ return aspd;};
  double angle(void){ return ang; };
};


// Constructor, set parameters
Pitot_Tube::Pitot_Tube(double Cv, double Ctheta, double C23, double adens, double offset, double s1, double s2, double s3){

  C_v = Cv;
  C_theta = Ctheta;
  C_23 = C23;
  rho = adens;
  angle_offset = offset;
  (psens[0]).set_sigma(s1);
  (psens[1]).set_sigma(s2);
  (psens[2]).set_sigma(s3);
}

// Measurement
void Pitot_Tube::measure( double true_aspd, double true_angle){

  double true_pressure[3];
  double measured_pressure[3];

  // Calculate true pressure values using pitot tube parameters
  true_pressure[0] = rho*(C_v+C_23)/2*pow(true_aspd,2);
  true_pressure[1] = true_pressure[0]*(1. + C_theta * true_angle/2. );
  true_pressure[2] = true_pressure[0]*(1. - C_theta * true_angle/2. );
//  cout << true_pressure[0] << " , " << true_pressure[1] << " , " << true_pressure[2] << endl;

  // Obtain measured pressure
  for(int i=0; i<3; i++){
    measured_pressure[i] = (psens[i]).measure(true_pressure[i]);
  }

  // Calculate airspeed and wind direction from measured values
  if(measured_pressure[0]<0) aspd = 0.0;
  else aspd = sqrt(2. * measured_pressure[0]/(rho * (C_v+C_23)));
  ang = 1/C_theta * (measured_pressure[1]-measured_pressure[2])/measured_pressure[0] + angle_offset;
}


int main(void){
 
	
	Pitot_Tube pt(0.551, 0.0511, 0.444, 1.23, -7.0, 8.08, 2.49, 4.74);

  for(int i=0; i<10; i++){
    pt.measure(15., 2.);
    cout << pt.airspeed() << "," << pt.angle() << endl;
  }
	return 0;
}