#include <stdio.h>
#include <string.h>

/* 非依存ヘッダ */
#include "common.h"
#include "ultra_sonic.h"

/* ユーティリティヘッダ */
#include <fifo_num.h>
#include <conv.h>

/* 依存ヘッダ */
#include <AKI3694.h>
#include <sci.h>
#include <timer.h>
#include <ad.h>
#include <wdt.h>

// サーボ(スピコン含む)チャンネル数
#define SERVO_CH 6
unsigned short output_pulse[SERVO_CH];
unsigned short input_pulse[SERVO_CH];

// 超音波チャンネル数
#define ULTRA_SONIC_CH 1

// 重力加速度、単位はmm/s^2
#define SCALE_1G 9806

// サーボに与えるパルス幅の設定
#define MAX_PULSE_WIDTH 22000
#define MIN_PULSE_WIDTH 8000
#define MIDDLE_PULSE_WIDTH 15000

// サーボのぴくつきを防止するか(1=する、0=しない)
#define ANTI_SHAKING 1
#if ANTI_SHAKING
#define SERVO_MEAN_NUMBER 16
unsigned short elevator_history[SERVO_MEAN_NUMBER];
unsigned short throttle_history[SERVO_MEAN_NUMBER];
unsigned short ludder_history[SERVO_MEAN_NUMBER];
#endif


/* 処理ルーチン */

/**
 * その他の初期化
 * 
 */
void misc_init(){
  
  int i;
  
  /* サーボパルス幅の初期化 */
  for(i = 0; i < SERVO_CH; i++){
    output_pulse[i] = MIDDLE_PULSE_WIDTH;
    input_pulse[i] = MIDDLE_PULSE_WIDTH;
  }
  
  /* サーボ履歴の初期化 */
#if ANTI_SHAKING
  for(i = 0; i < SERVO_MEAN_NUMBER; i++){
    elevator_history[i] = MIDDLE_PULSE_WIDTH;
    throttle_history[i] = MIDDLE_PULSE_WIDTH;
    ludder_history[i] = MIDDLE_PULSE_WIDTH;
  }
#endif
	
	/* LEDの有効化 */
	//IO.PCR8 = 0x03;
	IO.PCR8 = 0x01; // P81(LED2)はタイマW(FTIOA)で使用!!
  
  /* PORT50〜55はサーボにつながる */
  IO.PCR5 = 0x3F;
  
  /* タイマWの有効化
   * 
   * クロックはφ/2(0.1 us)
   * A => インプットキャプチャ(割り込み)
   * B => インプットキャプチャ(割り込み)
   * C => アウトプットコンペア
   * D => アウトプットコンペア
   */
  TW.TCRW.BYTE = 0x10;
  TW.TIOR0.BYTE = 0x47; // B => 立ち上がりエッジのみ検出
  TW.TIOR1.BYTE = 0x00;
  TW.TIERW.BYTE = 0x03;
  
  /* カウントスタート */
  TW.TMRW.BIT.CTS = 1;
}

#define servo_high(ch) (IO.PDR5.BYTE |=  (0x01 << ch))
#define servo_low(ch)  (IO.PDR5.BYTE &= ~(0x01 << ch))

unsigned char out_target_ch;

inline void start_pulse_timerw(unsigned char ch);

inline void stop_pulse_timerw(){
  
  /* ポートLow */
  servo_low(out_target_ch);
  
  /* 次のチャンネルへ */
  start_pulse_timerw(out_target_ch + 1);
}

inline void start_pulse_timerw(unsigned char ch){
  
  /* パラメータの設定 */
  if((out_target_ch = ch) >= SERVO_CH){
    /* 割り込み禁止 */
    TW.TIERW.BIT.IMIEC = 0;
    return;
  }
  
  TW.GRC = (TW.TCNT + output_pulse[ch]);
  
  /* ポートHigh */
  servo_high(ch);
  
  /* コンペアマッチCで割り込みがかかるようにする */
  if(ch == 0){
    TW.TSRW.BIT.IMFC = 0;
    TW.TIERW.BIT.IMIEC = 1;
  }
}

#define BIT_IMFA 0x01
#define BIT_IMFB 0x02
#define BIT_IMFC 0x04
#define BIT_IMFD 0x08

