/*
 * カルマンフィルタのシミュレーションを行います。
 *  センサ(6)     機体固定座標の加速度計3軸、ジャイロ3軸
 *  refセンサ(3)  絶対座標のGPSの位置情報
 * を想定しています。
 * 
 * 状態量としては
 *  x(3)     絶対座標での位置
 *  v(3)     絶対座標での速度
 *  q(4)     機体の姿勢情報(Quarternionで与える)
 *  omega(3) 機体の角速度(ロール、ピッチ、ヨー)
 * の13個がパラメータとなります。
 * 
 * センサの積分はEuler法で行っています。
 * またカルマンフィルタは連続系を離散系に変換して行っています。
 * 
 * @author M.Naruoka
 * 
 * @see kalman.h Kalmanフィルタ
 * @see matrix.h 行列演算
 * @see parameter.h 3要素ベクトル、Quarternion
 * @see util.h ユーティリティ(正規乱数など)
 * @see mt19937ar.h 乱数発生機構、メルセンヌツイスタ(標準rand()では完全な乱数にならない)
 * 
 * 以下、今回は使用しない
 * @see fixedp.h 小数点演算の整数演算化(計算速度をあげるため、実機では搭載予定) 
 * @see strategy.h 運動方程式など
 * @see complex.h 複素数
 */

using namespace std;

#include <iostream>
#include <math.h>

//#include "fixedp.h"
#include "util.h"

typedef double accuracy;

/*
typedef FixedPoint<20> accuracy;

inline accuracy _pow(double x, double y){return (accuracy)pow(x, y);}
#define pow(x, y) _pow(x, y)

inline accuracy _rand_regularized(double x, double y){return (accuracy)rand_regularized(x, y);}
#define rand_regularized(x, y) _rand_regularized(x, y)
*/

#ifndef PI
  #define PI 3.14159265358973
#endif

#define DEBUG 1

#include "strategy.h"
#include "parameter.h"
#include "kalman.h"
#include "matrix.h"

//センサ観測時間間隔[sec]
#define DELTA (accuracy)(1.0/32)
//refセンサ観測タイミング
#define OBSERVE 32
//終了ステップ数
#define LIMIT 5000

//White Noise標準偏差設定
//加速度計[m/s^2]
#define ERR_ACC (accuracy)0.3
//ジャイロ[rad/sec]
#define ERR_GYRO (accuracy)0.05
//GPS[m]
#define ERR_GPS (accuracy)3.0

//カルマンフィルタで用いるP行列の初期値設定
#define P_INIT1 (accuracy)3.0
#define P_INIT2 (accuracy)0.3
#define P_INIT3 (accuracy)0.05

