/*
 * INS vs INS_GPS
 * (センサモデル1,2)
 * 
 * 水平定常旋回飛行でシミュレーション
 * 
 * 初期状態は北方向に一定速度で進んでいる状態
 * 新しいフィルタプログラムで
 */
 
#include <iostream>
#include <strstream>
#include <math.h>
#include <time.h>

#if _MSC_VER >= 1400
#define sprintf sprintf_s
#endif

using namespace std;

#include "util/util.h"
#include "param/vector3.h"
#include "algorithm/kalman.h"

#include "INS_GPS2.h"

#include "sensor/MEMS.h"

typedef double accuracy;
#define DUMP_TARGET 9
const char *label[DUMP_TARGET] = {"東経[秒]", "北緯[秒]", "高度[m]", "北方向速度[m/s]", "東方向速度[m/s]", "重力方向速度[m/s]", "Ψ[deg]", "Θ[deg]", "Φ[deg]"};

#define DUMP_ALL 0
#define DUMP_INTERVAL 10  /* ダンプタイミング */

#define FREQUENCY 50      /* 更新周期[Hz] */
#define SIM_LIMIT 60 * 10 /* シミュレーション時間[sec] */
#define GPS_DELTA 1       /* GPS補正タイミング[sec] */

#define ROTATE_RADIUS 100  /* 旋回半径[m] */
#define ROTATE_PERIOD 120  /* 旋回周期[sec] */
#define ROTATE_OMEGA  (PI * 2 / ROTATE_PERIOD)        /* 旋回角速度 */
#define ROTATE_SPEED  (ROTATE_OMEGA * ROTATE_RADIUS)  /* 旋回速度 */
#define ROTATE_ACCEL  (ROTATE_OMEGA * ROTATE_SPEED)   /* 旋回加速度 */

#define INIT_LATITUDE   (35.  + 42. / 60 + 50.818 / 3600)    //初期北緯[rad]
#define INIT_LONGITUDE  (139. + 45. / 60 + 48.670 / 3600)    //初期東経[rad]
#define INIT_HEIGHT     0                                    //初期高度[m]


//#define GPS_Vd

#define GPS_V_NORM_SIGMA        1     // GPS速さに対する偏差[m]
#define GPS_V_DIRECTION_SIGMA   5     // GPS速度方向偏差[°]
#define GPS_LAT_SIGMA           20    // GPS緯度方向偏差[m]
#define GPS_LONG_SIGMA          20    // GPS経度方向偏差[m]
#define GPS_HEIGHT_SIGMA        30    // GPS高さ方向偏差[m]

string pretty_deg2(double degree){
  char buffer[128];
  (degree -= (int)degree) *= (60 * 60);
  sprintf(buffer, "%f", degree);
  return string(buffer);
}

