/**
 * @file Sylphide に搭載する制御ルーチン
 * 
 */

#ifndef __SYLPH_CONTROL_H__
#define __SYLPH_CONTROL_H__

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

#include "sylph_state.h"

// とりあえず車をいれてみる
#include "note/070423/controller/hino_car.h"

// 座標変換のために座標変換のコードが必要
#include "navigation/coordinate.h"

#include <cstring>

class Car : public AbstractCar {
  public:
    static const Uint16 SERVO_MIN;
    static const Uint16 SERVO_MAX;
    static const Uint16 SERVO_NEUTRAL;
  private:
    typedef AbstractCar super_t;
  protected:
    dmatrix_t x, u;
    System_LLH<> origin_llh;
    System_XYZ<> origin_xyz;
    
  public:
    Car() : super_t(), x(super_t::states, 1), u(super_t::controls, 1) {
      
    }
  
    /**
     * 入力の設定
     * 
     * Sets control input value
     * @param i input type
     * @param value input value
     */
    void set_input(const control_t i, const double &value){
      double neutral(0);
      switch(i){
        case ACCEL:
          neutral = 1515;
          break;
        case STEERING:
          neutral = 1613;
          break;
        default:
          neutral = SERVO_NEUTRAL;
      }
      u(i, 0) = value + SERVO_NEUTRAL;
    }
    
    void set_input(const dmatrix_t &values){
      set_input(ACCEL, const_cast<dmatrix_t &>(values)(ACCEL, 0));
      set_input(STEERING, const_cast<dmatrix_t &>(values)(STEERING, 0));
    }
    
    static Uint16 servo_limit(double v){
      if(v > SERVO_MAX){
        return SERVO_MAX;
      }else if(v < SERVO_MIN){
        return SERVO_MIN;
      }
      return (Uint16)v;
    }
    
    /**
     * 指令値の伝達
     * 
     * external_writeの先頭2文字を"CF"とするとFPGAへ伝達されるので、
     * それを利用する
     * 
     * 書式は [address_wr] [size] [contents(0)] [contents(1)]
     */
    void reflect_input() const {
      
      union {
        Uint16 val;
        unsigned char buf[2];
      } values[2];
      
      values[0].val = servo_limit(input_value(STEERING));
      values[1].val = servo_limit(input_value(ACCEL));
      
      unsigned char buf[] // ヘッダと2ch分(WriteはLSB=0、数値はビックエンディアン)
          = {'C', 'F', 
              (0x0A << 1) & (~0x01), sizeof(Uint16), values[0].buf[1], values[0].buf[0],  // ch.3
              (0x0B << 1) & (~0x01), sizeof(Uint16), values[1].buf[1], values[1].buf[0]}; // ch.4
      
      external_write((char *)buf, sizeof(buf));
    }
  
    /** 
     * state_value
     * 
     * returns designated state value
     * @param suffix suffix
     */
    double state_value(const state_t i) const {
      return const_cast<dmatrix_t &>(x)(i, 0);
    }
  
    /** 
     * input_value
     * 
     * returns designated control input value
     * @param suffix suffix
     */
    double input_value(const control_t i) const {
      return const_cast<dmatrix_t &>(u)(i, 0);
    }
    
    /**
     * 初期化を行う
     * 
     * @param current 現在の状態
     * @return bool 初期化に成功した場合
     */
    bool initialize(const ::state_t &current){
      
      if(!current.nav.valid){
        return false;
      }
      
      // 初期位置を原点とする
      origin_llh.latitude() = /*deg2rad(35.715269);*/ current.nav.latitude;
      origin_llh.longitude() = /*deg2rad(139.760813);*/ current.nav.longitude;
      origin_llh.height() = 0; //current.nav.height;
      origin_xyz = origin_llh.xyz();
      
      u(AbstractCar::ACCEL, 0)
          = current.servo.servo_in[3];
      u(AbstractCar::STEERING, 0)
          = current.servo.servo_in[2];
      
      return true;
    }
    
    /**
     * 指令地を伝達するとともに現在の状態から車の状態を作る
     * 
     * @param current 現在の状態
     * @return 状態が変化した場合
     */
    bool refresh(const ::state_t &current){
      
      u(AbstractCar::ACCEL, 0)
          = current.servo.servo_out[3];
      u(AbstractCar::STEERING, 0)
          = current.servo.servo_out[2];
      
      if(/*(!current.nav.updated) ||*/ (!current.nav.valid)){
        return false;
      }
      
      // 緯度、経度、高度からEast-North-Up座標に変換
      System_LLH<> current_llh(
          current.nav.latitude,
          current.nav.longitude,
          0); //current.nav.height);
      
      System_ENU<> enu(
          System_ENU<>::relative_rel(
              current_llh.xyz() - origin_xyz, origin_llh));
      
      // 現在の状態の取得
      x(AbstractCar::X, 0) = enu.north();
      x(AbstractCar::Y, 0) = enu.east();
      x(AbstractCar::U, 0) 
          = sqrt(pow(current.nav.v_north, 2) + pow(current.nav.v_east, 2));
      x(AbstractCar::THETA, 0)
          = current.nav.heading;
      
      return true;
    }
};

