#include <SoftwareSerial.h>
#include <Servo.h>

String time;
long temp;
//Connect:
//arduino >> bluetooth
// D10 >> TX
// D11 >> RX

Servo servo1, servo2;
SoftwareSerial mySerial(10, 11); //RX, TX

String readString; //main captured String
String pitch; //data String
String roll;
String throttle;

int ind1; // , locations
int ind2;
int ind3;

int s1, s2;


void setup() {
  pinMode(9, OUTPUT);
  servo1.attach(9); // Attaches pin D9 on nano to Servo! :)
  pinMode(5, OUTPUT);
  servo2.attach(5);
  Serial.begin(9600);

  servo1.writeMicroseconds(700); // initialize servos
  servo2.writeMicroseconds(700);

  mySerial.begin(9600);
  mySerial.setTimeout(50);

}

void loop() {

// Denne setupen forventer en string på formen: x,y,z*  -- Viktig at det er kommadelt og en stjerne på slutten! :-)
// Variable1(pitch),variable2(roll),variable3(throttle)*

  if (mySerial.available())  {
    char c = mySerial.read();  //gets one byte from serial buffer
    if (c == '*') {
      //do stuff
      ind1 = readString.indexOf(',');  //finds location of first ,
      roll = readString.substring(0, ind1);   //captures first data String
      ind2 = readString.indexOf(',', ind1+1 );   //finds location of second ,
      pitch = readString.substring(ind1+1, ind2);   //captures second data String
      ind3 = readString.indexOf(',', ind2+1 );
      throttle = readString.substring(ind2+1, ind3); // ind3+1

      Serial.print("Roll: ");
      Serial.println(roll);
      Serial.print("Pitch: ");
      Serial.println(pitch);
      Serial.print("Throttle: ");
      Serial.println(throttle);
      Serial.println("");

      s1 = pitch.toInt(); // Casts the string back to int or float, whatever we choose to use.
      s2 = roll.toInt();

      servo1.writeMicroseconds(s1);
      servo2.writeMicroseconds(s2);

      readString=""; //clears variable for new input
      pitch="";
      roll="";
      throttle="";
      mySerial.write("u"); // Sends signal back to python
    }
    else {
      readString += c; //makes the string readString
    }
  }
}