/**
 * @file Sylphide 航法ルーチン(リアルタイム)
 * 
 */

#include <std.h>
#include <log.h>
#include <tsk.h>
#include <mbx.h>

#include <cmath>

#include "navigation.h"
#include "kernel.h"

#include "param/constant.h"
#include "param/complex.h"
#include "param/vector3.h"
#include "param/quarternion.h"
#include "param/matrix.h"

#include "navigation/INS_GPS2.h"
#define BIAS_EST_MODE 2 // 初期ゼロ点推定含む
#define ZERO_CONST_ALPHA 0 //(FloatT(1) / 1E4) // 0
#include "navigation/INS_GPS_BE.h"

#include "navigation/MagneticField.h"

#include "util/fifo.h"

using namespace std;

template<class NumType>
static inline NumType pow2(const NumType &v){
  return v * v;
}

#if !defined(NAVLIB)
template<class T>
static inline void log_printf(String format, T &t){
  LOG_printf(&trace, format, t);
}
static inline void log_printf(String format){
  LOG_printf(&trace, format);
}
#else
template<class T>
static inline void log_printf(String format, T &t){}
static inline void log_printf(String format){}
#endif

struct NAVConst {
  static const bool use_udkf; ///< KFにUDKFを使うか(重い)
  static const bool bias_on;  ///< バイアス推定を行うか
};
const bool NAVConst::use_udkf = false;
const bool NAVConst::bias_on = true;

NavigationConfig navigation_config = {
  true, // dump_tu
  false,  // dump_mu
  USE_EXTERNAL_NAV_INFO, // use_external_nav_info
  {0, 0, 0}, // mag_hard_iron_offset
};

class NAV {
  public:
    virtual void init(
        const float_sylph_t &latitude, 
        const float_sylph_t &longitude, 
        const float_sylph_t &height,
        const float_sylph_t &v_north, 
        const float_sylph_t &v_east, 
        const float_sylph_t &v_down,
        const float_sylph_t &yaw, 
        const float_sylph_t &pitch, 
        const float_sylph_t &roll) = 0;
    virtual float_sylph_t &operator[](unsigned index) = 0;
    virtual NAV &update(
        const Vector3<float_sylph_t> &accel, 
        const Vector3<float_sylph_t> &gyro, 
        const float_sylph_t &deltaT) = 0;
    
    virtual NAV &correct(
        const GPS_UBLOX_3D<float_sylph_t> &gps,
        const float_sylph_t &deltaT) = 0;
    
    virtual void convert(NAVInfo &info) const = 0;

    virtual ~NAV() {}
};

