#include <Servo.h>
 
//ピン接続 | Pin assign
#define GT_DTR 4      //DTR
#define GT_DSR 6      //DSR
#define GT_TRDY 7      //TRDY <--未使用 | Disused
 
#define RX_PIN 13 // GT-SPからのシリアルデータを受け取るピン | Pin to receive serial data from GT-SP
 
Servo servoMotor;
 
void setup() {
  //ピン初期設定 | Pin setting
  pinMode(GT_DTR, INPUT);
  pinMode(GT_DSR, OUTPUT);
  pinMode(GT_TRDY, INPUT);
 
  // Servoモータの初期化 | Initialization of Servo motor
  servoMotor.attach(RX_PIN);
 
  // シリアル通信の開始 | Sserial communication start
  Serial.begin(38400);    //38400bps Baud rate
  Serial.setTimeout(100); //シリアルタイムアウト | Serial timeout
}
 
void loop() {
  // GT-SPデータ受信 | Receive one GT-SP response data
  if (Serial.available()) {
    // GT-SPから数値を取得 | Get values from GT-SP
    unsigned long gtspValue = gtsp_signal_read_numeric();
 
    // スライダーの値を0〜180の範囲にマッピング | Mapping slider value from 0 to 180
    int servoAngle = map(gtspValue, 0, 1023, 0, 180);
 
    // サーボモータを指定した角度に動かす | Move servo motor to the specified angle
    servoMotor.write(servoAngle);
 
    // GT-SPにサーボモータの角度を表示 | Display the angle of servo motors on GT-SP
    gtsp_ObjPrpSet_string(2, 0x40, String(servoAngle)+ " deg");
 
    // シリアルモニタにサーボの角度を表示（デバッグ用）| Display the angle of the servo on the serial monitor (for debugging)
    Serial.print("Servo Angle: ");
    Serial.println(servoAngle);
  }
}
/********************** 関数 | Function **********************/
////////////////////////////////////////////////////
//  データ受信（数値用） | Receive data from GT-SP (for Value)
////////////////////////////////////////////////////
unsigned long gtsp_signal_read_numeric() {
  byte res_dl[4] = "";
  unsigned long dl;
 
  if (Serial.find("RESb", 4)) {
    Serial.readBytes(res_dl, 4); //データ長抽出 | Read data length
    dl = (unsigned long)(res_dl[0] + (res_dl[1] << 8) + (res_dl[2] << 16) + (res_dl[3] << 24)); //データ長変換 | Data length
 
    // 数値を受信するためのバッファ | Buffer for receiving value
    byte res_data_buffer[4];
    // データを読み取り | Read data
    Serial.readBytes(res_data_buffer, dl);
 
    // 数値に変換してリターン
    return (unsigned long)(res_data_buffer[0] + (res_data_buffer[1] << 8) + (res_data_buffer[2] << 16) + (res_data_buffer[3] << 24));
  }
 
  // Returns 0 in case of error (needs to be modified for proper error handling)
  return 0;
}
 
 
 
 
////////////////////////////////////////////////////
//  オブジェクト制御コマンド - プロパティ設定 (文字列用) | Object Control Command -Property Setting (for String)
////////////////////////////////////////////////////
void gtsp_ObjPrpSet_string(int obj, int prp, String val )
{
  gt_print("CMD");     //コマンドヘッダ | Command header
  gt_put(0xd3);        //オブジェクト-プロパティ設定コマンド | Object-Property Setting
  gt_put(obj >> 0);    //オブジェクトNo. 下位バイト | Object No. Lower byte
  gt_put(obj >> 8);    //オブジェクトNo. 上位バイト| Object No. Upper byte
  gt_put(prp >> 0);    //プロパティNo. 下位バイト | Property No. Lower byte 
  gt_put(prp >> 8);    //プロパティNo. 上位バイト| Property No. Upper byte 
  gt_put(val.length() >> 0);    //データ長 最下位バイト | Data length Least significant byte
  gt_put(val.length() >> 8);    //データ長 下位バイト | Data length second byte
  gt_put(val.length() >> 16);   //データ長 上位バイト | Data length third byte
  gt_put(val.length() >> 24);   //データ長 最上位バイト| Data length Most significant byte
  gt_print(val);       //文字列送信 | Send String
}
 
////////////////////////////////////////////////////
//  1byte送信 | Send byte to GT-SP
////////////////////////////////////////////////////
void gt_put(unsigned char onebyte)
{
  while ( digitalRead(GT_DTR) == HIGH ) {}  //busycheck
  Serial.write(onebyte);
}
 
////////////////////////////////////////////////////
//  文字列送信 | Send String to GT-SP
////////////////////////////////////////////////////
void gt_print(String val)
{
  int val_i;
  //文字列を1文字ずつ送信 | Send string per one byte
  for (val_i = 0; val_i < val.length(); val_i++)
  {
    while ( digitalRead(GT_DTR) == HIGH ) {}  //busycheck
    Serial.print(val.substring(val_i, val_i+1));
  }
}
