/*
 * 1自由度でのシミュレーション
 * 
 */
#include <iostream>
#include <strstream>
#include <math.h>
#include <time.h>

using namespace std;

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

#include "sensor/MEMS.h"

typedef double accuracy;

#define DUMP_INTERVAL 20   /* ダンプタイミング */

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

#define CORRECT_SIGMA 5    // 角度補正ノイズ[deg]

#define DELTA_T (1. / FREQUENCY)

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

int main(){
  
  MEMS_Gyro gyro;
  
  accuracy gyro_true, gyro_model1, gyro_model2;
  accuracy theta_true(0), 
           theta_model1(0),
           theta_model1_KF1(0),
           theta_model2(0),
           theta_model2_KF2(0),
           theta_model2_KF3(0), theta_model2_bias(0);
         
  // フィルターの調整
  Matrix<accuracy> P1(1, 1);
  {
    P1(0, 0) = 1E-2;
  }
  Matrix<accuracy> Q1(1, 1);
  {
    Q1(0, 0) = pow(pow(DELTA_T, gyro.WN_SLOPE) * gyro.WN_INTERCEPT / gyro.SF, 2);
  }
  
  Matrix<accuracy> P2(1, 1);
  {
    P2(0, 0) = 1E-2;
  }
  Matrix<accuracy> Q2(1, 1);
  {
    Q2(0, 0) = pow(pow(DELTA_T, gyro.WN_SLOPE) * gyro.WN_INTERCEPT / gyro.SF, 2);
  }
  Matrix<accuracy> P3(2, 2);
  {
    P3(0, 0) = 1E-2;
    P3(1, 1) = 1E-2;
  }
  Matrix<accuracy> Q3(2, 2);
  {
    Q3(0, 0) = pow(pow(DELTA_T, gyro.WN_SLOPE) * gyro.WN_INTERCEPT / gyro.SF, 2);
    Q3(1, 1) = pow(gyro.ROOT_N / gyro.SF * sqrt(DELTA_T), 2);
  }
  
  KalmanFilterUD<accuracy> KF1(P1, Q1), KF2(P2, Q2), KF3(P3, Q3);
  KF1.init();
  KF2.init();
  KF3.init();
  
  cout << "t[sec],θ(true),θ(model1),θ(model1_KF1),θ(model2),θ(model2_KF2),θ(model2_KF3),ω(true),ω(model1),ω(model2),ω(bias),ω(bias_KF3)" << endl;
  
  char buffer[2048];
  strstream strout(buffer, sizeof(buffer), strstream::out);
  
  for(int step = 1; step <= FREQUENCY * SIM_LIMIT; step++){
   
    // センサの値で積分
    {
      gyro_true = 0;
      gyro_model1 = gyro_true + gyro.pure_WN(DELTA_T);
      gyro_model2 = gyro_model1 + gyro.pure_RD(DELTA_T);
    
      theta_true += gyro_true * DELTA_T;
      theta_model1 += gyro_model1 * DELTA_T;
      theta_model1_KF1 += gyro_model1 * DELTA_T;
      theta_model2 += gyro_model2 * DELTA_T;
      theta_model2_KF2 += gyro_model2 * DELTA_T;
      theta_model2_KF3 += (gyro_model2 - theta_model2_bias) * DELTA_T; 
      theta_model2_bias *= (1. - gyro.BETA * DELTA_T);
    }
    
    // Kalman Filterの修正
    {
      Matrix<accuracy> Phi12(1, 1);
      {
        Phi12(0, 0) = 1.;
      }
      Matrix<accuracy> Gamma12(1, 1);
      {
        Gamma12(0, 0) = DELTA_T;
      }
      KF1.predict(Phi12, Gamma12);
      KF2.predict(Phi12, Gamma12);
      
      Matrix<accuracy> Phi3(2, 2);
      {
        Phi3(0, 0) = 1.;
        Phi3(0, 1) = -DELTA_T;
        Phi3(1, 1) = 1. - gyro.BETA * DELTA_T;
      }
      Matrix<accuracy> Gamma3(2, 2);
      {
        Gamma3(0, 0) = DELTA_T;
        Gamma3(1, 1) = 1.;
      }
      KF3.predict(Phi3, Gamma3);
    }
    
    // 補正を使うかどうか
    if(step % (FREQUENCY * CORRECT_DELTA) == 0){
      
      accuracy observe_noise = rand_regularized(0, deg2rad(CORRECT_SIGMA));
      
      {
        Matrix<accuracy> z1(1, 1);
        Matrix<accuracy> z2(1, 1);
        z1(0, 0) = theta_model1_KF1 - theta_true + observe_noise;
        z2(0, 0) = theta_model2_KF2 - theta_true + observe_noise;
      
        Matrix<accuracy> H12(1, 1);
        Matrix<accuracy> R12(1, 1);
        H12(0, 0) = 1.;
        R12(0, 0) = pow(deg2rad(CORRECT_SIGMA), 2);
      
        Matrix<accuracy> K1(KF1.correct(H12, R12));
        Matrix<accuracy> x_hat1(K1 * z1);
        theta_model1_KF1 -= x_hat1(0, 0);
      
        Matrix<accuracy> K2(KF2.correct(H12, R12));
        Matrix<accuracy> x_hat2(K2 * z2);
        theta_model2_KF2 -= x_hat2(0, 0);
      }
      
      {
        Matrix<accuracy> H3(1, 2);
        Matrix<accuracy> R3(1, 1);
        H3(0, 0) = 1.;
        R3(0, 0) = pow(deg2rad(CORRECT_SIGMA), 2);
      
        Matrix<accuracy> z3(1, 1);
        z3(0, 0) = theta_model2_KF3 - theta_true + observe_noise;
      
        Matrix<accuracy> K3(KF3.correct(H3, R3));
        Matrix<accuracy> x_hat3(K3 * z3);
        theta_model2_KF3 -= x_hat3(0, 0);
        
        theta_model2_bias -= x_hat3(1, 0);
      }
    }
    
    strout << DELTA_T * step << ","
           << rad2deg(theta_true) << ","
           << rad2deg(theta_model1) << ","
           << rad2deg(theta_model1_KF1) << ","
           << rad2deg(theta_model2) << ","
           << rad2deg(theta_model2_KF2) << ","
           << rad2deg(theta_model2_KF3) << ","
           << gyro_true << ","
           << gyro_model1 << ","
           << gyro_model2 << ","
           << gyro_model2 - gyro_model1 << ","
           << theta_model2_bias
           << endl << ends;
    
    if(step % DUMP_INTERVAL == 0){cout << buffer;}
    strout.seekp(0);
  }
  
  return 0;
}