#ifndef __FILTERED_INS_ZC_H__
#define __FILTERED_INS_ZC_H__

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

template <
  class FloatT, 
  typename Filter = KalmanFilterUD<FloatT>,
  typename FINS = Filtered_INS<FloatT, Filter>
>
class Filtered_INS_ZeroCorrector : public FINS{
  protected:
    Vector3<FloatT> m_corrector_accel;
    Vector3<FloatT> m_corrector_gyro;
    
    Matrix<FloatT> m_Phi, m_Gamma;
  
  public:
    Filtered_INS_ZeroCorrector() 
      : FINS(), 
        m_corrector_accel(), m_corrector_gyro(),
        m_Phi(Matrix<FloatT>::getI(FINS::P_SIZE)), 
        m_Gamma(FINS::P_SIZE, FINS::Q_SIZE){}
    Filtered_INS_ZeroCorrector(const Matrix<FloatT> &P, const Matrix<FloatT> &Q) 
      : FINS(P, Q), 
        m_corrector_accel(), m_corrector_gyro(),
        m_Phi(Matrix<FloatT>::getI(FINS::P_SIZE)), 
        m_Gamma(FINS::P_SIZE, FINS::Q_SIZE){}
    
    /**
     * コピーコンストラクタ
     * 
     * @param orig コピー元
     * @param deepcopy ディープコピーを作成するかどうか
     */
    Filtered_INS_ZeroCorrector(
        const Filtered_INS_ZeroCorrector &orig, 
        const bool deepcopy = false) :
      FINS(orig, deepcopy),
      m_corrector_accel(deepcopy ? orig.m_corrector_accel.copy() : orig.m_corrector_accel),
      m_corrector_gyro(deepcopy ? orig.m_corrector_gyro.copy() : orig.m_corrector_gyro), 
      m_Phi(deepcopy ? orig.m_Phi.copy() : orig.m_Phi_gyro),
      m_Gamma(deepcopy ? orig.m_Gamma.copy() : orig.m_Gamma){
      
    }
    
    ~Filtered_INS_ZeroCorrector(){}
    
    Vector3<FloatT> &corrector_accel(){return m_corrector_accel;}
    Vector3<FloatT> &corrector_gyro(){return m_corrector_gyro;}
  
  protected:
    /**
     * 時間更新において後処理をするためのコールバック関数。
     * ゼロ点修正に必要な情報を集めます。
     * 
     * @param A A行列
     * @param B B行列
     * @patam deltaT 時間間隔
     */
    void before_update_INS(
        const Matrix<FloatT> &A, const Matrix<FloatT> &B, 
        const FloatT &deltaT
      ){
      
      Matrix<FloatT> delta_Phi(Matrix<FloatT>::getI(A.rows()) + A * deltaT);
      Matrix<FloatT> delta_Gamma(B * deltaT);
      
      m_Phi *= delta_Phi;
      m_Gamma = ((delta_Phi *= m_Gamma) += delta_Gamma);
    }

#define MU_EST_WINDOWS 10
//#define CORRECT_ZERO
#define DUMP_ZERO
    /**
     * 修正において後処理をするためのコールバック関数。
     * 適切なタイミングでゼロ点修正、ならびに修正前の影響除去を行います。
     * 
     * @param H H行列
     * @param R R行列
     * @patam K K行列、カルマンゲイン
     * @param v (z - H x)ここではEKFでxがゼロであるためzに等しい
     * @param x_hat 修正量
     */
    void before_correct_INS(
        const Matrix<FloatT> &H,
        const Matrix<FloatT> &R,
        const Matrix<FloatT> &K,
        const Matrix<FloatT> &v,
        Matrix<FloatT> &x_hat
      ){
      static int prod_count = 0;
      static Matrix<accuracy> prod_sum(FINS::P_SIZE, FINS::Q_SIZE);
      static Matrix<accuracy> v_sum(H.rows(), 1);
  
      /*
      prod_sum 
        = ((m_Phi * (Matrix<accuracy>::getI(FINS::P_SIZE) - K * H) *= prod_sum)
          += m_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_ZERO
        for(int i = 0; i < 3; i++){m_corrector_accel[i] += mu_est(i, 0);}
        for(int i = 0; i < 3; i++){m_corrector_gyro[i] += mu_est(i + 3, 0);}
        Matrix<FloatT> 
          delta_x_est(
              ((Matrix<accuracy>::getI(m_Phi.columns()) -= (K * H)) 
                *= prod_sum)
                *= mu_est
            );
        x_hat -= delta_x_est;
        
        v_sum.clear();
        prod_count = 0;
#endif
        
#ifdef DUMP_ZERO
        cerr << "v_est :\t" << v_est << endl;
        cerr << "Accel :\t" << m_corrector_accel << endl;
        cerr << "Gyro :\t" << m_corrector_gyro << endl;
#endif
      }
      */
      
      static int loop = 0;
      cerr << loop++ << ",";
      for(int i = 0; i < v.rows(); i++){
        cerr << const_cast<Matrix<accuracy> *>(&v)->operator()(i, 0) << ",";  
      }
      cerr << endl; 
            
      m_Phi = Matrix<accuracy>::getI(m_Phi.rows());
      m_Gamma.clear();
    }
  
  public:
    /**
     * 時間更新
     * 
     * @param accel 加速度計の値
     * @param gyro ジャイロの値
     * @param deltaT 時間間隔
     */
    void update(const Vector3<FloatT> &accel, const Vector3<FloatT> &gyro, const FloatT &deltaT){
      
      // ゼロ点の修正
      Vector3<FloatT> _accel(accel - m_corrector_accel);
      Vector3<FloatT> _gyro(gyro - m_corrector_gyro);
      
      FINS::update(_accel, _gyro, deltaT);
    }
};

#endif /* __FILTERED_INS_ZC_H__ */