/** @file
 * @brief システム同定用、縦の運動方程式、観測方程式
 * 
 * システム同定用の縦の運動に関する運動方程式、観測方程式
 */

#ifndef __LONGITUDINAL_H
#define __LONGITUDINAL_H

#include "algorithm/integral.h"
#include "param/matrix.h"
#include "WGS84.h"

#include "base.h"

/**
 * 航空機の縦の運動方程式(システム同定用)
 * 
 */
template <class FloatT>
class LongitudinalDynamicsSI{
  public:
    /**
     * 定常状態に関するインデックス
     */
    enum {W0, theta0, U0, num_of_stables};

    /**
     * 状態量(x)
     *
     */
    MAKE_VALUES(StateValues, 18, FloatT,
        MAKE_ALIAS(u, 0)
        MAKE_ALIAS(a, 1)
        MAKE_ALIAS(theta, 2)
        MAKE_ALIAS(q, 3)
        MAKE_ALIAS(Xu, 4)   // 以下、安定微係数
        MAKE_ALIAS(Xa, 5)
        MAKE_ALIAS(Xdt, 6)
        MAKE_ALIAS(Zu, 7)
        MAKE_ALIAS(Za, 8)
        MAKE_ALIAS(Zq, 9)
        MAKE_ALIAS(Zde, 10)
        MAKE_ALIAS(Zdt, 11)
        MAKE_ALIAS(Mu, 12)
        MAKE_ALIAS(Maa, 13)
        MAKE_ALIAS(Ma, 14)
        MAKE_ALIAS(Mq, 15)
        MAKE_ALIAS(Mde, 16)
        MAKE_ALIAS(Mdt, 17));
    typedef StateValues states_t;

    /**
     * 操舵量入力(u)
     */
    MAKE_VALUES(InputValues, 2, FloatT,
        MAKE_ALIAS(dt, 0)
        MAKE_ALIAS(de, 1));
    typedef InputValues input_t;

    const FloatT g;
  protected:
    FloatT sta[num_of_stables];     ///< 定常状態

  public:
#define make_accessor(variable, symbol) \
FloatT &variable ## _ ## symbol() {return variable[symbol];}
    make_accessor(sta, W0);
    make_accessor(sta, theta0);
    make_accessor(sta, U0);
#undef make_accessor
    const FloatT const_g() const {return g;}

