/* 縦の運動のシミュレータ */

#include <iostream>
#include <iomanip>
#include <fstream>

#include <cstdlib>
#include <cmath>
#include <cstring>

#define PITOT_ERROR_MODEL 0

#include "../RPE_KF/longitudinal.h"
#include "pitot.h"
#include "misc.h"
#include "util/CSV_reader.h"
#include "util/util.h"

using namespace std;

typedef double float_t;

int main(int argc, char *argv[]){

  cout << setprecision(10);
  cerr << setprecision(10);

  bool sim_error(true);

  if(argc < 2){
    cerr << "Usage: " << argv[0] << " input.csv [0 = error_free]" << endl;
    return -1;
  }else if(argc >= 3){
    if(strcmp(argv[2], "0") == 0){
      sim_error = false;
    }
  }

  fstream input_fin(argv[1], ios::in);
  if(input_fin.fail()){
    cerr << "Log file not found : " << argv[1] << endl;
    return -1;
  }

  CSV_Reader reader(input_fin);

  // 入力をスラスト[N]に変換
  ControlInput<float_t> raw2dt(4077, 0, 2.980562274, 0, 10);
  // 入力をエレベータ舵角[rad]に変換
  //ControlInput<float_t> raw2de(3868, 2387, 3047, deg2rad(-28.1), deg2rad(25.9), deg2rad(0.5));
  ControlInput<float_t> raw2de(3047, 0.00025, 0); // SFはrad

  
  typedef LongitudinalDynamics<float_t> dynamics_t;
  dynamics_t dynamics;
  typedef LongitudinalObservationEquation<float_t> obs_t;
  obs_t obs(dynamics);
  typedef dynamics_t::input_t input_t;
  typedef obs_t::observed_t observed_t;

#define set_coef(symbol, value) dynamics.coef_ ## symbol() = value
#define set_sta(symbol, value) dynamics.sta_ ## symbol() = value

  // ビジネスジェットの有次元安定微係数(安定軸)を入れる
  // 080806_watarase/bj_config.xls の U_0 = 17m/s より
  set_coef(Xu,  -0.351378075);
  set_coef(Xa,   2.263153404);
  set_coef(Xdt,  0);
  set_coef(Zu,  -1.154117647);
  set_coef(Za,  -135.0578111);
  set_coef(Zq,  -0.649906636399308);
  set_coef(Zde, -8.811050943);
  set_coef(Zdt, 0);
  set_coef(Mu,  0);
  set_coef(Maa, 0);//-4.248653103);
  set_coef(Ma,  -42.16313555);
  set_coef(Mq,  -8.082003388);
  set_coef(Mde, -109.5710362);
  set_coef(Mdt, 0);

  // 定常状態(安定軸)の設定
  set_sta(W0, 0);
  set_sta(theta0, 0);
  set_sta(U0, 17.0);

  // 状態量生成
  dynamics_t::states_t states;
  float_t previous_t(0);

#define set_state(symbol, value) states.symbol() = value

  // 初期値設定(開始時はトリム状態)
  set_state(u, 0);
  set_state(a, 0);
  set_state(theta, 0);
  set_state(q, 0);
  float_t trim_de(0), trim_dt(0);

  // ピトー管
#if PITOT_ERROR_MODEL == 0
  //Pitot_Tube pitot(0.551, 0.0511, 0.444, 1.23, -7.0, 0.7, 0.7, 0.0547, 0.0153, 0.033); // オフセット5deg
  Pitot_Tube pitot(0.551, 0.0511, 0.444, 1.23, 0.0, 0.0547, 0.0153, 0.033); // オフセットなし
#else
  //Pitot_Tube pitot(0.551, 0.0511, 0.444, 1.23, -7.0, 0.7, 0.7, 8.08, 2.49, 4.74);
  Pitot_Tube pitot(0.551, 0.0511, 0.444, 1.23, 0.0, 8.08, 2.49, 4.74);
#endif

  // INS/GPS関連のノイズジェネレータ
  PitchNoiseGenerator<float_t> noise_gen_pitch;
  QNoiseGenerator<float_t> noise_gen_q;

  for(unsigned loop(0); reader.seek(); ++loop){
    CSV_Reader::item_t item(reader.next());
    /*for(CSV_Reader::item_t::iterator it(item.begin());
        it != item.end();
        ++it){
      cout << *it << endl;
    }*/

    float_t t(atof(item[1].c_str()));
    unsigned dt_raw(atoi(item[8].c_str())), de_raw(atoi(item[6].c_str()));
    float_t dt(raw2dt.deflection(dt_raw)),  // スロットル => I列
            de(raw2de.deflection(de_raw));  // エレベータ => G列

    //cerr << de_raw << ", " << dt_raw << endl;
    
    dynamics.t = t;
    dynamics.delta_t = t - previous_t;
    
    // 制御入力
    input_t input;
    input.dt() = dt - trim_dt;
    input.de() = de - trim_de;

    if(loop > 0){
      cerr << t << ", " << input.dt() << ", " << input.de() << endl;
      states = dynamics(states, input);
    }else{
      trim_dt = raw2dt.deflection_init(dt_raw);
      trim_de = raw2de.deflection_init(de_raw);
    }

    // システム同定向けの状態量を生成
    // 出力は全て機体固定軸に変換されることに注意
    // Z 04 TASCG[M/S]   真大気速度(C.G.)  => U/W_{0}, u/w から生成
    // Z 05 ALFCG[RAD]   迎角(C.G.)        => alpha
    // Z 06 THE[RAD]     ピッチ(Theta)     => theta0 + theta
    // Z 07 Q[RAD/S]     ピッチ角速度      => q
    // Z 08 QDOT[RAD/S2] ピッチ角加速度    => qから運動方程式で生成
    // Z 09 AXCG[M/S2]   加速度X(C.G.)     => uから運動方程式で生成
    // Z 10 AZCG[M/S2]   加速度Z(C.G.)     => dot{w} = U_{0} * dot{alpha}
    obs.input = &input;
    observed_t observed(obs(states));
    
    // ここで観測誤差を加える
    if(sim_error){
      // まずはピトー管
      Pitot_Tube::values_t true_v;
      true_v.aspd = observed.tascg();
      true_v.ang = rad2deg(observed.alphacg());
      Pitot_Tube::values_t v(pitot.measure(true_v));//, rad2deg(states(dynamics_t::q, 0)));
      observed.tascg() = v.aspd;
      observed.alphacg() = deg2rad(v.ang);

      // GPS/INS系のエラー
      observed.theta() += deg2rad(noise_gen_pitch.next());
      observed.q() += deg2rad(noise_gen_q.next());
      //observed.qdot();
      observed.axcg() += rand_regularized(0, 0.1); //0.005);// STMicro 50 [ug/sqrt{Hz}] * 100 Hz
      observed.azcg() += rand_regularized(0, 0.1); //0.005);// STMicro 50 [ug/sqrt{Hz}] * 100 Hz
    }else{
      observed.alphacg() += (deg2rad(pitot.offset()));
    }

    cout << t << "," 
        << input.de() << ","
        << observed.join(",") << endl;
    previous_t = t;
  }

#undef set_state

#undef set_coef
#undef set_sta

  return 0;
}