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

#ifndef __LATERAL_H
#define __LATERAL_H

#include "WGS84.h"

#include "base.h"

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

    /**
     * 状態量(x)
     *
     */
    MAKE_VALUES(StateValues, 19, FloatT,
        MAKE_ALIAS(b, 0)
        MAKE_ALIAS(p, 1)
        MAKE_ALIAS(r, 2)
        MAKE_ALIAS(phi, 3)
        MAKE_ALIAS(psi, 4)
        MAKE_ALIAS(Yb, 5)   // 以下、安定微係数, 10〜19はpromed derivative
        MAKE_ALIAS(Yp, 6)
        MAKE_ALIAS(Yr, 7)
        MAKE_ALIAS(Ydr, 8)
        MAKE_ALIAS(Lbp, 9)
        MAKE_ALIAS(Lpp, 10)
        MAKE_ALIAS(Lrp, 11)
        MAKE_ALIAS(Ldap, 12)
        MAKE_ALIAS(Ldrp, 13)
        MAKE_ALIAS(Nbp, 14)
        MAKE_ALIAS(Npp, 15)
        MAKE_ALIAS(Nrp, 16)
        MAKE_ALIAS(Ndap, 17)
        MAKE_ALIAS(Ndrp, 18));
    typedef StateValues states_t;

    /**
     * 操舵量入力(u)
     */
    MAKE_VALUES(InputValues, 2, FloatT,
        MAKE_ALIAS(dr, 0)
        MAKE_ALIAS(da, 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, psi0);
    make_accessor(sta, U0);
#undef make_accessor
    const FloatT const_g() const {return g;}

  protected:
    struct DifferentialEquation {
      mutable input_t *vec_u;
      mutable LateralDynamicsSI *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(b)
            = (coef(Yb) * vec_x(b)
                + (sta(W0) + coef(Yp)) * vec_x(p)
                - (sta(U0) - coef(Yr)) * vec_x(r)
                + dynamics->g * cos(sta(theta0)) * vec_x(phi)
                + coef(Ydr) * vec_u(dr)) / sta(U0);
        vec_dx(p) 
            = coef(Lbp) * vec_x(b)
                + coef(Lpp) * vec_x(p)
                + coef(Lrp) * vec_x(r)
                + coef(Ldap) * vec_u(da)
                + coef(Ldrp) * vec_u(dr);
        vec_dx(r) 
            = coef(Nbp) * vec_x(b)
                + coef(Npp) * vec_x(p)
                + coef(Nrp) * vec_x(r)
                + coef(Ndap) * vec_u(da)
                + coef(Ndrp) * vec_u(dr);
        vec_dx(phi) = vec_x(p) + vec_x(r) * ::tan(sta(theta0));
        vec_dx(psi) = vec_x(r) / ::cos(sta(theta0));
        vec_dx(Yb) = vec_dx(Yp) = vec_dx(Yr) = vec_dx(Ydr)
            = vec_dx(Lbp) = vec_dx(Lpp) = vec_dx(Lrp) = vec_dx(Ldap) = vec_dx(Ldrp)
            = vec_dx(Nbp) = vec_dx(Npp) = vec_dx(Nrp) = vec_dx(Ndap) = vec_dx(Ndrp)
            = 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(b), j(b)) = X(Yb) / STA(U0);
        res(i(b), j(Yb)) = X(b) / STA(U0);
        res(i(b), j(p)) = (STA(W0) + X(Yp)) / STA(U0);
        res(i(b), j(Yp)) = X(p) / STA(U0);
        res(i(b), j(r)) = (X(Yr) / STA(U0)) - 1;
        res(i(b), j(Yr)) = X(r) / STA(U0);
        res(i(b), j(phi)) = dynamics->g * cos(STA(theta0)) / STA(U0);
        res(i(b), j(Ydr)) = U(dr) / STA(U0);
        
        res(i(p), j(b)) = X(Lbp);
        res(i(p), j(Lbp)) = X(b);
        res(i(p), j(p)) = X(Lpp);
        res(i(p), j(Lpp)) = X(p);
        res(i(p), j(r)) = X(Lrp);
        res(i(p), j(Lrp)) = X(r);
        res(i(p), j(Ldap)) = U(da);
        res(i(p), j(Ldrp)) = U(dr);
        
        res(i(r), j(b)) = X(Nbp);
        res(i(r), j(Nbp)) = X(b);
        res(i(r), j(p)) = X(Npp);
        res(i(r), j(Npp)) = X(p);
        res(i(r), j(r)) = X(Nrp);
        res(i(r), j(Nrp)) = X(r);
        res(i(r), j(Ndap)) = U(da);
        res(i(r), j(Ndrp)) = U(dr);
        
        res(i(phi), j(p)) = 1;
        res(i(phi), j(r)) = ::tan(STA(theta0));
        
        res(i(psi), j(r)) = 1.0 / ::cos(STA(theta0));
        
        res(i(Yb), j(Yb)) = res(i(Yp), j(Yp)) = res(i(Yr), j(Yr)) = res(i(Ydr), j(Ydr))
            = res(i(Lbp), j(Lbp)) = res(i(Lpp), j(Lpp)) = res(i(Lrp), j(Lrp)) = res(i(Ldap), j(Ldap)) = res(i(Ldrp), j(Ldrp))
            = res(i(Nbp), j(Nbp)) = res(i(Npp), j(Npp)) = res(i(Nrp), j(Nrp)) = res(i(Ndap), j(Ndap)) = res(i(Ndrp), j(Ndrp))
            = 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(b), j(dr)) = X(Ydr) / STA(U0);
        
        res(i(p), j(da)) = X(Ldap);
        res(i(p), j(dr)) = X(Ldrp);
        
        res(i(r), j(da)) = X(Ndap);
        res(i(r), j(dr)) = X(Ndrp);
