/**
 * @file
 * 
 * コスメイトのターンテーブルを自動制御するためのヘッダ
 * 
 * 使い方:
 * 
 * #include "cosmate_if.h"
 * 
 * CostmateIF cos_if(com_port_spec); 
 * 
 * 通信は
 *   38400bps
 *   stop: 1bit
 *   data: 8bits
 *   parity: none
 * を想定
 * 
 * coded by fenrir(M.Naruoka)
 */

#ifndef __COSMATE_IF_H__
#define __COSMATE_IF_H__

#include <cstdio>
#include <cstring>
#include <cmath>
#include <iostream>
#include <iomanip>

#include <Windows.h>

#include "util/comstream.h"

#if _MSC_VER >= 1400
#define _CRT_SECURE_NO_WARNINGS // sprintf_s等への推奨を止める
#endif

#ifndef min
#define min(x, y) (((x) < (y)) ? (x) : (y))
#else
#define ALREADY_MIN_DEFINED
#endif

#ifndef max
#define max(x, y) (((x) > (y)) ? (x) : (y))
#else
#define ALREADY_MAX_DEFINED
#endif

#define concat_str(str1, str2) str1 ## str2

class CosmateIF {
  public:
    const static int packet_size = 9;
    const static int data_per_packet = packet_size - 2;
  private:
    ComportStream comport;
    unsigned char rx_packet[packet_size];
    int rx_index_of_packet;
    
    static void debug_print(
        const unsigned char data[], 
        const unsigned int size){
      std::cerr << std::hex;
      for(int i(0); i < size; i++){
        std::cerr << std::setfill('0') 
                  << std::setw(2)
                  << (unsigned int)((unsigned char)data[i]) << ' ';
      }
      std::cerr << std::dec;
      std::cerr << std::endl;
    }
  
  public:
    /**
     * 送信処理を行う
     * 
     * @param data パケットのうちヘッダ、トレーラを除いた
     * index = 1からはじまるデータ
     * @return (bool) 送信後の状態　
     */
    bool write_to_TT(const unsigned char data[data_per_packet]){
      unsigned char packet[packet_size];
      packet[0] = packet[packet_size - 1] = 0x81;
      for(int i(0); i < data_per_packet; i++){
        packet[i + 1] = data[i];
        packet[packet_size - 1] += data[i];
      }
      packet[packet_size - 1] &= 0x7F;
      
      comport.write((const char *)packet, sizeof(packet));
      comport.flush();
#if DEBUG
      std::cerr << sizeof(packet) << "bytes TXed" << std::endl;
      debug_print(packet, sizeof(packet));
#endif
      return comport.good();
    }
    
    /**
     * 受信処理を行う
     * 
     * @param data データ格納用のバッファ
     * @param return 1パケット分受信できた場合はtrue
     */
    bool read_from_TT(unsigned char data[data_per_packet]){
      int size_RX;
      
      bool res(false);
      
      while(comport.good()){
        
        comport.read(
            (char *)&(rx_packet[rx_index_of_packet]), 
            (packet_size - rx_index_of_packet));
        size_RX = comport.gcount();
        
        if(!size_RX){continue;}
#if DEBUG
        std::cerr 
            << size_RX << "bytes RXed. index => "
            << rx_index_of_packet 
            << std::endl;
#endif
        rx_index_of_packet += size_RX;
#if DEBUG
        debug_print(rx_packet, rx_index_of_packet);
#endif
        int head_index(0);
        for(; head_index < rx_index_of_packet; head_index++){
          if(rx_packet[head_index] == 0x8F){break;}  
        }
        if(head_index > 0){
          rx_index_of_packet -= head_index;
          std::memmove(rx_packet, (rx_packet + head_index), rx_index_of_packet);
        }else if(rx_index_of_packet >= packet_size){
          // チェックサムの計算
          unsigned char checksum(0);
          for(int i(0); i < packet_size - 1; i++){
            checksum += rx_packet[i];
          }
#if DEBUG
          std::cerr << "checksum: " << (int)(checksum & 0x7f) << ", " << (int)(rx_packet[packet_size - 1]) << std::endl;
#endif          
          if((checksum & 0x7F) == rx_packet[packet_size - 1]){
            std::memcpy(data, &rx_packet[1], data_per_packet);
            res = true;
          }
          rx_index_of_packet = 0;
          break;
        } 
      }
      return res;
    }
    