template <class INS_GPS_Core>
class INS_GPS_NAV : public NAV {
  protected:
    class INS_GPS_RT;
    struct snapshot_content_t {
      INS_GPS_RT *ins_gps;
      Matrix<float_sylph_t> A;
      Matrix<float_sylph_t> GQGt;
      float_sylph_t deltaT_update;
      snapshot_content_t(){}
      snapshot_content_t(
          INS_GPS_RT *_ins_gps,
          const Matrix<float_sylph_t> &_A,
          const Matrix<float_sylph_t> &_GQGt, 
          const float_sylph_t &_deltaT)
          : ins_gps(_ins_gps), A(_A), GQGt(_GQGt), 
          deltaT_update(_deltaT){
      }
      snapshot_content_t &operator=(const snapshot_content_t &another){
        ins_gps = another.ins_gps;
        A = another.A;
        GQGt = another.GQGt;
        deltaT_update = another.deltaT_update;
        return *this;
      }
    };
    typedef FIFO<snapshot_content_t, operator_eq_t> snapshots_t;
    snapshots_t snapshots;
    class INS_GPS_RT : public INS_GPS_Core {
      protected:
        snapshots_t &snapshots;
      public:
        INS_GPS_RT(snapshots_t &_snapshots) 
            : INS_GPS_Core(), snapshots(_snapshots) {}
        INS_GPS_RT(
            const INS_GPS_RT &orig, 
            const bool deepcopy = false)
            : INS_GPS_Core(orig, deepcopy), snapshots(orig.snapshots){}
        INS_GPS_RT &operator=(const INS_GPS_RT &another){
          snapshots = another.snapshots;
          INS_GPS_Core::operator=(another);
          return *this;
        }
      protected:
        /**
         * 時間更新におけるコールバック関数。
         * 
         * @param A A行列
         * @param B B行列
         * @patam deltaT 時間間隔
         */
        void before_update_INS(
            const Matrix<float_sylph_t> &A, const Matrix<float_sylph_t> &B, 
            const float_sylph_t &deltaT){
          
          // 空きがなかったら古いものを捨てる
          if(!snapshots.has_margin()){
            delete snapshots[0].ins_gps;
            snapshots.skip(1);
          }
          
          Matrix<float_sylph_t> Gamma(B * deltaT);
          
          snapshot_content_t snapshot(new INS_GPS_RT(*this, false), 
              A, Gamma * INS_GPS_Core::getFilter().getQ() * Gamma.transpose(),
              deltaT);
          
          snapshots.push(&snapshot);
        }
    };
    INS_GPS_RT latest;
  public:
    INS_GPS_NAV() 
        : NAV(), snapshots(64), 
        latest(snapshots) {}
    ~INS_GPS_NAV() {}
    void init(
        const float_sylph_t &latitude, 
        const float_sylph_t &longitude, 
        const float_sylph_t &height,
        const float_sylph_t &v_north, 
        const float_sylph_t &v_east, 
        const float_sylph_t &v_down,
        const float_sylph_t &yaw, 
        const float_sylph_t &pitch, 
        const float_sylph_t &roll){
      
      latest.initPosition(latitude, longitude, height);
      latest.initVelocity(v_north, v_east, v_down);
      latest.initAttitude(yaw, pitch, roll);

      /**
       * Kalman Filterの誤差共分散行列(P)の初期値
       * 対角成分にある程度大きな値に設定のこと
       * それぞれの要素が持つ意味は
       *  0〜2 速度の分散(単位はm/s)
       *  3〜5 緯度、経度、アジマスをあわせてQuaternion表現した分散(1E-8程度で十分大きい)
       *  6 高度の分散(単位はm)
       *  7〜9 姿勢をQuaternion表現した分散(1E-4程度で十分大きい)
       */
      {
        Matrix<float_sylph_t> P(latest.getFilter().getP());
        P(0, 0) = P(1, 1) = P(2, 2) = 1E+1;
        P(3, 3) = P(4, 4) = P(5, 5) = 1E-8;
        P(6, 6) = 1E+2;
        P(7, 7) = P(8, 8) = P(9, 9) = 1E-4; //CRS03 1E-1;   //-4
        latest.getFilter().setP(P);
      }
      
      /**
       * Kalman Filterの誤差共分散行列(Q)の初期値
       * 対角成分にそれぞれ
       *  0〜2 : 加速度計X,Y,Zの分散(単位はm/s^2)
       *  3〜5 : ジャイロX,Y,Zの分散(単位はrad/s)
       *  6 : 重力加速度の分散(単位はm/s^2)、通常小さな値(1E-6)でよい
       * を設定のこと
       */
      {
        Matrix<float_sylph_t> Q(latest.getFilter().getQ());
        // TODO: 校正データより、下は9001とほぼ同じ
        Q(0, 0) = pow2(1E4 / 1.4E5);
        Q(1, 1) = pow2(1E4 / 1.4E5);
        Q(2, 2) = pow2(1E4 / 1.4E5);
        Q(3, 3) = pow2(4E3 / (3E4 * 180 / M_PI));
        Q(4, 4) = pow2(4E3 / (3E4 * 180 / M_PI));
        Q(5, 5) = pow2(4E3 / (3E4 * 180 / M_PI));
        Q(6, 6) = 1E-6; //1E-14
        latest.getFilter().setQ(Q);
      }
    }
    
    INS_GPS_Core &current(){return latest;}

    void convert(NAVInfo &info) const{
      info.longitude = latest.longitude();
      info.latitude = latest.latitude();
      info.height = latest.height();
      info.v_north = latest.v_north();
      info.v_east = latest.v_east();
      info.v_down = latest.v_down();
      info.heading = latest.heading();
      info.euler_phi = latest.euler_phi();
      info.euler_theta = latest.euler_theta();
      info.euler_psi = latest.euler_psi();
      info.azimuth = latest.azimuth();
    }
    
    float_sylph_t &operator[](unsigned index){return latest[index];}
    
