/** @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 "longitudinal.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_lng.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 LongitudinalDynamicsSI<float_t> dynamics_si_t;
  dynamics_si_t dynamics_si;
  typedef LongitudinalObservationEquationSI<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(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(u,      0, 1E-1);
    set_state(a,      0, 1E-2);
    set_state(theta,  0, 1E-2);
    set_state(q,      0, 1E-2);

    set_state(Xu,     0, 1E-1);
    set_state(Xa,     1, 1E-1);
    set_state(Xdt,    0, 1E+0);
    set_state(Zu,    -1, 1E-1);
    set_state(Za,  -100, 1E+1);
    set_state(Zq,    -1, 1E-1);
    set_state(Zde,  -10, 1E+1);
    set_state(Zdt,    0, 1E+0);
    set_state(Mu,     0, 1E-1);
    set_state(Maa,   -1, 1E-1);
    set_state(Ma,   -10, 1E+1);
    set_state(Mq,   -10, 1E+1);
    set_state(Mde, -100, 1E+1);
    set_state(Mdt,    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, dt, input_si_t, 1E-3);
    set_mat(Q, de, input_si_t, 1E-3);

    set_mat(R, tascg,   observed_si_t, 1E-1);  // [m/s]
    set_mat(R, alphacg, observed_si_t, 1E-2);  // [rad]
    set_mat(R, theta,   observed_si_t, 1E-2);  // [rad]
    set_mat(R, q,       observed_si_t, 1E-3);  // [rad/s]
    set_mat(R, qdot,    observed_si_t, 1E-2);  // [rad/s^2]
    set_mat(R, axcg,    observed_si_t, 1E+1);
    set_mat(R, azcg,    observed_si_t, 1E+1);
#undef set_mat

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

    kf.filter().init();

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

  // 入力をスラスト[N]に変換
  ControlInput<float_t> raw2dt(
      options_lng.dt_trim, options_lng.dt_sf, 0);

  // 入力をエレベータ舵角[rad]に変換
  ControlInput<float_t> raw2de(
      options_lng.de_trim, options_lng.de_sf, deg2rad(0));

  float_t t(0), previous_t(0);
  float_t sum_Vc(0), sum_theta(0), sum_alpha(0);
  float_t auto_dt_trim(0), auto_de_trim(0);
  float_t alpha0(dynamics_si.sta_W0() / dynamics_si.sta_U0());
  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(fabs(options.manuv_start - options.log_start) < 0.1){
        // ログ開始と操舵開始がほぼ等しい場合、初期値の再設定などをしない
        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_lng.dt_row]) >> input.dt();
    stringstream(item[options_lng.de_row]) >> input.de();

    observed_si_t observed;
    stringstream(item[rowstr2index("L")]) >> observed.tascg();
    observed.tascg() -= options.tascg_offset;
    stringstream(item[rowstr2index("M")]) >> observed.alphacg();
    observed.alphacg() -= options.aoffset;
    stringstream(item[rowstr2index("I")]) >> observed.theta();
    observed.theta() -= options.theta_offset;
    stringstream(item[rowstr2index("AI")]) >> observed.q();
    stringstream(item[rowstr2index("AL")]) >> observed.qdot();
    stringstream(item[rowstr2index("AE")]) >> observed.axcg();
    stringstream(item[rowstr2index("AG")]) >> observed.azcg();

    //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_STA(x, val) \
if(init_sta.find(#x "0") == init_sta.end()){ \
  dynamics_si.sta_ ## x ## 0() = val; \
  cerr << "Reset: " #x "0 => " << dynamics_si.sta_ ## x ## 0() << endl; \
}
        float_t _alpha0(sum_alpha / denom);
        float_t U0((sum_Vc / denom) * cos(_alpha0));
        REINITIZALIE_STA(U, U0);
        REINITIZALIE_STA(W, U0 * tan(_alpha0));
        REINITIZALIE_STA(theta, sum_theta / denom);
        alpha0 = dynamics_si.sta_W0() / dynamics_si.sta_U0();
#undef REINITIZALIE_STA
        
        // 初期値の再設定
        /*
         * V_wind = ((U_0 + u)^2 + (W_0 + w)^2)^0.5
         * thus, (U_0 + u) = (V_wind^2 - (W_0 + w)^2)^0.5
         */
        float_t _a(observed.alphacg() - alpha0),
            _theta(observed.theta() - dynamics_si.sta_theta0()), 
            _q(observed.q()), 
            _u(sqrt(pow(observed.tascg(), 2)
                - pow(dynamics_si.sta_W0() + (dynamics_si.sta_U0() * _a), 2))
                - dynamics_si.sta_U0());

#define REINITIZALIE(symbol) \
if(init_values.find(#symbol) == init_values.end()){ \
  states_si.symbol() = _ ## symbol; \
  cerr << "Reset: " #symbol " => " << states_si.symbol() << endl; \
}
        
        REINITIZALIE(u);
        REINITIZALIE(a);
        REINITIZALIE(theta);
        REINITIZALIE(q);
#undef REINITIZALIE
        
        auto_dt_trim /= loop;
        auto_de_trim /= loop;
        
        cerr << "Reset: auto_dt_trim => " << auto_dt_trim << endl;
        cerr << "Reset: auto_de_trim => " << auto_de_trim << endl;
      }

      // 操舵量の有次元化
      input.dt() = raw2dt.deflection(input.dt()) - auto_dt_trim;
      input.de() = raw2de.deflection(input.de()) - auto_de_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 += observed.theta();
      sum_alpha += observed.alphacg();
      auto_dt_trim += raw2dt.deflection_init(input.dt());
      auto_de_trim += raw2de.deflection_init(input.de());
    }

    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;
}