  protected:
    struct DifferentialEquation {
      mutable input_t *vec_u;
      mutable LongitudinalDynamicsSI *dynamics;
      states_t operator()(const FloatT &t, const states_t &vec_x) const {
        states_t dx;
#define vec_x(name) const_cast<states_t &>(vec_x).name()
#define coef(name) const_cast<states_t &>(vec_x).name()
#define vec_dx(name) dx.name()
#define vec_u(name) vec_u->name()
#define sta(name) dynamics->sta_ ## name()
        vec_dx(u)
            = (coef(Xu) * vec_x(u))
                + coef(Xa) * vec_x(a)
                - sta(W0) * vec_x(q)
                - dynamics->g * cos(sta(theta0)) * vec_x(theta)
                + coef(Xdt) * vec_u(dt);
        vec_dx(a)
            = (coef(Zu) * vec_x(u)
                + coef(Za) * vec_x(a)
                + (sta(U0) + coef(Zq)) * vec_x(q)
                - dynamics->g * sin(sta(theta0)) * vec_x(theta)
                + coef(Zde) * vec_u(de)
                + coef(Zdt) * vec_u(dt)) / sta(U0);
        vec_dx(theta)  = vec_x(q);
        vec_dx(q)
            = coef(Mu) * vec_x(u)
                + coef(Ma) * vec_x(a)
                + coef(Mq) * vec_x(q)
                + coef(Mde) * vec_u(de)
                + coef(Mdt) * vec_u(dt);
        vec_dx(Xu) = vec_dx(Xa) = vec_dx(Xdt)
            = vec_dx(Zu) = vec_dx(Za) = vec_dx(Zq) = vec_dx(Zde) = vec_dx(Zdt)
            = vec_dx(Mu) = vec_dx(Maa) = vec_dx(Ma) = vec_dx(Mq) = vec_dx(Mde) = vec_dx(Mdt)
            = 0;
#undef sta
#undef coef
#undef vec_u
#undef vec_dx
#undef vec_x
        return dx;
      }
      Matrix<FloatT> getA(const FloatT &t, const states_t &vec_x) const {
        Matrix<FloatT> res(states_t::variables(), states_t::variables());
#define i(name) states_t::index_ ## name()
#define j(name) i(name)
#define X(name) const_cast<states_t &>(vec_x).name()
#define U(name) vec_u->name()
#define STA(name) dynamics->sta_ ## name()
        res(i(u), j(u)) = X(Xu);
        res(i(u), j(Xu)) = X(u);
        res(i(u), j(a)) = X(Xa);
        res(i(u), j(Xa)) = X(a);
        res(i(u), j(q)) = -STA(W0);
        res(i(u), j(theta)) = - dynamics->g * cos(STA(theta0));
        res(i(u), j(Xdt)) = U(dt);
        
        res(i(a), j(u)) = X(Zu) / STA(U0);
        res(i(a), j(Zu)) = X(u) / STA(U0);
        res(i(a), j(a)) = X(Za) / STA(U0);
        res(i(a), j(Za)) = X(a) / STA(U0);
        res(i(a), j(q)) = (X(Zq) / STA(U0)) + 1;
        res(i(a), j(Zq)) = X(q) / STA(U0);
        res(i(a), j(theta)) = -dynamics->g * sin(STA(theta0)) / STA(U0);
        res(i(a), j(Zde)) = U(de) / STA(U0);
        res(i(a), j(Zdt)) = U(dt) / STA(U0);
        
        res(i(theta), j(q)) = 1;
        
        res(i(q), j(u)) = X(Mu);
        res(i(q), j(Mu)) = X(u);
        res(i(q), j(a)) = X(Ma);
        res(i(q), j(Ma)) = X(a);
        res(i(q), j(q)) = X(Mq);
        res(i(q), j(Mq)) = X(q);
        res(i(q), j(Mde)) = U(de);
        res(i(q), j(Mdt)) = U(dt);
        
        res(i(Xu), i(Xu)) = res(i(Xa), i(Xa)) = res(i(Xdt), i(Xdt))
            = res(i(Zu), i(Zu)) = res(i(Za), i(Za)) = res(i(Zq), i(Zq)) = res(i(Zde), i(Zde)) = res(i(Zdt), i(Zdt))
            = res(i(Mu), i(Mu)) = res(i(Maa), i(Maa)) = res(i(Ma), i(Ma)) = res(i(Mq), i(Mq)) = res(i(Mde), i(Mde)) = res(i(Mdt), i(Mdt))
            = 1;
#undef STA
#undef U
#undef X
#undef j
#undef i
        return res;
      }
      Matrix<FloatT> getB(const FloatT &t, const states_t &vec_x) const {
        Matrix<FloatT> res(states_t::variables(), input_t::variables());
#define i(name) states_t::index_ ## name()
#define j(name) input_t::index_ ## name()
#define X(name) const_cast<states_t &>(vec_x).name()
#define U(name) vec_u->name()
#define STA(name) dynamics->sta[name]
        res(i(u), j(dt)) = X(Xdt);
        
        res(i(a), j(de)) = X(Zde) / STA(U0);
        res(i(a), j(dt)) = X(Zdt) / STA(U0);
        
        res(i(q), j(de)) = X(Mde);
        res(i(q), j(dt)) = X(Mdt);
#undef STA
#undef U
#undef X
#undef j
#undef i
        return res;
      }
    } diff;

  public:
    FloatT t, delta_t;
    LongitudinalDynamicsSI() : g(WGS84::gravity(0)), diff(), t(0), delta_t(0.01) {
      for(unsigned int i(0); i < num_of_stables; i++){
        sta[i] = FloatT(0);
      }
      diff.dynamics = this;
    }
    ~LongitudinalDynamicsSI(){}