inline void check_pulse(unsigned char flag){
  static unsigned char target_ch = SERVO_CH;
  static unsigned short before_count;
  
  if(target_ch < SERVO_CH){
    // 奇数(1, 3, ...)
    if(target_ch & 0x01){
      if(flag & BIT_IMFA){
        unsigned short pulse_width 
          = (TW.GRA > before_count) ? (TW.GRA - before_count) : ~(before_count - TW.GRA);
        if(pulse_width < MAX_PULSE_WIDTH && pulse_width > MIN_PULSE_WIDTH){
        	input_pulse[target_ch] = pulse_width;
	        if(flag & BIT_IMFB){
	          before_count = TW.GRB;
	          target_ch++;
	          return;
	        }
        }
      }
    // 偶数(0, 2, ...)
    }else{
      if(flag & BIT_IMFB){
      	unsigned short pulse_width 
          = (TW.GRB > before_count) ? (TW.GRB - before_count) : ~(before_count - TW.GRB);
        if(pulse_width < MAX_PULSE_WIDTH && pulse_width > MIN_PULSE_WIDTH){
        	input_pulse[target_ch] = pulse_width;
	        if(flag & BIT_IMFA){
	          before_count = TW.GRA;
	          target_ch++;
	          return;
	        }
        }
      }
    }
    target_ch = SERVO_CH;
    TW.TIOR0.BYTE &= ~(0x30); // B => 立ち上がりエッジのみ検出
  }else{
    if((flag & (BIT_IMFA | BIT_IMFB)) == BIT_IMFB){
      before_count = TW.GRB;
      target_ch = 0;
      TW.TIOR0.BYTE |= 0x30; // B => 両エッジ検出
    }
  }
}
  
#pragma interrupt
void int_timerw(){
	
	unsigned char flag = TW.TSRW.BYTE;
	TW.TSRW.BYTE = 0x00;
		
	/* IMFC */
	if(flag & BIT_IMFC){
		stop_pulse_timerw();
	}

	/* IMFA, IMFB */
	if(flag & (BIT_IMFA | BIT_IMFB)){
	  check_pulse(flag);
	}
}

#define CALC_START 0x01
#define COMMAND_INIT 0x02
unsigned char status = 0;

/**
 * タイマAインターバルによる呼び出しルーチン
 * 6.4msごとに呼び出される
 */
void call_every_6m4s(){
	static unsigned char loop = 0;
	switch(++loop){
    case 2:
      start_pulse_timerw(0);
      break;
    case 3:
      status |= CALC_START;
		  //IO.PDR8.BIT.B1 = (IO.PDR8.BIT.B1 ? 0 : 1); // P81(LED2)はタイマW(FTIOA)で使用!!
      loop = 0;
	}
}

#define pulse_in_range(command) (max(min(command, MAX_PULSE_WIDTH), MIN_PULSE_WIDTH))
#define pulse_in_range2(command, range) (max(min(command, MIDDLE_PULSE_WIDTH + range), MIDDLE_PULSE_WIDTH - range))
#define pulse_in_range3(command, range) (max(min(command, range), - range))

/**
 * 指令値の計算
 * 
 * @param accel[] 
 * 	 3軸の加速度 0,1,2の順にX,Y,Z軸、単位はmm/s^2、
 *   例えば静止状態では
 *   accel[0],accel[1]には約0(mm/s^2)
 *   accel[2]には約9806(mm/s^2)
 *   がセットされている
 * 
 * @param ultra_sonic[] 
 *   超音波センサから得られた距離(未実装なので使わないこと)
 */
