#include <cstdio>
#include <cstdlib>
#include <climits>
#include <ctime>

#include <string>
#include <sstream>
#include <Windows.h>
#include <direct.h>

using namespace std;

#define DEBUG 1
#include "cosmate_if.h"
#include "espec_if.h"

//#define STEP_INTERVAL 1000 * 60 * 1 //[ms]  // 1分間
#define STEP_INTERVAL 1000 * 15 //[ms]  // 20秒間

#define SPEED_UNIT 10.0 // deg/s
#define SPEED_STEPS 22 // max/min => * SPEED_UNIT

#ifndef PI
 #define PI 3.14159265358979323846
#endif

#define RUBY_BIN "C:\\cygwin\\bin\\ruby"
string bot_path;
#define BOT_SCRIPT "u_tokyo_13_bot.rb"
#define NOTIFY_TARGET "@fenrir_n"

void notify(const string &msg){
  string command;
  command /*.append("cd ").append(bot_path).append(" && ")*/
      .append(RUBY_BIN).append(" ")
      .append(BOT_SCRIPT).append(" ")
      .append("\"")
        .append(NOTIFY_TARGET).append(" ")
        .append(msg).append("\"");
  cerr << command << endl;
  system(command.c_str());
}

#define SPEED_CALC ((speed_step > 15) ? (50.0 * (speed_step - 15) + 150) : ((speed_step < -15) ? (50.0 * (15 + speed_step) - 150) : (SPEED_UNIT * speed_step)))

void constant_temperature_loop(CosmateIF &cos_if, const double &temperature){
  Sleep(STEP_INTERVAL);
    
  int speed_step(0);
  double speed;
  unsigned char rcv_data[CosmateIF::data_per_packet];
  
  // 速度指定ドライブ1 (0 -> +LIMIT_SPEED)
  for(speed = 0; speed_step < SPEED_STEPS; speed_step++, speed = SPEED_CALC){
    cout << "Info: Deg " << (int)speed << "; Temp " << (int)temperature << endl;
    while(true){
      cos_if.set_speed_drive(speed);
      if(cos_if.wait_speed(rcv_data, speed, 10)){break;}
    }
    Sleep(STEP_INTERVAL);
  }
  
  {
    stringstream ss;
    ss << "現在、最高速度 " << speed << "deg/s @ " << temperature << "℃";
    notify(ss.str());
  }
  
  // 速度指定ドライブ2 (+LIMIT_SPEED -> -LIMIT_SPEED)
  for(; speed_step > -SPEED_STEPS; speed_step--, speed = SPEED_CALC){
    cout << "Info: Deg " << (int)speed << "; Temp " << (int)temperature << endl;
    while(true){
      cos_if.set_speed_drive(speed);
      if(cos_if.wait_speed(rcv_data, speed, 10)){break;}
    }
    Sleep(STEP_INTERVAL);
  }
  
  {
    stringstream ss;
    ss << "現在、逆最高速度 " << speed << "deg/s @ " << temperature << "℃";
    notify(ss.str());
  }
  
  // 速度指定ドライブ1 (-LIMIT_SPEED -> 0)
  for(; speed_step <= 0; speed_step++, speed = SPEED_CALC){
    cout << "Info: Deg " << (int)speed << "; Temp " << (int)temperature << endl;
    while(true){
      cos_if.set_speed_drive(speed);
      if(cos_if.wait_speed(rcv_data, speed, 10)){break;}
    }
    Sleep(STEP_INTERVAL);
  }
}

int main(int argc, const char *argv[]){
  
  cerr << "Usage: " << argv[0] 
      << " Cosmete_COM Espec_COM" << endl;
  
  /*{
    char buf[256];
    _getcwd(buf, sizeof(buf) - 1);
    bot_path = buf;
    bot_path.append("\\");
    cerr << "Bot_path: " << bot_path << endl;
  }*/
  
  
  if(argc <= 2){
    cerr << "Arguments are not enough !!" << endl;
    exit(-1);
  }
  
  CosmateIF cos_if(argv[1]);
  EspecIF espec_if(argv[2]);
  
  unsigned char rcv_data[CosmateIF::data_per_packet];
  
  // 動いていたら停止
  while(true){
    if(cos_if.read_from_TT(rcv_data)){
      cout << (int)rcv_data[0] << endl;
      switch(rcv_data[0]){
        case 2: // 角度指定ドライブ中
        case 3: // 速度指定ドライブ中
        case 5: // 遠心加速度ドライブ中
          while(true){
            cos_if.set_speed_drive(0);
            //cos_if.set_stop();
            if(cos_if.wait_msg(rcv_data, 0, 10)){break;}
          }
          break;
      }
      break;
    } 
  }
  
  // リミット解除
  cos_if.unset_limit();
  //cos_if.wait_msg(rcv_data, 8);
  
  // 原点設定
  while(true){
    cos_if.set_origin();
    if(cos_if.wait_msg_not(rcv_data, 1, 10)){break;}
  }
  
  {
    stringstream ss;
    ss << "テストを開始します ターンテーブル: " 
        << argv[1] << " 恒温槽: " << argv[2];
    notify(ss.str());
  }
  
  // 温度を変えつつ、回転数を変える
  //double temperatures_list[] = {20, 30, 40, -10, 0, 10, 20};
  double temperatures_list[] = {25, 40, -10, 0, 15};
  
  for(int i(0); 
      i < sizeof(temperatures_list) / sizeof(temperatures_list[0]);
      ++i){
    double temperature(temperatures_list[i]);
    
    {
      stringstream ss;
      ss << "温度を設定中です " << temperature << "℃";
      notify(ss.str());
    }
    espec_if.set_temperature(temperature);
    {
      stringstream ss;
      ss << "回転テスト開始 " << temperature << "℃";
      notify(ss.str());
    }
    
    constant_temperature_loop(cos_if, temperature);
    {
      stringstream ss;
      ss << "回転テスト終了 " << temperature << "℃";
      notify(ss.str());
    }
  }
  espec_if.standby(); // スタンバイ状態に移行
  {
    stringstream ss;
    ss << "全テスト終了、面を変えるか終了しましょう!!";
    notify(ss.str());
  }
}