    states_t operator()(const states_t &x, const input_t &u) const {
      diff.vec_u = const_cast<input_t *>(&u);
      return nextByRK4(diff, t, x, delta_t);
    }
    states_t operator()(const states_t &x, const input_t &u,
        const Matrix<FloatT> &u_deviation) const {
      input_t u_copy(u.copy());
      for(int i(0); i < u_copy.variables(); i++){
        u_copy[i] += const_cast<Matrix<FloatT> &>(u_deviation)(i, i);
      }
      diff.vec_u = &u_copy;
      return nextByRK4(diff, t, x, delta_t);
    }
    states_t delta_1st(const states_t &x, const input_t &u) const {
      diff.vec_u = const_cast<input_t *>(&u);
      return diff(t, x);
    }
    Matrix<FloatT> getA(const states_t &x, const input_t &u) const {
      diff.vec_u = const_cast<input_t *>(&u);
      return diff.getA(t, x);
    }
    Matrix<FloatT> getA(const MaskedValues<states_t> &x, const input_t &u) const {
      Matrix<FloatT> res(getA((states_t &)x, u));
      for(MaskedValues<states_t>::index_table_void_t::iterator it(MaskedValues<states_t>::masked().begin());
          it != MaskedValues<states_t>::masked().end(); ++it){
        res = res.coMatrix(it->first, it->first);
      }
      return res;
    }
    Matrix<FloatT> getB(const states_t &x, const input_t &u) const {
      diff.vec_u = const_cast<input_t *>(&u);
      return diff.getB(t, x);
    }
    Matrix<FloatT> getB(const MaskedValues<states_t> &x, const input_t &u) const {
      Matrix<FloatT> orig(getB((states_t &)x, u));
      Matrix<FloatT> res(orig.rows() - MaskedValues<states_t>::masked().size(), orig.columns());
      for(int i(0), k(0); i < orig.rows(); i++){
        if(MaskedValues<states_t>::masked().find(i) != MaskedValues<states_t>::masked().end()){
          continue;
        }
        for(int j(0); j < orig.columns(); j++){res(k, j) = orig(i, j);}
        k++;
      }
      return res;
    }
};

/**
 * 航空機の縦の運動の観測方程式(システム同定用)
 * 
 */