    NAV &update(
        const Vector3<float_sylph_t> &accel, 
        const Vector3<float_sylph_t> &gyro, 
        const float_sylph_t &deltaT){
      latest.update(accel, gyro, deltaT);
      return *this;
    }
  protected:
    /**
     * snapshotsの調整
     * GPS情報と同期するタイミングの情報がsnapshotsの先頭になるようにする
     * 
     * @param deltaT GPSのタイミング(負の値が前の時刻)
     * @return bool 調整がうまくいった場合true
     */
    bool pre_correct_info(float_sylph_t deltaT){
      // 未来の時間を返すのはおかしい 
      if(deltaT > 0){return false;}
      
      for(int i(-1); i >= -snapshots.stored(); i--){
        deltaT += snapshots[i].deltaT_update;
        if(deltaT > float_sylph_t(-(1E-3 * NAV_UPDATE_RATIO_ms / 2))){ // 一番近い時刻を探してくる
          int bounds(snapshots.stored() + i);
          for(int j(0); j < bounds; j++){
            delete snapshots[j].ins_gps;
          }
          snapshots.skip(snapshots.stored() + i);
          return true;
        }
      }
      
      // 予定したものよりも古い時刻のGPSデータなので使えない
      return false;
    }
    
    /**
     * 得られた修正情報を加工し、実際に修正を加えるまでを行う
     * 
     * @param info 修正情報
     */
    void post_correct_info(CorrectInfo<float_sylph_t> &info){
      Matrix<float_sylph_t> sum_A(info.H.columns(), info.H.columns());
      Matrix<float_sylph_t> sum_GQGt(sum_A.rows(), sum_A.rows());
      float_sylph_t bar_delteT(0);
      int i(0);
      for(; i < snapshots.stored(); i++){
        sum_A += snapshots[i].A;
        sum_GQGt += snapshots[i].GQGt;
        bar_delteT += snapshots[i].deltaT_update;
      }
      if(i > 0){
        bar_delteT /= i;
        Matrix<float_sylph_t> sum_A_GQGt(sum_A * sum_GQGt);
        info.R += info.H
            * (sum_GQGt -= ((sum_A_GQGt + sum_A_GQGt.transpose()) *= (bar_delteT * (i + 1) / (2 * i))))
            * info.H.transpose();
        info.H *= (Matrix<float_sylph_t>::getI(sum_A.rows()) - sum_A * bar_delteT);
      }
      latest.correct(info);
    }
  
  public:
    /**
     * アーム長を使わないモード
     * 
     * @param gps GPSの情報
     * @param deltaT いつのタイミングの情報か(負の値が前の時刻)
     */
    NAV &correct(
        const GPS_UBLOX_3D<float_sylph_t> &gps,
        const float_sylph_t &deltaT){
      if(pre_correct_info(deltaT)){
        CorrectInfo<float_sylph_t> info(
            snapshots[0].ins_gps->correct_info(gps));
        post_correct_info(info);
      }
      return *this;
    }
};

template <class INS_GPS_BE_Core>
class INS_GPS_BE_NAV : public INS_GPS_NAV<INS_GPS_BE_Core> {
  public:
    typedef INS_GPS_NAV<INS_GPS_BE_Core> super_t;
    INS_GPS_BE_NAV() : super_t() {}
    ~INS_GPS_BE_NAV(){}

    void init(
            const float_sylph_t &latitude,
            const float_sylph_t &longitude,
            const float_sylph_t &height,
            const float_sylph_t &v_north,
            const float_sylph_t &v_east,
            const float_sylph_t &v_down,
            const float_sylph_t &yaw,
            const float_sylph_t &pitch,
            const float_sylph_t &roll){

      /**
       * バイアス項に関する設定を行う
       */
      super_t::latest.beta_accel() *= 0.1;
      super_t::latest.beta_gyro() *= 0.1;//mems_g.BETA;
      
      {
        Matrix<float_sylph_t> P(super_t::latest.getFilter().getP());
        P(10, 10) = P(11, 11) = P(12, 12) = 1E-2;
        P(13, 13) = P(14, 14) = P(15, 15) = 1E-6;
        super_t::latest.getFilter().setP(P);
      }
      {
        Matrix<float_sylph_t> Q(super_t::latest.getFilter().getQ());
        Q(7, 7) = 1E-6;
        Q(8, 8) = 1E-6;
        Q(9, 9) = 1E-6;
        Q(10, 10) = 1E-8;
        Q(11, 11) = 1E-8;
        Q(12, 12) = 1E-8;
        super_t::latest.getFilter().setQ(Q);
      }

      super_t::init(
          latitude, longitude, height,
          v_north, v_east, v_down,
          yaw, pitch, roll);
    }

