/**
 * プロポをコントロールする基板に指令をあたえるための
 * PC側ソフトウェア。
 * 
 * 使い方:
 * 
 * #include "propo_host.h"
 * 
 * PropoHost propo(com_port_no = 1); //引数なしの場合はCOM1が設定される
 * propo.set(int channel, double ratio);
 * 
 * 同じディレクトリのexample.cppも参考のこと。
 * 
 * coded by fenrir(M.Naruoka) 2006.
 */

#ifndef __PROPO_HOST_H__
#define __PROPO_HOST_H__

#include <string>
#include <exception>

#include <Windows.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <iostream>

using namespace std;

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

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

#define COM_STR "COM"
#define COM_DEFAULT 1
#define PROPO_HOST_BANDRATE CBR_9600

//トレーナーケーブルを流れるPWM信号の最大値、最小値
#define PROPO_HIGH  1600  //[us]
#define PROPO_LOW   800   //[us]

#define PROPO_PACKET_SIZE 3

class PropoHostException : public exception{
  private:
    const string what_str;
  public:
    PropoHostException(const string &what_arg) : what_str(what_arg){}
    ~PropoHostException() throw(){}
    /**
     * エラー内容を取得します。
     * 
     * @return (chsr *) エラー内容
     */
    const char *what() const throw(){
      return what_str.c_str(); 
    }
};

class PropoHost{
  private:
    HANDLE hComm;
    DCB dcb;
    
    void writeToContoller(const unsigned char packet[PROPO_PACKET_SIZE]) const{
      DWORD size_TX, size_RX;
      char buf[16];
      WriteFile(hComm, packet, PROPO_PACKET_SIZE, &size_TX, NULL);
      if(ReadFile(hComm, buf, sizeof(buf), &size_RX, NULL)){
        buf[size_RX]=0;
      }else{buf[0] = 0;}
#if DEBUG
      printf("%x %x %x : %d byte TXed. -> RX: %s\n", packet[0], packet[1], packet[2], size_TX, buf);
#endif
    }
  private:
    void init(const int &com_port) throw(PropoHostException){
     
      char com_str[16];
      sprintf(com_str, "%s%d", COM_STR, com_port);
      cout << "TARGET_PORT:\t" << com_str << endl;
      
      hComm = CreateFile( 
                          com_str,
                          GENERIC_READ | GENERIC_WRITE,
                          0,
                          0,
                          OPEN_EXISTING,
                          FILE_ATTRIBUTE_NORMAL,
                          0
                        );

      if(hComm == INVALID_HANDLE_VALUE){
        throw PropoHostException("Couldn't open port.");
      }

      GetCommState(hComm, &dcb); // DCB を取得
      dcb.BaudRate = PROPO_HOST_BANDRATE;
      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制御なし 
      SetCommState(hComm, &dcb); // DCB を設定
    }
    
  public:
    PropoHost(const int &com_port) throw(PropoHostException){
      init(com_port);
    }
    PropoHost() throw(PropoHostException){
      init(COM_DEFAULT);
    }
    
    ~PropoHost(){
    
      CloseHandle(hComm);
    }
    
    /**
     * プロポをコントロールします。
     * 
     * @param channel チャンネル(0からはじまることに注意)
     * @param ratio 指令値(0.0〜1.0、1.0がフル)
     */
    void set(const unsigned char &channel, double ratio) const{
      unsigned char packet[PROPO_PACKET_SIZE];
      
      packet[0] = (unsigned char)channel;
      
      //指令値の計算
      if(ratio < 0){ratio = 0;}
      else if(ratio > 1){ratio = 1;}
      unsigned int command_width = (unsigned int)(ratio * (PROPO_HIGH - PROPO_LOW)) + PROPO_LOW;
      //printf("%d\n", command_width);
      
      packet[1] = (unsigned char)((command_width >> 8) & 0xFF);
      packet[2] = (unsigned char)(command_width & 0xFF);
      
      writeToContoller(packet);
    }
};

#endif /* __SERVO_HOST_H__ */