// Yellow bot code (now with cereal box)
// By Tyler Bletsch (Tyler.Bletsch@gmail.com)

#include <SimpleSoftwareServo.h> // this is my library; you can replace with regular servo library too)
#include <SoftwareSerial.h>

int noSignal = 0;

SoftwareSerial bluetooth(A0,A1); //RX,TX  note that RX will go to the TX on the bluetooth, and TX will go to the RX on the bluetooth

#define control_serial bluetooth // set to Serial for wired, bluetooth for wireless

SimpleSoftwareServo servo1;
int servo1_pin = 4;

#define    STX          0x02
#define    ETX          0x03

// for Joystick BT Commander
int joyX;
int joyY;
bool button_state[6] = {0};
long sendInterval = 750;                               // interval between Buttons status transmission (milliseconds)

int motorL[] = {5,6};
int motorR[] = {9,10};

void motor_setSpeed(int pin1, int pin2, int speed) {
  if (speed<0) { // backward
    speed = -speed;
    analogWrite(pin1,0);
    analogWrite(pin2,speed);
  } else {
    analogWrite(pin1,speed);
    analogWrite(pin2,0);
  }
}

#define motorL_setSpeed(speed) motor_setSpeed(motorL[0],motorL[1],speed)
#define motorR_setSpeed(speed) motor_setSpeed(motorR[0],motorR[1],speed)

void setup(){
  Serial.begin(9600);
  Serial.println("Robot booted.");
  bluetooth.begin(9600);
  motorL_setSpeed(0);
  motorR_setSpeed(0);
  servo1.attach(servo1_pin);

  pinMode(motorL[0],OUTPUT);
  pinMode(motorL[1],OUTPUT);
  pinMode(motorR[0],OUTPUT);
  pinMode(motorR[1],OUTPUT);


  while (0) {
    digitalWrite(5,1);
    digitalWrite(6,0);
  }
  //myServo.attach(9);

}

long latency = -1;

void loop() {
  long tStart = micros();
  // interact with Joystick BT Commander via bluetooth serial, populates globals joyX, joyY (in range -100..+100), and button_state[]
  receiveBluetooth();
  sendBluetooth();

  motor_setSpeed_curve(-joyY/100.0,-joyX/100.0);

  int servo_angle = button_state[0] ? 45 : 180;

  servo1.write(servo_angle);
  SimpleSoftwareServo::refresh();

  latency = micros() - tStart;

  //delay(40);
}

void motor_setSpeed_curve(float mag, float curve) {
  float sensitivity = 0.5;
  float leftOutput, rightOutput; // -1..+1
  float value,ratio;
  if (curve < 0) {
    value = log(-curve);
    ratio = (value - sensitivity) / (value + sensitivity);
    if (ratio == 0) {
      ratio = .0000000001;
    }
    leftOutput = mag / ratio;
    rightOutput = mag;
  } else if (curve > 0) {
    value = log(curve);
    ratio = (value - sensitivity) / (value + sensitivity);
    if (ratio == 0) {
        ratio = .0000000001;
    }
    leftOutput = mag;
    rightOutput = mag / ratio;
  } else {
    leftOutput = mag;
    rightOutput = mag;
  }

  motorL_setSpeed(255*leftOutput);
  motorR_setSpeed(255*rightOutput);

}

// The bluetooth format code from Joystick BT Commander, adapted and cleaned up from 'AndroTest_V20_POSTED' - http://forum.arduino.cc/index.php?topic=173246.msg1766646#msg1766646
void receiveBluetooth() {
  byte cmd[8] = {0};                 // bytes received
  if(control_serial.available())  {                           // data received from smartphone
    delay(2);
    cmd[0] =  control_serial.read();
    if(cmd[0] == STX)  {
      int i=1;
      while(control_serial.available())  {
        delay(1);
        cmd[i] = control_serial.read();
        if(cmd[i]>127 || i>7)                 break;     // Communication error
        if((cmd[i]==ETX) && (i==2 || i==7))   break;     // Button or Joystick data
        i++;
      }
      if     (i==2)          getButtonState(cmd[1]);    // 3 Bytes  ex: < STX "C" ETX >
      else if(i==7)          getJoystickState(cmd);     // 6 Bytes  ex: < STX "200" "180" ETX >
    }
  }
}

void sendBluetooth()  {
  static long previousMillis = 0;
  long currentMillis = millis();
  if(currentMillis - previousMillis > sendInterval) {   // send data back to smartphone
    previousMillis = currentMillis;

// Data frame transmitted back from Arduino to Android device:
// < 0X02   Buttons state   0X01   DataField#1   0x04   DataField#2   0x05   DataField#3    0x03 >
// < 0X02      "01011"      0X01     "120.00"    0x04     "-4500"     0x05  "Motor enabled" 0x03 >    // example

    control_serial.print((char)STX);                                             // Start of Transmission
    control_serial.print("butstat?");  control_serial.print((char)0x1);   // buttons status feedback    // (no evidence this is used --tkb)
    control_serial.print(millis());            control_serial.print((char)0x4);   // datafield #1
    control_serial.print(latency);          control_serial.print((char)0x5);   // datafield #2
    control_serial.print("Sup");                                         // datafield #3
    control_serial.print((char)ETX);                                             // End of Transmission
  }
}

void getJoystickState(byte data[8])    {
  int _joyX = (data[1]-'0')*100 + (data[2]-'0')*10 + (data[3]-'0');       // obtain the Int from the ASCII representation
  int _joyY = (data[4]-'0')*100 + (data[5]-'0')*10 + (data[6]-'0');
  _joyX = _joyX - 200;                                                  // Offset to avoid
  _joyY = _joyY - 200;                                                  // transmitting negative numbers

  if(_joyX<-100 || _joyX>100 || _joyY<-100 || _joyY>100)     return;      // commmunication error

  joyX = _joyX;
  joyY = _joyY;
}

void button_on(int b) {
  button_state[b] = true;
  Serial.print("Button "); Serial.print(b); Serial.println(" ON");
}

void button_off(int b) {
  button_state[b] = false;
  Serial.print("Button "); Serial.print(b); Serial.println(" off");
}

void getButtonState(byte bStatus)  {
  switch (bStatus) {
    case 'A':  button_on(0); break;
    case 'B': button_off(0); break;
    case 'C':  button_on(1); break;
    case 'D': button_off(1); break;
    case 'E':  button_on(2); break;
    case 'F': button_off(2); break;
    case 'G':  button_on(3); break;
    case 'H': button_off(3); break;
    case 'I':  button_on(4); break;
    case 'J': button_off(4); break;
    case 'K':  button_on(5); break;
    case 'L': button_off(5); break;
  }
}