#undef STA
#undef U
#undef X
#undef j
#undef i
        return res;
      }
    } diff;

  public:
    FloatT t, delta_t;
    LateralDynamicsSI() : 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;
    }
    ~LateralDynamicsSI(){}

    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 LateralObservationEquationSI{
  public:
    /**
     * 観測量
     * システム同定向けの状態量を生成
     * 出力は全て機体固定軸に変換されることに注意
     * tascg[m/s]   真大気速度(C.G.)  => U_{0}, v から生成
     * betacg[rad]  横滑り角(C.G.)    => beta
     * phi[rad]     ロール(phi)       => phi
     * psi[rad]     ヨー(psi)         => psi0 + psi
     * p[rad]       ロール角速度      => p
     * r[rad]       ヨー角速度        => q
     * pdot[rad/s2] ロール角加速度    => pから運動方程式で生成
     * rdot[rad/s]  ヨー角加速度      => rから運動方程式で生成
     * aycg[m/s2]   加速度Y(C.G.)     => dot{v} = U_{0} * dot{beta}
     */
    MAKE_VALUES(ObservedValues, 9, FloatT,
        MAKE_ALIAS(tascg, 0)
        MAKE_ALIAS(betacg, 1)
        MAKE_ALIAS(phi, 2)
        MAKE_ALIAS(psi, 3)
        MAKE_ALIAS(p, 4)
        MAKE_ALIAS(r, 5)
        MAKE_ALIAS(pdot, 6)
        MAKE_ALIAS(rdot, 7)
        MAKE_ALIAS(aycg, 8));
    typedef ObservedValues observed_t;
    typedef LateralDynamicsSI<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;
    LateralObservationEquationSI(dynamics_t &model) 
        : dynamics(model), input(NULL) {}
    ~LateralObservationEquationSI() {}
    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_beta(vec_dx(b)),
          v(vec_x(b) * dynamics.sta_U0()),
          d_v(d_beta * dynamics.sta_U0());

      vec_z(tascg) = ::sqrt(::pow(dynamics.sta_U0(), 2)
          + ::pow(v, 2));
      vec_z(betacg) = vec_x(b);
      vec_z(phi) = vec_x(phi);
      vec_z(psi) = vec_x(psi) + dynamics.sta_psi0();
      vec_z(p) = vec_x(p);
      vec_z(r) = vec_x(r);
      vec_z(pdot) = vec_dx(p);
      vec_z(rdot) = vec_dx(r);
      vec_z(aycg) = d_v;
#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());
#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 U(name) input->name()
#define STA(name) dynamics.sta_ ## name()
        res(i(tascg), j(b)) = (STA(U0) * X(b));
        res(i(betacg), j(b)) = 1;
        res(i(phi), j(phi)) = 1;
        res(i(psi), j(psi)) = 1;
        res(i(p), j(p)) = 1;
        res(i(r), j(r)) = 1;
        res(i(pdot), j(b)) = X(Lbp);
        res(i(pdot), j(Lbp)) = X(b);
        res(i(pdot), j(p)) = X(Lpp);
        res(i(pdot), j(Lpp)) = X(p);
        res(i(pdot), j(r)) = X(Lrp);
        res(i(pdot), j(Lrp)) = X(r);
        res(i(pdot), j(Ldap)) = U(da);
        res(i(pdot), j(Ldrp)) = U(dr);
        res(i(rdot), j(b)) = X(Nbp);
        res(i(rdot), j(Nbp)) = X(b);
        res(i(rdot), j(p)) = X(Npp);
        res(i(rdot), j(Npp)) = X(p);
        res(i(rdot), j(r)) = X(Nrp);
        res(i(rdot), j(Nrp)) = X(r);
        res(i(rdot), j(Ndap)) = U(da);
        res(i(rdot), j(Ndrp)) = U(dr);
        res(i(aycg), j(b)) = X(Yb);
        res(i(aycg), j(Yb)) = X(b);
        res(i(aycg), j(p)) = STA(W0) + X(Yp);
        res(i(aycg), j(Yp)) = X(p);
        res(i(aycg), j(r)) = X(Yr) - STA(U0);
        res(i(aycg), j(Yr)) = X(r);
        res(i(aycg), j(phi)) = dynamics.const_g() * cos(STA(theta0));
        res(i(aycg), j(Ydr)) = U(dr);
#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 LateralDynamics{
  public:
    typedef LateralDynamicsSI<FloatT> dynamics_si_t;
    /**
     * 定常状態に関するインデックス
     */
    const static unsigned int num_of_stables = dynamics_si_t::num_of_stables;
    /**
     * 安定微係数に関するインデックス
     */
    enum {Yb, Yp, Yr, Ydr, Lbp, Lpp, Lrp, Ldap, Ldrp, Nbp, Npp, Nrp, Ndap, Ndrp};
    const static unsigned int num_of_coefs = 14;

    /**
     * 状態量(x)
     *
     */
    MAKE_VALUES(StateValues, 5, FloatT,
        MAKE_ALIAS(b, 0)
        MAKE_ALIAS(p, 1)
        MAKE_ALIAS(r, 2)
        MAKE_ALIAS(phi, 3)
        MAKE_ALIAS(psi, 4));
    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, psi0);
    make_accessor(sta, U0);
