/** @file
 * @brief 縦の運動からパラメータ推定を行う
 * 軸は任意に設定可能だが、定常値の計算を任せると機体軸になる(安定軸ではない!)
 */


#if defined(__GNUC__)
#define FLT_EVAL_METHOD 0
#endif

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

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

#include "common.h"
#include "common_options.h"

#include "../RPE_KF/si_common.h"
#include "../RPE_KF/longitudinal.h"
#include "../sim/misc.h"

using namespace std;

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);
  
  typedef Options::vector_cp_t vector_cp_t;
  vector_cp_t add_coef_specs;

  // 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;
    }
    if(std::strstr(argv[arg_index], "--add=") == argv[arg_index]){
      char *spec(argv[arg_index] + strlen("--add="));
      cerr << "add: " << spec << endl;
      add_coef_specs.push_back(spec);
      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_R;
  extract_values(options.obs_init_R, init_R);
  map<string, float_t> init_sta;
  extract_values(options.stationary_values, init_sta);

  // 観測方程式、状態量、入力、観測量について型の設定
  typedef LongitudinalDynamics<float_t> dynamics_t;
  dynamics_t dynamics;
  typedef LongitudinalObservationEquation<float_t> obs_t;
  obs_t obs(dynamics);
  typedef dynamics_t::states_t states_t;
  typedef dynamics_t::input_t input_t;
  typedef obs_t::observed_t observed_t;
  
  // パラメータリスト(システム方程式の展開)
  // 中には(係数のID, pair<左辺のID、右辺のID>)が記録
  typedef map<int, pair<int, int> > parameter_list_t;
  parameter_list_t parameter_list_states;
  parameter_list_t parameter_list_input;
  parameter_list_t *parameter_list_all[] = {
      &parameter_list_states, &parameter_list_input};
  
#define add_list1(lhs_state, rhs_coef, rhs_state) \
parameter_list_states.insert(make_pair(dynamics_t::rhs_coef, \
    make_pair( \
        states_t::index_ ## lhs_state(), \
        states_t::index_ ## rhs_state())))
#define add_list2(lhs_state, rhs_coef, rhs_input) \
parameter_list_input.insert(make_pair(dynamics_t::rhs_coef, \
    make_pair( \
        states_t::index_ ## lhs_state(), \
        input_t::index_ ## rhs_input())))
  add_list1(u, Xu, u);
  add_list1(u, Xa, a);
  add_list2(u, Xdt, dt);
  add_list1(a, Zu, u);
  add_list1(a, Za, a);
  add_list1(a, Zq, q);
  add_list2(a, Zde, de);
  add_list2(a, Zdt, dt);
  add_list1(q, Mu, u);
  add_list1(q, Ma, a);
  add_list1(q, Mq, q);
  add_list2(q, Mde, de);
  add_list2(q, Mdt, dt);
#undef add_list1
#undef add_list2
  
  
  // 追加的に推定を行うパラメータの設定
  add_coefs_t add_coefs;
  for(vector_cp_t::iterator it(add_coef_specs.begin());
      it != add_coef_specs.end();
      ++it){
    add_coef_item_t add_coef_item;
    add_coef_item.id = dynamics_t::num_of_coefs + distance(add_coef_specs.begin(), it);
    add_coef_item.name = *it;
    string str(*it);
    string::size_type pos(0);
    int i(0);
    while(true){
      pos = str.find_first_of(",");
      string item(str.substr(0, pos));
      bool exit_loop(false);
      switch(++i){
        case 1:
          *((*it) + pos) = '\0';
          break;
        case 2:
          if((add_coef_item.lhs_id = states_t::index(item.c_str())) >= 0){
            break;
          }
          exit_loop = true;
          break;
        case 3:
          if((add_coef_item.rhs_id = states_t::index(item.c_str())) >= 0){
            parameter_list_states.insert(
                make_pair(add_coef_item.id,
                    make_pair(add_coef_item.lhs_id, add_coef_item.rhs_id)));
            break;
          }else if((add_coef_item.rhs_id = input_t::index(item.c_str())) >= 0){
            parameter_list_input.insert(
                make_pair(add_coef_item.id,
                    make_pair(add_coef_item.lhs_id, add_coef_item.rhs_id)));
            break;
          }
          exit_loop = true;
          break;
        case 4:
          stringstream(item) >> add_coef_item.init;
          break;
        case 5:
          stringstream(item) >> add_coef_item.init_P;
          exit_loop = true;
          break;
      }
      if(exit_loop || (pos == str.npos)){break;}
      str = str.substr(pos + 1);
    }
    if(i >= 3){
      add_coefs.push_back(add_coef_item);
      cerr << "adding... " << add_coef_item << endl;
      continue;
    }else{
      cerr << "Unknown add spec: " << *it << endl;
      exit(-1);
    }
  }
  
  // 推定を行わないパラメータの除去  
  for(vector<char *>::iterator it(options.skip_coef.begin());
      it != options.skip_coef.end();
      ++it){
    
    int index(dynamics_t::coef_str2index(*it));
    if(index < 0){
      cerr << "Not found coefficient (" << *it << ") !!" << endl;
      exit(-1);
    }else{
      for(int i(0); 
          i < sizeof(parameter_list_all) / sizeof(parameter_list_all[0]); 
          i++){
        parameter_list_all[i]->erase(index);
      }
      
      cerr << "Coefficient (" << *it << " => " << index  
          << ") will not be estimated." << endl;
    }
  }
  states_t states;
  
  // 左辺と右辺の対応関係を整理する
  typedef map<int, int> lhs_rhs_size_t;
  lhs_rhs_size_t lhs_rhs_size;
  ///< (左辺のID, 右辺のIDの個数)
  parameter_list_t coef_lhs_rhs_index;
  ///< (係数のID, 左辺のID, 右辺のインデックス)
  EquationErrorMethod::lhs_rhs_indexes_t rhs_states_indexes;
  ///< (左辺のID, 右辺で状態量に相当する部分のインデックスの集合)
  EquationErrorMethod::lhs_rhs_indexes_t rhs_input_indexes;
  ///< (左辺のID, 右辺で入力に相当する部分のインデックスの集合)
  
  for(int i(0); 
      i < sizeof(parameter_list_all) / sizeof(parameter_list_all[0]); 
      i++){
    for(parameter_list_t::iterator it(parameter_list_all[i]->begin());
        it != parameter_list_all[i]->end();
        ++it){
      int index;
      coef_lhs_rhs_index.insert(make_pair(it->first, 
          make_pair(
              it->second.first, 
              (index = lhs_rhs_size[it->second.first]++))));
      
      if(parameter_list_all[i] == &parameter_list_states){
        rhs_states_indexes[it->second.first].push_back(index);
      }else if(parameter_list_all[i] == &parameter_list_input){
        rhs_input_indexes[it->second.first].push_back(index);
      }
    }
  }
  
  for(parameter_list_t::iterator it(coef_lhs_rhs_index.begin());
      it != coef_lhs_rhs_index.end();
      ++it){
    cerr << "coef:" << it->first << " => (" 
        << it->second.first << ", " 
        << it->second.second << ")" << endl;
  }
  
  // 推定対象となるパラメータ用の行列の生成
  EquationErrorMethod::matrix_list_t Ks; ///< (左辺ID, 係数行列)
  EquationErrorMethod::matrix_list_t Ps; ///< (左辺ID, 係数行列の共分散)
  for(lhs_rhs_size_t::iterator it(lhs_rhs_size.begin());
      it != lhs_rhs_size.end();
      ++it){
    int size(it->second);
    Ks.insert(make_pair(it->first, matrix_t(size, 1))); // 列ベクトル(縦)
    Ps.insert(make_pair(it->first, matrix_t(size, size)));
  }
  
  // 適用する手法の選択
  struct {
    EquationErrorMethod *method;
    const char *name;
  } methods[] = {
      {new RLS(Ks, Ps), "rls"},
      {(new FTR(Ks, Ps, options.ftr_autodiff))
          ->init_convert_table_freq(
              options.ftr_freq_min, 
              options.ftr_freq_max,
              options.ftr_freq_step), "ftr"},
      {new WFR(Ks, Ps, rhs_input_indexes,
          options.wfr_cascade,
          options.wfr_threshold, options.wfr_ifading), "wfr"}};
  
  int method_index(sizeof(methods) / sizeof(methods[0]) - 1);
  if(options.method_type){
    while(method_index >= 0){
      if(strcmp(options.method_type, methods[method_index].name) == 0){break;}
      method_index--;
    }
    if(method_index < 0){
      cerr << "Unknown method: " << options.method_type << endl;
      exit(-1);
    }
  }else{
    method_index = 0;
  }
  EquationErrorMethod &method(*(methods[method_index].method));
  cerr << "setMethod: " << methods[method_index].name << endl;
  
  // FTRのとき、左辺を差分ではなく状態量をそのまま送りたいのでフラグをつけておく
  bool method_is_ftr(strcmp(methods[method_index].name, "ftr") == 0);

  Matrix<float_t> R(observed_t::variables(), observed_t::variables());
  
  // 初期値を設定する
  {

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

#define set_coef(symbol, default_value, P_default_value) \
{ float_t value( \
    (init_values.find(#symbol) == init_values.end()) \
        ? default_value \
        : init_values[#symbol]); \
cerr << "set: " #symbol " => " << value << endl; \
if(coef_lhs_rhs_index.find(dynamics_t::symbol) != coef_lhs_rhs_index.end()){ \
  int lhs_id(coef_lhs_rhs_index[dynamics_t::symbol].first); \
  int rhs_index(coef_lhs_rhs_index[dynamics_t::symbol].second); \
  Ks[lhs_id](rhs_index, 0) = value; \
  float_t P_value( \
      (init_P.find(#symbol) == init_P.end()) \
          ? P_default_value \
          : init_P[#symbol]); \
  Ps[lhs_id](rhs_index, rhs_index) = P_value; \
cerr << "setP: " #symbol " => " << P_value << endl; \
}else{ \
  dynamics.coef_ ## symbol() = value; \
}}
    // 初期値、分散値の設定(推定しないものについて分散値は設定しない)
    set_coef(Xu,     0, 1E-1);
    set_coef(Xa,     1, 1E-1);
    set_coef(Xdt,    0, 1E+0);
    set_coef(Zu,    -1, 1E-1);
    set_coef(Za,  -100, 1E+1);
    set_coef(Zq,    -1, 1E-1);
    set_coef(Zde,  -10, 1E+1);
    set_coef(Zdt,    0, 1E+0);
    set_coef(Mu,     0, 1E-1);
    set_coef(Maa,   -1, 1E-1);
    set_coef(Ma,   -10, 1E+1);
    set_coef(Mq,   -10, 1E+1);
    set_coef(Mde, -100, 1E+1);
    set_coef(Mdt,    0, 1E+0);
#undef set_coef
    
    // 追加的な項目に対する設定
    for(add_coefs_t::iterator it(add_coefs.begin());
        it != add_coefs.end();
        ++it){
      int rhs_index(coef_lhs_rhs_index[it->id].second);
      
      Ks[it->lhs_id](rhs_index, 0) = it->init;
      cerr << "set: " << it->name << " => " << it->init << endl;
      Ps[it->lhs_id](rhs_index, rhs_index) = it->init_P;
      cerr << "setP: " << it->name << " => " << it->init_P << endl;
    }

#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(R, tascg,   observed_t, 1E-1);  // [m/s]
    set_mat(R, alphacg, observed_t, 1E-2);  // [rad]
    set_mat(R, theta,   observed_t, 1E-2);  // [rad]
    set_mat(R, q,       observed_t, 1E-3);  // [rad/s]
    set_mat(R, qdot,    observed_t, 1E-2);  // [rad/s^2]
    set_mat(R, axcg,    observed_t, 1E+1);
    set_mat(R, azcg,    observed_t, 1E+1);
#undef set_mat
    
    // t(0)の出力
    method.dump(cerr, 0, states, dynamics, coef_lhs_rhs_index);
  }

  // 入力をスラスト[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);
  states_t previous_states;
  
  //fstream tmp_f("tmp.txt", ios_base::out | ios_base::trunc);
  
  float_t sum_Vc(0), sum_theta(0), sum_alpha(0);
  float_t auto_dt_trim(0), auto_de_trim(0);
  bool perform_auto_reset(true);
  float_t alpha0(dynamics.sta_W0() / dynamics.sta_U0());
  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;
      }
      if(loop == 0){
        options.log_start = t;
        continue; // delta_tの計算ができないため
      }
    }
    if(t > options.log_end){break;} // ログ終了後

    input_t input;
    stringstream(item[options_lng.dt_row]) >> input.dt();
    stringstream(item[options_lng.de_row]) >> input.de();

    observed_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.t = t;
      dynamics.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.sta_ ## x ## 0() = val; \
  cerr << "Reset: " #x "0 => " << dynamics.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.sta_W0() / dynamics.sta_U0();
#undef REINITIZALIE_STA
        
        auto_dt_trim /= denom;
        auto_de_trim /= denom;
        
        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;
      
      // 状態量の設定
      /*
       * 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
       */
      states.a() 
          = observed.alphacg() - alpha0;
      states.theta() 
          = observed.theta() - dynamics.sta_theta0();
      states.q()
          = observed.q();
      states.u()
          = sqrt(pow(observed.tascg(), 2) 
              - pow(dynamics.sta_W0() + (dynamics.sta_U0() * states.a()), 2))
              - dynamics.sta_U0();
      
      //tmp_f 
      //    << states.a() << ", " << states.theta() << ", " << states.q() << ", " << states.u() << ", "
      //    << observed.alphacg() << ", " << observed.theta() << ", " << observed.q() << ", " << observed.tascg() << endl; 
      
      // 状態量の微分量の設定(差分で作る)
      // TODO: この差分だと0.5ステップ前の値になるが大丈夫か?
      states_t delta_states(
          (states - previous_states) / dynamics.delta_t);
      
      // 右辺にある推定に寄与しない項を左辺から引いておく
      // (推定しないものについてのみdynamicsに係数あり)
      delta_states -= dynamics.delta_1st(states, input);
      
      // 左辺の分散
#define getR(name) R(observed_t::index_ ## name(), observed_t::index_ ## name())
      matrix_t lhs_R(1, states.variables());
      lhs_R(0, states_t::index_a()) 
          = getR(alphacg);
      lhs_R(0, states_t::index_theta())
          = getR(theta);
      lhs_R(0, states_t::index_q())
          = getR(q);
      lhs_R(0, states_t::index_u())
          = getR(tascg);
      lhs_R *= (2. / pow(dynamics.delta_t, 2));
      // E[ ((a1 - a2) / delta)^2 ] = (E(a1^{2}) + E(a2^{2})) / pow(delta^2)
      
      // 差分よりも良い状態量の微分値がある場合はそれで上書きする
      delta_states.q() = observed.qdot();
      lhs_R(0, states_t::index_q()) = getR(qdot);
#undef detR
      
      // 推定量更新の準備
      method.prepare_next_update(dynamics.delta_t);
      
      // 推定量の更新
      for(lhs_rhs_size_t::iterator it(lhs_rhs_size.begin());
          it != lhs_rhs_size.end();
          ++it){
        
        // it->first => 左辺のID, it->second => 右辺の個数
        matrix_t lhs(1, 1); // 状態量の微分を入れる
        matrix_t rhs(1, it->second); // 状態量や入力を挿入する
        
        // 右辺を穴埋めする
        for(int i(0), j(0); 
            i < sizeof(parameter_list_all) / sizeof(parameter_list_all[0]);
            i++){
          for(parameter_list_t::iterator it2(parameter_list_all[i]->begin());
              it2 != parameter_list_all[i]->end();
              ++it2){
            if(it2->second.first != it->first){continue;}
            
            // 左辺のIDが一致したら右辺の値を引っ張ってくる
            if(parameter_list_all[i] == &parameter_list_states){
              // 状態量から
              rhs(0, j++) = states[it2->second.second];
            }else if(parameter_list_all[i] == &parameter_list_input){
              // 入力から
              rhs(0, j++) = input[it2->second.second];
            }
          }
        }
        
        // 左辺の算出
        if(method_is_ftr && options.ftr_autodiff){
          lhs(0, 0) = states[it->first];
        }else{
          lhs(0, 0) = delta_states[it->first];
        }
        
        //cerr << "L: " << lhs << endl << "R: " << rhs << endl; 
        
        // 係数行列を更新
        method.update(
            it->first, lhs, rhs, 
            lhs_R.partial(1, 1, 0, it->first));
      }
    }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());
    }
    
    // 出力
    method.dump(cout, t, states, dynamics, coef_lhs_rhs_index);

    if(!states.good()){exit(-1);}
    
    previous_states = states.copy();
  }
  
  for(int i(0); i < sizeof(methods) / sizeof(methods[0]); i++){
    delete methods[i].method;
  }

  return 0;
}