/*
 * Adaptive Kalman Filter‚ΜƒeƒXƒg
 * Q„’θ‚ΖR„’θ‚πŽŽ‚έ‚ι
 * 
 */
#include <iostream>
#include <iomanip>
#include <strstream>
#include <exception>
#include <deque>
#include <math.h>
#include <time.h>

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

using namespace std;

typedef double accuracy;

#include "param/vector3.h"
#include "param/matrix.h"
#include "param/quarternion.h"
#include "util/util.h"

#include "algorithm/kalman.h"

#ifndef pow2
  #define pow2(x) ((x)*(x))
#endif

#define DEBUG

/*
 * d/dt [ x ] = [ 0  1 ] [ x ] + [ 0 ] [ a ]
 *      [ v ]   [ 0  0 ] [ v ]   [ 1 ]
 * i.e.
 * 
 * [ x ]       = [ 1  t ] [ x ]     + [ t^{2}/2 ] [ a ]
 * [ v ]_{k+1}   [ 0  1 ] [ v ]_{k}   [ t       ]
 * 
 * P_{k+1} = A P_{k} A^{T} + B Q B^{T}
 * 
 * ŠΟ‘ͺ‚Νx‚Μ‚έ
 * [ x_{observe} ] = [ 1  0 ] [ x ]  + v
 *                            [ v ]
 * 
 * x_{k+1} = A x_{k} + B u 
 * x_{k+2} = A x_{k+1} + B u
 *         = A (A x_{k} + B u) + B u
 *         = A^{2} x_{k} + (A + I) B u
 * x_{k+3} = A x_{k+2} + B u
 *         = A (A^{2} x_{k} + (A + I) B u) + B u
 *         = A^{3} x_{k} + (A^{2} + A + I) B u
 *  ....
 * x_{k+n} = A^{n} x_{k} + (\sigma_{j = 0}^{n-1} A^{j}) B u
 * 
 * 
 * P_{k+1} = A P_{k} A^{T} + B Q B^{T}
 * P_{k+2} = A P_{k+1} A^{T} + B Q B^{T}
 *         = A (A P_{k} A^{T} + B Q B^{T}) A^{T} + B Q B^{T}
 *         = A^{2} P_{k} A^{T}^{2} + (A + I) B Q B^{T} (I + A^{T})
 * P_{k+3} = A P_{k+1} A^{T} + B Q B^{T}
 *         = A (A^{2} P_{k} A^{T}^{2} + (A + I) B Q B^{T} (I + A^{T})) A^{T} + B Q B^{T}
 *         = A^{3} P_{k} A^{T}^{3} + (A^{2} + A + I) B Q B^{T} (I + A^{T} + A^{T}^{2})
 *  ....
 * P_{k+n} = A^{n} P_{k} A^{T}^{n} + (\sigma_{j = 0}^{n-1} A^{j}) B Q B^{T} (\sigma_{j = 0}^{n-1} A^{T}^{j})
 * 
 * A^{2} = [ 1  t ] [ 1  t ] = [ 1  2t ]
 *         [ 0  1 ] [ 0  1 ]   [ 0   1 ]
 * A^{3} = [ 1  t ] [ 1  2t ] = [ 1  3t ]
 *         [ 0  1 ] [ 0   1 ]   [ 0   1 ]
 *  ....
 * A^{n} = [ 1  nt ]
 *         [ 0   1 ]
 * 
 * \sigma_{j = 0}^{n-1} A^{j} = \sigma_{j = 0}^{n-1} [ 1  jt ]
 *                                                   [ 0   1 ]
 *      = [ n  t*n(n-1)/2 ]
 *        [ 0           n ]
 */

#define DELTA_T 0.01
#define MAX_T 30.
#define PERIOD_T 2.
#define AMP 5. 

#define UPDATE_PERIOD 0.25
#define UPDATE_STEP (int)(UPDATE_PERIOD / DELTA_T)

#define ACCEL_MU -5.0 //-1.0
#define ACCEL_SIGMA 1.0
#define OBSERVE_SIGMA 0.1

#define EST_INIT_ERROR_0 OBSERVE_SIGMA
#define EST_INIT_ERROR_1 1.

#define EST_WINDOWS 20
#define Q_EST_WINDOWS EST_WINDOWS
#define R_EST_WINDOWS EST_WINDOWS
#define MU_EST_WINDOWS EST_WINDOWS

#define CORRECT_MU

