/** @file
 * @brief 横の運動からパラメータ推定を行う
 *
 */

#include <iostream>
#include <fstream>
#include <sstream>
#include <iomanip>
#include <string>
#include <exception>
#include <cstring>
#include <vector>
#include <map>
#include <algorithm>

#include "si_common.h"

#include "algorithm/integral.h"
#include "algorithm/kalman.h"

#include "util/util.h"
#include "util/CSV_reader.h"

#include "base.h"
#include "lateral.h"
#include "../sim/misc.h"

#include "../PE_EquationError/common_options.h"

using namespace std;

typedef double float_t;

int main(int argc, char *argv[]){

  cout << setprecision(8);
  cerr << setprecision(8);

  if(argc < 2){
    cerr << "Usage: " << argv[0] << " input.csv [options]" << endl;
    return -1;
  }

  fstream input_fin(argv[1], ios::in);
  istream *input_in(NULL);
  if(strcmp(argv[1], "-") == 0){
    input_in = &cin;
  }else{
    if(input_fin.fail()){
      cerr << "Log file not found : " << argv[1] << endl;
      return -1;
    }
    input_in = &input_fin;
  }

  CSV_Reader reader(*input_in);

  // option check...
  cerr << "Option checking..." << endl;
  for(int arg_index(2); arg_index < argc; arg_index++){

    if(options_lat.check_spec(argv[arg_index])){
      continue;
    }
    if(options.check_spec(argv[arg_index])){
      continue;
    }
    
    cerr << "Unknown option: " << argv[arg_index] << endl;
    exit(-1);
  }

  // 初期値の展開
  map<string, float_t> init_values;
  extract_values(options.state_init_values, init_values);
  map<string, float_t> init_P;
  extract_values(options.state_init_P, init_P);
  map<string, float_t> init_Q;
  extract_values(options.input_init_Q, init_Q);
  map<string, float_t> init_R;
  extract_values(options.obs_init_R, init_R);
  map<string, float_t> init_sta;
  extract_values(options.stationary_values, init_sta);

  // 運動方程式、観測方程式、状態量、入力、観測量について型の設定
  typedef LateralDynamicsSI<float_t> dynamics_si_t;
  dynamics_si_t dynamics_si;
  typedef LateralObservationEquationSI<float_t> obs_si_t;
  obs_si_t obs_si(dynamics_si);
  typedef MaskedValues<dynamics_si_t::states_t> states_si_t;
  typedef dynamics_si_t::input_t input_si_t;
  typedef obs_si_t::observed_t observed_si_t;


  // 推定を行わないパラメータの除去
  states_si_t::init();
  for(vector<char *>::iterator it(options.skip_states.begin());
      it != options.skip_states.end();
      ++it){

    int index(states_si_t::index(*it));
    if(index < 0){
      cerr << "Not found state value (" << *it << ") !!" << endl;
      exit(-1);
    }else if(states_si_t::mask(index)){
      cerr << "State value (" << *it << " => " << index  << ") masked." << endl;
    }
  }
  states_si_t states_si;

#define FILTER_NAME(filter) \
filter<dynamics_si_t, obs_si_t, \
    states_si_t, input_si_t, observed_si_t, \
    float_t>

  struct {
    FILTER_NAME(AbstractKalmanFilter) *filter;
    const char *name;
  } filters[] = {
      {new FILTER_NAME(UKF)(dynamics_si, obs_si), "ukf"},         // UKFの設定
      {new FILTER_NAME(EKF)(dynamics_si, obs_si), "ekf"},         // EKFの設定
      {new FILTER_NAME(Simulator)(dynamics_si, obs_si), "sim"}};  // 運動方程式のシミュレータ
  
  int filter_index(sizeof(filters) / sizeof(filters[0]) - 1);
  if(options.method_type){
    while(filter_index >= 0){
      if(strcmp(options.method_type, filters[filter_index].name) == 0){break;}
      filter_index--;
    }
    if(filter_index < 0){
      cerr << "Unknown filter: " << options.method_type << endl;
      exit(-1);
    }
  }else{
    filter_index = 0;
  }
  
  FILTER_NAME(AbstractKalmanFilter) &kf(*(filters[filter_index].filter));
  cerr << "setFilter: " << filters[filter_index].name << endl;

  Matrix<float_t> R(observed_si_t::variables(), observed_si_t::variables());

  {
    Matrix<float_t> &P(kf.filter().getP()), &Q(kf.filter().getQ());

#define set_sta(symbol, value) \
if(init_sta.find(#symbol) == init_sta.end()){dynamics_si.sta_ ## symbol() = value;} \
else{dynamics_si.sta_ ## symbol() = init_sta[#symbol];} \
cerr << "setSTA: " #symbol " => " << dynamics_si.sta_ ## symbol() << endl;
    // 定常状態(安定軸)の設定
    set_sta(W0, 0);
    set_sta(theta0, 0);
    set_sta(psi0, 0);
    set_sta(U0, 17);
#undef set_sta

#define set_state(symbol, value, P_value) \
if(init_values.find(#symbol) == init_values.end()){states_si.symbol() = value;} \
else{states_si.symbol() = init_values[#symbol];} \
cerr << "set: " #symbol " => " << states_si.symbol() << endl; \
{ \
  unsigned orig_i(states_si.index_ ## symbol()); \
  if(states_si_t::masked().find(orig_i) == states_si_t::masked().end()){ \
    states_si_t::index_table_t &table(states_si_t::unmasked()); \
    int i(distance(table.begin(), \
        find(table.begin(), table.end(), orig_i))); \
    if(init_P.find(#symbol) == init_P.end()){P(i, i) = P_value;} \
    else{P(i, i) = init_P[#symbol];} \
    cerr << "setP: " #symbol " => " << P(i, i) << endl; \
  }else{ \
    states_si.masked()[orig_i] = states_si.symbol(); \
  } \
}
    // 初期値、分散値の設定(推定しないものについて分散値は設定しない)
    set_state(p,      0, 1E-1);
    set_state(b,      0, 1E-2);
    set_state(r,      0, 1E-2);
    set_state(phi,    0, 1E-2);
    set_state(psi,    0, 1E-2);

    set_state(Yb,     0, 1E-1);
    set_state(Yp,     1, 1E-1);
    set_state(Yr,     0, 1E+0);
    set_state(Ydr,   -1, 1E-1);
    set_state(Lbp, -100, 1E+1);
    set_state(Lpp,   -1, 1E-1);
    set_state(Lrp,  -10, 1E+1);
    set_state(Ldap,   0, 1E+0);
    set_state(Ldrp,   0, 1E-1);
    set_state(Nbp,   -1, 1E-1);
    set_state(Npp,  -10, 1E+1);
    set_state(Nrp,  -10, 1E+1);
    set_state(Ndap,-100, 1E+1);
    set_state(Ndrp,   0, 1E+0);
#undef set_state

#define set_mat(mat, symbol, symbol_table, value) \
{ \
  int index(symbol_table::index(#symbol)); \
  if(index >= 0){ \
    if(init_ ## mat.find(#symbol) == init_ ## mat.end()){mat(index, index) = value;} \
    else{mat(index, index) = init_ ## mat[#symbol];} \
    cerr << "set" #mat ": " #symbol " => " << mat(index, index) << endl; \
  } \
}
    set_mat(Q, dr, input_si_t, 1E-4);
    set_mat(Q, da, input_si_t, 1E-4);

    set_mat(R, tascg,   observed_si_t, 1E-1);
    set_mat(R, betacg,  observed_si_t, 1E-1);
    set_mat(R, phi,     observed_si_t, 1E-2);
    set_mat(R, psi,     observed_si_t, 1E-2);
    set_mat(R, p,       observed_si_t, 1E-2);
    set_mat(R, r,       observed_si_t, 1E-2);
    set_mat(R, pdot,    observed_si_t, 1E-1);
    set_mat(R, rdot,    observed_si_t, 1E-1);
    set_mat(R, aycg,    observed_si_t, 1E+1);
#undef set_mat

    //ukf.alpha()
    //ukf.beta()
    //ukf.kappa()

    kf.filter().init();
    
    kf.dump(cerr, 0, states_si);
  }

  // 入力をラダー舵角[rad]に変換
  ControlInput<float_t> raw2dr(
      options_lat.dr_trim, options_lat.dr_sf, 0); //deg2rad(5));

  // 入力をエルロン舵角[rad]に変換
  ControlInput<float_t> raw2da(
      options_lat.da_trim, options_lat.da_sf, 0); //deg2rad(5));

  float_t t(0), previous_t(0);
  float_t sum_Vc(0), sum_theta(0), sum_psi(0);
  float_t auto_dr_trim(0), auto_da_trim(0);
  bool perform_auto_reset(true);
  for(int loop(0), loop_start_log(-1); reader.seek(); ++loop, previous_t = t){
    CSV_Reader::item_t item(reader.next());
    /*for(CSV_Reader::item_t::iterator it(item.begin());
        it != item.end();
        ++it){
      cout << *it << endl;
    }*/

    /*
     * 項目は
     * 0) GPS時刻
     * 1-10) 状態量(位置(long,lat,height)、速度(N,E,D)、姿勢(Y,P,R,Z))
     * 11-13) 風データ(大気速度、迎角、横滑り角)
     * 14-29) 制御データ(入力8、出力8)
     * 30-38) 微分情報(加速度3軸(X,Y,Z)、角速度3軸(X,Y,Z)、角加速度3軸(X,Y,Z))
     * 角度は位置以外すべてラジアン
     */
    
    stringstream(item[rowstr2index("A")]) >> t;
    
    if(t < options.log_start){
      // ログ開始前
      continue;
    }else if(loop_start_log < 0){
      // ログ開始直後
      loop_start_log = loop;
      if(options.manuv_start < options.log_start){
        options.manuv_start = t + 0.5; // 現在から0.5秒間後から操舵開始とする
      }else if(options.manuv_start == options.log_start){
        // ログ開始と操舵開始が等しい場合、初期値の再設定などをしない
        perform_auto_reset = false;
      }
      // 初回はdeltaT = 0となるようにして時間更新が行われないようにする
      options.log_start = previous_t = t;
    }
    if(t > options.log_end){break;} // ログ終了後

    input_si_t input;
    stringstream(item[options_lat.dr_row]) >> input.dr();
    stringstream(item[options_lat.da_row]) >> input.da();

    observed_si_t observed;
    stringstream(item[rowstr2index("L")]) >> observed.tascg();
    observed.tascg() -= options.tascg_offset;
    stringstream(item[rowstr2index("N")]) >> observed.betacg();
    observed.betacg() -= options.boffset;
    stringstream(item[rowstr2index("J")]) >> observed.phi();
    observed.phi() -= options.phi_offset;
    stringstream(item[rowstr2index("H")]) >> observed.psi();
    observed.psi() -= options.psi_offset;
    // TODO: consider attitude offset
    stringstream(item[rowstr2index("AH")]) >> observed.p();
    stringstream(item[rowstr2index("AJ")]) >> observed.r();
    stringstream(item[rowstr2index("AK")]) >> observed.pdot();
    stringstream(item[rowstr2index("AM")]) >> observed.rdot();
    stringstream(item[rowstr2index("AF")]) >> observed.aycg();
    
    float_t theta;
    stringstream(item[rowstr2index("I")]) >> theta;
    
    //cerr << t << ","
    //     << input.join(",") << ","
    //     << observed.join(",") << endl;

    // 操舵開始
    if(t >= options.manuv_start){

      // 時刻の処理
      dynamics_si.t = t;
      dynamics_si.delta_t = t - previous_t;

      // 再初期化
      if(perform_auto_reset){
        perform_auto_reset = false;
        
        int denom(loop - loop_start_log);
        
#define REINITIZALIE(x, val) \
if(init_sta.find(#x "0") == init_sta.end()){ \
  dynamics_si.sta_ ## x ## 0() = val / denom; \
  cerr << "Reset: " #x "0 => " << dynamics_si.sta_ ## x ## 0() << endl; \
}
        REINITIZALIE(U, sum_Vc);
        REINITIZALIE(theta, sum_theta);
        REINITIZALIE(psi, sum_psi);
        
#undef REINITIZALIE
        
        auto_dr_trim /= denom;
        auto_da_trim /= denom;
        
        cerr << "Reset: auto_dr_trim => " << auto_dr_trim << endl;
        cerr << "Reset: auto_da_trim => " << auto_da_trim << endl;
      }

      // 操舵量の有次元化
      input.dr() = raw2dr.deflection(input.dr()) - auto_dr_trim;
      input.da() = raw2da.deflection(input.da()) - auto_da_trim;

      // 時間更新
      try{
        kf.predict(states_si, input);
      }catch(exception &e){
        cerr << e.what() << endl;
        exit(-1);
      }

      // 観測更新
      try{
        obs_si.input = &input;
        kf.correct(states_si, observed, R);
      }catch(exception &e){
        cerr << e.what() << endl;
        exit(-1);
      }
    }else{
      sum_Vc += observed.tascg();
      sum_theta += theta;
      sum_psi += observed.psi();
      auto_dr_trim += raw2dr.deflection_init(input.dr());
      auto_da_trim += raw2da.deflection_init(input.da());
    }
    
    kf.dump(cout, t, states_si);

    if(!states_si.good()){exit(-1);}
  }
  
  for(int i(0); i < sizeof(filters) / sizeof(filters[0]); i++){
    delete filters[i].filter;
  }

  return 0;
}