    void convert(NAVInfo &info) const{
      super_t::convert(info);

      // バイアスの補正
      Vector3<float_sylph_t>
          &bias_accel(const_cast<INS_GPS_BE_NAV *>(this)->latest.bias_accel()),
          &bias_gyro(const_cast<INS_GPS_BE_NAV *>(this)->latest.bias_gyro());
      for(int i(0); i < sizeof(info.corrected_accel) / sizeof(info.corrected_accel[0]); i++){
        info.corrected_accel[i] += bias_accel.get(i);
      }
      for(int i(0); i < sizeof(info.corrected_omega) / sizeof(info.corrected_omega[0]); i++){
        info.corrected_omega[i] += bias_gyro.get(i);
      }
    }
};

// TIのコンパイラが賢くないので、特殊化で回避(定数を定数として認識しない!!)
template <>
const unsigned Filtered_INS_BiasEstimated<
    float_sylph_t, 
    KalmanFilter<float_sylph_t>, 
    Filtered_INS2<float_sylph_t, KalmanFilter<float_sylph_t> > >::P_SIZE
        = 16;
template <>
const unsigned Filtered_INS_BiasEstimated<
    float_sylph_t, 
    KalmanFilterUD<float_sylph_t>, 
    Filtered_INS2<float_sylph_t, KalmanFilterUD<float_sylph_t> > >::P_SIZE
        = 16;

// どのタイプのINS/GPSを使うか
template <bool bias_on, bool use_udkf>
struct NAV_instance_t { 
  typedef INS_GPS_NAV<
                INS_GPS2<
                    float_sylph_t,
                    KalmanFilter<float_sylph_t> > > type_name;
};

template <>
struct NAV_instance_t<true, true> { 
  typedef INS_GPS_BE_NAV<
              INS_GPS2_BiasEstimated<float_sylph_t> > type_name;
};

template <>
struct NAV_instance_t<true, false> { 
  typedef INS_GPS_BE_NAV<
              INS_GPS2_BiasEstimated<
                  float_sylph_t,
                  KalmanFilter<float_sylph_t> > > type_name;
};

template <>
struct NAV_instance_t<false, true> { 
  typedef INS_GPS_NAV<
              INS_GPS2<float_sylph_t> > type_name;
};

/**
 * 現在の状態を表現するクラス
 * 
 */
class NAVStatus {
  public:
    static const float_sylph_t INTERVAL_THRESHOLD;
    static const float_sylph_t INTERVAL_FORCE_VALUE;
  protected:    
    bool initialized;
    typedef NAV_instance_t<
        NAVConst::bias_on, NAVConst::use_udkf>::type_name nav_t;
    nav_t nav;
    int yaw_correct_with_velocity;
    int yaw_correct_with_magnetic;
    
    // 初期化などのため、ある程度TUデータをためておく
    unsigned int tu_info_counter, tu_info_latest_index;
    static const int tu_info_pool_size = 0x40;
    TimeUpdateInfo tu_info_pool[tu_info_pool_size];

    // GPS観測と同期させるなどのため、磁気データをためておく
    unsigned int mag_info_counter;
    static const int mag_info_pool_size = 8;
    MagInfo mag_info_pool[mag_info_pool_size];
    
    ///< 地磁気モデル更新後の経過カウンタ
    int mag_model_counter_after_update;
    ///< 地磁気モデル(少しの移動であれば再計算の手間を省くため)
    MagneticField::filed_components_res_t mag_model;
    ///< 磁気偏角(磁北と真北のずれ)
    float_sylph_t magnetic_declination;

  public:
    /**
     * 最後に登録された地磁気情報を返す
     * 
     * @return 結果
     */
    MagInfo get_mag_info(){
      return mag_info_pool[(mag_info_counter + (mag_info_pool_size - 1)) % mag_info_pool_size];
    }

    /**
     * 指定された時刻の地磁気情報を返す
     * 
     * @param itow 指定時刻
     * @return 結果、ただし指定時刻にあったデータがない場合、itowが一致しない
     */
    MagInfo get_mag_info(const float_sylph_t &itow){
      MagInfo res;
      int i(mag_info_pool_size - 1), j(0);
      if((mag_info_pool[i].itow > itow) || (mag_info_pool[j].itow <= itow)){
        for(i = 0, j = 1; j < mag_info_pool_size; i++, j++){
          if((mag_info_pool[i].itow <= itow) && (mag_info_pool[j].itow > itow)){
            break;
          }
        }
      }
      
      if(j == mag_info_pool_size){ // 該当なし
        res.itow = 0;
        res.mag_raw[0] = 1;
        res.mag_raw[1] = res.mag_raw[2] = 0;
        return res;
      }
      
      float_sylph_t 
          weight_i(
              (float_sylph_t)(mag_info_pool[j].itow - itow) 
              / (mag_info_pool[j].itow - mag_info_pool[i].itow)), 
          weight_j(1. - weight_i); // 線形補完重み
      
      res.itow = itow;
      for(int k(0); k < sizeof(res.mag_raw) / sizeof(res.mag_raw[0]); k++){
        res.mag_raw[k] = (mag_info_pool[i].mag_raw[k] * weight_i) 
            + (mag_info_pool[j].mag_raw[k] * weight_j);
      }
      return res;
    }
    