int main(){

  //真値
  Vector3<accuracy> x_true(0.0, 0.0, 0.0);          //[m]
  Vector3<accuracy> v_true(0.0, 0.0, 0.0);          //[m/s]
  Quarternion<accuracy> q_true(1.0, 0.0, 0.0, 0.0); //
  Vector3<accuracy> omega_true(0.0, 0.0, 0.0);      //[rad/s]
  
  //システム推定値
  Vector3<accuracy> x = x_true;         //[m]
  for(int i = 0; i < 3; i++){x[i] += rand_regularized(0, ERR_GPS);}
  Vector3<accuracy> v = v_true;         //[m/s]
  Quarternion<accuracy> q = q_true;     //
  Vector3<accuracy> omega = omega_true; //[rad/s]
  
  //センサの値
  Vector3<accuracy> acc_true, acc;    //[m/s^2]
  Vector3<accuracy> gyro_true, gyro;  //[rad/s]
  
  //カルマンフィルターで用いる値
  Matrix<accuracy> P(10, 10);
  for(unsigned int i = 0; i < 3; i++){P(i, i) = P_INIT1;}
  for(unsigned int i = 0; i < 3; i++){P(i + 3, i + 3) = P_INIT2;}
  for(unsigned int i = 0; i < 4; i++){P(i + 6, i + 6) = P_INIT3;}
  
  Matrix<accuracy> Q(6, 6);
  for(unsigned int i = 0; i < 3; i++){Q(i, i) = pow(ERR_ACC, 2);}
  for(unsigned int i = 0; i < 3; i++){Q(i + 3, i + 3) = pow(ERR_GYRO, 2);}
  
  Matrix<accuracy> R(3, 3);
  for(unsigned int i = 0; i < R.rows(); i++){R(i, i) = pow(ERR_GPS, 2);}
  
  KalmanFilter filter(P, Q);

#if !DEBUG  
  cout << "TIME(s)" << ","
       << "x_true(m)" << "," << "x(m)" << ","
       << "y_true(m)" << "," << "y(m)" << ","
       << "z_true(m)" << "," << "z(m)" << ","
       << "v_x_true(m/s)" << "," << "v_x(m/s)" << ","
       << "v_y_true(m/s)" << "," << "v_y(m/s)" << ","
       << "v_z_true(m/s)" << "," << "v_z(m/s)" << ","
       << "q_0_true" << "," << "q_0" << ","
       << "q_1_true" << "," << "q_1" << ","
       << "q_2_true" << "," << "q_2" << ","
       << "q_3_true" << "," << "q_3" << ","
       << "ω_x_true(rad/s)" << "," << "ω_x(rad/s)" << ","
       << "ω_y_true(rad/s)" << "," << "ω_y(rad/s)" << ","
       << "ω_z_true(rad/s)" << "," << "ω_z(rad/s)" << ","
       << "p" << endl;
#endif

  accuracy time = DELTA;
  
  for(int step = 1; step <= LIMIT; step++, time += DELTA){
   
    //真値
    {
      {
        acc_true[0] = 0;
        acc_true[1] = 0;
        acc_true[2] = 0;
      }
      {
        gyro_true[0] = 0;
        gyro_true[1] = 0;
        gyro_true[2] = 0;
      }
      
      //誤差混入
      {
        for(int i = 0; i < 3; i++){acc[i] = acc_true[i] + rand_regularized(0, ERR_ACC);}
        for(int i = 0; i < 3; i++){gyro[i] = gyro_true[i] + rand_regularized(0, ERR_GYRO);}
      }  
    }
    
    //A
    Matrix<accuracy> A(10, 10);
    {
      A(0, 3) = 1.0;
      A(1, 4) = 1.0;
      A(2, 5) = 1.0;
      
      A(3, 6) = 2 * ( q[0] * acc[0] + q[3] * acc[1] - q[2] * acc[2]);
      A(3, 7) = 2 * ( q[1] * acc[0] + q[2] * acc[1] + q[3] * acc[2]);
      A(3, 8) = 2 * (-q[2] * acc[0] + q[1] * acc[1] - q[0] * acc[2]);
      A(3, 9) = 2 * (-q[3] * acc[0] + q[0] * acc[1] - q[1] * acc[2]);
     
      A(4, 6) = 2 * (-q[3] * acc[0] + q[0] * acc[1] + q[1] * acc[2]);
      A(4, 7) = 2 * ( q[2] * acc[0] - q[1] * acc[1] + q[0] * acc[2]);
      A(4, 8) = 2 * ( q[1] * acc[0] + q[2] * acc[1] + q[3] * acc[2]);
      A(4, 9) = 2 * (-q[0] * acc[0] - q[3] * acc[1] + q[2] * acc[2]);
      
      A(5, 6) = 2 * (-q[2] * acc[0] - q[1] * acc[1] + q[0] * acc[2]);
      A(5, 7) = 2 * ( q[3] * acc[0] - q[0] * acc[1] - q[1] * acc[2]);
      A(5, 8) = 2 * (-q[0] * acc[0] - q[3] * acc[1] - q[2] * acc[2]);
      A(5, 9) = 2 * ( q[1] * acc[0] + q[2] * acc[1] + q[3] * acc[2]);
     
      A(6, 7)  = -omega[0]/2;
      A(6, 8)  = -omega[1]/2;
      A(6, 9)  = -omega[2]/2;
      
      A(7, 6)  =  omega[0]/2;
      A(7, 8)  =  omega[2]/2;
      A(7, 9)  = -omega[1]/2;
      
      A(8, 6)  =  omega[1]/2;
      A(8, 7)  = -omega[2]/2;
      A(8, 9)  =  omega[0]/2;
      
      A(9, 6)  =  omega[2]/2;
      A(9, 7)  =  omega[1]/2;
      A(9, 8)  = -omega[0]/2;
    }
    
    //B
    Matrix<accuracy> B(10, 6);
    {
      B(3, 0) = (accuracy)1.0 - (pow(q[2], 2) + pow(q[3], 2)) * 2;
      B(3, 1) = (q[1] * q[2] + q[0] * q[3]) * 2;
      B(3, 2) = (q[1] * q[3] - q[0] * q[2]) * 2;
      
      B(4, 0) = (q[1] * q[2] - q[0] * q[3]) * 2;
      B(4, 1) = (accuracy)1.0 - (pow(q[1], 2) + pow(q[3], 2)) * 2;
      B(4, 2) = (q[2] * q[3] + q[0] * q[1]) * 2;
      
      B(5, 0) = (q[1] * q[3] + q[0] * q[2]) * 2;
      B(5, 1) = (q[2] * q[3] - q[0] * q[1]) * 2;
      B(5, 2) = (accuracy)1.0 - (pow(q[1], 2) + pow(q[2], 2)) * 2;
      
      B(6, 3) = -q[1]/2;
      B(6, 4) = -q[2]/2;
      B(6, 5) = -q[3]/2;
      
      B(7, 3) =  q[0]/2;
      B(7, 4) = -q[3]/2;
      B(7, 5) =  q[2]/2;
      
      B(8, 3) =  q[3]/2;
      B(8, 4) =  q[0]/2;
      B(8, 5) = -q[1]/2;

      B(9, 3) = -q[2]/2;
      B(9, 4) =  q[1]/2;
      B(9, 5) =  q[0]/2;
    }
             
    //refセンサの観測、それによる修正を行うか
    if(step % OBSERVE == 0){
      
      //観測あり
      //z(Refサンセよりの情報)、観測ノイズをいれる
      Matrix<accuracy> z(3, 1);
      //GPSデータ
      for(int i = 0; i < 3; i++){z(i, 0) = (x[i] - x_true[i]) + accuracy(rand_regularized(0, ERR_GPS));}
#if DEBUG
      cout << "z:" << z << endl;
#endif
      
      //H
      Matrix<accuracy> H(3, 10);
      {
        H(0, 0) = 1.0;
        H(1, 1) = 1.0;
        H(2, 2) = 1.0;
      }
#if DEBUG > 1
      cout << "H:" << H << endl;
#endif

      //修正
      Matrix<accuracy> x_hat(filter.correct(H, R) * z);
      for(int i = 0; i < 3; i++){x[i] -= x_hat(i, 0);}
      for(int i = 0; i < 3; i++){v[i] -= x_hat(i + 3, 0);}
      for(int i = 0; i < 4; i++){q[i] -= x_hat(i + 6, 0);}
      q = q.regularize();
#if DEBUG
      cout << "x_hat:" << x_hat << endl;
#endif
    }
    
    //P更新
    filter.predict(A, B, DELTA);
    
    // 状態量積分
    {

      // 真値、Euler法で積分
      for(int i = 0; i < 3; i++){x_true[i] += v_true[i] * DELTA;}
      for(int i = 0; i < 3; i++){
        for(int j = 0; j < 3; j++){v_true[i] += (q_true.getDCM())(i, j) * acc_true[j] * DELTA;}
      }
      for(int i = 0; i < 3; i++){omega_true[i] = gyro_true[i];}
      {
        Quarternion<accuracy> q_true_delta(
          (-q_true[1] * omega_true[0] - q_true[2] * omega_true[1] - q_true[3] * omega_true[2]) / 2,
          ( q_true[0] * omega_true[0] - q_true[3] * omega_true[1] - q_true[2] * omega_true[2]) / 2,
          ( q_true[3] * omega_true[0] + q_true[0] * omega_true[1] - q_true[1] * omega_true[2]) / 2,
          (-q_true[2] * omega_true[0] + q_true[1] * omega_true[1] + q_true[0] * omega_true[2]) / 2
        );
        q_true = (q_true + (q_true_delta * DELTA)).regularize();
      }
    
      // システム推定値、Euler法で積分
      for(int i = 0; i < 3; i++){x[i] += v[i] * DELTA;}
      for(int i = 0; i < 3; i++){
        for(int j = 0; j < 3; j++){v[i] += (q.getDCM())(i, j) * acc[j] * DELTA;}
      }
      for(int i = 0; i < 3; i++){omega[i] = gyro[i];}
      {
        Quarternion<accuracy> q_delta(
          (-q[1] * omega[0] - q[2] * omega[1] - q[3] * omega[2]) / 2,
          ( q[0] * omega[0] - q[3] * omega[1] - q[2] * omega[2]) / 2,
          ( q[3] * omega[0] + q[0] * omega[1] - q[1] * omega[2]) / 2,
          (-q[2] * omega[0] + q[1] * omega[1] + q[0] * omega[2]) / 2
        );
        q = (q + (q_delta * DELTA)).regularize();
      }
    }

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

    //出力
    cout << time << ",";
    for(int i = 0; i < 3; i++){cout << x_true[i] << "," << x[i] << ",";}
    for(int i = 0; i < 3; i++){cout << v_true[i] << "," << v[i] << ",";}
    for(int i = 0; i < 4; i++){cout << q_true[i] << "," << q[i] << ",";}
    for(int i = 0; i < 3; i++){cout << omega_true[i] << "," << omega[i] << ",";}
    cout << sqrt(p) << endl;
#endif

#if DEBUG
    //出力2
    cout << time << ",";
    for(int i = 0; i < 3; i++){cout << x_true[i] - x[i] << ",";}
    for(int i = 0; i < 3; i++){cout << v_true[i] - v[i] << ",";}
    for(int i = 0; i < 4; i++){cout << q_true[i] - q[i] << ",";}
    for(int i = 0; i < 3; i++){cout << omega_true[i] - omega[i] << ",";}
    cout << endl;
#endif
  }
  return 0;
}