inline void calc_command(short accel[3], unsigned char ultra_sonic[1]){
	
	/*
	 * 補足説明
	 * 
	 * 1.定義済み変数について
	 * 
	 * 関数の外部で次の変数が定義されています。
	 * input_pulse 受信機より得られたパルス幅(0〜5)
	 * output_pulse 実際に送信するパルス幅(0〜5)
	 * 
	 * パルス幅の単位は[0.1us]です。
	 * 例えば、双葉のサーボなら
	 * 15200[0.1us]が中心位置(0度)で
	 * 20200[0.1us]が+90度
	 * 10200[0.1us]が-90度
	 * 
	 * 詳しくは、
	 * http://berry.sakura.ne.jp/technics/servo_control_p3.html
	 * 等に情報があります。
	 * 
	 * 例えば
	 * int i;
	 * for(i = 0; i < 6; i++){
	 *   output_pulse[i] = input_pulse[i];
	 * }
	 * はマイコンが間に挿入されていないのと同じ状態、
	 * つまり受信機とサーボやスピコンを直繋ぎしたのと同じ状態
	 * を実現します。
	 * 
	 * 2.コーディング上の注意
	 * 
	 * 残念ながらマイコンの計算能力が高くないので、
	 * 整数演算を使用するようにしてください。
	 * 小数点演算や三角関数が入ると非常にスピードが落ちます。
	 * 例えば、0.25をかける場合は 
	 *  >> 2 (ビットシフト演算) や 
	 *  / 4 (整数演算) 
	 * としてください。
	 * どうしても三角関数等を使用したい場合は相談してください。
	 * 
	 * 3.この関数の呼び出し速度
	 * 
	 * この関数は約50Hzで呼び出されます。
	 */
	static unsigned char calc_invoked = 0;
	calc_invoked++;
  
#if ANTI_SHAKING
  //ぴくつき防止用
	static int elevator_sum = MIDDLE_PULSE_WIDTH * SERVO_MEAN_NUMBER;
	static int throttle_sum = MIDDLE_PULSE_WIDTH * SERVO_MEAN_NUMBER;
	static int ludder_sum = MIDDLE_PULSE_WIDTH * SERVO_MEAN_NUMBER;

	unsigned char history_index = calc_invoked % SERVO_MEAN_NUMBER;
#endif
	
	unsigned short elevator_command;
	unsigned short ludder_command;
	unsigned short throttle_command;
	
	if(input_pulse[4] > MIDDLE_PULSE_WIDTH){	
		//自動
		//エレベータ
		{
			//const short KE_1 = 1;
      //const short KE_2 = 1;
			/*
      elevator_command = MIDDLE_PULSE_WIDTH - 500;
      elevator_command += KE_1 * accel[0];
      elevator_command += KE_2 * (accel[2] - SCALE_1G);	//エレベータのパルス幅[0.1us]
      elevator_command = pulse_in_range2(elevator_command, 2000);
      */
      elevator_command = input_pulse[1];
		}
		
		//スロットル
		{
			
      const short TARGET_HIGHT = 300; // 目標高度[*10 mm]
			const short KT = 10; // スロットルゲイン
			short h = ((20000 - input_pulse[2]) / 1000) * 100;	// プロポから送られてきた高度
			throttle_command = pulse_in_range(18500 + KT * ( TARGET_HIGHT - h )); // スロットルのパルス幅[0.1us]
      
      //throttle_command = input_pulse[2];
		}
		
		//ラダー
		{
      const short KR_1 = 2;
      //const short KR_2 = 2;
			ludder_command = MIDDLE_PULSE_WIDTH - 1000;
      ludder_command -= accel[1] / KR_1;
      //ludder_command = MIDDLE_PULSE_WIDTH;
      //ludder_command += accel[1] / KR_1; 	//ラダーのパルス幅[0.1us]
      //ludder_command += KR_2 * (input_pulse[3] - MIDDLE_PULSE_WIDTH);   //ラダーのパルス幅[0.1us]
      //ludder_command = pulse_in_range2(ludder_command, 3500);
		}
	
		output_pulse[0] = input_pulse[2];
#if ANTI_SHAKING
    //ぴくつき防止用
		output_pulse[1] = (elevator_sum / SERVO_MEAN_NUMBER);
		output_pulse[2] = (throttle_sum / SERVO_MEAN_NUMBER);
		output_pulse[3] = (ludder_sum / SERVO_MEAN_NUMBER);
#else
    //こっちだとぴくつく
		output_pulse[1] = elevator_command;
		output_pulse[2] = throttle_command;
		output_pulse[3] = ludder_command;
#endif
		output_pulse[4] = input_pulse[0];
		output_pulse[5] = input_pulse[1];
	}else{
		//手動
		int i;
		for(i = 0; i < 6; i++){
		  output_pulse[i] = input_pulse[i];
		}
		
		elevator_command = input_pulse[1];
		throttle_command = input_pulse[2];
		ludder_command = input_pulse[3];
	}

#if ANTI_SHAKING
  //ぴくつき防止用
	elevator_sum -= elevator_history[history_index];
	elevator_sum += (elevator_history[history_index] = elevator_command);
	throttle_sum -= throttle_history[history_index];
	throttle_sum += (throttle_history[history_index] = throttle_command);
	ludder_sum -= ludder_history[history_index];
	ludder_sum += (ludder_history[history_index] = ludder_command);

#define DUMP_COMMAND 0
#if DUMP_COMMAND
	if(calc_invoked % 8 == 0){
	  char buf[32];
	  char *buf_index = buf;
	  buf_index = ushort2string(elevator_command, buf_index);
	  *(buf_index++) = ' ';
	  buf_index = ushort2string(throttle_command, buf_index);
	  *(buf_index++) = ' ';
	  buf_index = ushort2string(ludder_command, buf_index);
    null_char(crlf(buf_index));
    sci_dump(buf);
	}
#endif
#endif
}

