/**
 * Equation Error Methodの実装テスト
 * 2次系で遊んでみる
 * 
 */

#include <iostream>
#include <vector>
#include <map>
#include <deque>
#include <queue>
#include <fstream>
#include <limits>
#include <sstream>

#include <cstdarg>
#include <io.h>
#include <fcntl.h>

#define NOMINMAX

#include "algorithm/least_square.h"
#include "algorithm/fft.h"
#include "algorithm/wavelet.h"
#include "algorithm/digital_filter.h"
#include "param/matrix.h"
#include "param/complex.h"
#include "util/pipestream.h"
#include "util/tempfile.h"

#include "../../090115_ukftest/2nd_order_system.h"

#include "common.h"

/**
 * 観測量、位置のみ観測できるものとする
 * 位置が[0]
 * 
 */
MAKE_VALUES(ObservedValues, 1, double,
  MAKE_ALIAS(pos, 0));

struct ObservationEquation {
  ObservedValues operator()(const StateValues &x){
    ObservedValues obs;
    obs.pos() = const_cast<StateValues &>(x).pos();
    return obs;
  }
};

typedef double float_t;
typedef Matrix<float_t> matrix_t;
typedef Complex<float_t> complex_t;
typedef Matrix<complex_t> cmatrix_t;

// TODO: Matrix<Complex<FloatT> >::adjointできるようにすることで実数と複素数で処理を統一する
// とりあえず特殊化で回避
template <>
cmatrix_t LeastSquare::operator()(
    const cmatrix_t &y, const cmatrix_t &K){

  //adjoint matrix、随伴行列
  cmatrix_t K_adj(K.columns(), K.rows()); 
  for(int i(0); i < K_adj.rows(); i++){
    for(int j(0); j < K_adj.columns(); j++){
      K_adj(i, j) = const_cast<cmatrix_t &>(K)(j, i).conjugate();
    }
  }
  cmatrix_t first_c(K_adj * K), second_c(K_adj * y);
  matrix_t 
      first_r(first_c.rows(), first_c.columns()), 
      second_r(second_c.rows(), second_c.columns());
#define copy_real(src, dist) \
for(int i(0); i < src.rows(); i++){ \
  for(int j(0); j < src.columns(); j++){ \
    dist(i, j) = src(i, j).real(); \
  } \
}
      copy_real(first_c, first_r);
      copy_real(second_c, second_r);
#undef copy_op
      
  matrix_t res_r(first_r.inverse() * second_r);
  cmatrix_t res_c(res_r.rows(), res_r.columns());
  for(int i(0); i < res_r.rows(); i++){
    for(int j(0); j < res_r.columns(); j++){
      res_c(i, j) = res_r(i, j);
    }
  }
  
  return res_c;
}

using namespace std;

void print_states(ostream &out, unsigned int states, ...){
  va_list ap;
  va_start(ap, states);
  while(states--){
    StateValues *x(va_arg(ap, StateValues *));
    out << x->vel() << ", " << x->pos();
    if(states){out << ", ";}
  }
  va_end(ap);
}

struct Options {
  float_t deltaT;
  float_t final_t;
  int rand_seed;
  float_t omega_sigma; // 入力(角速度)プロセスノイズ
  float_t pos_sigma; // 観測量(位置)に対する観測ノイズ
  float_t model_k, model_a, model_m, model_F0, model_omega;
  std::istream *model_scenario;
  float_t ftr_freq_min, ftr_freq_max;
  int ftr_freq_step;
  bool ftr_autodiff;
  int wfr_cascade;
  float_t wfr_threshold, wfr_ifading;
  float_t forgetting_factor;
  std::istream *input_fetch;
  bool input_analyze;
  bool input_lpf;
  typedef std::map<const char *, std::iostream *> iostream_pool_t;
  iostream_pool_t iostream_pool;
  
  enum filter_id_t {
    sim = -1, ls = 0, wls, rwls, ftr, wfr, wfr2
  } filter_id;
  
  Options() 
      : deltaT(0.02), 
      final_t(100), 
      rand_seed(0),
      model_k(30), model_a(5), model_m(10), 
      model_F0(50), model_omega(deg2rad(100)),
      model_scenario(NULL),
      omega_sigma(0),
      pos_sigma(5E-4),
      ftr_freq_min(0.2), ftr_freq_max(5), ftr_freq_step(25),
      ftr_autodiff(false),
      wfr_cascade(0),
      wfr_threshold(1E2), wfr_ifading(0), filter_id(ls),
      forgetting_factor(1),
      input_fetch(NULL), input_analyze(false),
      input_lpf(false),
      iostream_pool() {}
  ~Options(){
    for(iostream_pool_t::iterator it(iostream_pool.begin());
        it != iostream_pool.end();
        ++it){
      it->second->flush();
      delete it->second;
    }
  }
  