template <class FloatT>
class LongitudinalObservationEquationSI{
  public:
    /**
     * 観測量
     * システム同定向けの状態量を生成
     * 出力は全て機体固定軸に変換されることに注意
     * tascg[m/s]   真大気速度(C.G.)  => U/W_{0}, u/w から生成
     * alphacg[rad] 迎角(C.G.)        => alpha
     * theta[rad]   ピッチ            => theta0 + theta
     * q[rad/s]     ピッチ角速度      => q
     * qdot[rad/s2] ピッチ角加速度    => qから運動方程式で生成
     * axcg[m/s2]   加速度X(C.G.)     => uから運動方程式で生成
     * azcg[m/s2]   加速度Z(C.G.)     => dot{w} = U_{0} * dot{alpha}
     */
    MAKE_VALUES(ObservedValues, 7, FloatT,
        MAKE_ALIAS(tascg, 0)
        MAKE_ALIAS(alphacg, 1)
        MAKE_ALIAS(theta, 2)
        MAKE_ALIAS(q, 3)
        MAKE_ALIAS(qdot, 4)
        MAKE_ALIAS(axcg, 5)
        MAKE_ALIAS(azcg, 6));
    typedef ObservedValues observed_t;
    typedef LongitudinalDynamicsSI<FloatT> dynamics_t;
    typedef typename dynamics_t::states_t states_t;
    typedef typename dynamics_t::input_t input_t;
  protected:
    dynamics_t &dynamics;
  public:
    input_t *input;
    LongitudinalObservationEquationSI(dynamics_t &model) 
        : dynamics(model), input(NULL) {}
    ~LongitudinalObservationEquationSI() {}
    observed_t operator()(const states_t &x) const {
      observed_t obs;

      states_t dx(dynamics.delta_1st(x, *input));
#define vec_x(name) const_cast<states_t &>(x).name()
#define vec_dx(name) dx.name()
#define vec_z(name) obs.name()
      FloatT d_alpha(vec_dx(a)),
          w(vec_x(a) * dynamics.sta_U0()),
          d_u(vec_dx(u)),
          d_w(d_alpha * dynamics.sta_U0());

      vec_z(tascg) = ::sqrt(::pow(dynamics.sta_U0() + vec_x(u), 2)
          + ::pow(dynamics.sta_W0() + w, 2));
      vec_z(alphacg) = vec_x(a)
          + dynamics.sta_W0() / dynamics.sta_U0();
      vec_z(theta) = dynamics.sta_theta0() + vec_x(theta);
      vec_z(q) = vec_x(q);
      vec_z(qdot) = vec_dx(q);
      // 加速度計の出力は純慣性系に対する加速度と地球重力の和
      // 運動計算に使う場合は重力項をキャンセルしてから運動方程式に代入のこと
      vec_z(axcg) = (d_u
          + (vec_x(q) * (dynamics.sta_W0() + w))
          + (dynamics.const_g() * ::sin(vec_z(theta))));
      vec_z(azcg) = d_w
          - (vec_x(q) * (dynamics.sta_U0() + vec_x(u)))
          - (dynamics.const_g() * ::cos(vec_z(theta)));
#undef vec_x
#undef vec_dx
#undef vec_z

      return obs;
    }
    Matrix<FloatT> getH(const states_t &vec_x, const observed_t &vec_z) const {
      Matrix<FloatT> res(observed_t::variables(), states_t::variables());
      states_t vec_dx(dynamics.delta_1st(vec_x, *input));
#define i(name) observed_t::index_ ## name()
#define j(name) states_t::index_ ## name()
#define X(name) const_cast<states_t &>(vec_x).name()
#define Z(name) const_cast<observed_t &>(vec_z).name()
#define U(name) input->name()
#define STA(name) dynamics.sta_ ## name()
        FloatT d_alpha(vec_dx.a()),
            w(X(a) * STA(U0)),
            d_u(vec_dx.u()),
            d_w(d_alpha * STA(U0));
      
        res(i(tascg), j(u)) = (STA(U0) + X(u)) / Z(tascg);
        res(i(tascg), j(a)) = (STA(W0) + w) * STA(U0) / Z(tascg);
        res(i(alphacg), j(a)) = 1;
        res(i(theta), j(theta)) = 1;
        res(i(q), j(q)) = 1;
        res(i(qdot), j(u)) = X(Mu);
        res(i(qdot), j(Mu)) = X(u);
        res(i(qdot), j(a)) = X(Ma);
        res(i(qdot), j(Ma)) = X(a);
        res(i(qdot), j(q)) = X(Mq);
        res(i(qdot), j(Mq)) = X(q);
        res(i(qdot), j(Mde)) = U(de);
        res(i(qdot), j(Mdt)) = U(dt);
        
        // 観測方程式と符号一致
        res(i(axcg), j(u)) = X(Xu);
        res(i(axcg), j(Xu)) = X(u);
        res(i(axcg), j(a)) = X(Xa) + STA(U0) * X(q);
        res(i(axcg), j(Xa)) = X(a);
        res(i(axcg), j(q)) = STA(U0) * X(a);
        res(i(axcg), j(Xdt)) = U(dt);

        res(i(azcg), j(u)) = X(Zu) - X(q);
        res(i(azcg), j(Zu)) = X(u);
        res(i(azcg), j(a)) = X(Za);
        res(i(azcg), j(Za)) = X(a);
        res(i(azcg), j(q)) = (X(Zq) - X(u));
        res(i(azcg), j(Zq)) = X(q);
        res(i(azcg), j(Zde)) = U(de);
        res(i(azcg), j(Zdt)) = U(dt);
#undef STA
#undef U
#undef X
#undef j
#undef i
      return res;
    }
    Matrix<FloatT> getH(const MaskedValues<states_t> &vec_x, const observed_t &vec_z) const {
      Matrix<FloatT> orig(getH((states_t &)vec_x, vec_z));
      Matrix<FloatT> res(orig.rows(), orig.columns() - MaskedValues<states_t>::masked().size());
      for(int i(0); i < orig.rows(); i++){
        for(int j(0), k(0); j < orig.columns(); j++){
          if(MaskedValues<states_t>::masked().find(j) != MaskedValues<states_t>::masked().end()){
            continue;
          }
          res(i, k) = orig(i, j);
          k++;
        }
      }
      return res;
    }
};

