/**
 * @file Sylphide の状態量を定めたファイル
 * 
 */

#ifndef __SYLPH_STATE_H__
#define __SYLPH_STATE_H__

#include <cstring>

#include "util/util.h"
#include "vector3.h"

#include <sem.h>

typedef double float_sylph_t;

const unsigned int NAV_UPDATE_RATIO_ms = 20; ///< 航法の更新頻度はここでコントロール

/**
 * 
 * 航法用データ
 */
class NAVData {
  public:
    virtual float_sylph_t longitude() const = 0;
    virtual float_sylph_t latitude() const = 0;
    virtual float_sylph_t height() const = 0;
    virtual float_sylph_t v_north() const = 0;
    virtual float_sylph_t v_east() const = 0;
    virtual float_sylph_t v_down() const = 0;
    virtual float_sylph_t heading() const = 0;
    virtual float_sylph_t euler_phi() const = 0;
    virtual float_sylph_t euler_theta() const = 0;
    virtual float_sylph_t euler_psi() const = 0;
    virtual float_sylph_t azimuth() const = 0;
    
  public:
    /**
     * N0 Packetを作成
     * 
     */
    void encode_N0(
        const float_sylph_t &itow,
        char buf[32], 
        unsigned char seq_num,
        const bool valid = true) 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));
    }
};

/**
 * 同期をとるためのユーティリティオブジェクト
 * 
 */
struct Synchronizer{
  private:
    const SEM_Handle &_handle;
  public:
    Synchronizer(const SEM_Handle &handle) : _handle(handle){
      SEM_pendBinary(_handle, SYS_FOREVER);
    }
    ~Synchronizer(){
      SEM_postBinary(_handle);
    }
};

/*
Synchronizer lock(sem_synchronize_delay_slot);
TSK_yield();
TSK_setpri(TSK_self(), TSK_getpri(&tsk_RT));
*/

void external_write(char *c, unsigned bytes);
void telemetry_write(char *c, unsigned bytes);
void debug_write(char *c, unsigned bytes);

/**
 * 全体で共有する状態量の定義
 * 
 */
struct state_t {
  
  static SEM_Handle sem_update_notifier; ///< 同期用セマフォ
  
  void update_done(){
    //SEM_post(sem_update_notifier);
  }
  
  // 航法情報
  struct nav_t {
    float_sylph_t itow; ///< GPS時刻[ms]
    float_sylph_t longitude, latitude, height;
    float_sylph_t v_north, v_east, v_down;
    float_sylph_t roll, pitch, heading;
    bool valid;
    bool updated;
  } nav;
 
  void update_nav(
      const float_sylph_t &new_itow, 
      const NAVData &new_nav,
      const bool valid = true){
    nav.itow = new_itow;
    nav.longitude = new_nav.longitude();
    nav.latitude = new_nav.latitude();
    nav.height = new_nav.height();
    nav.v_north = new_nav.v_north();
    nav.v_east = new_nav.v_east();
    nav.v_down = new_nav.v_down();
    nav.roll  = new_nav.euler_phi();
    nav.pitch = new_nav.euler_theta();
    nav.heading = new_nav.heading();
    nav.valid = valid;
    nav.updated = true;
    update_done();
  }
  
  // 加速度計やジャイロに由来し、航法補正がかかった加速度と角速度
  struct {
    float_sylph_t itow; ///< GPS時刻[ms]
    Vector3<float_sylph_t> accel, omega;
    bool updated;
  } accel_omega;
  
  void update_accel_omega(
      const float_sylph_t &new_itow,
      const Vector3<float_sylph_t> &new_accel,
      const Vector3<float_sylph_t> &new_omega){
    //Synchronizer lock(sem_RW_lock);
    accel_omega.itow = new_itow;
    accel_omega.accel = new_accel;
    accel_omega.omega = new_omega;
    accel_omega.updated = true;
    update_done();
  }
  
  // サーボの値
  static const unsigned servo_pages = 1;  ///< サーボはとりあえず入力と出力でれぞれ8ch分用意しておく
  static const unsigned servo_channels = servo_pages * 8; 
  struct {
    float_sylph_t itow; ///< GPS時刻[ms]
    unsigned int servo_in[servo_channels], servo_out[servo_channels];
    bool updated;
  } servo;
  
  template <class Container>
  void update_servo(
      const float_sylph_t &new_itow,
      const Container &servo_data,
      const unsigned int page_index = 0){
    if(page_index >= servo_pages){return;}
    servo.itow= new_itow;
    for(unsigned i(0), j(page_index * 8); i < 8; i++, j++){
      servo.servo_in[j] = servo_data.servo_in[i];
      servo.servo_out[j] = servo_data.servo_out[i];
    }
    servo.updated = true;
    update_done();
  }
  
  // TODO: 風のデータ
  
  // TODO: 磁気のデータ
  
  // TODO: コマンドなどの通信関係
  
  /**
   * 現在状態をコピーとして取得する
   * 加えて、更新フラグを全て解除する
   * 
   * 排他制御との関連があるので、
   * 使っている値が変わらないで欲しい場合は必ずfetchでコピーをとること
   */
  state_t fetch() {
    //SEM_pendBinary(sem_update_notifier, SYS_FOREVER);
    
    // セマフォがうまく動かないので、タスクを眠らせることでごまかしておく
    TSK_sleep(NAV_UPDATE_RATIO_ms);
    
    state_t res(*this);
    nav.updated = false;
    accel_omega.updated = false;
    servo.updated = false;
    return res;
  }
} current;

SEM_Handle state_t::sem_update_notifier = NULL;

#endif /* __SYLPH_STATE_H__ */