  istream &spec2istream(const char *spec){
    
    if(strcmp(spec, "-") == 0){
      // 標準入力は'-'で指定
      cerr << "[cin]" << endl;
#if defined(_MSC_VER)
      setmode(fileno(stdin), O_BINARY);
#endif
      return cin;
    }
    
    cerr << spec;
    fstream *fin(new fstream(spec, ios::in | ios::binary));    
    if(fin->fail()){
      cerr << " => File not found!!" << endl;
      exit(-1);
    }
    cerr << endl;
    iostream_pool[spec] = fin;
    return *fin;
  }
  
  enum filter_id_t check_filter(char *str){
#define CHECK_NAME(name) \
if(std::strstr(str, #name) == str){ \
  return name; \
}
    CHECK_NAME(ls);
    CHECK_NAME(wls);
    CHECK_NAME(rwls);
    CHECK_NAME(ftr);
    CHECK_NAME(wfr2);
    CHECK_NAME(wfr);
    
#undef CHECK_NAME
    return sim;
  }
  
  bool check_spec(char *str){
#define CHECK_OPTION(name, operation) \
if(std::strstr(str, "--" #name "=") == str){ \
  char *spec(str + strlen("--" #name "=")); \
  std::cerr << #name << ": " << spec << std::endl; \
  {operation;} \
  return true; \
}
    // 時刻関係の処理
    // --deltaT=(time)
    CHECK_OPTION(deltaT,
        deltaT = atof(spec));
    // --final_t=(time)
    CHECK_OPTION(final_t,
        final_t = atof(spec));
    
    // 乱数に関する処理
    CHECK_OPTION(rand,
        rand_seed = atoi(spec));
    
    // 誤差に関する設定
    // --omega_sigma=(sigma)
    CHECK_OPTION(omega_sigma,
        omega_sigma = atof(spec));
    // --pos_sigma=(sigma)
    CHECK_OPTION(pos_sigma,
        pos_sigma = atof(spec));
    
    // モデルに関するパラメータ
    CHECK_OPTION(model_k,
        model_k = atof(spec));
    CHECK_OPTION(model_a,
        model_a = atof(spec));
    CHECK_OPTION(model_m,
        model_m = atof(spec));
    CHECK_OPTION(model_F0,
        model_F0 = atof(spec));
    CHECK_OPTION(model_omega,
        model_omega = atof(spec));
    // モデルを途中で変更する場合はシナリオを取り込むようにする
    CHECK_OPTION(model_scenario,
        model_scenario = &spec2istream(spec));
    
    // 入力に関する設定
    // 入力を外部から取り込むか
    CHECK_OPTION(input_fetch,
        input_fetch = &spec2istream(spec));
    // 入力を解析するか
    CHECK_OPTION(input_analyze,
        input_analyze = (strcmp(spec, "on") == 0));
    // 入力に対してLPF(IIR5)を適用するか
    CHECK_OPTION(input_lpf,
        input_lpf = (strcmp(spec, "on") == 0));
    
    // FTRに関するパラメータ
    // --ftr_freq_min=(freq)
    CHECK_OPTION(ftr_freq_min,
        ftr_freq_min = atof(spec));
    // --ftr_freq_max=(freq)
    CHECK_OPTION(ftr_freq_max,
        ftr_freq_max = atof(spec));
    // --ftr_freq_step=(int)
    CHECK_OPTION(ftr_freq_step,
        ftr_freq_step = atoi(spec));
    // --ftr_freq_step=(0:off, 1:on)
    CHECK_OPTION(ftr_autodiff,
        ftr_autodiff = (atoi(spec) != 0));
    
    // WFRに関するパラメータ
    // --wfr_cascade=(int)
    CHECK_OPTION(wfr_cascade, // カスケードの段数
        wfr_cascade = atoi(spec));
    // --wfr_threshold=(value)
    CHECK_OPTION(wfr_threshold, // 閾値
        wfr_threshold = atof(spec));
    // --wfr_ifading=(value)
    CHECK_OPTION(wfr_ifading,   // 入力のフェーディング
        wfr_ifading = atof(spec));
    
    // 使用するフィルタの選択
    CHECK_OPTION(filter,
        filter_id = check_filter(spec));
    
    // その他
    CHECK_OPTION(ffactor,   // 忘却係数
        forgetting_factor = atof(spec));    
#undef CHECK_OPTION
    
    return false;
  }
} options;

struct Scenario {
  // {時刻 => {値の名前 => 値}}
  typedef std::queue<std::pair< float_t, std::map<std::string, float_t> > > content_t;
  content_t content;
  Scenario(){}
  ~Scenario(){}
  void read(std::istream &in){
    typedef std::map<float_t, std::map<std::string, float_t> > items_t;
    items_t items;
    // 1行に 時刻 値 値の名前 を書いていく
    while(!in.eof()){
      std::string str;
      std::getline(in, str);
      std::stringstream ss(str);
      float_t time, value;
      string name;
      ss >> time;
      if(!ss.good()){continue;}
      ss >> value;
      if(!ss.good()){continue;}
      ss >> name;
      if(name.length() == 0){continue;}
      std::cerr << "scenario: " 
          << name << " = " << value << " @ " << time << " [s]" << std::endl;
      items[time][name] = value;
    }
    
    for(items_t::iterator it(items.begin());
        it != items.end();
        ++it){
      content.push(make_pair(it->first, it->second));
    }
  }
} scenario;

class SystemModel {
  protected:
    typedef vector< pair<float_t, pair<float_t, float_t> > > inputs_history_t;
    mutable inputs_history_t inputs_history;
    SystemModelBase &model;
  public:
    SystemModel(SystemModelBase &_model) 
        : inputs_history(), model(_model) {}
    virtual ~SystemModel(){
      if((!options.input_analyze) || inputs_history.empty()){return;}
      
      // 入力の解析を行う
      
      // TODO: Ruby側のパイプが変…
      //PipeStream analyze_io("ruby ~/src/eclipse/autopilot/note/080821_si/wavelet_analyze.rb - --prefix=input");
      
      TempFile temp;
      iostream &analyze_io(temp.stream());
      
      for(inputs_history_t::iterator it(inputs_history.begin());
          it != inputs_history.end();
          ++it){
        analyze_io 
            << it->first << "," 
            << it->second.first << ","
            << it->second.second << endl; 
      }
      analyze_io.flush();
      
      // 外部コマンドの起動
      stringstream cmdstr;
      cmdstr
          << "ruby.exe "
          << "~/src/eclipse/autopilot/note/080821_si/wavelet_analyze.rb "
          << temp.get_fname() << " "
          << "--prefix=input "
          << "--window=" 
          << (1 << ((options.wfr_cascade <= 0) ? WFR::default_cascade_depth : options.wfr_cascade));
      cerr << cmdstr.str() << endl;
      system(cmdstr.str().c_str());
    }
    virtual InputValues get_external_u(const float_t &t, StateValues &x_true) const = 0;
    
    void update_model(const float_t &t){
      // シナリオを参照しながら、モデル(k, a, m)を更新する
      if(scenario.content.empty()){return;}
      Scenario::content_t::value_type &item(scenario.content.front());
      if(item.first > t){return;}
      Scenario::content_t::value_type::second_type &name_values(item.second);
#define update_value(name) \
if(name_values.find( #name ) != name_values.end()){ \
  model.set_ ## name(name_values[ #name ]); \
}
      update_value(k);
      update_value(a);
      update_value(m);
#undef update_value
      scenario.content.pop();
    }
    virtual void update_system(const float_t &t, StateValues &x_true) = 0;
    virtual void update(const float_t &t, StateValues &x_true){
      update_model(t);
      update_system(t, x_true);
    }
};

/**
 * 
 * 入力はサイン波
 */
class SystemModel_SW : public SystemModel {
  protected:
    SystemModelSW model;
  public:
    SystemModel_SW() 
        : SystemModel(model),
        model(options.model_k, options.model_a, options.model_m, 
            options.model_F0, options.model_omega){}
    InputValues get_external_u(const float_t &t, StateValues &x_true) const {
      return model.get_external_u(t, x_true);
    }
    void update_system(const float_t &t, StateValues &x_true){
      float_t acc_without_noise(model.get_external_u(t, x_true).acc());
      
      // 必要であれば、ここでプロセスノイズをいれる
      model.phase_error() = rand_regularized(0, options.omega_sigma); // ランダムノイズ
      float_t acc_with_noise(model.get_external_u(t, x_true).acc());
      
      // システムの真値の更新
      x_true = nextByRK4(model, t, x_true, options.deltaT);
    
      // プロセスノイズの解除
      model.phase_error() = 0;
      
      inputs_history.push_back(
          make_pair(t, 
              make_pair(acc_without_noise, acc_with_noise)));
    }
};

/**
 * 
 * 入力を外部から取り込む
 */
class SystemModel_InputFetch : public SystemModel {
  protected:
    struct SystemModelInputFetch : public SystemModelBase {
      mutable double external_acc;
      SystemModelInputFetch(
          const double &k, const double &a, const double &m)
          : SystemModelBase(k, a, m), external_acc(0) {
        
      }
      InputValues get_external_u(const double &t, const StateValues &x) const {
        InputValues res;
        res.acc() = external_acc;
        return res;
      }
    } model;
    mutable float_t current_t, acc_without_noise, acc_with_noise;
  public:
    SystemModel_InputFetch() 
        : SystemModel(model),
        model(options.model_k, options.model_a, options.model_m),
        current_t(numeric_limits<double>::min()), acc_without_noise(0), acc_with_noise(0) {}
    ~SystemModel_InputFetch(){
      if(options.input_fetch){
        options.input_fetch->sync();
        options.input_fetch->seekg(ios_base::end);
      }
    }
    
    void update_acc(const float_t &t) const {
      if(t != current_t){
        float_t *target[] = {&acc_without_noise, &acc_with_noise};
        float_t v;
        for(int i(0); i < sizeof(target) / sizeof(target[0]); i++){
          while(true){
            float_t v;
            *(options.input_fetch) >> v;
            if(!options.input_fetch->fail()){
              *target[i] = v;
              break;
            }
            options.input_fetch->clear();
            char c;
            (*options.input_fetch) >> c;
          }
        }
        current_t = t;
        inputs_history.push_back(
            make_pair(t, 
                make_pair(acc_without_noise, acc_with_noise)));
      }
      //cerr << current_t << ": " << acc_without_noise << ", " << acc_with_noise << endl;
    }
    
    InputValues get_external_u(const float_t &t, StateValues &x_true) const {
      update_acc(t);
      model.external_acc = acc_without_noise;
      return model.get_external_u(t, x_true);
    }
    void update_system(const float_t &t, StateValues &x_true){
      update_acc(t);
      model.external_acc = acc_with_noise;
      
      // システムの真値の更新
      x_true = nextByRK4(model, t, x_true, options.deltaT);
    }
};

// IIRフィルタ
struct IIR5 {
  float_t sec0[3];
  static const float_t coef_l0[3];
  static const float_t coef_r0[3];
  float_t sec1[3];
  static const float_t coef_l1[3];
  static const float_t coef_r1[3];
  float_t sec2[2];
  static const float_t coef_l2[2];
  static const float_t coef_r2[2];
  IIR5(){
#define init_array(index) \
for(int i(0); i < sizeof(sec ## index) / sizeof(sec0[index]); i++){ \
  sec ## index[i] = 0; \
}
    init_array(0);
    init_array(1);
    init_array(2);
#undef init_array
  }
  ~IIR5(){}
  void shift() {
#define init_array(index) \
for(int i(1); i < sizeof(sec ## index) / sizeof(sec0[index]); i++){ \
  sec ## index[i] = sec ## index[i - 1]; \
}
    init_array(0);
    init_array(1);
    init_array(2);
#undef init_array    
  }
  float_t operator()(const float_t &in){
    shift();
    sec0[0] = in;
#define end_p(v) (v + (sizeof(v) / sizeof(v[0])))
    sec0[0] = DigitalFilter::fir(sec0, coef_l0, end_p(coef_l0));
    sec1[0] = DigitalFilter::fir(sec0, coef_r0, end_p(coef_r0));
    sec1[0] = DigitalFilter::fir(sec1, coef_l1, end_p(coef_l1));
    sec2[0] = DigitalFilter::fir(sec1, coef_r1, end_p(coef_r1));
    sec2[0] = DigitalFilter::fir(sec2, coef_l2, end_p(coef_l2));
    return DigitalFilter::fir(sec2, coef_r2, end_p(coef_r2));
#undef end_p
  }
};

// 例えば5次のIIRフィルタ (LPF, チェビシェフ, bandpass(0.5dB): 0.2, stopband(35dB): 0.3)
const float_t IIR5::coef_l0[] = {1.46246314826535023e-01, 5.30285468718403763e-01, -8.09655568506156498e-01};
const float_t IIR5::coef_r0[] = {2.18701254336773054e+00, 4.37402508673546109e+00, 2.18701254336773054e+00};
const float_t IIR5::coef_l1[] = {1.49022374998750984e-01, 8.92145499942275322e-01, -4.92212500037740419e-01};
const float_t IIR5::coef_r1[] = {1.00667265586878241e+00, 2.01334531173756481e+00, 1.00667265586878241e+00};
const float_t IIR5::coef_l2[] = {3.95807412908426748e-01, 5.83229651633706880e-01};
const float_t IIR5::coef_r2[] = {5.26481231495677493e-01, 5.26481231495677493e-01};

void update_P(matrix_t &P){
  if(options.forgetting_factor == float_t(1)){return;}
  P /= options.forgetting_factor;
}

void update_Ps(EquationErrorMethod::matrix_list_t &Ps){
  if(options.forgetting_factor == float_t(1)){return;}
  for(EquationErrorMethod::matrix_list_t::iterator it(Ps.begin());
      it != Ps.end();
      ++it){
    (it->second) /= options.forgetting_factor;
  }
}

int main(int argc, char *argv[]) {
  
  cerr << "Usage : " << argv[0] << endl;
  
  // オプションの取り出し
  for(int i(1); i < argc; i++){
    options.check_spec(argv[i]);
  }
  
  rand_init(options.rand_seed); // 乱数の固定
  
  // シナリオの取り出し
  if(options.model_scenario){
    scenario.read(*options.model_scenario);
  }
  
  // 真値
  StateValues x_true;
  x_true.pos() = x_true.vel() = 0;
  
  // 運動方程式
  SystemModel_InputFetch model_input_fetch;
  SystemModel_SW model_sw;
  SystemModel &model(
      options.input_fetch 
          ? (SystemModel &)model_input_fetch   // 外部から入力(加速度)を取り込む
          : (SystemModel &)model_sw);          // 組み込みのサイン波を入力を使う
  // デフォルトでは (30, 5, 10, 50, deg2rad(100))
  //  => 正解は {{-0.5, -3, 1}, {1, 0, 0}}
  //     固有モードは -0.25 +/- 1.714 i (虚数解)
  
  // 観測方程式等
  EquationOfMotion eom;
  ObservationEquation oe;
  matrix_t R(ObservedValues::variables(), ObservedValues::variables());
  R(0, 0) = pow(options.pos_sigma, 2);
  
  // 時刻/状態量/入力量/観測量をためておくためのスプール
  typedef vector<StateValues> spool_x_true_t;
  spool_x_true_t spool_x_true;
  typedef vector<InputValues> spool_u_t;
  spool_u_t spool_u;
  typedef vector<ObservedValues> spool_z_t;
  spool_z_t spool_z;
  typedef vector<float_t> spool_time_t;
  spool_time_t spool_time;
  
  for(float_t t(0); t < options.final_t; t += options.deltaT){
    
    // 入力量(加速度)をスプールにためる
    spool_u.push_back(model.get_external_u(t, x_true));
    
    // 真値更新
    model.update(t, x_true);
    
    // 状態量の真値の保管
    spool_x_true.push_back(x_true);
    
    // 観測量の作成
    ObservedValues z;
    z.pos() = x_true.pos() + rand_regularized(0, options.pos_sigma);
    
    // 観測量スプールにためる
    spool_z.push_back(z);
    
    // 時刻も入れておく
    spool_time.push_back(t);
  }
  
  if(spool_u.size() < 2){exit(-1);}
  
  // 右辺(状態量と入力)と左辺(状態量の微分)の行列を作る
  matrix_t 
      rhs(StateValues::variables() + InputValues::variables(), spool_u.size()), 
      lhs(StateValues::variables(), spool_u.size());
  
  // LPFの準備
  IIR5 filter_v, filter_a;
  
  // 要素を埋めていく(いわゆる前処理も同時にこなしながら)
  for(int i(1); i < spool_u.size()-1; ++i){
    //cout << spool_z[i].pos() << endl;
    //cout << spool_u[i].acc() << endl;
    
    // 微分量は時間差分で作成する
    StateValues delta;
    // p:   *   v   v   v   *
    // v:   *   * a v a *   *
    // a:   *   *   a   *   *
    delta.pos() = (spool_z[i+1].pos() - spool_z[i-1].pos()) / (options.deltaT * 2);
    delta.vel() 
        = (((spool_z[i+1].pos() - spool_z[i].pos()) / (options.deltaT))
            - ((spool_z[i].pos() - spool_z[i-1].pos()) / (options.deltaT))) 
                / (options.deltaT);
    
    // LPFを適用する場合
    if(options.input_lpf){
      delta.pos() = filter_v(delta.pos());
      delta.vel() = filter_a(delta.vel());
    }
    
    rhs(StateValues::index("vel"), i) = delta.pos();
    rhs(StateValues::index("pos"), i) = spool_z[i].pos();
    rhs(StateValues::variables() + InputValues::index("acc"), i) 
        = spool_u[i].acc();
    
    lhs(StateValues::index("vel"), i) = delta.vel();
    lhs(StateValues::index("pos"), i) = delta.pos();
    
    //cout << spool_z[i].pos() << "," << delta.pos() << "," << delta.vel() << endl;
  }
  
  // 係数行列と初期値の設定
  typedef vector<matrix_t> spool_K_t;
  spool_K_t spool_K;
  matrix_t K(lhs.rows(), rhs.rows());
  {
    K(0, 0) = -2;
    K(0, 1) = -1;
    K(0, 2) = 1;
    K(1, 0) = 0.5;
    K(1, 1) = 0;
    K(1, 2) = 0;
  }
  // 初期値での固有モードは -1 +/- 0.707 (実数解)
  
  // オフラインアルゴリズムについては最後の手前まで初期値を書いておく
  switch(options.filter_id){
    case options.ls:
    case options.wls:
      for(int i(0); i < lhs.columns() - 1; i++){
        spool_K.push_back(K.copy());
      }
      break;
  }
  
  // 最小二乗法
  if(options.filter_id == options.ls){
    LeastSquareCoef ls;
    K = ls(rhs, lhs);
    cerr << "OLS:" << K << endl;
    spool_K.push_back(K);
  }
  
  // 重み付最小二乗法
  if(options.filter_id == options.wls){
    WeightedLeastSquareCoef wls;
    struct WeightCollection {
      matrix_t *weights;
      WeightCollection(
          unsigned int lhs_rows, unsigned int lhs_columns)
          : weights(new matrix_t[lhs_rows]) {
        for(int i(0); i < lhs_rows; i++){
          // とりあえず重みは全て等しくしておく
          weights[i] = matrix_t::getI(lhs_columns);
        }
      }
      ~WeightCollection(){
        delete [] weights;
      }
      matrix_t operator[](unsigned int i) const {
        return weights[i];
      }
    } W(lhs.rows(), lhs.columns());
    K = wls(rhs, lhs, W);
    cerr << "WLS:" << K << endl;
    spool_K.push_back(K);
  }
  
  
  // 再帰的重み付最小二乗法
  if(options.filter_id == options.rwls){    
    EquationErrorMethod::matrix_list_t Ks;
    EquationErrorMethod::matrix_list_t Ps;
    for(int i(0); i < lhs.rows(); i++){
      // KとPの初期値
      Ks.insert(make_pair(i, K.rowVector(i).transpose()));
      Ps.insert(make_pair(i, matrix_t::getI(rhs.rows())));
    }
    
    RLS ls(Ks, Ps);
    matrix_t weight(matrix_t::getI(1));
    for(int i(0); i < lhs.columns(); i++){
      for(int j(0); j < lhs.rows(); j++){
        ls.update(j, lhs.partial(1, 1, j, i), rhs.columnVector(i).transpose(), weight);
      }
      update_Ps(Ps);
      spool_K.push_back(K.copy());
    }
    cerr << "RWLS:" << K << endl;
  }
  
#if 0
  // 周波数領域で最小二乗法を行う
  {
    LeastSquareCoef ls;
    
    cmatrix_t rhs_fd(rhs.rows(), rhs.columns());
    cmatrix_t lhs_fd(lhs.rows(), lhs.columns());
    for(int i(0); i < rhs.rows(); i++){
      int j;
      
      vector<float_t> data_rhs_td;
      for(j = 0; j < rhs.columns(); j++){
        data_rhs_td.push_back(rhs(i, j));
      }
      vector<complex_t> data_rhs_fd(FFT::fft_ct(data_rhs_td));
      j = 0;
      for(vector<complex_t>::iterator it(data_rhs_fd.begin());
          it != data_rhs_fd.end(); ++it, j++){
        rhs_fd(i, j) = *it;
        //lhs_fd(i, j) = *it * complex_t(0, 2. * M_PI * j / data_rhs_fd.size());
        // 左辺については j omega が係数としてついてくる <= うまくいかない
      }
    }
    for(int i(0); i < lhs.rows(); i++){
      int j;
      
      vector<float_t> data_lhs_td;
      for(j = 0; j < lhs.columns(); j++){
        data_lhs_td.push_back(lhs(i, j));
      }
      vector<complex_t> data_lhs_fd(FFT::fft_ct(data_lhs_td));
      j = 0;
      for(vector<complex_t>::iterator it(data_lhs_fd.begin());
          it != data_lhs_fd.end(); ++it, j++){
        
        lhs_fd(i, j) = *it;
      }
    }
    cmatrix_t res_c(
        ls(
            rhs_fd.partial(rhs_fd.rows(), 50, 0, 20), 
            lhs_fd.partial(lhs_fd.rows(), 50, 0, 20)));
    matrix_t res_r(res_c.rows(), res_c.columns());
    for(int i(0); i < res_c.rows(); i++){
      for(int j(0); j < res_c.columns(); j++){
        res_r(i, j) = res_c(i, j).real();
      }
    }
    
    cerr << "OLS_FD:" << res_r << endl;
  }
#endif
  
  // FTR (Fourier Transform Regression)
  if(options.filter_id == options.ftr){
    
    EquationErrorMethod::matrix_list_t Ks;
    EquationErrorMethod::matrix_list_t Ps;
    for(int i(0); i < lhs.rows(); i++){
      // KとPの初期値
      Ks.insert(make_pair(i, K.rowVector(i).transpose()));
      Ps.insert(make_pair(i, matrix_t::getI(rhs.rows())));
    }
    
    FTR ftr(Ks, Ps, options.ftr_autodiff);
    ftr.init_convert_table_freq(options.ftr_freq_min, options.ftr_freq_max, options.ftr_freq_step);
    matrix_t weight(matrix_t::getI(1));
    for(int i(0); i < lhs.columns(); i++){
      ftr.prepare_next_update(options.deltaT);
      for(int j(0); j < lhs.rows(); j++){
        //ftr.update(j, rhs.partial(1, 1, j, i), rhs.columnVector(i).transpose(), weight);
        ftr.update(j, lhs.partial(1, 1, j, i), rhs.columnVector(i).transpose(), weight);
      }
      update_Ps(Ps);
      spool_K.push_back(K.copy());
    }
    cerr << "FTR:" << K << endl;
  }
  
  // Waveletを使って再帰的重み付最小二乗法
  if(options.filter_id == options.wfr){

    EquationErrorMethod::matrix_list_t Ks;
    EquationErrorMethod::matrix_list_t Ps;
    for(int i(0); i < lhs.rows(); i++){
      // KとPの初期値
      Ks.insert(make_pair(i, K.rowVector(i).transpose()));
      Ps.insert(make_pair(i, matrix_t::getI(rhs.rows())));
    }
    
    EquationErrorMethod::lhs_rhs_indexes_t rhs_input_indexes;
    ///< (左辺のID, 右辺のインデックスの集合)
    {
      rhs_input_indexes[0].push_back(2);
      rhs_input_indexes[1].push_back(2);
    }
    
    WFR wfr(Ks, Ps, rhs_input_indexes,
        options.wfr_cascade,
        options.wfr_threshold, options.wfr_ifading);
    matrix_t weight(matrix_t::getI(1));
    
    for(int i(0); i < lhs.columns(); i++){
      for(int j(0); j < lhs.rows(); j++){
        if(wfr.update(j, lhs.partial(1, 1, j, i), rhs.columnVector(i).transpose(), weight)){
          update_P(Ps[j]);
        }
      }
      spool_K.push_back(K.copy());
    }
    
    cerr << "WFR" << K << endl;
  }
  
  // Waveletを使って再帰的重み付最小二乗法(復元機能付)
  if(options.filter_id == options.wfr2){
    // 重み付最小二乗法の準備
    EquationErrorMethod::matrix_list_t Ps;
    for(int i(0); i < lhs.rows(); i++){
      // とりあえずPの初期値
      Ps.insert(make_pair(i, matrix_t::getI(rhs.rows())));
    }
    RecursiveWeightedLeastSquareCoef<float_t> rwls(Ps);

    matrix_t weights[] = {
        matrix_t::getI(1), matrix_t::getI(1)};
    
    // Wavelet変換の準備
    //typedef DaubechiesCascade<4, matrix_t> cascade_t;
    typedef DaubechiesCascade<6, matrix_t> cascade_t;
    const int cascade_depth = 8;
    
    struct Hook {
      cascade_t cascade[cascade_depth];
      
      typedef map<int, matrix_t> spool_t;
      spool_t spool;
      Hook() : spool() {
        for(int i((sizeof(cascade) / sizeof(cascade[0])) - 2);
            i >= 0; --i){
          new(&cascade[i]) cascade_t(cascade[i + 1]);
        }
      }
      ~Hook() {}
      void operator()(const int &depth, const matrix_t &value){
        //cerr << depth << endl;
        //cerr << value << endl;
        spool.insert(make_pair(depth, value));
      }
      void update(matrix_t &new_item){
        cascade[0].propagate(new_item, *this);
      }
      void terminate(){
        cascade[0].terminate_propagate(*this);
      }
    } hook_lhs, hook_rhs, hook_u;
    
    struct RevertHook {
      cascade_t cascade[cascade_depth];
      
      typedef map<int, deque<matrix_t > > spool_t;
      spool_t spool;
      RevertHook() : spool() {
        for(int i((sizeof(cascade) / sizeof(cascade[0])) - 2);
            i >= 0; --i){
          new(&cascade[i]) cascade_t(cascade[i + 1]);
        }
      }
      ~RevertHook() {}
      matrix_t &operator()(const int &depth){
        spool[depth].push_back(spool[depth].front());
        spool[depth].pop_front();
        return spool[depth].back();
      }
      vector<matrix_t> revert() {
        vector<matrix_t> res;
        int deepest(sizeof(cascade) / sizeof(cascade[0]) - 1);
        spool_t::mapped_type::iterator top(spool[deepest].begin());
        do{
          res.push_back(cascade[0].back_propagate(*this));
        }while(top != spool[deepest].begin());
        return res;
      }
    } rhook_lhs, rhook_rhs;
    
    matrix_t zero_matrix_lhs(lhs.rows(), 1), zero_matrix_rhs(rhs.rows(), 1);
    
    for(int i(0); i < lhs.columns(); i++){
      hook_lhs.update(lhs.columnVector(i));
      hook_rhs.update(rhs.columnVector(i));
      hook_u.update(rhs.partial(1, 1, 2, i));
      
      bool updated(false);
      for(Hook::spool_t::iterator it(hook_u.spool.begin());
          it != hook_u.spool.end(); ++it){
        
        Hook::spool_t::iterator it_lhs(hook_lhs.spool.find(it->first));
        Hook::spool_t::iterator it_rhs(hook_rhs.spool.find(it->first));
        if((it_lhs == hook_lhs.spool.end()) 
            || (it_rhs == hook_rhs.spool.end())){
          continue;
        }
        
        bool is_use(true);
        
        if((it->first < 0)
            || (pow(it->second(0, 0), 2) < options.wfr_threshold)){
          is_use = false;
        }
        
        rhook_lhs.spool[it->first].push_back(
            is_use ? it_lhs->second : zero_matrix_lhs);
        rhook_rhs.spool[it->first].push_back(
            is_use ? it_rhs->second : zero_matrix_rhs);
        
        if(!is_use){continue;}
        //cerr << it->first << ", " << it->second(0, 0) << endl;
        
        rwls(it_lhs->second, it_rhs->second, weights, K);
        updated = true;
        
        /*
        // 重みを変えてみる
        matrix_t weights2[] = {
            matrix_t::getScalar(1, abs(it->second(0, 0))), 
            matrix_t::getScalar(1, abs(it->second(0, 0)))};
        rwls(it_lhs->second, it_rhs->second, weights2, K);
        */
      }
      
      if(updated){update_Ps(Ps);}
      spool_K.push_back(K.copy());
      
      hook_lhs.spool.clear();
      hook_rhs.spool.clear();
    }
    hook_lhs.terminate();
    hook_rhs.terminate();
    hook_u.terminate();
    
    // 情報を抽出
    struct {
      RevertHook &hook;
      const char *name;
      matrix_t &zero;
    } targets[] = {
        {rhook_lhs, "LHS", zero_matrix_lhs},
        {rhook_rhs, "RHS", zero_matrix_rhs}};
    
    for(int i(0); i < sizeof(targets) / sizeof(targets[0]); i++){
      for(RevertHook::spool_t::iterator it(targets[i].hook.spool.begin());
          it != targets[i].hook.spool.end(); ++it){
        
        int invoked(0);
        for(RevertHook::spool_t::mapped_type::iterator it2(it->second.begin());
            it2 != it->second.end(); ++it2){
          if(targets[i].zero != (*it2)){invoked++;}
        }
        
        cerr << targets[i].name 
            << " depth : " << it->first << " invoked " 
            << invoked << " times." << endl; 
      }
    }
    
    // 復元
    rhook_lhs.revert();
    rhook_rhs.revert();
    
    cerr << "WFR2" << K << endl;
  }
  
  // まとめて出力
  if(spool_K.empty()){
    
    // シミュレータのみ
    for(int i(0); i < lhs.columns(); i++){
      cout << spool_time[i] << ", ";
      print_states(cout, 1, &(spool_x_true[i]));
      cout << ", "
          << spool_u[i].acc() << ", "
          << spool_z[i].pos() << endl;
    }
  }else{
    for(int i(0); i < lhs.columns(); i++){
      cout << spool_time[i] << ", ";
      print_states(cout, 1, &(spool_x_true[i]));
      cout << ", "
          << spool_u[i].acc() << ", "
          << spool_z[i].pos();
      for(int j(0); j < K.rows(); j++){
        for(int k(0); k < K.columns(); k++){
          cout << ", " << spool_K[i](j, k);
        }
      }
      cout << endl;
    }
  }
  
  return 0;
}