/**
 * 共分散行列(P)の後処理用プログラム
 * 
 */

#include <iostream>
#include <fstream>
#include <iomanip>
#include <string>
#include <exception>
#include <sstream>

#include <vector>

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

#include "util/util.h"
#include "util/logger.h"

using namespace std;
typedef double float_sylph_t;

#include "inspector.h"

typedef Matrix<float_sylph_t> matrix_t;
typedef Quarternion<float_sylph_t> quarternion_t;

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

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

class DummyNAV {
  protected:
    matrix_t &mat_P;
  public:
    DummyNAV(matrix_t &P) : mat_P(P){
      
    }
    ~DummyNAV(){}
    
    DummyNAV &getFilter(){return *this;}
    matrix_t &getP(){return mat_P;}
};

int main(int argc, char *argv[]){
  
  //Logger logger("p_log_mod.csv");
  Logger logger;
  logger.out() << setprecision(16);
  
  matrix_t P;
  DummyNAV nav(P);
  FilterP_Observer<DummyNAV> observer(nav);
  logger.add(observer);
  
  cerr << setprecision(10);
  cout << setprecision(10);
  
  cerr << "Usage: (exe) processed p_log" << endl;
  
  if(argc < 3){exit(-1);}
  
  cerr << "processed: " << argv[1] << endl;
  cerr << "p_log: " << argv[2] << endl;
  
  fstream fin_processed(argv[1], ios::in | ios::binary);    
  if(fin_processed.fail()){
    cerr << "Processed file not found : " << argv[1] << endl;
    return -1;
  }
  // 1行目は飛ばす
  {
    char c[16384];
    fin_processed.getline(c, sizeof(c) - 1);
  }
  
  fstream fin_p(argv[2], ios::in | ios::binary);    
  if(fin_p.fail()){
    cerr << "P_log file not found : " << argv[2] << endl;
    return -1;
  }
  
  bool do_read_processed(true), do_read_p(true);
  vector<float_sylph_t> v_processed, v_p;
  
  while(true){
    char c[16384];
    float_sylph_t f;
    
    // 処理済みファイルの読み込み
    /*
     *  0 itow
     *  1 longitude
     *  2 latitude
     *  3 height
     *  4 v_north
     *  5 v_east
     *  6 v_down
     *  7 Yaw(Ψ)
     *  8 Pitch(Θ)
     *  9 Roll(Φ)
     * 10 Azimuth(α)
     * 11 bias_accel(X)
     * 12 bias_accel(Y)
     * 13 bias_accel(Z)
     * 14 bias_gyro(X)
     * 15 bias_gyro(Y)
     * 16 bias_gyro(Z)
     * 
     */
    if(do_read_processed){
      v_processed.clear();
      fin_processed.getline(c, sizeof(c) - 1);
      if(fin_processed.fail()){break;}
      {
        stringstream ss(c);
        char label[16];
        ss >> label;  // はじめの項目はラベル
        while(true){
          ss >> f;
          if(ss.fail()){break;}
          v_processed.push_back(f);
        }
      }
    }
    
    // Pログの読み込み
    if(do_read_p){
      v_p.clear();
      fin_p.getline(c, sizeof(c) - 1);
      if(fin_p.fail()){break;}
      {
        stringstream ss(c);
        while(true){
          ss >> f;  // はじめの項目は時刻であることに注意
          if(ss.fail()){break;}
          v_p.push_back(f);
        }
      }
    }
    
    if(v_processed[0] != v_p[0]){  // 時刻チェック
      cerr << "Unsynchronized !! itow => " << v_processed[0]
           << ", " << v_p[0] << endl;
      if(v_processed[0] > v_p[0]){
        do_read_processed = false;
      }else{
        do_read_p = false;
      }
      continue;
    }
    
    do_read_processed = do_read_p = true;
    
    // q_n2bの復元
    float_sylph_t yaw(v_processed[7] - v_processed[10]),
            pitch(v_processed[8]),
            roll(v_processed[9]);
    
    quarternion_t q_n2b(
        cos(yaw / 2) * cos(pitch / 2) * cos(roll / 2)
          + sin(yaw / 2) * sin(pitch / 2) * sin(roll / 2),
        cos(yaw / 2) * cos(pitch / 2) * sin(roll / 2)
          - sin(yaw / 2) * sin(pitch / 2) * cos(roll / 2),
        cos(yaw / 2) * sin(pitch / 2) * cos(roll / 2)
          + sin(yaw / 2) * cos(pitch / 2) * sin(roll / 2),
        sin(yaw / 2) * cos(pitch / 2) * cos(roll / 2)
          - cos(yaw / 2) * sin(pitch / 2) * sin(roll / 2));
    
    unsigned rows((unsigned)sqrt((double)v_p.size()));
    
    // P行列の復元
    P = matrix_t(rows, rows);
    {
      unsigned i(0);
      for(vector<float_sylph_t>::iterator it(++(v_p.begin()));
          it != v_p.end();
          ++it, ++i){
        P(i / rows, i % rows) = *it;
      }
    }
    
    // misc.pdfの5章を参考に変換する
    // 式(109)
    matrix_t convert_L;
    {
      matrix_t convert_L1(3, 4);
      {
        convert_L1(0, 0) =  q_n2b[3];
        convert_L1(0, 1) =  q_n2b[2];
        convert_L1(0, 2) =  q_n2b[1];
        convert_L1(0, 3) =  q_n2b[0];
        
        convert_L1(1, 0) =  q_n2b[2];
        convert_L1(1, 1) = -q_n2b[3];
        convert_L1(1, 2) =  q_n2b[0];
        convert_L1(1, 3) = -q_n2b[1];
        
        convert_L1(2, 0) =  q_n2b[1];
        convert_L1(2, 1) =  q_n2b[0];
        convert_L1(2, 2) =  q_n2b[3];
        convert_L1(2, 3) =  q_n2b[2];
      }
      convert_L1 *= 2;
      
      // 式(109) 左2
      matrix_t convert_L2(4, 3);
      {
        convert_L2(0, 0) = -q_n2b[1];
        convert_L2(0, 1) = -q_n2b[2];
        convert_L2(0, 2) = -q_n2b[3];
        
        convert_L2(1, 0) =  q_n2b[0];
        convert_L2(1, 1) =  q_n2b[3];
        convert_L2(1, 2) = -q_n2b[2];
        
        convert_L2(2, 0) = -q_n2b[3];
        convert_L2(2, 1) =  q_n2b[0];
        convert_L2(2, 2) =  q_n2b[1];
        
        convert_L2(3, 0) =  q_n2b[2];
        convert_L2(3, 1) = -q_n2b[1];
        convert_L2(3, 2) =  q_n2b[0];
      }
      
      convert_L = convert_L1 * convert_L2;
      
      float_sylph_t denom_psi((pow2(q_n2b[0]) + pow2(q_n2b[1])) * 2 - 1);
      float_sylph_t denom_phi((pow2(q_n2b[0]) + pow2(q_n2b[3])) * 2 - 1);
      
      convert_L.partial(1, 3, 0, 0)
          /= (pow2(denom_psi) + pow2(q_n2b[1] * q_n2b[2] + q_n2b[0] * q_n2b[3]) * 4) 
            / (pow3(denom_psi));
      convert_L.partial(1, 3, 1, 0)
          /= cos(pitch);
      convert_L.partial(1, 3, 2, 0)
          /= (pow2(denom_phi) + pow2(q_n2b[2] * q_n2b[3] - q_n2b[0] * q_n2b[1]) * 4) 
            / (pow3(denom_phi));
    }
    matrix_t convert_R(convert_L.transpose());
    
    // P行列のインデックス(7,8,9)が姿勢に該当する部分
    //cerr << "before:" << P.partial(3, 3, 7, 7) << endl;
    // 行からせめる
    for(unsigned j(0); j < P.columns(); ++j){
      matrix_t target_vector(P.partial(3, 1, 7, j));
      matrix_t modified(convert_L * target_vector);
      //target_vector = modified;   // コンパイラのせい?
      for(unsigned i(0); i < target_vector.rows(); i++){
        target_vector(i, 0) = modified(i, 0);
      }
    }
    // 列もせめる
    for(unsigned i(0); i < P.rows(); ++i){
      matrix_t target_vector(P.partial(1, 3, i, 7));
      matrix_t modified(target_vector * convert_R);
      //target_vector = modified;   // コンパイラのせい?
      for(unsigned j(0); j < target_vector.columns(); j++){
        target_vector(0, j) = modified(0, j);
      }
    }
    //cerr << "after:" << P.partial(3, 3, 7, 7) << endl;
    
    // 出力
    observer.time_stamp() = v_processed[0];
    logger.flush();
    
    cerr << observer.time_stamp() << endl;
  }
  
  return 0;  
}