// 加速度計のゼロ点調整はこの値を変更すること
#define ACCEL_X_0G 32760
#define ACCEL_Y_0G 32500
#define ACCEL_Z_1G 22575

inline void polling(){
  while(!(status & CALC_START));
  status &= ~(CALC_START);
  
	static unsigned char invoked_polling = 0;
	invoked_polling++;

  unsigned char i, j;
  
  num_t accel_raw[3];
  
  // AD変換値の計算(16回して平均をとる)
  {
	  num_t num_buf;
	  accel_raw[0] = accel_raw[1] = accel_raw[2] = 0;
	  for(i = 0; i < 16; i++){
	    ad_start();
	    for(j = 0; j < 3; j++){
	      while(!fifo_num_read(&(fifo_ad[j]), &num_buf));
	      accel_raw[j] += (num_buf >> 4);
	    }
	  }
  }
  
  short accel[3];
  
  // 変換値の加工(値がmm/s^2になるようにする)
  {
  	/*
  	 * ACCEL_1G 13107 // = (1 << 16) / 5
  	 * EARTH_1G 9.80665
  	 * ACCEL_1mm_s2 (4 / 3) // = (ACCEL_1G / (EARTH_1G * 1000)) = 1.33654204
  	 * AD_RAW / ACCEL_1mm_s2 = accel [mm/s^2]
  	 */
	  // X軸
	  accel[0] = ((short)(accel_raw[0] >> 2) - (ACCEL_X_0G >> 2)) * 3;
  	// Y軸
  	accel[1] = ((accel_raw[1] >> 2) - (short)(ACCEL_Y_0G >> 2)) * 3;
	  // Z軸
	  accel[2] = (((ACCEL_Z_1G >> 2) - (short)(accel_raw[2] >> 2)) * 3) + SCALE_1G;
  }
  
  
#if DEBUG
	// 加速度計の値出力
  if(invoked_polling % 8 == 0){
	  char buf[32];
	  char *buf_index = buf;
	  //null_char(ushort2string((unsigned short)invoked_polling, buf));
	  for(i = 0; i < 3; i++){
	  	*(buf_index++) = i + '0';
	  	*(buf_index++) = ':';
	    //buf_index = ushort2string(accel_raw[i], buf_index);
      buf_index = short2string(accel[i], buf_index);
      *(buf_index++) = ' ';
    }
    null_char(crlf(buf_index));
    sci_dump(buf);
	}
#endif
  
  // 超音波センサの値を読み込む
  unsigned char us[ULTRA_SONIC_CH];
  for(i = 0; i < ULTRA_SONIC_CH; i++){
  	us[i] = us_read(i);
  }
  
#if DEBUG
	// パススルー
  for(i = 0; i < SERVO_CH; i++){
    output_pulse[i] = input_pulse[i];
  }
#else
	// ここに制御を埋め込むこと
	calc_command(accel, us);
#endif
}

/**
 * メイン関数
 * 
 */
int main(){
	
	// Wait
	//int i;
  //for(i = 0; i < 100000; i++);
 
  // 初期設定
	misc_init();
	us_init();
	timer_init(call_every_6m4s);
	ad_init();
	sci3_init();
	wdt_init();

  enableIRQ(); /* 割り込み有効 */
 
  sci_dump("Test Program started!\r\n");

	wdt_start();
  
  while(1){
    polling();

		wdt_reset(12); // WDTを100msで動作 20E6 / 8192 * 100 * 1E-3 = 244.14
  	
  	IO.PDR8.BIT.B0 = (IO.PDR8.BIT.B0 ? 0 : 1);
  }
 
  disableIRQ(); /* 割り込み無効 */
  
  return 0;
}