    /**
     * 地磁気情報の登録
     * 
     * @param mag 地磁気情報
     */
    void set_mag_info(const MagInfo &mag){
      mag_info_pool[(mag_info_counter++) % mag_info_pool_size] = mag;
    }
    
    /**
     * 地磁気からヨー角補正量を求める
     * 
     * @param attitude 姿勢
     * @return ヨー角補正量[rad]
     */
    float_sylph_t get_mag_delta_yaw(
        const Quarternion<float_sylph_t> &attitude,
        const MagInfo &mag){
      
      typedef Vector3<float_sylph_t> vec_t;
      typedef Quarternion<float_sylph_t> quat_t;
      
      // 姿勢の影響をキャンセルする(逆回転)
      vec_t mag_vec(mag.mag_raw[0], mag.mag_raw[1], mag.mag_raw[2]);
      vec_t mag_horizontal((attitude * quat_t(0, mag_vec) * attitude.conj()).vector());
      
      // 地磁気モデルを考慮してヨー角補正量を求める
      return magnetic_declination - atan2(mag_horizontal[1], mag_horizontal[0]);
    }
    
    /**
     * 地磁気からヨー角を求める
     *
     * @param pitch ピッチ角[rad]
     * @param roll ヨー角[rad]
     * @return ヨー角[rad]
     */
    float_sylph_t get_mag_yaw(
        const float_sylph_t &pitch, const float_sylph_t &roll,
        const MagInfo &mag){

      float_sylph_t ct(cos(pitch / 2)), st(sin(pitch / 2)),
          cp(cos(roll / 2)), sp(sin(roll / 2));

      return get_mag_delta_yaw(
          Quarternion<float_sylph_t>(ct * cp, ct * sp, st * cp, -st * sp), mag);
    }

    /**
     * 地磁気のモデルを更新する
     *
     */
    void update_mag_model(
        const float_sylph_t &latitude, const float_sylph_t &longitude, const float_sylph_t &altitude){
      // 地磁気モデル修正
      mag_model = MagneticField::filed_components(IGRF2012::IGRF2015,
              latitude, longitude, altitude);
      // 偏角計算
      magnetic_declination = atan2(mag_model.east, mag_model.north);
    }

    struct pseudo_gravity_vector {
      float_sylph_t roll, pitch;
    };

    /**
     * 加速度計から疑似重力を求める
     *
     */
    pseudo_gravity_vector get_pseudo_gravity_vector() const {
      pseudo_gravity_vector res;

      typedef Vector3<float_sylph_t> vec_t;

      vec_t acc(0, 0, 0);

      // これまでの平均値を求める(最後の割り算は次の正規化があるので省略)
      for(int i(0); i < tu_info_pool_size; i++){
        acc[0] += tu_info_pool[i].accel[0];
        acc[1] += tu_info_pool[i].accel[1];
        acc[2] += tu_info_pool[i].accel[2];
      }

      // 正規化
      vec_t acc_reg(-acc / acc.abs());

      // ピッチ角を求める, x方向は最後のロール回転で影響をうけないことを利用
      res.pitch = -asin(acc_reg[0]);
      // 続いてロール角を求める、xとz成分の比から
      res.roll = atan2(acc_reg[1], acc_reg[2]);

      return res;
    }

  public:
    NAVStatus() 
        : initialized(false), 
        nav(), 
        yaw_correct_with_velocity(0), yaw_correct_with_magnetic(0),
        tu_info_counter(0), tu_info_latest_index(tu_info_pool_size - 1), tu_info_pool(),
        mag_info_counter(0), mag_info_pool(),
        mag_model_counter_after_update(0), mag_model(), magnetic_declination(0) {
      for(int i(0); i < mag_info_pool_size; i++){
        mag_info_pool[i].itow = 0;
        mag_info_pool[i].mag_raw[0] = 1; // 北向きで初期化
        mag_info_pool[i].mag_raw[1] = mag_info_pool[i].mag_raw[2] = 0;
      }
      // 地磁気モデル初期値(磁北と真北は一致、下方向成分はない)、偏角は0
      mag_model.north = 1;
      mag_model.east = mag_model.down = 0;
    }
    ~NAVStatus(){}
  