  private:
#ifdef _WIN32
    static void reconfig_dcb(DCB &dcb) {
      dcb.BaudRate = CBR_38400;
      dcb.ByteSize = 8;
      dcb.Parity = NOPARITY;
      dcb.fParity = FALSE;
      dcb.StopBits = ONESTOPBIT;
      dcb.fBinary = TRUE; // バイナリモード
      dcb.fNull = FALSE;  // NULLバイトは破棄しない
      dcb.fOutX = FALSE;  // XONなし
      dcb.fInX = FALSE;   // XOFFなし
      //dcb.fOutxCtsFlow = FALSE;   // CTSフロー制御なし
      //dcb.fOutxDsrFlow = FALSE;   // DSRフロー制御なし
      //dcb.fDsrSensitivity = FALSE;         // DSR制御なし 
    }
#else
#error Assume non-Windows? Unsupported!!
#endif
    
  public:
    CosmateIF(const char *port_spec) throw(std::ios_base::failure)
        : comport(port_spec), rx_index_of_packet(0) {
      comport.buffer().config_dcb(reconfig_dcb);
      std::cerr << "TARGET_PORT:\t" << port_spec << std::endl;
      char buf[64];
      while(comport.readsome(buf, sizeof(buf)));
    }
    ~CosmateIF(){}
  protected:
    static void make_command(
        unsigned char data[data_per_packet], 
        unsigned char command_num){
      std::memset(&(data[1]), 0, (sizeof(data) - sizeof(data[0])));
      data[0] = command_num;
    }
    
    static void make_data1(
        unsigned char data[data_per_packet], 
        unsigned int value,
        bool flag){
#if DEBUG
      std::cerr << "make_data1 : " << value 
          << "; flag : " << (flag ? '1' : '0')
          << std::endl;
#endif
      for(int i(1); i < 5; i++){
        data[i] = (unsigned char)(value & 0x7F);
        value >>= 7;
      }
      if(flag){data[4] |= 0x40;}
    }
    
    static void make_data2(
        unsigned char data[data_per_packet], 
        unsigned int value){
#if DEBUG
      std::cerr << "make_data2 : " << value << std::endl;
#endif
      for(int i(5); i < 7; i++){
        data[i] = (unsigned char)(value & 0x7F);
        value >>= 7;
      }
    }
  
  public:
    bool set_origin(){
      unsigned char data[data_per_packet];
      make_command(data, 1);
      return write_to_TT(data);
    }
    
    bool set_zero(){
      unsigned char data[data_per_packet];
      make_command(data, 2);
      return write_to_TT(data);
    }
    
    bool set_stop(){
      unsigned char data[data_per_packet];
      make_command(data, 3);
      make_data1(data, 0, false);
      make_data2(data, 0);
      return write_to_TT(data);
    }
    
    bool set_angle_drive(const double &angle, const double &speed){
      unsigned char data[data_per_packet];
      make_command(data, 4);
      unsigned int _angle(min(std::abs(angle * 10), 36000)), 
          _speed(min(std::abs(speed), 1080));
      make_data1(data, _angle, (angle < 0));
      make_data2(data, _speed);
      return write_to_TT(data);
    }
    
    bool set_speed_drive(const double &speed){
      if((speed < 0.5) && (speed > -0.5)){return set_stop();}
      unsigned char data[data_per_packet];
      make_command(data, 5);
      unsigned int _speed;
      if(speed >= 0){
        _speed = min(speed, 1080);
        if(speed - _speed > 0.5){_speed++;}
        make_data1(data, 0, false);
      }else{
        double speed_abs(-speed);
        _speed = min(speed_abs, 1080);
        if(speed_abs - _speed > 0.5){_speed++;}
        make_data1(data, 0, true);
      }
      make_data2(data, _speed);
      return write_to_TT(data);
    }
    