int main(){
 
  rand_init(0);
  
  INS<accuracy> ins_true;         //真値用INS
  INS<accuracy> ins_sim1, ins_sim2;              //誤差いり、INSのみ
  //INS_GPS2<accuracy, KalmanFilter<accuracy> > ins_gps_sim1, ins_gps_sim2;  //誤差いり、INS_GPS2
  INS_GPS2<accuracy> ins_gps_sim1, ins_gps_sim2;  //誤差いり、INS_GPS2
  
  // 初期値
  ins_true.initPosition(deg2rad(INIT_LATITUDE),
                        deg2rad(INIT_LONGITUDE),
                        INIT_HEIGHT);
  accuracy INIT_SIM_LATITUDE(ins_true.latitude() + rand_regularized(0, ins_true.meter2lat(GPS_LAT_SIGMA))),
           INIT_SIM_LONGITUDE(ins_true.longitude() + rand_regularized(0, ins_true.meter2long(GPS_LONG_SIGMA))),
           INIT_SIM_HEIGHT(ins_true.height() + rand_regularized(0, GPS_HEIGHT_SIGMA));
  ins_sim1.initPosition(INIT_SIM_LATITUDE, INIT_SIM_LONGITUDE, INIT_SIM_HEIGHT);
  ins_gps_sim1.initPosition(INIT_SIM_LATITUDE, INIT_SIM_LONGITUDE, INIT_SIM_HEIGHT);
  ins_sim2.initPosition(INIT_SIM_LATITUDE, INIT_SIM_LONGITUDE, INIT_SIM_HEIGHT);
  ins_gps_sim2.initPosition(INIT_SIM_LATITUDE, INIT_SIM_LONGITUDE, INIT_SIM_HEIGHT);
  
  ins_true.initVelocity(ROTATE_SPEED, 0, 0);
  ins_sim1.initVelocity(ins_true.v_north(), ins_true.v_east(), ins_true.v_down());
  ins_gps_sim1.initVelocity(ins_true.v_north(), ins_true.v_east(), ins_true.v_down());
  ins_sim2.initVelocity(ins_true.v_north(), ins_true.v_east(), ins_true.v_down());
  ins_gps_sim2.initVelocity(ins_true.v_north(), ins_true.v_east(), ins_true.v_down());
  
  ins_true.initAttitude(0, 0, -atan(ROTATE_ACCEL / Earth::gravity(ins_true.latitude())));
  ins_sim1.initAttitude(0, 0, -atan(ROTATE_ACCEL / Earth::gravity(ins_true.latitude())));
  ins_gps_sim1.initAttitude(0, 0, -atan(ROTATE_ACCEL / Earth::gravity(ins_true.latitude())));
  ins_sim2.initAttitude(0, 0, -atan(ROTATE_ACCEL / Earth::gravity(ins_true.latitude())));
  ins_gps_sim2.initAttitude(0, 0, -atan(ROTATE_ACCEL / Earth::gravity(ins_true.latitude())));
  
  // センサーモデル
  MEMS_Accelerometer mems_a_X, mems_a_Y, mems_a_Z;
  MEMS_Gyro mems_g_X, mems_g_Y, mems_g_Z;
  
  // フィルターの調整
  Matrix<accuracy> P1(ins_gps_sim1.getFilter().getP());
  {
    P1(0, 0) = P1(1, 1) = P1(2, 2) = 1E+1;
    P1(3, 3) = P1(4, 4) = P1(5, 5) = 1E-8;
    P1(6, 6) = 1E+2;
    P1(7, 7) = P1(8, 8) = P1(9, 9) = 1E-5;
  }
  Matrix<accuracy> Q1(ins_gps_sim1.getFilter().getQ());    
  {
    Q1(0, 0) = pow(pow(1. / FREQUENCY, mems_a_X.WN_SLOPE) * mems_a_X.WN_INTERCEPT / mems_a_X.SF, 2);
    Q1(1, 1) = pow(pow(1. / FREQUENCY, mems_a_Y.WN_SLOPE) * mems_a_Y.WN_INTERCEPT / mems_a_Y.SF, 2);
    Q1(2, 2) = pow(pow(1. / FREQUENCY, mems_a_Z.WN_SLOPE) * mems_a_Z.WN_INTERCEPT / mems_a_Z.SF, 2);
    Q1(3, 3) = pow(pow(1. / FREQUENCY, mems_g_X.WN_SLOPE) * mems_g_X.WN_INTERCEPT / mems_g_X.SF, 2);
    Q1(4, 4) = pow(pow(1. / FREQUENCY, mems_g_Y.WN_SLOPE) * mems_g_Y.WN_INTERCEPT / mems_g_Y.SF, 2);
    Q1(5, 5) = pow(pow(1. / FREQUENCY, mems_g_Z.WN_SLOPE) * mems_g_Z.WN_INTERCEPT / mems_g_Z.SF, 2);
    Q1(6, 6) = 1E-14;
  }
  //Q1 *= 2;
  Matrix<accuracy> P2(ins_gps_sim2.getFilter().getP());
  {
    P2(0, 0) = P2(1, 1) = P2(2, 2) = 1E+1;
    P2(3, 3) = P2(4, 4) = P2(5, 5) = 1E-8;
    P2(6, 6) = 1E+2;
    P2(7, 7) = P2(8, 8) = P2(9, 9) = 1E-4;
  }
  Matrix<accuracy> Q2(ins_gps_sim2.getFilter().getQ());    
  {
    Q2(0, 0) = pow(pow(1. / FREQUENCY, mems_a_X.WN_SLOPE) * mems_a_X.WN_INTERCEPT / mems_a_X.SF, 2);
    Q2(1, 1) = pow(pow(1. / FREQUENCY, mems_a_Y.WN_SLOPE) * mems_a_Y.WN_INTERCEPT / mems_a_Y.SF, 2);
    Q2(2, 2) = pow(pow(1. / FREQUENCY, mems_a_Z.WN_SLOPE) * mems_a_Z.WN_INTERCEPT / mems_a_Z.SF, 2);
    Q2(3, 3) = pow(pow(1. / FREQUENCY, mems_g_X.WN_SLOPE) * mems_g_X.WN_INTERCEPT / mems_g_X.SF, 2);
    Q2(4, 4) = pow(pow(1. / FREQUENCY, mems_g_Y.WN_SLOPE) * mems_g_Y.WN_INTERCEPT / mems_g_Y.SF, 2);
    Q2(5, 5) = pow(pow(1. / FREQUENCY, mems_g_Z.WN_SLOPE) * mems_g_Z.WN_INTERCEPT / mems_g_Z.SF, 2);
    Q2(6, 6) = 1E-14;
  }
  //Q2 *= 2;
  ins_gps_sim1.getFilter().init();
  ins_gps_sim2.getFilter().init();
  
  char buffer[2048];
  strstream strout(buffer, sizeof(buffer), strstream::out);

  char *target_label[] = {"真値", "INSのみ(モデル1)", "INS/GPS(モデル1)", "INSのみ(モデル2)", "INS/GPS(モデル2)"};
  INS<accuracy> *target[] = {&ins_true, &ins_sim1, &ins_gps_sim1, &ins_sim2, &ins_gps_sim2};

  // ラベル
  cout << "[SEC]" << "\t";
  for(unsigned i = 0; i < sizeof(target) / sizeof(INS<accuracy> *); i++){
    for(int j = 0; j < DUMP_TARGET; j++) cout << label[j] << "(" << target_label[i] << ')' << "\t"; 
  }
  
  cout << "GPS東経" << "\t"
       << "GPS北緯" << "\t"
       << "GPS高度" << "\t"
       << "GPS速度" << "\t"
       << "GPS真方位" << "\t";
  
  cout << "加速度(真値)[X^b]\t" << "加速度計[X^b]\t" 
       << "加速度(真値)[Y^b]\t" << "加速度計[Y^b]\t" 
       << "加速度(真値)[Z^b]\t" << "加速度計[Z^b]\t"
       << "角速度(真値)[ω_X^b]\t" << "ジャイロ1[ω_X^b]\t" << "ジャイロ2[ω_X^b]\t"
       << "角速度(真値)[ω_Y^b]\t" << "ジャイロ1[ω_Y^b]\t" << "ジャイロ2[ω_Y^b]\t"
       << "角速度(真値)[ω_Z^b]\t" << "ジャイロ1[ω_Z^b]\t" << "ジャイロ2[ω_Z^b]\t";
#if DUMP_ALL
  for(int i = 0; i < 3; i++){
    for(unsigned j = 0; j < INS<accuracy>::STATE_VALUES; j++){
      cout << "状態量[" << j << "]" << "(" << target_label[i] << ')' << "\t"; 
    }
  }
#endif
  cout << endl;
  
  long t = (long)time(NULL);    //実行時間測定用
  
  accuracy theta = 0;   //旋回軌道上の位置、角度であらわすことにする
  
  for(int step = 1; step <= FREQUENCY * SIM_LIMIT; step++){

    //旋回軌道上の位置の計算
    theta += (ROTATE_OMEGA / FREQUENCY);
  
    //加速度の生成
    Vector3<accuracy> accel_true;
    {
     
      // g-frameで加速度を生成
#define pow2(x) ((x) * (x))
      accuracy earth_rotate_a(ins_true.beta() * pow2(Earth::Omega_Earth));
      Vector3<accuracy> accel_g(
                            -ROTATE_ACCEL * sin(theta)
                              + earth_rotate_a * sin(ins_true.latitude()),
                            -ROTATE_ACCEL * cos(theta),
                            -Earth::gravity(ins_true.latitude())
                              + earth_rotate_a * cos(ins_true.latitude())
                          );
                          //- pow(ROTATE_SPEED, 2) / Earth::R_e
#undef pow2
    
      // n-frameに変換
      Vector3<accuracy> accel_n(accel_g[0] * cos(ins_true.azimuth()) 
                                  + accel_g[1] * sin(ins_true.azimuth()),
                                -accel_g[0] * sin(ins_true.azimuth()) 
                                  + accel_g[1] * cos(ins_true.azimuth()),
                                accel_g[2]);
    
    
      // b-frameに変換
      accel_true = (ins_true.n2b().conj() * accel_n * ins_true.n2b()).vector();
    }
    
    //角速度の生成
    Vector3<accuracy> gyro_true;
    {
      // g-frameで角速度を生成
      Vector3<accuracy> gyro_g(
                            Earth::Omega_Earth * cos(ins_true.latitude()),
                            0,
                            -Earth::Omega_Earth * sin(ins_true.latitude())
                              - ROTATE_OMEGA
                          );
      
      // n-frameに変換
      Vector3<accuracy> gyro_n(gyro_g[0] * cos(ins_true.azimuth()) 
                                  + gyro_g[1] * sin(ins_true.azimuth()),
                               -gyro_g[0] * sin(ins_true.azimuth()) 
                                  + gyro_g[1] * cos(ins_true.azimuth()),
                               gyro_g[2]);
      
      // b-frameに変換
      gyro_true = (ins_true.n2b().conj() * gyro_n * ins_true.n2b()).vector();
     
    }
    
    // 真値の更新
    ins_true.update(accel_true, gyro_true, 1. / FREQUENCY);
    
    // モデルに従って、誤差の混入
    Vector3<accuracy> accel(mems_a_X.convert(accel_true[0], 1. / FREQUENCY), 
                            mems_a_Y.convert(accel_true[1], 1. / FREQUENCY),
                            mems_a_Z.convert(accel_true[2], 1. / FREQUENCY));
    Vector3<accuracy> gyro1(mems_g_X.pure_WN(1. / FREQUENCY) + gyro_true[0],
                            mems_g_Y.pure_WN(1. / FREQUENCY) + gyro_true[1],
                            mems_g_Z.pure_WN(1. / FREQUENCY) + gyro_true[2]);
    /*Vector3<accuracy> gyro1(gyro_true[0],
                            gyro_true[1],
                            gyro_true[2]);*/
    Vector3<accuracy> gyro2(mems_g_X.pure_RD(1. / FREQUENCY) + gyro1[0],
                            mems_g_Y.pure_RD(1. / FREQUENCY) + gyro1[1],
                            mems_g_Z.pure_RD(1. / FREQUENCY) + gyro1[2]);
                            
    // シミュレーションモデルの更新
    ins_sim1.update(accel, gyro1, 1. / FREQUENCY);      // INSのみ(モデル1)
    ins_gps_sim1.update(accel, gyro1, 1. / FREQUENCY);  // INS_GPS2(モデル1)
    ins_sim2.update(accel, gyro2, 1. / FREQUENCY);      // INSのみ(モデル2)
    ins_gps_sim2.update(accel, gyro2, 1. / FREQUENCY);  // INS_GPS2(モデル2)
    
    
    // 状態の出力
    strout << (1. / FREQUENCY) * step << "\t";
    for(unsigned i = 0; i < sizeof(target) / sizeof(INS<accuracy> *); i++){
      strout << pretty_deg2(rad2deg(target[i]->longitude())) << "\t"
             << pretty_deg2(rad2deg(target[i]->latitude())) << "\t"
             << target[i]->height() << "\t" 
             << target[i]->v_north() << "\t"
             << target[i]->v_east() << "\t"
             << target[i]->v_down() << "\t"
             << rad2deg(target[i]->euler_psi()) << "\t" 
             << rad2deg(target[i]->euler_theta()) << "\t" 
             << rad2deg(target[i]->euler_phi()) << "\t";
    }
         
    // GPS補正を使うかどうか
    if(step % (FREQUENCY * GPS_DELTA) == 0){

      //for(unsigned i = 0; i < INS<accuracy>::STATE_VALUES; i++) cout << "[" << i << "]" << ins_true[i] - ins_gps_sim[i] << endl;
      
      // GPS用真値を求める
#define pow2(x) ((x) * (x))     
      accuracy v = sqrt(pow2(ins_true.v_north()) + pow2(ins_true.v_east()));
      accuracy v_heading(acos(ins_true.v_north() / v));
      accuracy v_d(ins_true.v_down());
      if(ins_true.v_east() < 0){v_heading = -v_heading;}
      accuracy latitude(ins_true.latitude());
      accuracy longitude(ins_true.longitude());
      accuracy height(ins_true.height());
      
      // 誤差混入、GPS出力値の生成
      v += rand_regularized(0, GPS_V_NORM_SIGMA);
      v_heading += rand_regularized(0, deg2rad(GPS_V_DIRECTION_SIGMA));
      v_d += rand_regularized(0, GPS_V_NORM_SIGMA);
      if(_abs(v_heading) > PI){v_heading = (v_heading > 0 ? -v_heading : v_heading) + 2 * PI;}
      latitude += rand_regularized(0, ins_true.meter2lat(GPS_LAT_SIGMA));
      longitude += rand_regularized(0, ins_true.meter2long(GPS_LONG_SIGMA));
      height += rand_regularized(0, GPS_HEIGHT_SIGMA);
#undef pow2

#ifdef GPS_Vd
      GPS_NMEA_SIM_3D_with_Vd<accuracy> gps_packet;
#else
      GPS_NMEA_SIM_3D<accuracy> gps_packet;
#endif
      {
        gps_packet.v = v;
        gps_packet.v_heading = v_heading;
#ifdef GPS_Vd
        gps_packet.v_down = v_d;
#endif
        gps_packet.v_norm_sigma = GPS_V_NORM_SIGMA;
        gps_packet.v_direction_sigma = GPS_V_DIRECTION_SIGMA;
        gps_packet.latitude = latitude;
        gps_packet.longitude = longitude;
        gps_packet.height = height;
        gps_packet.long2D_sigma = GPS_LAT_SIGMA;
        gps_packet.short2D_sigma = GPS_LONG_SIGMA;
        gps_packet.height_sigma = GPS_HEIGHT_SIGMA;
      }
      
      // GPSデータ書き出し
      strout << pretty_deg2(rad2deg(longitude)) << "\t"
             << pretty_deg2(rad2deg(latitude)) << "\t"
             << height << "\t"
             << v << "\t"
             << rad2deg(v_heading) << "\t";
      
      // 修正
      ins_gps_sim1.correct(gps_packet);
      ins_gps_sim2.correct(gps_packet);

      //for(unsigned i = 0; i < INS<accuracy>::STATE_VALUES; i++) cout << "[" << i << "]" << ins_true[i] - ins_gps_sim[i] << endl;
    }else{strout << "\t" << "\t" << "\t" << "\t" << "\t";}

#if DUMP_ALL
    //加速度、ジャイロの値
    for(unsigned i = 0; i < Vector3<accuracy>::OUT_OF_INDEX; i++){
      strout << accel_true[i] << "\t" << accel[i] << "\t";
    }
    for(unsigned i = 0; i < Vector3<accuracy>::OUT_OF_INDEX; i++){
      strout << gyro_true[i] << "\t" << gyro1[i] "\t" << gyro2[i] << "\t";
    }
    strout << ins_true << "\t" << ins_sim << "\t" << ins_gps_sim << "\t";
#endif

    strout << endl << ends;
    
    if(step % DUMP_INTERVAL == 0){cout << buffer;}
    strout.seekp(0);
  }
  
  t = (long)time(NULL) - t;
  
  //データ書き出し
  
  cout << "INS/GPSシミュレーション" << endl;
  
  cout << "[日時] " << (long)time(NULL) << endl;
  
  cout << "[条件]" << endl
       << "初期位置: " << "φ=" << pretty_deg(INIT_LATITUDE) 
                      << "; λ=" << pretty_deg(INIT_LONGITUDE) 
                      << "; h=" << INIT_HEIGHT << endl
       << "旋回半径: " << ROTATE_RADIUS << "[m]" << endl
       << "旋回周期: " << ROTATE_PERIOD << "[sec]" << endl
       << "旋回角速度: " << ROTATE_OMEGA << "[1/rad]" << endl
       << "旋回速度: " << ROTATE_SPEED <<"[m/s]" << endl
       << "積分周波数" << FREQUENCY << "[Hz]" << endl
       << "加速度計モデル" << TO_STRING(ACCELEROMETER_MODEL) << endl
       << "ジャイロモデル" << TO_STRING(GYRO_MODEL) << endl
       << "GPS補正タイミング" << GPS_DELTA  << "[sec]" << endl
       << "GPS速さに対する偏差" << GPS_V_NORM_SIGMA << "[m]" << endl
       << "GPS速度方向偏差" << GPS_V_DIRECTION_SIGMA << "[°]" << endl
       << "GPS緯度方向偏差" << GPS_LAT_SIGMA << "[m]" << endl
       << "GPS経度方向偏差" << GPS_LONG_SIGMA << "[m]" << endl
       << "GPS高さ方向偏差" << GPS_HEIGHT_SIGMA << "[m]" << endl;
  
  cout << "[実行時間] " << t << "[sec]" << endl;
  
  cout << "[最終位置]" << endl 
       << "<<真値>>     "
       << "φ=" << pretty_deg(rad2deg(ins_true.latitude()))
       << ": λ=" << pretty_deg(rad2deg(ins_true.longitude()))
       << ": h=" << ins_true.height() << endl
       << "<<INSのみ(モデル1)>>  "
       << "φ=" << pretty_deg(rad2deg(ins_sim1.latitude()))
       << "; λ=" << pretty_deg(rad2deg(ins_sim1.longitude()))
       << "; h=" << ins_sim1.height() << endl
       << "<<INS/GPS(モデル1)>>  "
       << "φ=" << pretty_deg(rad2deg(ins_gps_sim1.latitude()))
       << "; λ=" << pretty_deg(rad2deg(ins_gps_sim1.longitude()))
       << "; h=" << ins_gps_sim1.height() << endl
       << "<<INSのみ(モデル2)>>  "
       << "φ=" << pretty_deg(rad2deg(ins_sim2.latitude()))
       << "; λ=" << pretty_deg(rad2deg(ins_sim2.longitude()))
       << "; h=" << ins_sim2.height() << endl
       << "<<INS/GPS(モデル2)>>  "
       << "φ=" << pretty_deg(rad2deg(ins_gps_sim2.latitude()))
       << "; λ=" << pretty_deg(rad2deg(ins_gps_sim2.longitude()))
       << "; h=" << ins_gps_sim2.height() << endl;
  
  cout << ins_gps_sim1.getFilter().getP() << endl;
  cout << ins_gps_sim2.getFilter().getP() << endl;
  
  return 0;
}