  public:
    NAV &navigation(){return nav;}
    
    void convert(NAVInfo &info) const {
      info.itow = tu_info_pool[tu_info_latest_index].itow;
      std::memcpy(info.corrected_accel, tu_info_pool[tu_info_latest_index].accel, sizeof(info.corrected_accel));
      std::memcpy(info.corrected_omega, tu_info_pool[tu_info_latest_index].omega, sizeof(info.corrected_omega));

      nav.convert(info);
      if(!initialized){
        if(tu_info_counter > tu_info_pool_size){
          // 加速度計から疑似姿勢を計算する
          pseudo_gravity_vector gravity_vector(get_pseudo_gravity_vector());
          info.euler_phi = gravity_vector.roll;
          info.euler_theta = gravity_vector.pitch;
        }
      }
    }
    operator NAVInfo() const {
      NAVInfo res;
      convert(res);
      return res;
    }
    
    /**
     * 時間更新を行う関数
     * 
     * 一定時間間隔で行われるAD変換結果より得られる
     * 加速度(加速度計に依存)、角速度(ジャイロに依存)を利用する。
     * 
     * @param a_packet AD変換結果
     */
    void time_update(const TimeUpdateInfo &tu_info){
      
      //log_printf("TU : %f", tu_info.itow);
      
      if(initialized){
        //前回時刻よりの差分を計算
        float_sylph_t how_old(tu_info_pool[tu_info_latest_index].how_old(tu_info));
        if((how_old < 0) || (how_old >= INTERVAL_THRESHOLD)){
          // 時刻の不連続点を観測したら、強制的に書き換える
          how_old = INTERVAL_FORCE_VALUE;
        }
        Vector3<float_sylph_t>
            accel(tu_info.accel[0], tu_info.accel[1], tu_info.accel[2]),
            omega(tu_info.omega[0], tu_info.omega[1], tu_info.omega[2]);
        nav.update(accel, omega, how_old);
      }
      
      // 履歴として保存
      if((++tu_info_latest_index) >= tu_info_pool_size){
        tu_info_latest_index = 0;
      }
      tu_info_pool[tu_info_latest_index] = tu_info;
      tu_info_counter++;
    }
    
