#include <iostream>

#ifndef PI
  #define PI 3.14159265358973
#endif

#define DEBUG 0

#define DELTA 0.01
#define OBSERVE 1.0
#define LIMIT 100.0

#define I_xx 1.9
#define I_yy 1.6
#define I_zz 2.0

#define NOISE_SYS sqrt(0.01)
#define NOISE_OBS sqrt(0.01)

#define P_INIT 0.1

#include "util.h"
#include "strategy.h"
#include "parameter.h"
#include "equation.h"

typedef double accuracy;

using namespace std;

int main(){ 
  
  //真値
  Vector3<accuracy> omega_true(0.1, 17.0 * (PI * 2 / 60) + 1.0, 0.0);
  Quarternion<accuracy> q_true(1.0, 0.0, 0.0, 0.0);
  
  //システム推定値
  Vector3<accuracy> omega_estimate(0.1 + rand_regularized(0, NOISE_SYS),
                                   17.0 * (PI * 2 / 60) + 1.0
                                        + rand_regularized(0, NOISE_SYS),
                                   0.0 + rand_regularized(0, NOISE_SYS));
  Quarternion<accuracy> q_estimate 
      = Quarternion<accuracy>(1.0 + rand_regularized(0, NOISE_SYS),
                              0.0 + rand_regularized(0, NOISE_SYS),
                              0.0 + rand_regularized(0, NOISE_SYS),
                              0.0 + rand_regularized(0, NOISE_SYS)).regularize();

  Vector3<accuracy> torque;
  Vector3<accuracy> productI(I_xx, I_yy, I_zz);
 
  EulerEoM<accuracy> euler;
  QuarternionEoM<accuracy> qEoM;
  
  //カルマンフィルターで用いる値
  Matrix<accuracy> P(7, 7);
  for(int i = 0; i < P.rows(); i++){P(i, i) = P_INIT;}
  Matrix<accuracy> Q(3, 3);
  for(int i = 0; i < Q.rows(); i++){Q(i, i) = pow(NOISE_SYS, 2);}
  Matrix<accuracy> R(3, 3);
  for(int i = 0; i < R.rows(); i++){R(i, i) = pow(NOISE_OBS, 2);}
  
  Vector3<accuracy> random;

#if !DEBUG  
  cout << "TIME(s)" << ","
       << "ω_x(rad/s)" << ","
       << "ω_x_estimate(rad/s)" << ","
       << "ω_y(rad/s)" << ","
       << "ω_y_estimate(rad/s)" << ","
       << "ω_z(rad/s)" << ","
       << "ω_z_estimate(rad/s)" << ","
       << "q_0" << ","
       << "q_0_estimate" << ","
       << "q_1" << ","
       << "q_1_estimate" << ","
       << "q_2" << ","
       << "q_2_estimate" << ","
       << "q_3" << ","
       << "q_3_estimate" << ","
       << "p" << endl;
#endif

  for(accuracy time = DELTA; time <= LIMIT; time += DELTA){
   
    //真値
    {
      qEoM.setOmega(omega_true);
      q_true = nextByRK4(qEoM, time, q_true, DELTA).regularize();
    
      //外乱トルクの作成
      for(int i = 0; i < 3; i++) random[i] = rand_regularized(0, NOISE_SYS);
      
      euler.set(torque + random, productI);
      omega_true = nextByRK4(euler, time, omega_true, DELTA);
    }
    
    //システム推定値
    {
      qEoM.setOmega(omega_estimate);
      q_estimate = nextByRK4(qEoM, time, q_estimate, DELTA).regularize();
      
      euler.set(torque, productI);
      omega_estimate = nextByRK4(euler, time, omega_estimate, DELTA);
    }

    //A
    Matrix<accuracy> A(7, 7);
    {
      A(0, 1) = -omega_estimate[0]/2;
      A(0, 2) = -omega_estimate[1]/2;
      A(0, 3) = -omega_estimate[2]/2;
      A(0, 4) = -q_estimate[1]/2;
      A(0, 5) = -q_estimate[2]/2;
      A(0, 6) = -q_estimate[3]/2;
      
      A(1, 0) =  omega_estimate[0]/2;
      A(1, 2) =  omega_estimate[2]/2;
      A(1, 3) = -omega_estimate[1]/2;
      A(1, 4) =  q_estimate[0]/2;
      A(1, 5) = -q_estimate[3]/2;
      A(1, 6) =  q_estimate[2]/2;
      
      A(2, 0) =  omega_estimate[1]/2;
      A(2, 1) = -omega_estimate[2]/2;
      A(2, 3) =  omega_estimate[0]/2;
      A(2, 4) =  q_estimate[3]/2;
      A(2, 5) =  q_estimate[0]/2;
      A(2, 6) = -q_estimate[1]/2;
      
      A(3, 0) =  omega_estimate[2]/2;
      A(3, 1) =  omega_estimate[1]/2;
      A(3, 2) = -omega_estimate[0]/2;
      A(3, 4) = -q_estimate[2]/2;
      A(3, 5) =  q_estimate[1]/2;
      A(3, 6) =  q_estimate[0]/2;
      
      A(4, 5) = (I_yy - I_zz) / I_xx * omega_estimate[2];
      A(4, 6) = (I_yy - I_zz) / I_xx * omega_estimate[1];
      
      A(5, 4) = (I_zz - I_xx) / I_yy * omega_estimate[2];
      A(5, 6) = (I_zz - I_xx) / I_yy * omega_estimate[0];
      
      A(6, 4) = (I_xx - I_yy) / I_zz * omega_estimate[1];
      A(6, 5) = (I_xx - I_yy) / I_zz * omega_estimate[0];
    }
      
    //B
    Matrix<accuracy> B(7, 3);
    {
      B(4, 0) = -1.0/I_xx;
      B(5, 1) = -1.0/I_yy;
      B(6, 2) = -1.0/I_zz;
    }
    
    //φ  
    Matrix<accuracy> Phi = Matrix<accuracy>::getI(A.rows()) + A * DELTA;  
#if DEBUG > 2
    cout << "Phi:" << Phi << endl;
#endif
    
    //Γ
    Matrix<accuracy> Gamma = B * DELTA;
#if DEBUG > 2
    cout << "Gamma:" << Gamma << endl;
#endif
      
    //M
    Matrix<accuracy> M = (Phi * P * Phi.transpose()) + (Gamma * Q * Gamma.transpose());
#if DEBUG > 1
    cout << "M:" << M << endl;
#endif
         
    //観測、修正を行うか
    if((int)(time / DELTA) % (int)(OBSERVE / DELTA) != 0){
     
      //P更新
      P = M;
    }else{
      
      //DCMベクトル
      int dcm_index = rand() % 3;
      Matrix<accuracy> dcm_true = q_true.getDCM().columnVector(dcm_index);
      Matrix<accuracy> dcm_estimate = q_estimate.getDCM().columnVector(dcm_index);
      
      //z、観測ノイズをいれる
      Matrix<accuracy> z(3, 1);
      for(int i = 0; i < 3; i++){z(i, 0) = dcm_estimate(i, 0) - (dcm_true(i, 0) + rand_regularized(0, NOISE_OBS));}
#if DEBUG
      cout << "z:" << z << endl;
#endif
      
      //H
      Matrix<accuracy> H(3, 7);
      {
        switch(dcm_index){
          case 0:
            H(0, 0) = q_estimate[0];  H(0, 1) = q_estimate[1];  H(0, 2) =-q_estimate[2];  H(0, 3) =-q_estimate[3];
            H(1, 0) =-q_estimate[3];  H(1, 1) = q_estimate[2];  H(1, 2) = q_estimate[1];  H(1, 3) =-q_estimate[0];
            H(2, 0) = q_estimate[2];  H(2, 1) = q_estimate[3];  H(2, 2) = q_estimate[0];  H(2, 3) = q_estimate[1];
            break;
          case 1:
            H(0, 0) = q_estimate[3];  H(0, 1) = q_estimate[2];  H(0, 2) = q_estimate[1];  H(0, 3) = q_estimate[0];
            H(1, 0) = q_estimate[0];  H(1, 1) =-q_estimate[1];  H(1, 2) = q_estimate[2];  H(1, 3) =-q_estimate[3];
            H(2, 0) =-q_estimate[1];  H(2, 1) =-q_estimate[0];  H(2, 2) = q_estimate[3];  H(2, 3) = q_estimate[2];
            break;
          case 2:
            H(0, 0) =-q_estimate[2];  H(0, 1) = q_estimate[3];  H(0, 2) =-q_estimate[0];  H(0, 3) = q_estimate[1];
            H(1, 0) = q_estimate[1];  H(1, 1) = q_estimate[0];  H(1, 2) = q_estimate[3];  H(1, 3) = q_estimate[2];
            H(2, 0) = q_estimate[0];  H(2, 1) =-q_estimate[1];  H(2, 2) =-q_estimate[2];  H(2, 3) = q_estimate[3];
            break;
        }
        H *= 2;
      }
#if DEBUG > 1
      cout << "H(" << dcm_index << "):" << H << endl;
#endif
      
      //P更新
      //P = (M.inverse() + H.transpose() * R.inverse() * H).inverse();
      P = M - M * H.transpose() * (H * M * H.transpose() + R).inverse() * H * M;
#if DEBUG
      cout << "P:" << P << endl;
#endif
      
      //K
      Matrix<accuracy> K = P * H.transpose() * R.inverse();
#if DEBUG > 1
      cout << "K:" << K << endl;
#endif
      
      //修正
      Matrix<accuracy> x_hat = K * z;
      for(int i = 0; i < 4; i++){q_estimate[i] -= x_hat(i, 0);}
      for(int i = 0; i < 3; i++){omega_estimate[i] -= x_hat(i + 4, 0);}
#if DEBUG
      cout << "x_hat:" << x_hat << endl;
#endif
    }

#if !DEBUG
    //対角成分計算
    accuracy p(0);
    for(int i = 0; i < P.rows(); i++){p += pow(P(i, i), 2);}
    p = pow(p, 0.5);

    //出力
    cout << time << ","
         << omega_true[0] << ","
         << omega_estimate[0] << ","
         << omega_true[1] << ","
         << omega_estimate[1] << ","
         << omega_true[2] << ","
         << omega_estimate[2] << ","
         << q_true[0] << ","
         << q_estimate[0] << ","
         << q_true[1] << ","
         << q_estimate[1] << ","
         << q_true[2] << ","
         << q_estimate[2] << ","
         << q_true[3] << ","
         << q_estimate[3] << ","
         << p << endl;     
#endif

#if DEBUG
    //出力2
    cout << time << ","
         << q_true[0] - q_estimate[0] << ","
         << q_true[1] - q_estimate[1] << ","
         << q_true[2] - q_estimate[2] << ","
         << q_true[3] - q_estimate[3] << ","
         << omega_true.getX() - omega_estimate.getX() << ","
         << omega_true.getY() - omega_estimate.getY() << ","
         << omega_true.getZ() - omega_estimate.getZ() << endl;
#endif
  }
  return 0;
}