int main(){
 
  rand_init(0);
  
  accuracy k(2. * PI / PERIOD_T);
 
  // ^’l
  Matrix<accuracy> x(2, 1); // [ x  v ]
  {
    x(0, 0) = 0.; // x = 0; 
    x(1, 0) = AMP / k;
  }
  
  // „’θ’l
  Matrix<accuracy> x_est(2, 1); // [ x  v ]
  {
    x_est(0, 0) = x(0, 0) + rand_regularized(0., EST_INIT_ERROR_0);
    x_est(1, 0) = x(1, 0) + rand_regularized(0., EST_INIT_ERROR_1);
  }
  
  Matrix<accuracy> A(2, 2);
  {
    A(0, 0) = 1.;
    A(0, 1) = DELTA_T;
    A(1, 0) = 0.;
    A(1, 1) = 1.;
  }
  Matrix<accuracy> B(2, 1);
  {
    B(0, 0) = pow2(DELTA_T) / 2; //0;
    B(1, 0) = DELTA_T;
  }
  
#ifdef CORRECT_MU
  Matrix<accuracy> mu(B.columns(), 1);
#endif
  
  /*
  Matrix<accuracy> A_n(2, 2);
  {
    A_n(0, 0) = 1.;
    A_n(0, 1) = DELTA_T * UPDATE_STEP;
    A_n(1, 0) = 0.;
    A_n(1, 1) = 1.;
  }
  Matrix<accuracy> Phi(A);
  cout << A_n << endl;
  Matrix<accuracy> A_n2(A.copy());
  {
    for(int i = 0; i < UPDATE_STEP - 1; i++){A_n2 *= A;}
  }
  cout << A_n2 << endl;
  
  Matrix<accuracy> A_sum(2, 2);
  {
    A_sum(0, 0) = UPDATE_STEP;
    A_sum(0, 1) = DELTA_T * UPDATE_STEP * (UPDATE_STEP - 1) / 2;
    A_sum(1, 0) = 0.;
    A_sum(1, 1) = UPDATE_STEP;
  }
  //cout << A_sum << endl;
  Matrix<accuracy> Gamma(A_sum * B);
  */
  
  // KF
  Matrix<accuracy> P_INIT(2, 2);
  {
    P_INIT(0, 0) = 1.;
    P_INIT(1, 1) = 1.;
  }
  Matrix<accuracy> Q(1, 1);
  {
    Q(0, 0) = pow2(ACCEL_SIGMA);
  }
  Matrix<accuracy> R(1, 1);
  {
    R(0, 0) = pow2(OBSERVE_SIGMA);
  }
  KalmanFilterUD<accuracy> filter(P_INIT, Q);
  filter.init();
  
  cout << "t,"
       << "u,u_noisy,mean(noise),sigma2(noise),"
       << "x0,x0_est,x0_upper,x0_lower,"
       << "x1,x1_est,x1_upper,x1_lower,"
       << "v,delta_x,delta_v,"
       << "R_est,Q_est00,Q_est01,Q_est11,"
       << "mu,"
       << endl;
       
  Matrix<accuracy> Phi(Matrix<accuracy>::getI(A.rows()));
  Matrix<accuracy> Gamma;
  
  Matrix<accuracy> A_sum(A.rows(), A.columns());
  
  accuracy t = 0.;
  for(int step = 1; t <= MAX_T; step++, t += DELTA_T){
    
    // Žžo—Ν
    cout << t << ",";
    
    // ‰Α‘¬“x‚ΝƒTƒCƒ“”g
    Matrix<accuracy> u(1, 1);
    {
      u(0, 0) = -sin(k * t) * AMP;
      cout << u(0, 0) << ",";
    }
    Matrix<accuracy> u_noisy(1, 1);
    {
      u_noisy(0, 0) = u(0, 0) + rand_regularized(ACCEL_MU, ACCEL_SIGMA);
      
#ifdef CORRECT_MU
      for(int i = 0; i < u.rows(); i++){
        u_noisy(i, 0) -= mu(i, 0);
      }
#endif
      
      cout << u_noisy(0, 0) << ",";
      static accuracy u_sum(0);
      static accuracy u2_sum(0);
      accuracy delta_u(u_noisy(0, 0) - u(0, 0));
      u_sum += delta_u;
      u2_sum += pow2(delta_u);
      accuracy u_mean(u_sum / step);
      accuracy u_sigma2(u2_sum / step - pow2(u_mean));
      cout << u_mean << "," << u_sigma2 << ",";
    }
    
    Phi *= A;
    A_sum = A * A_sum + Matrix<accuracy>::getI(A.rows());
      
    Gamma = A_sum * B;
    
    
    // ^’l
    {
      // Ο•ͺ
      x = A * x + B * u;
    }
    
    // „’θ’l
    {
      // Update
      {
        // Ο•ͺ
        x_est = A * x_est + B * u_noisy;
        filter.predict(A, B);
      }
      
      // o—Ν
      for(int i = 0; i < x.rows(); i++){
        cout << x(i, 0) << ",";
        cout << x_est(i, 0) << ",";
        cout << x_est(i, 0) + sqrt((filter.getP())(i, i)) << ",";
        cout << x_est(i, 0) - sqrt((filter.getP())(i, i)) << ",";
      }
            
      // Correct
      if(step % UPDATE_STEP == 0){
        Matrix<accuracy> z(1, 1);
        {
          z(0, 0) = x(0, 0) + rand_regularized(0., OBSERVE_SIGMA);
        }
       
        Matrix<accuracy> H(1, 2);
        {
          H(0, 0) = 1.;
          H(0, 1) = 0.;
        }
        
        Matrix<accuracy> P_before_correct(filter.getP().copy());
        
        Matrix<accuracy> K(filter.correct(H, R)); // ƒJƒ‹ƒ}ƒ“ƒQƒCƒ“
        //Matrix<accuracy> mu(1, 0);
        //mu(0, 0) = ACCEL_MU;
        Matrix<accuracy> v(z - H * x_est); // v
        Matrix<accuracy> delta_x_est(K * v); // C³—Κ
        x_est += delta_x_est;
        
        for(int i = 0; i < v.rows(); i++){
          cout << v(i, 0) << ",";
        }
        for(int i = 0; i < delta_x_est.rows(); i++){
          cout << delta_x_est(i, 0) << ",";
        }
        
        // R‚̐„’θ’l
        Matrix<accuracy> v_sigma2(v.rows(), v.rows());
        {
          typedef deque<Matrix<accuracy> > v_queue_t;
          static v_queue_t v_queue;
          
          v_queue.push_back(v.copy());
          if(v_queue.size() > R_EST_WINDOWS){v_queue.pop_front();}
          
          for(v_queue_t::iterator it = v_queue.begin();
                it < v_queue.end();
                it++){
            v_sigma2 += (*it) * (*it).transpose();
          }
          v_sigma2 /= v_queue.size();
          
          Matrix<accuracy> R_est(v_sigma2 - H * P_before_correct * H.transpose());
          for(int i = 0; i < R_est.rows(); i++){
            for(int j = i; j < R_est.columns(); j++){
              cout << R_est(i, j) << ",";  
            }
          }
        }
        
        // Q‚̐„’θ’l
        {
          typedef deque<Matrix<accuracy> > x_queue_t;
          static x_queue_t delta_x_queue;
          static Matrix<accuracy> P_previous(P_INIT);
          
          //cout << P_previous << "," << filter.getP() << endl;
          
          delta_x_queue.push_back(delta_x_est.copy());
          if(delta_x_queue.size() > Q_EST_WINDOWS){delta_x_queue.pop_front();}
          
          Matrix<accuracy> delta_x_sigma2(x.rows(), x.rows());
          for(x_queue_t::iterator it = delta_x_queue.begin();
                it < delta_x_queue.end();
                it++){
            delta_x_sigma2 += (*it) * (*it).transpose();
          }
          delta_x_sigma2 /= delta_x_queue.size();
          /*for(int i = 0; i < delta_x_sigma2.rows(); i++){
            cout << delta_x_sigma2(i, i) << ","; 
          }*/
          
          Matrix<accuracy> Q_est(delta_x_sigma2 + filter.getP() - Phi * P_previous * Phi.transpose());
          //Matrix<accuracy> Q_est(K * v_sigma2 * K.transpose());
          
          Q_est = A_sum.inverse() * Q_est * A_sum.transpose().inverse();
          
          //cout << "Q_est:" << Q_est;
          for(int i = 0; i < Q_est.rows(); i++){
            for(int j = i; j < Q_est.columns(); j++){
              cout << Q_est(i, j) / (B(i, 0) * B(j, 0)) << ",";
              //cout << Q_est(i, j) << ",";
            }
          }
          
          P_previous = filter.getP().copy();
        }
        
        // mu‚̐„’θ
        {
          static int prod_count = 0;
          static Matrix<accuracy> prod_sum(Gamma.rows(), Gamma.columns());
          static Matrix<accuracy> v_sum(H.rows(), 1);

          prod_sum 
            = Phi * (Matrix<accuracy>::getI(Phi.columns()) - K * H) * prod_sum
              + Gamma;
          
          v_sum += v;

          
          if(++prod_count % MU_EST_WINDOWS == 0){
            Matrix<accuracy> v_est(v_sum / prod_count);
            Matrix<accuracy> brace(H * prod_sum);
            
            Matrix<accuracy> 
                mu_est(
                    -(brace.transpose() * brace).inverse()
                      * brace.transpose()
                      * v_est
                  );

#ifdef CORRECT_MU
            mu += mu_est;
            x_est -= (Matrix<accuracy>::getI(Phi.columns()) - K * H) * prod_sum * mu_est;
            for(int i = 0; i < mu_est.rows(); i++){
              cout << mu(i, 0) << ",";
            }
            v_sum.clear();
            prod_count = 0;
#else
            for(int i = 0; i < mu_est.rows(); i++){
              cout << mu_est(i, 0) << ",";
            }
#endif
          }
        }
        
        Phi = Matrix<accuracy>::getI(Phi.rows());
        A_sum.clear();
      }
    }
    
    cout << endl;
  }
  
  return 0;
}