    /**
     * 観測更新を行う関数
     * 
     * GPS受信機より観測量(位置、速度)が得られたときに行う
     * 
     * @param mu_info GPS受信機より得られる観測量
     */
    void measurement_update(const MeasurementUpdateInfo &mu_info){
      
      if(initialized){
        log_printf("MU : %f", mu_info.itow);
        
        GPS_UBLOX_3D<float_sylph_t> ubx;
        {
          ubx.v_n = mu_info.vel_ned[0];
          ubx.v_e = mu_info.vel_ned[1];
          ubx.v_d = mu_info.vel_ned[2];
          ubx.sigma_vel = mu_info.acc_vel;
          ubx.latitude = deg2rad(mu_info.llh[0]);
          ubx.longitude = deg2rad(mu_info.llh[1]);
          ubx.height = mu_info.llh[2];
          ubx.sigma_2d = mu_info.acc_2d;
          ubx.sigma_height = mu_info.acc_vertical;
        }
        
        float_sylph_t deltaT_rollback(tu_info_pool[tu_info_latest_index].how_old(mu_info));
        // deltaT_rollback will be large positive value when ITOW is rolled over.
        if(deltaT_rollback >= (((float_sylph_t)60 * 60 * 24 * 7) - INTERVAL_THRESHOLD)){
          deltaT_rollback -= ((float_sylph_t)60 * 60 * 24 * 7);
        }
        nav.correct(ubx, deltaT_rollback);
        
        // 地磁気モデルの更新チェック
        if(++mag_model_counter_after_update >= (5 * 60 * 5)){ // about 5min; 5Hz * (60 * 5)s times
          mag_model_counter_after_update = 0;
          update_mag_model(nav.current().latitude(), nav.current().longitude(), nav.current().height());
        }

        float_sylph_t vel_2d_abs2(
            pow2(mu_info.vel_ned[0]) + pow2(mu_info.vel_ned[1])); // 2D速度二乗を計算しておく

        // ヨーを速度方向から補正する
        if(YAW_CORRECT_WITH_VELOCITY != 0){
          // 速度チェック
          if(vel_2d_abs2 >= pow2(YAW_CORRECT_WITH_VELOCITY_WHEN_SPEED_MORE_THAN)){
            // 補正回数チェック
            if((YAW_CORRECT_WITH_VELOCITY < 0)
                || (yaw_correct_with_velocity < YAW_CORRECT_WITH_VELOCITY)){
              float_sylph_t delta_psi(atan2(mu_info.vel_ned[1], mu_info.vel_ned[0]) - nav.current().heading()); // atan2 => Y, Xの順
              if(YAW_CORRECT_VIA_KF){
                nav.current().correct_yaw(
                    delta_psi, pow2(deg2rad((float_sylph_t)5))); // 誤差分散は5度にしておく
              }else{
                nav.current().mod_euler_psi(delta_psi);
              }
              yaw_correct_with_velocity++;
            }
          }else{
            if(YAW_CORRECT_WITH_VELOCITY > 0){
              yaw_correct_with_velocity = 0; // 速度が低下した場合、補正回数制限がある場合は補正回数をリセット
            }
          }
        }

        // ヨーを地磁気から補正する
        if(YAW_CORRECT_WITH_MAGNETIC_WHEN_SPEED_LESS_THAN > 0){
          // 速度チェック
          if(vel_2d_abs2 <= pow2(YAW_CORRECT_WITH_MAGNETIC_WHEN_SPEED_LESS_THAN)){
            float_sylph_t delta_psi(get_mag_delta_yaw(nav.current().n2b(), get_mag_info()));
            if(YAW_CORRECT_VIA_KF){
              nav.current().correct_yaw(
                  delta_psi, pow2(deg2rad((float_sylph_t)2))); // 誤差分散は2度にしておく
            }else{
              nav.current().mod_euler_psi(delta_psi);
            }
            yaw_correct_with_magnetic++;
          }
        }
      }else if((tu_info_counter >= 0x200) // 初期姿勢推定のため、ある程度待つ
          && (mu_info.acc_2d <= 20.) && (mu_info.acc_vertical <= 10.)){

        /*
         * GPS受信機の出力する推定2D位置誤差が20m以内、
         * 高度誤差が10m以内になったときをフィルター開始時点とする
         * 以後、Time UpdateとMeasurement Updateが有効になる
         */
        
        float_sylph_t yaw(0), pitch(0), roll(0);
        float_sylph_t latitude(deg2rad(mu_info.llh[0])), longitude(deg2rad(mu_info.llh[1]));

        // 加速度計と地磁気を使って初期姿勢を推定する(静止状態を仮定)
        {
          pseudo_gravity_vector gravity_vector(get_pseudo_gravity_vector());
          pitch = gravity_vector.pitch;
          roll = gravity_vector.roll;
          
          // 地磁気モデル、偏角修正
          update_mag_model(latitude, longitude, mu_info.llh[2]);
          
          // 最後に地磁気を利用してヨー角を求める
          yaw = get_mag_yaw(pitch, roll, get_mag_info(mu_info.itow));
        }
        
        log_printf("Init : %f", mu_info.itow);
        nav.init(
            latitude, longitude, mu_info.llh[2],
            mu_info.vel_ned[0], mu_info.vel_ned[1], mu_info.vel_ned[2],
            yaw, pitch, roll);
        initialized = true;
        
        // TODO: ON時バイアスの推定
      }
      
      log_printf("MU_TU done");
    }
    
    bool is_initialized(){return initialized;}
} nav_status;

const float_sylph_t NAVStatus::INTERVAL_THRESHOLD = 10;
const float_sylph_t NAVStatus::INTERVAL_FORCE_VALUE = 1E-3 * NAV_UPDATE_RATIO_ms;