    bool set_cf_drive(const double &length, const double &cf){
      unsigned char data[data_per_packet];
      make_command(data, 7);
      unsigned int _length(min(std::abs(length), 200)),
          _cf(min(std::abs(cf * 10), 100));
      make_data1(data, _length, (length < 0));
      make_data2(data, _cf);
      return write_to_TT(data);
    }
    
    bool set_accel(const double &pos_accel, const double &neg_accel){
      unsigned char data[data_per_packet];
      make_command(data, 8);
      unsigned int _pos_accel(min(std::abs(pos_accel), 10)),
          _neg_accel(min(std::abs(neg_accel), 10));
      make_data1(data, _pos_accel, false);
      make_data2(data, _neg_accel);
      return write_to_TT(data);
    }
    
    bool set_limit(const double &angle, const double &speed){
      unsigned char data[data_per_packet];
      make_command(data, 9);
      unsigned int _angle(min(std::abs(angle), 3600)), 
          _speed(min(std::abs(speed), 1080));
      make_data1(data, _angle, (angle < 0));
      make_data2(data, _speed);
      return write_to_TT(data);
    }
    
    bool unset_limit(){
      return set_limit(0, 1080);
    }
    
    bool set_rcv(const int &interval){
      unsigned char data[data_per_packet];
      make_command(data, 9);
      unsigned int _interval(min(std::abs(interval), 50));
      make_data1(data, _interval, false);
      return write_to_TT(data);
    }
    
    bool wait_msg(unsigned char rcv_data[data_per_packet],
        const unsigned char &msg_num,
        unsigned int retry){
      bool inf_loop(retry == 0);
      while(inf_loop || (retry--)){
        if(read_from_TT(rcv_data)){
          if((rcv_data[0] & 0x3F) == msg_num){return true;} 
        }
      }
      return false;
    }
    bool wait_msg(unsigned char rcv_data[data_per_packet],
        const unsigned char &msg_num){
      return wait_msg(rcv_data, msg_num, 0);
    }
    bool wait_msg_not(unsigned char rcv_data[data_per_packet],
        const unsigned char &msg_num,
        unsigned int retry){
      bool inf_loop(retry == 0);
      while(inf_loop || (retry--)){
        if(read_from_TT(rcv_data)){
          if((rcv_data[0] & 0x3F) != msg_num){return true;} 
        }
      }
      return false;
    }
    bool wait_msg_not(unsigned char rcv_data[data_per_packet],
        const unsigned char &msg_num){
      return wait_msg(rcv_data, msg_num, 0);
    }
    bool wait_speed(unsigned char rcv_data[data_per_packet],
        const double &speed,
        unsigned int retry){
      if((speed < 0.5) && (speed > -0.5)){
        return wait_msg(rcv_data, 0, retry);
      }
      
      bool inf_loop(retry == 0);
      double speed_abs(speed > 0 ? speed : -speed);
      int speed_rcv;
      while(inf_loop || (retry--)){
        if(read_from_TT(rcv_data)){
          (speed_rcv = (rcv_data[5] & 0x3F)) <<= 7;
          speed_rcv |= (rcv_data[4] & 0x7F);
          double speed_diff = speed_abs - speed_rcv;
          if((rcv_data[5] & 0x40)
              && (speed_diff <= 1)
              && (speed_diff >= -1)){return true;} 
        }
      }
      return false;
    }
    bool wait_speed(unsigned char rcv_data[data_per_packet],
        const double &speed){
      return wait_speed(rcv_data, speed, 0);
    }
};

#ifndef  ALREADY_MIN_DEFINED
#undef min
#endif

#ifndef ALREADY_MAX_DEFINED
#undef max
#endif

#endif /* __COSMATE_IF_H__ */