#ifndef __PITOT_H__
#define __PITOT_H__

/**
 * ピトー管のエラーモデル
 * 
 */

#include "util/util.h"

#ifndef PITOT_ERROR_MODEL
#define PITOT_ERROR_MODEL 0
#endif

// 3 Orifice pitot tube class
class Pitot_Tube{
  protected:
    
    // Pressure Sensor Class
    class Psens{
      protected:
        double _sigma;
      public:
        // Measurement, return true value + noise
        double measure(double true_p) const {
#if PITOT_ERROR_MODEL == 0
          return true_p * (1. + rand_regularized(0, _sigma));
#else
          return true_p + rand_regularized(0, _sigma);
#endif
        }
        // Constructor, set standard deviation
        Psens(double s = 0.0) : _sigma(s){}
        double &sigma(){return _sigma;}
    };
  
  protected:
    Psens psens[3];                   // Pressure sensors
    double C_v, C_theta, C_23, rho;  // Pitot Tube Parameters
    double angle_offset;             // Pitot_tube offset angle
    double q_coef, dist_cg;          // Effect of pitch rate and distance from C.G.
  public:
    double &offset() {return angle_offset;}
    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), 
          q_coef(0), dist_cg(0) {
      psens[0].sigma() = s1;
      psens[1].sigma() = s2;
      psens[2].sigma() = s3;
    }
    Pitot_Tube(
        double Cv, double Ctheta, double C23, 
        double adens, double offset,
        double c_q, double d_cg, 
        double s1, double s2, double s3)
        : C_v(Cv), C_theta(Ctheta), C_23(C23), rho(adens), angle_offset(offset), 
          q_coef(c_q), dist_cg(d_cg) {
      psens[0].sigma() = s1;
      psens[1].sigma() = s2;
      psens[2].sigma() = s3;
    }
    struct values_t{
      double aspd, ang; 
    };
    /**
     * ピッチ角速度の影響あり版
     */
    values_t measure(const values_t &true_v,
        const double &pitch_rate) const {
      double true_pressure[3];
      double measured_pressure[3];
      
      double measured_angle(
          true_v.ang + angle_offset 
          - rad2deg(atan(deg2rad(pitch_rate) * dist_cg / true_v.aspd)) * q_coef);

      // Calculate true pressure values using pitot tube parameters
      true_pressure[0] = rho * (C_v + C_23) / 2 * pow(true_v.aspd, 2);
      true_pressure[1] = true_pressure[0] * (1. + C_theta * measured_angle / 2);
      true_pressure[2] = true_pressure[0] * (1. - C_theta * measured_angle / 2);
      // std::cerr << 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
      values_t res;
      res.aspd = (measured_pressure[0] < 0) 
          ? 0 
          : sqrt(2. * measured_pressure[0] / (rho * (C_v + C_23)));
      res.ang = 1.0 / C_theta 
          * (measured_pressure[1] - measured_pressure[2]) / measured_pressure[0];
      return res;
    }
    /**
     * ピッチ角速度の影響なし版
     */
    values_t measure(const values_t &true_v) const {
      return measure(true_v, 0);
    }
};

#endif __PITOT_H__