void NAVInfo::encode_N0(
    char buf[32], 
    unsigned char seq_num,
    const bool valid) const {
  typedef Uint32 v_u32_t;
  typedef Int32 v_s32_t;
  typedef Int16 v_s16_t;
  
  v_u32_t t(itow * 1000);
  v_s32_t lat(rad2deg(latitude) * 1E7), 
      lng(rad2deg(longitude) * 1E7), 
      h(height * 1E4);
  v_s16_t v_n(v_north * 1E2), 
      v_e(v_east * 1E2), 
      v_d(v_down * 1E2);
  v_s16_t psi(rad2deg(heading) * 1E2), 
      theta(rad2deg(euler_theta) * 1E2), 
      phi(rad2deg(euler_phi) * 1E2);
  buf[0] = 'N';
  buf[1] = seq_num;
  buf[2] = '\0';
  buf[3] = '\0';
  
  // 有効なデータでない場合はフラグを立てておく
  if(!valid){buf[3] |= 0x80;}
  
  memcpy(&buf[4], &t, sizeof(v_u32_t));
  memcpy(&buf[8], &lat, sizeof(v_s32_t));
  memcpy(&buf[12], &lng, sizeof(v_s32_t));
  memcpy(&buf[16], &h, sizeof(v_s32_t));
  memcpy(&buf[20], &v_n, sizeof(v_s16_t));
  memcpy(&buf[22], &v_e, sizeof(v_s16_t));
  memcpy(&buf[24], &v_d, sizeof(v_s16_t));
  memcpy(&buf[26], &psi, sizeof(v_s16_t));
  memcpy(&buf[28], &theta, sizeof(v_s16_t));
  memcpy(&buf[30], &phi, sizeof(v_s16_t));
}

extern "C" {
void nav_time_update(){
  
  //TSK_Handle my_tsk(TSK_self());
  
  MBX_Handle mbx_tu(
      Kernel::get_instance().mbx(Kernel::MBX_TU_INFO));
  MBX_Handle mbx_nav(
    Kernel::get_instance().mbx(Kernel::MBX_NAV_INFO));
  TimeUpdateInfo tu_info;
  NAVInfo nav_info;
  
  while(true){
    // メールボックスから時間更新情報取得
    MBX_pend(mbx_tu, &tu_info, SYS_FOREVER);
        
    nav_status.time_update(tu_info);
    
    if(navigation_config.use_external_nav_info){continue;} //外部情報使う場合は時間更新しない

    if(navigation_config.dump_tu){
      nav_status.convert(nav_info);
      MBX_post(mbx_nav, &nav_info, 0);
    }
  }
}

void nav_measurement_update(){
  
  MBX_Handle mbx_mu(
      Kernel::get_instance().mbx(Kernel::MBX_MU_INFO));
  MBX_Handle mbx_mag(
      Kernel::get_instance().mbx(Kernel::MBX_MAG_INFO));
  MBX_Handle mbx_nav(
    Kernel::get_instance().mbx(Kernel::MBX_NAV_INFO));
  MeasurementUpdateInfo mu_info;
  MagInfo mag_info;
  NAVInfo nav_info;
  
  while(true){
    // メールボックスから観測更新情報取得
    MBX_pend(mbx_mu, &mu_info, SYS_FOREVER);
    
    // 地磁気情報があれば先に登録しておく
    while(MBX_pend(mbx_mag, &mag_info, 0)){
      for(int i(0); i < sizeof(mag_info.mag_raw) / sizeof(mag_info.mag_raw[0]); ++i){
        mag_info.mag_raw[i] -= navigation_config.mag_hard_iron_offset[i];
      }
      nav_status.set_mag_info(mag_info);
    }
    
    if(navigation_config.use_external_nav_info){continue;} //外部情報使う場合は観測更新しない

    nav_status.measurement_update(mu_info);
    
    if(navigation_config.dump_mu){
      nav_status.convert(nav_info);
      MBX_post(mbx_nav, &nav_info, 0);
    }
  }
}
}

ExternalNAVInfo::operator NAVInfo() const{
  NAVInfo nav_info;
  nav_info.itow = itow;
  nav_info.longitude = longitude_rad;
  nav_info.latitude = latitude_rad;
  nav_info.height = height_m;
  nav_info.v_north = v_north_ms;
  nav_info.v_east = v_east_ms;
  nav_info.v_down = v_down_ms;
  nav_info.heading = heading_rad;
  nav_info.euler_phi = roll_rad;
  nav_info.euler_theta = pitch_rad;
  nav_info.euler_psi = heading_rad;
  nav_info.azimuth = 0;
  std::memcpy(nav_info.corrected_accel, accel_ms2, sizeof(nav_info.corrected_accel));
  std::memcpy(nav_info.corrected_omega, omega_rads, sizeof(nav_info.corrected_omega));
  return nav_info;
}

void ExternalNAVInfo::push() const {
  if(!navigation_config.use_external_nav_info){return;} //外部情報使う場合のみ有効
  NAVInfo nav_info(this->operator NAVInfo());
  MBX_post(Kernel::get_instance().mbx(Kernel::MBX_NAV_INFO), &nav_info, 0);
}