/**
 * 航空機の縦の運動方程式
 * 
 */
template <class FloatT>
class LongitudinalDynamics{
  public:
    typedef LongitudinalDynamicsSI<FloatT> dynamics_si_t;
    /**
     * 定常状態に関するインデックス
     */
    const static unsigned int num_of_stables = dynamics_si_t::num_of_stables;
    /**
     * 安定微係数に関するインデックス
     */
    enum {Xu, Xa, Xdt, Zu, Za, Zq, Zde, Zdt, Mu, Maa, Ma, Mq, Mde, Mdt};
    const static unsigned int num_of_coefs = 14;

    /**
     * 状態量(x)
     *
     */
    MAKE_VALUES(StateValues, 4, FloatT,
        MAKE_ALIAS(u, 0)
        MAKE_ALIAS(a, 1)
        MAKE_ALIAS(theta, 2)
        MAKE_ALIAS(q, 3));
    typedef StateValues states_t;

    /**
     * 操舵量入力(u)
     */
    typedef typename dynamics_si_t::input_t input_t;

    const FloatT g;
  protected:
    FloatT sta[num_of_stables];     ///< 定常状態
    FloatT coef[num_of_coefs];      ///< 安定微係数

  public:
#define make_accessor(variable, symbol) \
FloatT &variable ## _ ## symbol() {return variable[dynamics_si_t::symbol];}
    make_accessor(sta, W0);
    make_accessor(sta, theta0);
    make_accessor(sta, U0);
#undef make_accessor
#define make_accessor(variable, symbol) \
FloatT &variable ## _ ## symbol() {return variable[symbol];}
    make_accessor(coef, Xu);
    make_accessor(coef, Xa);
    make_accessor(coef, Xdt);
    make_accessor(coef, Zu);
    make_accessor(coef, Za);
    make_accessor(coef, Zq);
    make_accessor(coef, Zde);
    make_accessor(coef, Zdt);
    make_accessor(coef, Mu);
    make_accessor(coef, Maa);
    make_accessor(coef, Ma);
    make_accessor(coef, Mq);
    make_accessor(coef, Mde);
    make_accessor(coef, Mdt);
#undef make_accessor
    FloatT &coefficient(unsigned &index){
      return coef[index];
    }
    static const int coef_str2index(const char *str){
#define check_str(symbol) \
if(::strcmp(str, #symbol) == 0){return symbol;}    
      check_str(Xu);
      check_str(Xa);
      check_str(Xdt);
      check_str(Zu);
      check_str(Za);
      check_str(Zq);
      check_str(Zde);
      check_str(Zdt);
      check_str(Mu);
      check_str(Maa);
      check_str(Ma);
      check_str(Mq);
      check_str(Mde);
      check_str(Mdt);
#undef check
      return -1;
    }
    const FloatT const_g() const {return g;}

  protected:
    struct DifferentialEquation {
      mutable input_t *vec_u;
      mutable LongitudinalDynamics *dynamics;
      states_t operator()(const FloatT &t, const states_t &vec_x) const {
        states_t dx;
#define vec_x(name) const_cast<states_t &>(vec_x).name()
#define vec_dx(name) dx.name()
#define vec_u(name) vec_u->name()
#define coef(name) dynamics->coef[name]
#define sta(name) dynamics->sta[dynamics_si_t::name]
        vec_dx(u)
            = (coef(Xu) * vec_x(u))
                + coef(Xa) * vec_x(a)
                - sta(W0) * vec_x(q)
                - dynamics->g * cos(sta(theta0)) * vec_x(theta)
                + coef(Xdt) * vec_u(dt);
        vec_dx(a)
            = (coef(Zu) * vec_x(u)
                + coef(Za) * vec_x(a)
                + (sta(U0) + coef(Zq)) * vec_x(q)
                - dynamics->g * sin(sta(theta0)) * vec_x(theta)
                + coef(Zde) * vec_u(de)
                + coef(Zdt) * vec_u(dt)) / sta(U0);
        vec_dx(theta)  = vec_x(q);
        vec_dx(q)
            = coef(Mu) * vec_x(u)
                + coef(Ma) * vec_x(a)
                + coef(Mq) * vec_x(q)
                + coef(Mde) * vec_u(de)
                + coef(Mdt) * vec_u(dt);
#undef sta
#undef coef
#undef vec_u
#undef vec_dx
#undef vec_x
        return dx;
      }
    } diff;

  public:
    FloatT t, delta_t;
    LongitudinalDynamics() : g(WGS84::gravity(0)), diff(), t(0), delta_t(0.01) {
      for(unsigned int i(0); i < num_of_stables; i++){
        sta[i] = FloatT(0);
      }
      for(unsigned int i(0); i < num_of_coefs; i++){
        coef[i] = FloatT(0);
      }
      diff.dynamics = this;
    }
    ~LongitudinalDynamics(){}

    states_t operator()(const states_t &x, const input_t &u) const {
      diff.vec_u = const_cast<input_t *>(&u);
      return nextByRK4(diff, t, x, delta_t);
    }
    states_t delta_1st(const states_t &x, const input_t &u) const {
      diff.vec_u = const_cast<input_t *>(&u);
      return diff(t, x);
    }
};

/**
 * 航空機の縦の運動の観測方程式
 * 
 */
template <class FloatT>
class LongitudinalObservationEquation {
  public:
    typedef LongitudinalObservationEquationSI<FloatT> obs_si_t;
    typedef typename obs_si_t::observed_t observed_t;
    typedef LongitudinalDynamics<FloatT> dynamics_t;
    typedef typename dynamics_t::states_t states_t;
    typedef typename dynamics_t::input_t input_t;
  protected:
    dynamics_t &dynamics;
  public:
    input_t *input;
    LongitudinalObservationEquation(dynamics_t &model) : dynamics(model) {}
    ~LongitudinalObservationEquation() {}
    observed_t operator()(const states_t &x) const {
      observed_t obs;

      states_t dx(dynamics.delta_1st(x, *input));
#define vec_x(name) const_cast<states_t &>(x).name()
#define vec_dx(name) dx.name()
#define vec_z(name) obs.name()
      FloatT d_alpha(vec_dx(a)),
          w(vec_x(a) * dynamics.sta_U0()),
          d_u(vec_dx(u)),
          d_w(d_alpha * dynamics.sta_U0());

      vec_z(tascg) = ::sqrt(::pow(dynamics.sta_U0() + vec_x(u), 2)
          + ::pow(dynamics.sta_W0() + w, 2));
      vec_z(alphacg) = (dynamics.sta_W0() / dynamics.sta_U0()) + vec_x(a);
      vec_z(theta) = dynamics.sta_theta0() + vec_x(theta);
      vec_z(q) = vec_x(q);
      vec_z(qdot) = vec_dx(q);
      // TODO: +d_u + qW ではない??
      vec_z(axcg) = -d_u
          - (vec_x(q) * (dynamics.sta_W0() + w))
          + (dynamics.const_g() * ::sin(vec_z(theta)));
      vec_z(azcg) = d_w
          - (vec_x(q) * (dynamics.sta_U0() + vec_x(u)))
          - (dynamics.const_g() * ::cos(vec_z(theta)));
#undef vec_x
#undef vec_dx
#undef vec_z

      return obs;
    }
};

#endif /* __LONGITUDINAL_H */