#ifndef __GUIDANCE_CONTROL_H__
#define __GUIDANCE_CONTROL_H__

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

///< 誘導制御に使う情報
struct GCInfo {
  // 時刻
  float_sylph_t itow; ///< GPS時刻[s]
  
  // 加速度、角速度
  float_sylph_t accel_ms2[3]; ///< 加速度[m/s^2] X,Y,Z(機体座標、速報値)
  float_sylph_t omega_rads[3]; ///< 角速度[rad/s] X,Y,Z(機体座標、速報値)
  
  // 航法情報
  float_sylph_t longitude_rad; ///< 経度[rad]
  float_sylph_t latitude_rad; ///< 緯度[rad]
  float_sylph_t height_meter; ///< WGS84高度[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 corrected_accel_ms2[3]; ///< 修正加速度[m/s^2] X,Y,Z(機体座標、バイアス等考慮済)
  float_sylph_t corrected_omega_rads[3]; ///< 修正角速度[rad/s] X,Y,Z(機体座標、バイアス等考慮済)
  
  // 空気データ
  float_sylph_t tas_ms; ///< 真対気速度[m/s]
  float_sylph_t alpha_rad; ///< 迎角[rad]
  float_sylph_t beta_rad; ///< 横滑り角[rad]
  float_sylph_t pressure_alt_meter; ///< 気圧高度[m]
  
  // 受信機データ
  Uint16 servo_in[8]; ///< R/C受信機サーボパルス幅[ms] 通常1500ms中立

  // 状態
  Uint32 health; ///< データの状態
  static const Uint32 ACCEL_OMEGA_UPDATED = 0x0001;
  static const Uint32 NAV_UPDATED = 0x0010;
  static const Uint32 NAV_INITIALIZED = 0x0020;
  static const Uint32 ADS_UPDATED  = 0x0100;
  static const Uint32 SERVO_UPDATED  = 0x1000;
};

/**
 * 誘導制御ルーチンを組み込み済み以外のものに切り替える
 * 
 * @func 誘導制御ルーチン(NULL指定で元のものに戻る)
 */
void switch_guidance_control(void (*func)(const GCInfo &info));

#endif /* __GUIDANCE_CONTROL__ */