#define MOTOR_LEFT_A 15 // IN1
#define MOTOR_LEFT_B 14 // IN2

#define MOTOR_RIGHT_A 1 // IN1.2
#define MOTOR_RIGHT_B 2 // IN2.2

#define LEFT 0
#define RIGHT 1

#define DEAD_ZONE 30
#define MID_X 505
#define MID_Y 512

#define STEER_PERCENT 0.7


#define speaker 9

void setup() {
  // put your setup code here, to run once:
   //
   // set up serial
   Serial.begin(115200);
   Serial1.begin(115200);// wake up Serial1 at the same rate  (check that this is the rate your bluetooth module wants)   
   // setting up the bluetooth device
   delay(100);
   Serial1.print("$$$"); //no line ending
   delay(100);
   Serial1.print("+\r"); //carriage return
   delay(100);
   Serial1.print("SS,C0\r"); //carriage return
   delay(100);
   //Serial1.flush();
   
   //
   // set pins to output
   //

   write_motor(0, LEFT);
   write_motor(0, RIGHT);

   //need to update the inputs later according to the values read in Serial
}

String input1_str = ""; // input from the controller x-axis
String input2_str = "";
String serial_receive = ""; //[number1 number2]
int x;
int y;

void write_motor(float percent, int left_or_right) {
  // 100% : max forward
  // 0% : stopleft_or_right
  // -100% : max backwards

  int value_A, value_B;

  if (percent > 0) {
    value_A = percent * 255;
    value_B = 0;
  } else if (percent < 0) {
    value_A = 0;
    value_B = -percent * 255;
  } else {
    value_A = 0;
    value_B = 0;
  }
  
  if (left_or_right == LEFT) {
    analogWrite(MOTOR_LEFT_A, value_A);
    analogWrite(MOTOR_LEFT_B, value_B);
  } else {
    analogWrite(MOTOR_RIGHT_A, value_A);
    analogWrite(MOTOR_RIGHT_B, value_B);
  }
}


void loop() {
//   // enabling the bluetooth
//    while (Serial.available()) {      // If anything comes in Serial USB,
//      Serial1.write(Serial.read());   // read it and send it out Serial1 (PA10/11)
//    }
    while (Serial1.available()) {     // If anything comes in Serial1  (PA10/11)
      serial_receive = Serial1.readStringUntil('\n');
      //converting the incoming str to int
      input1_str = serial_receive.substring(0, serial_receive.indexOf(" "));
      input2_str = serial_receive.substring(serial_receive.indexOf(" ")+1, serial_receive.length());
      x = input1_str.toInt();
      y = input2_str.toInt();
      
      //    LEFT    
      Serial.print(x); //used for debugging
      Serial.print(" ");
      Serial.println(y);
      float value_up_down;

      if (x >0 && x<=2000 && y >0 && y<=2000){
        if (x == 2000 && y == 2000) {
          // honk
          tone(speaker, 1000, 200);
          continue;
        }
  
        if (x >= MID_X - DEAD_ZONE && x <= MID_X + DEAD_ZONE) {
          // middle band
          value_up_down = -1.0 + 2.0 * (float)y / 1023; //make reverse negative and forward positive
          if (y >= MID_Y - DEAD_ZONE && y <= MID_Y + DEAD_ZONE) {
            // don't move
            write_motor(0, LEFT);
            write_motor(0, RIGHT);
          } else {
            // move forward or backward
            write_motor(value_up_down, LEFT);
            write_motor(value_up_down, RIGHT);
          }
        } else if (x < MID_X - DEAD_ZONE) {
          // going left
          float value_left = STEER_PERCENT + (1.0 - STEER_PERCENT) * (float) x / MID_X; // 0.5 to 1
          value_up_down = -1.0 + 2.0 * (float)y / 1023;
  //        write_motor(value_left * value_up_down, LEFT);
  //        write_motor(value_up_down, RIGHT);
            write_motor(-value_left * value_up_down, LEFT); //left wheel moves back
            write_motor(value_left * value_up_down, RIGHT); //right wheel moves forward
        } else {
          // going right
          float value_right = 1.0 - (1.0 - STEER_PERCENT) * (float) (x - MID_X) / (1023 - MID_X); // 1 to 0.5
          value_up_down = -1.0 + 2.0 * (float)y / 1023;
  //        write_motor(value_up_down, LEFT);
  //        write_motor(value_right * value_up_down, RIGHT);
            write_motor(value_right * value_up_down, LEFT); //left wheel moves forward
            write_motor(-value_right * value_up_down, RIGHT); //right wheel moves back
        }
  
        // to calculate the factor (accounts for the forward/reverse speed)
        //factor: dependent on speed 
      }
      else{
        Serial.print("weird values");
        write_motor(0, LEFT);
        write_motor(0, RIGHT);
      }
    }
}