#undef make_accessor
#define make_accessor(variable, symbol) \
FloatT &variable ## _ ## symbol() {return variable[symbol];}
    make_accessor(coef, Yb);
    make_accessor(coef, Yp);
    make_accessor(coef, Yr);
    make_accessor(coef, Ydr);
    make_accessor(coef, Lbp);
    make_accessor(coef, Lpp);
    make_accessor(coef, Lrp);
    make_accessor(coef, Ldap);
    make_accessor(coef, Ldrp);
    make_accessor(coef, Nbp);
    make_accessor(coef, Npp);
    make_accessor(coef, Nrp);
    make_accessor(coef, Ndap);
    make_accessor(coef, Ndrp);
#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(Yb);
      check_str(Yp);
      check_str(Yr);
      check_str(Ydr);
      check_str(Lbp);
      check_str(Lpp);
      check_str(Lrp);
      check_str(Ldap);
      check_str(Ldrp);
      check_str(Nbp);
      check_str(Npp);
      check_str(Nrp);
      check_str(Ndap);
      check_str(Ndrp);
#undef check
      return -1;
    }
    const FloatT const_g() const {return g;}

  protected:
    struct DifferentialEquation {
      mutable input_t *vec_u;
      mutable LateralDynamics *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(b)
            = (coef(Yb) * vec_x(b)
                + (sta(W0) + coef(Yp)) * vec_x(p)
                - (sta(U0) - coef(Yr)) * vec_x(r)
                + dynamics->g * cos(sta(theta0)) * vec_x(phi)
                + coef(Ydr) * vec_u(dr)) / sta(U0);
        vec_dx(p) 
            = coef(Lbp) * vec_x(b)
                + coef(Lpp) * vec_x(p)
                + coef(Lrp) * vec_x(r)
                + coef(Ldap) * vec_u(da)
                + coef(Ldrp) * vec_u(dr);
        vec_dx(r) 
            = coef(Nbp) * vec_x(b)
                + coef(Npp) * vec_x(p)
                + coef(Nrp) * vec_x(r)
                + coef(Ndap) * vec_u(da)
                + coef(Ndrp) * vec_u(dr);
        vec_dx(phi) = vec_x(p) + vec_x(r) * ::tan(sta(theta0));
        vec_dx(psi) = vec_x(r) / ::cos(sta(theta0));
#undef sta
#undef coef
#undef vec_u
#undef vec_dx
#undef vec_x
        return dx;
      }
    } diff;

  public:
    FloatT t, delta_t;
    LateralDynamics() : 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;
    }
    ~LateralDynamics(){}

    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 LateralObservationEquation {
  public:
    typedef LateralObservationEquationSI<FloatT> obs_si_t;
    typedef typename obs_si_t::observed_t observed_t;
    typedef LateralDynamics<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;
    LateralObservationEquation(dynamics_t &model) : dynamics(model) {}
    ~LateralObservationEquation() {}
    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_beta(vec_dx(b)),
          v(vec_x(b) * dynamics.sta_U0()),
          d_v(d_beta * dynamics.sta_U0());

      vec_z(tascg) = ::sqrt(::pow(dynamics.sta_U0(), 2)
          + ::pow(v, 2));
      vec_z(betacg) = vec_x(b);
      vec_z(phi) = vec_x(phi);
      vec_z(psi) = vec_x(psi) + dynamics.sta_psi0();
      vec_z(p) = vec_x(p);
      vec_z(r) = vec_x(r);
      vec_z(pdot) = vec_dx(p);
      vec_z(rdot) = vec_dx(r);
      vec_z(aycg) = d_v;
#undef vec_x
#undef vec_dx
#undef vec_z

      return obs;
    }
};

#endif /* __LATERAL_H */