/**
 * @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){
  
}

static void (*external_guidance_control)(const GCInfo &info)(NULL);
void switch_guidance_control(void (*func)(const GCInfo &info)){
  external_guidance_control = func;
}

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));
  MBX_post(mbx_servo_write, &msg_servo_write, 0); // サーボを脱力させる
  
  char buf[0x40];
  
  unsigned int loop_count(0), init_count(0);
  float_sylph_t init_yaw(0), init_pitch(0), init_roll(0); 
  
  static const int num_of_servo_out_histories(16);
  float_sylph_t servo_out_histories[num_of_servo_out_histories][8];
  for(int i(0); i < sizeof(servo_out_histories) / sizeof(servo_out_histories[0]); i++){
    for(int j(0); j < sizeof(servo_out_histories[0]) / sizeof(servo_out_histories[0][0]); j++){
      servo_out_histories[i][j] = 1500;
    }
  }
  
  while(true){
    
    // 情報の更新を確認
    if(!MBX_pend(mbx_gc, &gc_info, NAV_UPDATE_RATIO_ms / 2)){ // 125Hz で timeout させる
      // 情報が更新されていない場合
      continue;
    }
    loop_count++;
    
    if(external_guidance_control){ // 外部の誘導制御則へのフック
      external_guidance_control(gc_info);
      continue;
    }
    
    // sprintfで最後の'\0'はカウントされていないことに注意
    int size(std::snprintf(buf, sizeof(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); // ダウンリンクで送信
    }
    
#if 0
    // ゼロだしをする
    {
      static const unsigned int init_using_from_count(10 * 1000 / NAV_UPDATE_RATIO_ms); // 10s
      static const unsigned int init_using_to_count(15 * 1000 / NAV_UPDATE_RATIO_ms); // 15s
      static const unsigned int init_using_range(
          init_using_to_count - init_using_from_count);
      if(init_count < init_using_to_count){
        if(gc_info.health & GCInfo::NAV_UPDATED){
          init_count++;
          if(init_count >= init_using_from_count){
            init_yaw += (gc_info.heading_rad / init_using_range);
            init_pitch += (gc_info.pitch_rad / init_using_range);
            init_roll += (gc_info.roll_rad / init_using_range);
            if(init_count % 10 == 0){
              char led_status[1]; 
              kernel.open(Kernel::OBCLED, NULL).receive(led_status, sizeof(led_status));
              led_status[0] ^= 0x01; // LED1 => toggle
              kernel.open(Kernel::OBCLED, NULL).transmit(led_status, sizeof(led_status));
            }
          }
        }
        continue;
      }else if(init_count == init_using_to_count){
        char led_status[1]; 
        kernel.open(Kernel::OBCLED, NULL).receive(led_status, sizeof(led_status));
        led_status[0] |= 0x01; // LED1 => ON
        kernel.open(Kernel::OBCLED, NULL).transmit(led_status, sizeof(led_status));
      }
    }
    
    if(gc_info.health & GCInfo::NAV_UPDATED){ // Maker Faire Tokyo 2012
      int current_index(loop_count % num_of_servo_out_histories);
      int prevoius_index((current_index - 1 + num_of_servo_out_histories) % num_of_servo_out_histories); // consider servo delay?

      { // ピッチ
        float_sylph_t pitch_diff_deg(rad2deg(gc_info.pitch_rad - init_pitch));
        float_sylph_t p_gain(std::fabs(pitch_diff_deg) < 4 ? 0.5 : 2);
        float_sylph_t command_diff(
            (pitch_diff_deg * p_gain) + (rad2deg(gc_info.omega_rads[1]) * 0.0));
        servo_out_histories[current_index][0] = servo_out_histories[prevoius_index][0] + command_diff;
      }
      { // ロール
        float_sylph_t roll_diff_deg(rad2deg(gc_info.roll_rad - init_roll));
        float_sylph_t p_gain(std::fabs(roll_diff_deg) < 4 ? 0.5 : 2);
        float_sylph_t command_diff(
            (roll_diff_deg * p_gain) + (rad2deg(gc_info.omega_rads[0]) * 0.0));
        servo_out_histories[current_index][1] = servo_out_histories[prevoius_index][1] + command_diff;
      }

      for(int j(0); j < sizeof(servo_out_histories[0]) / sizeof(servo_out_histories[0][0]); j++){
        if(servo_out_histories[current_index][j] > 2200){servo_out_histories[current_index][j] = 2200;}
        else if(servo_out_histories[current_index][j] < 800){servo_out_histories[current_index][j] = 800;}
        msg_servo_write.ch[j] = (Uint16)(servo_out_histories[current_index][j] + 0.5);
      }
      MBX_post(mbx_servo_write, &msg_servo_write, 0); // サーボ指令値が変化したことを通知
    }
#endif
  }
}
}