/**
 * @file SYlphide 誘導制御ルーチン
 * 
 */

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

#include <cstdio>
#include <cstring>

#include "guidance_control.h"
#include "kernel.h"
#include "data_matrix.h"

#include "param/constant.h"

static void uplink_notify(char *buf, const unsigned int &received){
  
}

extern "C" {
void guidance_control_main(){
  
  Kernel &kernel(Kernel::get_instance());
  
  MBX_Handle mbx_gc(kernel.mbx(Kernel::MBX_GC_INFO));
  MBX_Handle mbx_servo_write(kernel.mbx(Kernel::MBX_SERVO_WRITE));
  
  GCInfo gc_info;
  Kernel::msg_servo_write_t msg_servo_write;
  
  // ログファイルの指定(log.datという名前にはしないこと)
  Kernel::IO user_log(kernel.open(Kernel::FAT_FILE, "user_log.dat"));
  
  // ダウンリンクとアップリンクの設定
  {
    // 全てユーザ側で制御したければ以下2行を有効にすること
    //downlink_use_builtin_protocol(false);
    //uplink_use_builtin_protocol(false);
    
    // 受信時動作する関数を登録
    uplink_read(uplink_notify);
  }
  
  std::memset(msg_servo_write.ch, 0, sizeof(msg_servo_write.ch));
  
  char buf[0x40];
  
  unsigned int loop_count(0);
  
  while(true){
    loop_count++;
    
    // 情報の更新を確認
    if(!MBX_pend(mbx_gc, &gc_info, 40)){ // 25Hz で timeout させる
      // 情報が更新されていない場合
      //continue;
    }
    
    msg_servo_write.ch[0]++; // サーボの0chを少しずつ増加させる
    MBX_post(mbx_servo_write, &msg_servo_write, 0); // サーボ指令値が変化したことを通知
    
    // sprintfで最後の'\0'はカウントされていないことに注意
    int size(std::sprintf(buf, "%11.3f,%7.2f,%7.2f,%7.2f\r\n", 
        gc_info.itow,
        rad2deg(gc_info.heading_rad), rad2deg(gc_info.pitch_rad), rad2deg(gc_info.roll_rad)));
    
    user_log.transmit(buf, size); // ログに記録
    
    if(loop_count % 0x10 == 0){ // たまには地上に情報を送る
      downlink_write(buf, size); // ダウンリンクで送信
    }
  }
}
}