#ifndef __NAVIGATION_H__
#define __NAVIGATION_H__

#include <std.h>
#include "config.h"

///< 時刻ラベル付情報
struct TimeStampedInfo {
  float_sylph_t itow;
  
  /**
   * 異なるパケットよりどれだけ以前のものか計算する関数
   * 
   * @param new_info 異なるパケット
   */
  float_sylph_t how_old(const TimeStampedInfo &new_info) const {
    return new_info.itow - itow;
  }
};

///< 慣性力など時間更新情報
struct TimeUpdateInfo : public TimeStampedInfo {
  float_sylph_t accel[3];
  float_sylph_t omega[3];
};

///< GPS受信機より位置、速度など観測更新情報
struct MeasurementUpdateInfo : public TimeStampedInfo {
  float_sylph_t llh[3];
  float_sylph_t acc_2d, acc_vertical;
  float_sylph_t vel_ned[3];
  float_sylph_t acc_vel;
};

///< 地磁気情報
struct MagInfo : public TimeStampedInfo {
  float_sylph_t mag_raw[3];
};

///< 航法情報
struct NAVInfo : public TimeStampedInfo {
  float_sylph_t longitude; ///< 経度[rad]
  float_sylph_t latitude; ///< 緯度[rad]
  float_sylph_t height; ///< 高度[m]
  float_sylph_t v_north; ///< 北方向速度[m/s]
  float_sylph_t v_east; ///< 東方向速度[m/s]
  float_sylph_t v_down; ///< 下方向速度[m/s]
  float_sylph_t heading; ///< ヘディング(ヨー+アジマス)[rad]
  float_sylph_t euler_phi; ///< ロール[rad]
  float_sylph_t euler_theta; ///< ピッチ[rad]
  float_sylph_t euler_psi; ///< ヨー[rad]
  float_sylph_t azimuth; ///< アジマス[rad]
  float_sylph_t corrected_accel[3]; ///< 修正加速度[m/s2]
  float_sylph_t corrected_omega[3]; ///< 修正角速度[rad/s]
  
  ///< N0 Packetを作成
  void encode_N0(
      char buf[32], 
      unsigned char seq_num,
      const bool valid = true) const;
};

extern struct NavigationConfig {
  bool dump_tu; ///< 時間更新(Time Update)の際に現在の値を表示するか
  bool dump_mu; ///< 観測更新(Measurement Update)の際に現在の値を表示するか
  bool use_external_nav_info; ///< Hardware_in_the_Loopのように外部から航法情報を取り込むか
  float_sylph_t mag_hard_iron_offset[3]; ///< 地磁気のオフセット
} navigation_config;

///< 外部航法情報
struct ExternalNAVInfo : public TimeStampedInfo {
  float_sylph_t longitude_rad; ///< 経度[rad]
  float_sylph_t latitude_rad; ///< 緯度[rad]
  float_sylph_t height_m; ///< 高度[m]
  float_sylph_t v_north_ms; ///< 北方向速度[m/s]
  float_sylph_t v_east_ms; ///< 東方向速度[m/s]
  float_sylph_t v_down_ms; ///< 下方向速度[m/s]
  float_sylph_t heading_rad; ///< ヘディング[rad]
  float_sylph_t roll_rad; ///< ロール[rad]
  float_sylph_t pitch_rad; ///< ピッチ[rad]
  float_sylph_t accel_ms2[3]; ///< 加速度[m/s2]
  float_sylph_t omega_rads[3]; ///< 角速度[rad/s]

  operator NAVInfo() const;

  void push() const;
};

#endif /* __NAVIGATION_H__ */