#include <SoftwareSerial.h>

// Pin definition
#define PWMA  6
#define PWMB  11

#define AIN1  9
#define AIN2  7
#define STBY  8
#define BIN1  10
#define BIN2  12

// Serial definition
#define WHEEL_HEAD_CHAR   'W'
#define BUFFER_LENGTH     8
#define BAUDRATE          9600

// Control limit
#define MAX_SPEED   510
#define MIN_SPEED   0
#define STOP_SPEED  255

#define RESP  "WOK"

//#define DEBUG

#define BTrec 3
#define BTtra 5


SoftwareSerial BT(BTrec, BTtra);
//定義Arduino BTrec, BTtra分別為RX及TX腳位
//請注意Arduino的TX要接藍牙模組的RX, 反之亦然

int num;
int r,g,b;
int LED = 13;

int bufIndex = 0;
char recvBuf[BUFFER_LENGTH] = {0};
int recvCompelete = 0;

int standby = HIGH;

int motorL_speed = 150;
int motorL_in1 = 0;
int motorL_in2 = 0;

int motorR_speed = 150;
int motorR_in1 = 0;
int motorR_in2 = 0;

void ctrl_wheel()
{
  digitalWrite(STBY, standby);

  analogWrite(PWMA, motorR_speed);
  digitalWrite(AIN1, motorR_in1);
  digitalWrite(AIN2, motorR_in2);

  analogWrite(PWMB, motorL_speed);
  digitalWrite(BIN1, motorL_in1);
  digitalWrite(BIN2, motorL_in2);
  delay(100);
}

void setdir(int di){
  switch (di) {
      case 1://前
          motorL_in1 = LOW;//正轉
          motorL_in2 = HIGH;
          motorR_in1 = LOW;//正轉
          motorR_in2 = HIGH;
        break;//後
      case 2:
          motorL_in1 = HIGH;//反轉
          motorL_in2 = LOW;
          motorR_in1 = HIGH;//反轉
          motorR_in2 = LOW;
        break;//左
      case 3:
        motorL_in1 = HIGH;//反轉
          motorL_in2 = LOW;
          motorR_in1 = LOW;//正轉
          motorR_in2 = HIGH;
        break;//右
      case 4:
          motorL_in1 = LOW;//正轉
          motorL_in2 = HIGH;
          motorR_in1 = HIGH;//反轉
          motorR_in2 = LOW;
        break;
      default://停
        motorL_in1 = HIGH;
          motorL_in2 = HIGH;
          motorR_in1 = LOW;
          motorR_in2 = HIGH;
  }
  ctrl_wheel();
}


void setup()
{
  // Initial pin mode
  pinMode(PWMA, OUTPUT);
  pinMode(PWMB, OUTPUT);
  pinMode(AIN1, OUTPUT);
  pinMode(AIN2, OUTPUT);
  pinMode(STBY, OUTPUT);
  pinMode(BIN1, OUTPUT);
  pinMode(BIN2, OUTPUT);

  digitalWrite(PWMA, LOW);
  digitalWrite(PWMB, LOW);
  digitalWrite(AIN1, LOW);
  digitalWrite(AIN2, LOW);
  digitalWrite(STBY, LOW);
  digitalWrite(BIN1, LOW);
  digitalWrite(BIN2, LOW);

  // Setup serial

  Serial.begin(BAUDRATE);
  BT.begin(BAUDRATE);
  //while (!Serial);

  // Send startup response
  Serial.println(RESP);

}

//f=102
//l=108
//r=114
//b=98
//R=82
//G=71
//B=66
//space=32
void loop() {
  byte cmmd[3];
  int insize;
  while(1){
    //read message from bluetooth
    if ((insize=(BT.available()))>0){
      //Serial.print("input size = ");
      Serial.print("it is:");
      //Serial.println(insize);
      for (int i=0; i<insize; i++){
        Serial.print(cmmd[i]=char(BT.read()));
        Serial.print(",");
      }
      Serial.print("\n");
    }
    //根據所收到訊號執行對應動作

     //f=102
     //l=108
     //r=114
     //b=98
     //R=82
     //G=71
     //B=66
     //space=32
    switch (cmmd[0]) {//讀取第一個字元
    case 102://for
      Serial.println("for");
      setdir(1);
      digitalWrite(LED,HIGH);
      break;

    case 98://bac
      Serial.println("bac");
      setdir(2);
      digitalWrite(LED,LOW);
      break;
    case 108://lef
      Serial.println("lef");
      setdir(3);
      break;
    case 114://rig
      Serial.println("rig");
      setdir(4);
      break;
    case 82://R
      //num=(cmmd[1]-48)*100 + (cmmd[2]-48)*10 + cmmd[3]-48;
      Serial.print("R");
      //Serial.println(num);
      delay(10);
      break;
    case 71://G
      //num=(cmmd[1]-48)*100 + (cmmd[2]-48)*10 + cmmd[3]-48;
      Serial.print("G");
      //Serial.println(num);
      delay(10);
      break;
    case 66://B
      //num=(cmmd[1]-48)*100 + (cmmd[2]-48)*10 + cmmd[3]-48;
      Serial.print("B");
      //Serial.println(num);
      delay(10);
      break;
    }

  }
}