/**
 * TinyFeather Autopilot データ処理/中継/記録
 * 
 */
#include "data_matrix.h"

#include <std.h>

#include "config.h"

// 標準C/C++ヘッダ
#include <cstdio>
#include <cstring>
#include <cmath>
#include <xutility>
#include <new>

// DSP BIOS ヘッダ
#include <log.h>
#include <idl.h>
#include <sem.h>
#include <tsk.h>
#include <swi.h>

#include "kernel.h"
#include "navigation.h"
#include "guidance_control.h"
#include "calibration.h"

#include "rtklib/rtklib_bridge.h"

#include "peripheral/ublox.h"

#include "note/070423/SylphideStream.h"
#include "note/070423/SylphideProcessor.h"

using namespace std;

struct SylphidePagePacket {
  char buffer[
      SylphideProtocol::capsule_head_size
      + SylphideProtocol::payload_fixed_length
      + SylphideProtocol::capsule_tail_size];
  SylphidePagePacket(){
    SylphideProtocol::Encoder::preprocess(buffer);
  }
  char *payload() {
    return &(buffer[SylphideProtocol::capsule_head_size]);
  }
  void update(const Uint16 &sequence_num){
    SylphideProtocol::Encoder::postprocess(
        buffer, sequence_num, sizeof(buffer));
  }
};

static class Telemetry {
  protected:
    Kernel::IO tx_link;
    Uint16 telemetry_count;
    bool sem_initialized;
    SEM_Obj sem_tx_link;
  public:
    Telemetry() : tx_link(), telemetry_count(0), 
        sem_initialized(false), sem_tx_link() {}
    ~Telemetry(){}
    void init(const Kernel::IO &another_link){
      if(!sem_initialized){
        sem_initialized = true;
        SEM_new(&sem_tx_link, 1);
      }
      SEM_pendBinary(&sem_tx_link, SYS_FOREVER);
      tx_link = another_link;
      SEM_postBinary(&sem_tx_link);
    }
    void operator()(const void *buf, const unsigned int &buf_size){
      tx_link.transmit((char *)buf, buf_size);
    }
    void send(SylphidePagePacket &packet){
      packet.update(telemetry_count++);
      SEM_pendBinary(&sem_tx_link, SYS_FOREVER);
      tx_link.transmit(packet.buffer, sizeof(packet.buffer));
      SEM_postBinary(&sem_tx_link);
    }
    void send(const char *raw_data, const unsigned int &data_size){
      SEM_pendBinary(&sem_tx_link, SYS_FOREVER);
      SylphideProtocol::Encoder::send(*this, telemetry_count++, raw_data, data_size);
      SEM_postBinary(&sem_tx_link);
    }
} telemetry;

static void (*uplink_handler)(char *, const unsigned int &)(NULL);

static struct _BuiltinTelemetry : public BuiltinTelemetry {
  unsigned int frequency[NUM_OF_ITEMS];
  _BuiltinTelemetry(){
    frequency[NAV] = 4;
    frequency[GPS] = 2;
    frequency[ADS] = 32;
    frequency[SERVO] = 16;
    frequency[MAG] = 4;
  }
  ~_BuiltinTelemetry(){}
  void update(const item_t &item,
      const unsigned int &count,
      SylphidePagePacket &packet) const {
    if((frequency[item] > 0) && (count % frequency[item] == 0)){
      telemetry.send(packet); // 情報をテレメトリ送信
    }
  }
} builtin_telemetry;

static struct builtin_gps_telemetry_t {
  const char mclass;
  const char mid;
  unsigned int updated;
} builtin_gps_telemetry[] = {{0x01, 0x30, 0}};

unsigned int downlink_builtin_rate(const BuiltinTelemetry::item_t &item, const unsigned int &rate){
  return (builtin_telemetry.frequency[item] = rate);
}

static ProcessedInfo processed_info = {
    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, processed_info.gps.NO_FIX}, // GPS
    {{0}, {0}}, // 加速度、角速度
    {0, 0, 0, 0}, {{0}},// 空気、地磁気
    {0, 0, 0, 0, 0, 0, 0, 0, 0, {0}, {0}}, // 航法データ
    },
    _processed_info(processed_info);
const ProcessedInfo &latest_processed_info(_processed_info);