const Uint16 Car::SERVO_MIN = 1200;
const Uint16 Car::SERVO_MAX = 1900;
const Uint16 Car::SERVO_NEUTRAL = (Car::SERVO_MIN + Car::SERVO_MAX) / 2;

extern "C" {
// 制御タスク
void control_task(){
  const double time_step(1E-3 * NAV_UPDATE_RATIO_ms); ///< 50Hz
  
  WaypointTracker tracker(time_step);
  
  // ウェイポイントの設定(単位はm)
  Waypoint &wp(tracker.wp);
  // X軸は北、Y軸は東!!
  Waypoint::item_t waypoints[] = {
      {0, 4, 3, Waypoint::point},
      {-4, 4, 3, Waypoint::point},
      {-4, 0, 3, Waypoint::point},
      {0, 0, 3, Waypoint::point},
      {0, 4, 3, Waypoint::loop}};
  wp.change_waypoints(sizeof(waypoints) / sizeof(waypoints[0]));
  for(unsigned int i(0); i < wp.waypoints(); i++){
    wp.set_waypoint(i, waypoints[i]);
  }
  
  // PIDゲインの設定
  tracker.accel_con.set_gain(pid_controller_c::P, -100);
  tracker.steering_con.set_gain(pid_controller_c::P, -80);
  tracker.heading_con.set_gain(pid_controller_c::P, 0.4);
  tracker.heading_con.set_gain(pid_controller_c::D, 0.2);
  
  Car car;
  static const int MANUAL_CONTROL_THRESHOLD = 5; // チャタリング防止のため
  int manual_controlled(MANUAL_CONTROL_THRESHOLD);  ///< 非負の場合、手動
  bool need_reset(true);
  
  int loop_count(0);
  
  while(true){
    state_t sylphide_state(current.fetch()); // 現在状態のコピー

#if defined(ORCA_SPECIAL)
    { // エコーテスト
      union {
        Uint16 val;
        unsigned char buf[2];
      } values;
      
      unsigned char buf[64] // ヘッダとある分だけ(WriteはLSB=0、数値はビックエンディアン)
          = {'C', 'F'};
      int index(2);
      
      for(int i(0); i < state_t::servo_channels; i++){
        values.val = sylphide_state.servo.servo_in[i];
        buf[index++] = ((0x08 + i) << 1) & (~0x01);
        buf[index++] = sizeof(Uint16);
        buf[index++] = values.buf[1];
        buf[index++] = values.buf[0];
        if((i & 0x07) == 0x07){ // 8chごとに書き込む
          external_write((char *)buf, index);
          index = 2;
        }
      }
    }
    continue;
#endif
    
#if defined(SERVO_TEST)
    car.refresh(sylphide_state);
    car.set_input(Car::ACCEL,
        ((car.input_value(Car::ACCEL) < Car::SERVO_MAX)
            ? (car.input_value(Car::ACCEL) + 1)
            : Car::SERVO_MIN) - Car::SERVO_NEUTRAL);
    car.set_input(Car::STEERING,
        ((car.input_value(Car::STEERING) < Car::SERVO_MAX)
            ? (car.input_value(Car::STEERING) + 1)
            : Car::SERVO_MIN) - Car::SERVO_NEUTRAL);
    car.reflect_input();
    continue;
#endif
    
    // 手動か自動か判定、手動 => 自動のタイミングでリセットをかける
    //if(sylphide_state.servo.updated){
      // 自動の指示
      if(sylphide_state.servo.servo_in[7] > Car::SERVO_NEUTRAL){
        need_reset = (manual_controlled >= 0);
        if((--manual_controlled) < -MANUAL_CONTROL_THRESHOLD){
          manual_controlled = -MANUAL_CONTROL_THRESHOLD;
        }
      }else{
        // 手動
        if((++manual_controlled) > MANUAL_CONTROL_THRESHOLD){
          manual_controlled = MANUAL_CONTROL_THRESHOLD;
        }
      }
    //}
      
    if((++loop_count % 200) == 0){
      char buf[32] = "hoge";
      snprintf(buf, sizeof(buf) - 1, "DWaypoint: %d at itow = %f",
          tracker.current_waypoint, sylphide_state.nav.itow);
      telemetry_write(buf, strlen(buf));
    }
      
    // 手動の場合はループの先頭へ
    if(manual_controlled >= 0){continue;}
    
    if(need_reset){
      // 前回手動だった場合などはリセット
      if(car.initialize(sylphide_state)){
        tracker.current_waypoint = 0;
        need_reset = false;
      }
    }else if(car.refresh(sylphide_state)){
      // 状態が変化したら、自動航法の計算
      tracker.update_target(car); // 制御器の更新
      car.set_input(tracker.get_control_value(car));  // 指令値をセット
      car.reflect_input(); // 指令値を反映
    }
  }
}
}

#endif /* __SYLPH_CONTROL_H__ */