//#include <SoftwareSerial.h>
#include <Wire.h>
#include <MPU6050.h>
#include <Servo.h>
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();


//SoftwareSerial mySerial(2, 3); // RX, TX


#define SERVOMIN  150
#define SERVOMAX  600
#define SERVO_FREQ 50
#define motor1_pin1 5
#define motor1_pin2 6
#define motor2_pin1 9
#define motor2_pin2 10

Servo myServo;
Servo myServo2;

int state = 20;
char c;

String dataIn;
int8_t indexOfA, indexOfB, indexOfC, indexOfD, indexOfE, indexOfF, indexOfG, indexOfH;
String data1, data2, data3, data4, data5, data6, data7, data8;

String angle1_txt = "";
String angle2_txt = "";
String angle3_txt = "";
String angle4_txt = "";
String angle5_txt = "";
String angle6_txt = "";
String angle7_txt = "";
String angle8_txt = "";
int switch_data = 0;
int angle1;
int angle2;
int angle3;
int angle4;
int angle5;
int angle6;
int angle7;
int angle8;
MPU6050 gy_521;
int16_t ax, ay, az;
int16_t gx, gy, gz;
int motor1_speed;
int motor2_speed;

void setup() {

  pwm.begin();
  pwm.setPWMFreq(SERVO_FREQ);
  //Wire.begin( );
  Serial.begin(9600);//38400); // Default communication rate of the Bluetooth module
  Serial1.begin(9600);
  // Serial.println ("Initializing MPU and testing connections");
  gy_521.initialize ( );
  // Serial.println(gy_521.testConnection( ) ? "Successfully Connected" : "Connection failed");
  //  delay(1000);
  // Serial.println("Reading Values");

}

void loop() {

  while (Serial1.available() > 0) {
    c = Serial1.read();
    //delay(10);
    if (c == '\n') {
      break;
    }
    else       {
      dataIn += c;
    }
  }
  if (c == '\n') {
    parseData();

    resetData();
  }

}//END OF LOOP
/*
  angle1 = angle1_txt.toInt();
  angle2 = angle2_txt.toInt();
  angle3 = angle3_txt.toInt();
  angle4 = angle4_txt.toInt();
  angle5 = angle5_txt.toInt();
  angle6 = angle6_txt.toInt();
  angle7 = angle7_txt.toInt();
  angle8 = angle8_txt.toInt();

  Serial.println("^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^");
  Serial.println(angle1);
  Serial.println(angle2);
  Serial.println(angle3);
  Serial.println(angle4);
  Serial.println(angle5);
  Serial.println(angle6);
  Serial.println(angle7);
  Serial.println(angle8);
  Serial.println("~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~");
  angle1_txt = "";
  angle2_txt = "";
  angle3_txt = "";
  angle4_txt = "";
  angle5_txt = "";
  angle6_txt = "";
  angle7_txt = "";
  angle8_txt = "";
  switch_data = 0;
  break;
  }

  if (c == '@') {
  switch_data ++;
  continue;
  }

  if (switch_data == 0) angle1_txt += c;
  else if (switch_data == 1) angle2_txt += c;
  else if (switch_data == 2) angle3_txt += c;
  else if (switch_data == 3) angle4_txt += c;
  else if (switch_data == 4) angle5_txt += c;
  else if (switch_data == 5) angle6_txt += c;
  else if (switch_data == 6) angle7_txt += c;
  else if (switch_data == 7) angle8_txt += c;
  }


  pwm.setPWM(0, 0, angle1);
  pwm.setPWM(1, 0, angle2);
  pwm.setPWM(2, 0, angle3);
  pwm.setPWM(3, 0, angle4);
  pwm.setPWM(4, 0, angle5);
  pwm.setPWM(5, 0, angle6);
  delay(10);
  gy_521.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  ax = map(ax, -17000, 17000, -125, 125);
  motor1_speed = 125 + ax;  //To move first motor
  motor2_speed = 125 - ax;  //To move second motor
  // Serial.print ("Motor1 Speed = ");
  // Serial.print (motor1_speed, DEC);
  // Serial.print (" && ");
  // Serial.print ("Motor2 Speed = ");
  // Serial.println (motor2_speed, DEC);

  //Serial.println(angle7);
  if (angle7 == HIGH) {

  analogWrite (motor1_pin1, motor1_speed );
  analogWrite (motor2_pin2, motor2_speed);
  delay (20);
  }
  else {
  analogWrite (motor1_pin1, LOW);
  analogWrite (motor2_pin2, LOW);
  delay(20);
  }

  Serial.println(angle8);
  if (angle8 == HIGH) {

  analogWrite (motor1_pin2, motor1_speed );
  analogWrite (motor2_pin1, motor2_speed);
  delay (20);
  }
  else {
  analogWrite (motor1_pin2, LOW);
  analogWrite (motor2_pin1, LOW);
  delay(20);
  }

  }*/



void parseData() {
  indexOfA = dataIn.indexOf("A");
  indexOfB = dataIn.indexOf("B");
  indexOfC = dataIn.indexOf("C");
  indexOfD = dataIn.indexOf("D");
  indexOfE = dataIn.indexOf("E");
  indexOfF = dataIn.indexOf("F");
  indexOfG = dataIn.indexOf("G");
  indexOfH = dataIn.indexOf("H");

  data1 = dataIn.substring(0, indexOfA);
  data2 = dataIn.substring(indexOfA + 1, indexOfB);
  data3 = dataIn.substring(indexOfB + 1, indexOfC);
  data4 = dataIn.substring(indexOfC + 1, indexOfD);
  data5 = dataIn.substring(indexOfD + 1, indexOfE);
  data6 = dataIn.substring(indexOfE + 1, indexOfF);
  data7 = dataIn.substring(indexOfF + 1, indexOfG);
  data8 = dataIn.substring(indexOfG + 1, indexOfH);

  /*if (data1.length() > 3 || data2.length() > 3 || data3.length() > 3 || data4.length() > 3 || data5.length() > 3 || data6.length() >3 || data7.length() > 3 || data8.length() > 3) {
    data1 = "0";
    data2 = "0";
    data3 = "0";
    data4 = "0";
    data5 = "0";
    data6 = "0";
    data7 = "0";
    data8 = "0";
  }
  else{*/
    ShowRxData();
  //}
}

void ShowRxData() {
  Serial.println("D1: " + data1);
  Serial.println("D2: " + data2);
  Serial.println("D3: " + data3);
  Serial.println("D4: " + data4);
  Serial.println("D5: " + data5);
  Serial.println("D6: " + data6);
  Serial.println("D7: " + data7);
  Serial.println("D8: " + data8);
}

void resetData() {
  c = 0;
  dataIn = "";
}