static const unsigned int deorder_buffer_size = 0x400;
static class StreamDecorder
    : public AbstractSylphideProcessor<float_sylph_t> {
  protected:
    typedef AbstractSylphideProcessor<float_sylph_t> super_t;
    typedef G_Packet_Observer<float_sylph_t> G_Observer_t;
    
    ///< Gページ(u-bloxのGPS)のデコーダ
    struct GHandler : public G_Observer_t  {
      unsigned int itow_ms_0x0102, itow_ms_0x0112;
      bool previous_seek;
      G_Observer_t::solution_t solution;
      float_sylph_t itow_solution;
      unsigned int invoked;
      unsigned int good_satellites;
      SylphidePagePacket telemetry_g_packet;
      char *telemetry_g_page;
      Kernel &kernel;
      MBX_Handle mbx_mu;
      Kernel::IO ublox_io;
      Ublox ublox;
      MeasurementUpdateInfo mu_info;
      Uint32 ephemeris_received_flags;
      
      GHandler() 
          : G_Observer_t(deorder_buffer_size), 
          itow_ms_0x0102(0), itow_ms_0x0112(0),
          solution(), itow_solution(0), 
          invoked(0),
          good_satellites(0),
          telemetry_g_packet(), telemetry_g_page(telemetry_g_packet.payload()),
          kernel(Kernel::get_instance()), mbx_mu(kernel.mbx(Kernel::MBX_MU_INFO)),
          ublox_io(kernel.open(Kernel::UBLOX)), ublox(ublox_io),
          ephemeris_received_flags(0) {
        previous_seek = ready();
        telemetry_g_page[0] = 'G';
        solution.fix_type = G_Observer_t::status_t::NO_FIX;
      }
      ~GHandler(){}
      
      /** 
       * Gページが見つかった際、コールバックされる関数
       * 
       * Gページの内容が正しいかvalidateで確認した後、実処理を行うこと。
       * {class, id} = {0x01, 0x02}のとき位置情報が得られる
       * {class, id} = {0x01, 0x03}のとき測位モード(NOFIX/2D/3D)
       * {class, id} = {0x01, 0x12}のとき速度情報が得られる
       * 
       * @param obsrever Gページのオブザーバー
       */
      void operator()(const G_Observer_t &observer){
        if(++invoked >= 0x100){
          invoked = 0;
          LOG_printf(&trace,"256 g_packet_s recieved!\n");
        }
        
        if(!observer.validate()){
          Uint32 buf(0);
          observer.inspect((char *)&buf, sizeof(buf));
          LOG_printf(&trace, "Corrupted, header(LE) is 0x%08x.", buf);
          /*for(Uint32 i = 0; i < observer.current_packet_size(); i++){
            LOG_printf(&trace, "%d, %02x", i, (unsigned char)observer[i]);
          }*/
          return;
        }

        bool change_itow(false);
        G_Observer_t::packet_type_t
            packet_type(observer.packet_type());
        switch(packet_type.mclass){
          case 0x01: {
            switch(packet_type.mid){
              case 0x02: { // 位置
                G_Observer_t::position_t
                    position(observer.fetch_position());
                G_Observer_t::position_acc_t
                    position_acc(observer.fetch_position_acc());
                
                itow_ms_0x0102 = observer.fetch_ITOW_ms();
                change_itow = true;

                mu_info.llh[0] = position.latitude;
                mu_info.llh[1] = position.longitude;
                mu_info.llh[2] = position.altitude;
                mu_info.acc_2d = position_acc.horizontal;
                mu_info.acc_vertical = position_acc.vertical;
                break;
              }
              case 0x04: { // DOP
                Uint16 gdop_pdop_tdop[3];
                observer.inspect((char *)gdop_pdop_tdop, sizeof(gdop_pdop_tdop), 6 + 4);
                break;
              }
              case 0x06: {
                solution = observer.fetch_solution();
                itow_solution = observer.fetch_ITOW();
                break;
              }
              case 0x12: { // 速度
                G_Observer_t::velocity_t
                    velocity(observer.fetch_velocity());
                G_Observer_t::velocity_acc_t
                    velocity_acc(observer.fetch_velocity_acc());

                itow_ms_0x0112 = observer.fetch_ITOW_ms();
                change_itow = true;
                
                mu_info.vel_ned[0] = velocity.north;
                mu_info.vel_ned[1] = velocity.east;
                mu_info.vel_ned[2] = velocity.down;
                mu_info.acc_vel = velocity_acc.acc;
                break;
              }
              case 0x20: { // GPS時刻
                char buf[2];
                observer.inspect(buf, sizeof(buf), 6 + 10);
                if((unsigned char)buf[1] & 0x04){ // Valid UTC
                  processed_info.gps.leap_seconds_gps_minus_utc = buf[0];
                }
                break;
              }
              case 0x30: { // 衛星情報
                int channels(observer[6 + 4]);
                good_satellites = 0;
                for(int i(0); i < channels; i++){
                  G_Observer_t::svinfo_t svinfo(observer.fetch_svinfo(i));

                  if(svinfo.svid <= 32){ // check ephemeris
                    Uint32 ephemeris_mask((Uint32)1 << (svinfo.svid - 1));
                    if(svinfo.flags & 0x08){ // ephemeris available
                      if(!(ephemeris_received_flags & ephemeris_mask)){
                        // request ephemeris immediately
                        ublox.poll_aid_eph(svinfo.svid);
                      }
                    }else{
                      ephemeris_received_flags &= ~ephemeris_mask;
                    }
                  }

                  // QI check:
                  // 4 => Code Lock on Signal
                  // 5, 6 => Code and Carrier locked;
                  // 7 => Code and Carrier locked, receiving 50bps data
                  if(svinfo.quality_indicator > 4){ // トンネル対策テストコード
                    good_satellites++;
                  }
                }
                break;
              }
            }
            break;
          }
          case 0x02: {
            switch(packet_type.mid){
              case 0x10: { // RXM-RAW
                Uint8 num_of_sv((unsigned char)observer[6 + 6]);
                LOG_printf(&trace, "Number of satellites : %d", num_of_sv);
                static const unsigned int gps_time_valid_mask(
                    G_Observer_t::solution_t::WN_VALID | G_Observer_t::solution_t::TOW_VALID);
                if((solution.status_flags & gps_time_valid_mask) == gps_time_valid_mask){
                  kernel.notify_gps_time(observer.fetch_WN(), observer.fetch_ITOW_ms()); // 時刻変更の補助
                }
                rtklib_notify_ubx_rxm_raw(observer);
                break;
              }
              case 0x11: rtklib_notify_ubx_rxm_sfrb(observer); break; // RXM-SFRB
            }
            break;
          }
          case 0x0b: {
            switch(packet_type.mid){
              case 0x02: rtklib_notify_ubx_aid_hui(observer); break; // AID-HUI
              case 0x31: // AID-EPH
                char buf[4];
                observer.inspect(buf, sizeof(buf), 6); // SVID
                Uint32 svid(le_char4_2_num<s32_t>(buf[0]));
                if(svid <= 32){
                  Uint32 ephemeris_mask((Uint32)1 << (svid - 1));
                  if(observer.current_packet_size() > (8 + 8)){
                    ephemeris_received_flags |= ephemeris_mask;
                    rtklib_notify_ubx_aid_eph(observer);
                  }else{
                    ephemeris_received_flags &= ~ephemeris_mask;
                  }
                }
                break;
            }
            break;
          }
          default:
            break;
        }
        
        const unsigned int frequency(
            builtin_telemetry.frequency[BuiltinTelemetry::GPS]);
        
        if(frequency > 0){ // テレメトリ送信
          for(int i(0); i < sizeof(builtin_gps_telemetry) / sizeof(builtin_gps_telemetry[0]); ++i){
            if(!packet_type.equals(builtin_gps_telemetry[i].mclass, builtin_gps_telemetry[i].mid)){
              continue;
            }
            if((++builtin_gps_telemetry[i].updated) < frequency){
              continue;
            }
            builtin_gps_telemetry[i].updated = 0;

            int size(observer.current_packet_size());
            int index(0);
            for(int loop(size / (PAGE_SIZE - 1));
                loop > 0;
                loop--, index += (PAGE_SIZE - 1)){
              observer.inspect(&telemetry_g_page[1], (PAGE_SIZE - 1), index);
              telemetry.send(telemetry_g_packet);
            }
            int remain(size - index); // 端数
            if(remain){
              std::memset(&telemetry_g_page[1], 0, (PAGE_SIZE - 1)); // 0クリア
              observer.inspect(&telemetry_g_page[1], remain, index);
              telemetry.send(telemetry_g_packet);
            }
            break;
          }
        }
        
        if(change_itow
            && (itow_ms_0x0102 == itow_ms_0x0112)){
          
          unsigned int fix_type = solution.fix_type;
          
          // 室内で受信したことにするためのコード
          if(INDOOR_FAKE_GPS){
            good_satellites = 4;
            itow_ms_0x0102 = (kernel.itow_ms() - NAV_UPDATE_RATIO_ms * 5);
            mu_info.llh[0] = mu_info.llh[1] = mu_info.llh[2] = 0;
            mu_info.acc_2d = mu_info.acc_vertical = 1;
            mu_info.vel_ned[0] = mu_info.vel_ned[1] = mu_info.vel_ned[2] = 0;
            mu_info.acc_vel = 1E-2;
            fix_type = G_Observer_t::status_t::FIX_3D;
          }
          
          mu_info.itow = (float_sylph_t)1E-3 * itow_ms_0x0102;
          
          /*LOG_printf(&trace, "Position(lat, long, alt) :");
          LOG_printf(&trace, "%f", mu_info.llh[0]);
          LOG_printf(&trace, "%f", mu_info.llh[1]);
          LOG_printf(&trace, "%f", mu_info.llh[2]);
          LOG_printf(&trace,"Position_Acc(h, v) : %f, %f",
              mu_info.acc_2d, mu_info.acc_vertical);
          LOG_printf(&trace,"Velocity(N, E, D) :");
          LOG_printf(&trace, "%f", mu_info.vel_ned[0]);
          LOG_printf(&trace, "%f", mu_info.vel_ned[1]);
          LOG_printf(&trace, "%f", mu_info.vel_ned[2]);
          LOG_printf(&trace,"Velocity_Acc : %f",
              mu_info.acc_vel);*/
          
          // 情報の更新
          processed_info.gps.longitude_deg = mu_info.llh[0];
          processed_info.gps.latitude_deg = mu_info.llh[1];
          processed_info.gps.height_meter = mu_info.llh[2];
          processed_info.gps.horizontal_position_accuracy_meter = mu_info.acc_2d;
          processed_info.gps.vertical_position_accuracy_meter = mu_info.acc_vel;
          processed_info.gps.v_north_ms = mu_info.vel_ned[0];
          processed_info.gps.v_east_ms = mu_info.vel_ned[1];
          processed_info.gps.v_down_ms = mu_info.vel_ned[2];
          processed_info.gps.velocity_accuracy_ms = mu_info.acc_vel;
          processed_info.gps.using_satellites = good_satellites;
          switch(fix_type){ // TODO: GPS_WITH_DR equals 3D_FIX?
            case G_Observer_t::status_t::FIX_2D : processed_info.gps.fix_type = processed_info.gps.FIX_2D; break;
            case G_Observer_t::status_t::FIX_3D : processed_info.gps.fix_type = processed_info.gps.FIX_3D; break;
            default: processed_info.gps.fix_type = processed_info.gps.NO_FIX; break;
          }
          
          switch(fix_type){
            case G_Observer_t::status_t::FIX_2D :
              // 垂直方向の推定が行われていないことを偏差を使ってあらわす
              mu_info.acc_vertical = 1E+3;
              // 現在高度に置き換え
              mu_info.llh[2] = processed_info.navigation.height_meter;
            case G_Observer_t::status_t::FIX_3D :
              // GPS観測量を用いてMeasurement Updateをする
              if(good_satellites < 4){
                // 状態がよい衛星が4以下になったら更新しない
                break;
              }
              MBX_post(mbx_mu, &mu_info, 0);
              break;
          }
          
          LOG_printf(&trace, "MU done.");
        }
      }
    } g_handler;
    
    ///< SylphideProtocol形式で入ってくるCページ用のデコーダ
    class CommandHandler 
        : public Sylphide_Packet_Observer<float_sylph_t> {
      protected:
        typedef Sylphide_Packet_Observer<float_sylph_t> super_t;
        typedef CommandHandler self_t;
        char buf[deorder_buffer_size];
      public:
        bool previous_seek;
        unsigned int invoked;
        CommandHandler() 
            : super_t(deorder_buffer_size), 
            previous_seek(false), invoked(0) {}
        ~CommandHandler() {}
        
        ///< パケットが見つかった際に呼び出される関数
        void operator()(const self_t &observer){
          if(!observer.validate()){return;}
          unsigned int packet_size(observer.current_packet_size());
          // TODO: 組み込みコマンド
          if(uplink_handler){
            observer.inspect(buf, packet_size); // コピーして連続性を保証
            uplink_handler(buf, packet_size);
          }
        }
    } c_handler;
    
    ///< SylphideProtocol形式で入ってくるDページ用のデコーダ
    class DebugHandler 
        : public Sylphide_Packet_Observer<float_sylph_t> {
      protected:
        typedef Sylphide_Packet_Observer<float_sylph_t> super_t;
        typedef DebugHandler self_t;
      public:
        bool previous_seek;
        unsigned int invoked;
        DebugHandler() 
            : super_t(deorder_buffer_size), 
            previous_seek(false), invoked(0) {}
        ~DebugHandler() {}
        
        void operator()(const self_t &observer){
          if(!observer.validate()){return;}
          // TODO
        }
    } d_handler;
    
  public:
    StreamDecorder() 
        : super_t(),
        g_handler(),
        c_handler(), d_handler() {
      
    }
    virtual ~StreamDecorder(){}
    
    void process_gps(char *buffer, int read_count){
      super_t::process_raw(
          buffer, read_count,
          g_handler, g_handler.previous_seek, g_handler);
    }

    void process_command(char *buffer, int read_count){
      super_t::process_raw(
          buffer, read_count,
          c_handler, c_handler.previous_seek, c_handler);
    }

    void process_debug(char *buffer, int read_count){
      super_t::process_raw(
          buffer, read_count,
          d_handler, d_handler.previous_seek, d_handler);
    }

    void process(char *buffer, int read_count){
      
      /*LOG_printf(&trace, "size: %d", bytes);
      for(int i = 0; i < bytes; i++){
        LOG_printf(&trace, "%d, %02x", i, (unsigned char)buffer[i]);
      }*/
                
      switch(buffer[0]){
        // Tiny Featherでは原則、データの経路ごとに意味が決まっている
        default:
          LOG_printf(&trace, "Unknown, %c, %d", buffer[0], read_count);
      }
    }
    
    unsigned char gps_status_flag() const {
      // GPS status flag for N page
      // bit[2..0] : Used satellites (0-7 : n ; >=7 : 7)
      // bit[3] : 3D navigation ON
      unsigned char flag;
      if(g_handler.solution.satellites_used > 7){
        flag = 0x07;
      }else{
        flag = g_handler.solution.satellites_used;
      }
      if(g_handler.solution.fix_type == G_Observer_t::solution_t::FIX_3D){
        flag |= 0x08;
      }
      return flag;
    }
    
    unsigned char gps_status_flag(const float_sylph_t &itow) const {
      // All flag bits are default(0) if difference between the current and modified times are under 5 seconds.
      if(std::abs(itow - g_handler.itow_solution) > 5){  
        return 0;
      }
      return gps_status_flag();
    }
} decorder;

static bool downlink_use_sylphide(true);
void downlink_use_builtin_protocol(const bool &use){
  downlink_use_sylphide = use;
  telemetry.init(Kernel::get_instance().open(downlink_use_sylphide ? (Kernel::GS_LINK) : (Kernel::NUL)));
}
void downlink_write(const char *raw_data, const unsigned int &data_size){
  if(downlink_use_sylphide){
    telemetry.send(raw_data, data_size);
  }else{
    Kernel::get_instance().open(Kernel::GS_LINK).transmit(raw_data, data_size);
  }
}

static bool uplink_use_sylphide(true);
void uplink_use_builtin_protocol(const bool &use){
  uplink_use_sylphide = use;
}
void uplink_read(void (*func)(char *buf, const unsigned int &buf_size)){
  uplink_handler = func;
}

extern "C" {
void data_matrix(){
  
  Kernel &kernel(Kernel::get_instance());
  Kernel::IO usb_uart(kernel.open(Kernel::USB_UART));
  Kernel::IO ublox(kernel.open(Kernel::UBLOX));
  Kernel::IO file(kernel.open(Kernel::FAT_FILE, "log.dat"));
  Kernel::IO gs_link(kernel.open(Kernel::GS_LINK));
  
  // ログファイルのバッファを指定
  static const unsigned int file_tx_rx_buffers[] = {0x1000, 0};
  file.ioctl(Kernel::IOCTL_BUFFER, file_tx_rx_buffers);
  
  // テレメトリ送信にGS_Linkを使う
  if(downlink_use_sylphide){
    telemetry.init(gs_link);
  }
  
  SylphidePagePacket g_packet, a_packet, f_packet, p_packet, m_packet, n_packet, c_packet;
  char 
      *g_page(g_packet.payload()), 
      *a_page(a_packet.payload()),
      *f_page(f_packet.payload()),
      *p_page(p_packet.payload()),
      *m_page(m_packet.payload()),
      *n_page(n_packet.payload()),
      *c_page(c_packet.payload());

  g_page[0] = 'G';
  a_page[0] = 'A';
  f_page[0] = 'F';
  f_page[1] = 0;
  f_page[2] = 0;
  p_page[0] = 'P';
  p_page[1] = 0;
  p_page[2] = 0;
  m_page[0] = 'M';
  m_page[1] = 0x80;
  m_page[2] = 0;
  c_page[0] = 'C';
  
  unsigned int g_page_index(1), c_page_index(1), p_page_index(8), m_page_index(8);
  unsigned int f_page_count(0), p_page_count(0), m_page_count(0), n_page_count(0);
  
  Uint16 packet_count(0), no_packet(0);
  MBX_Handle mbx_adc(kernel.mbx(Kernel::MBX_ADC));
  MBX_Handle mbx_servo_read(kernel.mbx(Kernel::MBX_SERVO_READ));
  MBX_Handle mbx_ads(kernel.mbx(Kernel::MBX_ADS));
  MBX_Handle mbx_mag(kernel.mbx(Kernel::MBX_MAG));
  MBX_Handle mbx_tu(kernel.mbx(Kernel::MBX_TU_INFO));
  MBX_Handle mbx_mag_info(kernel.mbx(Kernel::MBX_MAG_INFO));
  MBX_Handle mbx_nav(kernel.mbx(Kernel::MBX_NAV_INFO));
  MBX_Handle mbx_gc(kernel.mbx(Kernel::MBX_GC_INFO));
  
  new (&decorder) StreamDecorder();

  Kernel::msg_adc_t msg_adc, msg_adc_sum;
  for(int i(0); i < sizeof(msg_adc_sum.ch) / sizeof(msg_adc_sum.ch[0]); i++){
    msg_adc_sum.ch[i] = 0;
  }
  unsigned int msg_adc_sum_counter(0);

  Kernel::msg_ads_t msg_ads;
  Kernel::msg_servo_read_t msg_servo_read;
  Kernel::msg_mag_t msg_mag;
  GCInfo msg_gc_info;
  
  Uint32 previous_msg_adc_time_ms(0);
  
  TimeUpdateInfo tu_info;
  MagInfo mag_info;
  NAVInfo msg_nav_info;
  
  static Uint32 invoked(0);
  while(true){
    
    unsigned int packet_count_old(packet_count);
    
    // GPS data
    while(true){
      int new_received(ublox.receive(
          &g_page[g_page_index], PAGE_SIZE - g_page_index));
      // デコード、位置速度情報が得られたらMeasurement update
      decorder.process_gps(&g_page[g_page_index], new_received);
      g_page_index += new_received;
      if(g_page_index < PAGE_SIZE){break;}
      
      // 保存、送信
      g_packet.update(packet_count);
      usb_uart.transmit(g_packet.buffer, sizeof(g_packet.buffer));
      file.transmit(g_page, PAGE_SIZE);
      g_page_index = 1;
      packet_count++;
      //break;
    }
    
    TSK_sleep(1);
    
    // ADC data
    while(MBX_pend(mbx_adc, &msg_adc, 0)){
      // 保存、送信
      std::memcpy(&a_page[2], &(msg_adc.time_ms), sizeof(Uint32));
      a_page[1] = (Uint8)invoked; // 内部時刻
      union u32_t {
        Uint32 v;
        Uint8 c[4];
      } conv;
      for(int i(0), j(6); i < 8; i++){
        conv.v = msg_adc.ch[i];
        *(Uint8 *)(&a_page[j++]) = conv.c[2];
        *(Uint8 *)(&a_page[j++]) = conv.c[1];
        *(Uint8 *)(&a_page[j++]) = conv.c[0];
      }
      a_packet.update(packet_count);
      usb_uart.transmit(a_packet.buffer, sizeof(a_packet.buffer));
      file.transmit(a_page, PAGE_SIZE);
      packet_count++;
      
      // 間引く, 時刻ジャンプ考慮のこと
      if((msg_adc.time_ms > previous_msg_adc_time_ms)
          && ((msg_adc.time_ms - previous_msg_adc_time_ms) < NAV_UPDATE_RATIO_ms)){
        for(int i(0); i < sizeof(msg_adc.ch) / sizeof(msg_adc.ch[0]); i++){
          msg_adc_sum.ch[i] += msg_adc.ch[i];
        }
        msg_adc_sum_counter++;
        break;
      }

      previous_msg_adc_time_ms = msg_adc.time_ms;
      
      // 平均値を求める
      if(msg_adc_sum_counter > 0){
        msg_adc_sum_counter++;
        for(int i(0); i < sizeof(msg_adc.ch) / sizeof(msg_adc.ch[0]); i++){
          msg_adc.ch[i] += msg_adc_sum.ch[i];
          msg_adc.ch[i] /= msg_adc_sum_counter;
          msg_adc_sum.ch[i] = 0;
        }
        msg_adc_sum_counter = 0;
      }

      // 慣性力に変換する => Time update
      calibrator.convert(msg_adc, tu_info);
      
      MBX_post(mbx_tu, &tu_info, 0);
      
      // 誘導制御へ
      std::memcpy(msg_gc_info.accel_ms2, tu_info.accel, sizeof(msg_gc_info.accel_ms2));
      std::memcpy(msg_gc_info.omega_rads, tu_info.omega, sizeof(msg_gc_info.omega_rads));
      msg_gc_info.health |= GCInfo::ACCEL_OMEGA_UPDATED;

      // 情報の更新
      std::memcpy(processed_info.inertial_sensor.accel_ms2,
          tu_info.accel, sizeof(processed_info.inertial_sensor.accel_ms2));
      std::memcpy(processed_info.inertial_sensor.omega_rads,
          tu_info.omega, sizeof(processed_info.inertial_sensor.omega_rads));

      break;
    }
    
    TSK_sleep(1);
    
    // Servo data
    if(MBX_pend(mbx_servo_read, &msg_servo_read, 0)){
      // 保存、送信
      std::memcpy(&f_page[4], &(msg_servo_read.time_ms), sizeof(Uint32));
      f_page[3] = (Uint8)invoked; // 内部時刻
      for(int i(0), j(8); i < 8; i++){
        *(Uint8 *)(&f_page[j++]) = (Uint8)msg_servo_read.ch_in[i];
        *(Uint8 *)(&f_page[j++]) 
            = ((Uint8)(msg_servo_read.ch_in[i] >> 4) & 0xF0) | ((Uint8)(msg_servo_read.ch_out[i] >> 8) & 0x0F);
        *(Uint8 *)(&f_page[j++]) = (Uint8)msg_servo_read.ch_out[i];
      }
      f_packet.update(packet_count);
      usb_uart.transmit(f_packet.buffer, sizeof(f_packet.buffer));
      file.transmit(f_page, PAGE_SIZE);
      packet_count++;
      
      builtin_telemetry.update(BuiltinTelemetry::SERVO, f_page_count++, f_packet);

      // 誘導制御へ
      std::memcpy(msg_gc_info.servo_in, msg_servo_read.ch_in, sizeof(msg_gc_info.servo_in));
      msg_gc_info.health |= GCInfo::SERVO_UPDATED;
    }
    
    // ADS data
    while(MBX_pend(mbx_ads, &msg_ads, 0)){
      // 保存、送信
      std::memcpy(
          &p_page[p_page_index],
          msg_ads.raw_buf,
          sizeof(msg_ads.raw_buf));
      p_page_index += sizeof(msg_ads.raw_buf);
      if(p_page_index < PAGE_SIZE){break;}
      
      // 保存、送信
      std::memcpy(&p_page[4], &(msg_ads.time_ms), sizeof(Uint32));
      p_page[3] = (Uint8)invoked; // 内部時刻
      p_packet.update(packet_count);
      usb_uart.transmit(p_packet.buffer, sizeof(p_packet.buffer));
      file.transmit(p_page, PAGE_SIZE);
      p_page_index = 8;
      packet_count++;
      
      builtin_telemetry.update(BuiltinTelemetry::ADS, p_page_count++, p_packet);

      // TODO: 誘導制御へ
      //msg_gc_info.health |= GCInfo::ADS_UPDATED;

      // TODO: 情報の更新
      //latest_processed_info.ads;

      break;
    }
    
    // MAG data
    while(MBX_pend(mbx_mag, &msg_mag, 0)){
      
      mag_info.itow = (float_sylph_t)(1E-3 * msg_mag.time_ms);
      msg_mag.convert(mag_info.mag_raw);
      MBX_post(mbx_mag_info, &mag_info, 0); // 処理済データをコミット
      
      // 保存、送信
      std::memcpy(
          &m_page[m_page_index],
          msg_mag.raw_buf,
          sizeof(msg_mag.raw_buf));
      m_page_index += sizeof(msg_mag.raw_buf);
      if(m_page_index < PAGE_SIZE){break;}
      
      std::memcpy(&m_page[4], &(msg_mag.time_ms), sizeof(Uint32));
      m_page[3] = (Uint8)invoked; // 内部時刻
      m_packet.update(packet_count);
      usb_uart.transmit(m_packet.buffer, sizeof(m_packet.buffer));
      file.transmit(m_page, PAGE_SIZE);
      m_page_index = 8;
      packet_count++;
      
      builtin_telemetry.update(BuiltinTelemetry::MAG, m_page_count++, m_packet);
      
      // TODO: 情報の更新
      //latest_processed_info.magnetic_sensor;

      break;
    }
    
    TSK_sleep(1);
    
    // NAV data
    if(MBX_pend(mbx_nav, &msg_nav_info, 0)){
      // 保存、送信
      msg_nav_info.encode_N0(n_page, (Uint8)(n_page_count++));
      // Pass navigation flag by using extension field[3].
      n_page[3] |= decorder.gps_status_flag(msg_nav_info.itow);
      n_packet.update(packet_count);
      usb_uart.transmit(n_packet.buffer, sizeof(n_packet.buffer));
      file.transmit(n_page, PAGE_SIZE);
      packet_count++;
      
      builtin_telemetry.update(BuiltinTelemetry::NAV, n_page_count, n_packet);
      
      // 航法情報が得られたタイミングで誘導制御に渡す
      {
#define copy(item_dst, item_src) msg_gc_info.item_dst = msg_nav_info.item_src
        copy(itow, itow);
        copy(longitude_rad, longitude);
        copy(latitude_rad, latitude);
        copy(height_meter, height);
        copy(v_north_ms, v_north);
        copy(v_east_ms, v_east);
        copy(v_down_ms, v_down);
        copy(heading_rad, heading);
        copy(roll_rad, euler_phi);
        copy(pitch_rad, euler_theta);
#undef copy
        std::memcpy(msg_gc_info.corrected_accel_ms2, msg_nav_info.corrected_accel, sizeof(msg_gc_info.corrected_accel_ms2));
        std::memcpy(msg_gc_info.corrected_omega_rads, msg_nav_info.corrected_omega, sizeof(msg_gc_info.corrected_omega_rads));
        msg_gc_info.health |= GCInfo::NAV_UPDATED;
        
        // 送る
        MBX_post(mbx_gc, &msg_gc_info, 0);

        // フラグクリア
        msg_gc_info.health = 0;
      }

      // 情報の更新
      {
#define copy(item_dst, item_src) processed_info.navigation.item_dst = msg_nav_info.item_src
        copy(longitude_rad, longitude);
        copy(latitude_rad, latitude);
        copy(height_meter, height);
        copy(v_north_ms, v_north);
        copy(v_east_ms, v_east);
        copy(v_down_ms, v_down);
        copy(heading_rad, heading);
        copy(roll_rad, euler_phi);
        copy(pitch_rad, euler_theta);
#undef copy
        std::memcpy(processed_info.navigation.corrected_accel_ms2,
            msg_nav_info.corrected_accel, sizeof(processed_info.navigation.corrected_accel_ms2));
        std::memcpy(processed_info.navigation.corrected_omega_rads,
            msg_nav_info.corrected_omega, sizeof(processed_info.navigation.corrected_omega_rads));

        /* guidance_controlのタスクはより優先度が高いので、
         * guidance_controlのループ実行中にlatest_infoが書き換わる心配はない
         */
        TSK_disable(); // 同じタイミングで公開情報を更新
        SWI_disable();
        _processed_info = processed_info;
        SWI_enable();
        TSK_enable();
      }
    }
    
    // GS-Link (XBee etc.)
    while(true){
      int new_received(gs_link.receive(
          &c_page[c_page_index], PAGE_SIZE - c_page_index));
      if(uplink_use_sylphide){
        // デコード、SylphideProtocol形式のコマンドを解析
        decorder.process_command(&c_page[c_page_index], new_received);
      }else if(uplink_handler){
        uplink_handler(&c_page[c_page_index], new_received);
      }
      c_page_index += new_received;
      if(c_page_index < PAGE_SIZE){break;}

      // 保存、送信
      c_packet.update(packet_count);
      usb_uart.transmit(c_packet.buffer, sizeof(c_packet.buffer));
      file.transmit(c_page, PAGE_SIZE);
      c_page_index = 1;
      packet_count++;
    }

    // Debug via USB
    {
      char buf[0x20];
      unsigned int received;
      while(received = usb_uart.receive(buf, sizeof(buf))){
        decorder.process_debug(buf, received);
      }
    }

    if(packet_count_old == packet_count){
      if(++no_packet > 8){
        usb_uart.transmit(NULL, 0);
        no_packet = 0;
      }
    }else{
      no_packet = 0;
    }
    
    TSK_sleep(1);
    
    invoked++;
    if((invoked % 0x100) == 0){
      LOG_printf(&trace, "256 data_matrix() invoked.");
    }
  }
}
}