#include <Servo.h>

const bool testing = true;

int pos;
int temp;
unsigned long timeTaken;

const int angleStep = 1; // step in degrees to turn
const int angleStepDelay = 1; // delay in ms between turning 1 step

const float inputscale = 255/100;
const int MAXRANGE = 260; // max range of servo in degrees
const float servoscale = 180/MAXRANGE;

// minimum delay in ms between instructions are performed
// if our delay is 50 ms, and inst1 takes 10ms, it will wait 40ms before starting inst2
// if inst1 takes more than 50ms, no delay
// if inst1 is 0, no delay
const int delayBetweenInst = 0;

// delay in ms to wait before rolling if vertical and lateral are in use
const int rollDelay = 0;

// delay in ms to wait before using all servos to go backwards
const int backwardDelay = 0;

const bool clearSerialAfterLoop = false; // set to true if arduino is lagging behind in commands

const int topServopin = A0;
const int bottomServopin = A1;
const int leftServopin = A2;
const int rightServopin = A3;

const int mainThruster = 5;

const int topMotor = 3;
const int topDir = 999; //################# CHANGE ME ############
const int bottomMotor = 6;
const int bottomDir = 999; //################# CHANGE ME ############
const int leftMotor = 9;
const int leftDir = 8; //################# CHANGE ME ############
const int rightMotor = 11;
const int rightDir = 12; //################# CHANGE ME ############


float fowardinst;
float verticalinst;
float lateralinst;
float rollrateinst;
float pitchrateinst;
float yawrateinst;

Servo topServo;
Servo leftServo;
Servo rightServo;
Servo bottomServo;

// variable to store the servo position
int topServopos = 0;
int leftServopos = 0;
int rightServopos = 0;
int bottomServopos = 0;

void setup() {
  Serial.begin(9600);

  topServo.attach(topServopin);
  bottomServo.attach(bottomServopin);
  leftServo.attach(leftServopin);
  rightServo.attach(rightServopin);  // attaches the servo on pin to the servo object

  pinMode(mainThruster, OUTPUT);

  pinMode(topMotor, OUTPUT);
  pinMode(bottomMotor, OUTPUT);
  pinMode(leftMotor, OUTPUT);
  pinMode(rightMotor, OUTPUT);
  pinMode(topDir, OUTPUT);
  pinMode(bottomDir, OUTPUT);
  pinMode(leftDir, OUTPUT);
  pinMode(rightDir, OUTPUT);
}

// start should be current position
// end should be intended angle in degrees
// returns final position
int turn(Servo myservo, int start, int end) {
  end = end * servoscale;
  if (start < end) {
    for (pos = start; pos <= end; pos += angleStep) { // goes from start degrees to end degrees
      myservo.write(pos);              // tell servo to go to position in variable 'pos'
      delay(angleStepDelay);
    }
  }else {
    for (pos = start; pos >= end; pos -= angleStep) { // goes from start degrees to end degrees
      myservo.write(pos);              // tell servo to go to position in variable 'pos'
      delay(angleStepDelay);
    }
  }
  myservo.write(end);
  return end; //instead of having to write leftServopos = 270, it'll know it.
}

void loop() {

  while(Serial.available() > 0)           //I'm using a while instead of an if to ensure I get all the data at once.
  {                                       //Serial.available = how many coins left.

    if (testing) {
      fowardinst    = 0;
      verticalinst  = 50;
      lateralinst   = 0;
      rollrateinst  = 0;
      pitchrateinst = 0;
      yawrateinst   = 0;

      temp = 62; // don't change
    }else {
      do {
        temp = Serial.read();
      } while (temp != 60); // temp != "<"

      fowardinst    = Serial.parseFloat();
      verticalinst  = Serial.parseFloat();
      lateralinst   = Serial.parseFloat();
      rollrateinst  = Serial.parseFloat();
      pitchrateinst = Serial.parseFloat();
      yawrateinst   = Serial.parseFloat();

      temp = Serial.read();
    }


    if (temp == 62)     // temp == ">"
    //check flow across prop, change servo values accordingly
    {
      //do pitch first figure this out, set motor ops. or individual operation
      if (pitchrateinst != 0) {

        timeTaken = millis(); // time started

        // #################### CHECK ANGLES ######################
        topServopos = turn(topServo,topServopos,90);
        bottomServopos = turn(bottomServo,bottomServopos,90);
        if(pitchrateinst > 0){
          digitalWrite(topDir,HIGH);
          digitalWrite(bottomDir,LOW);
        }else{
          pitchrateinst = pitchrateinst * -1;
          digitalWrite(topDir,LOW);
          digitalWrite(bottomDir,HIGH);
        }
        analogWrite(topMotor, inputscale*pitchrateinst);
        analogWrite(bottomMotor, inputscale*pitchrateinst);

        timeTaken = millis() - timeTaken; // (current time) - (time started)
        if (timeTaken < delayBetweenInst) {
          delay(delayBetweenInst - timeTaken);
        }
      }else{ // pitch inst is 0
        analogWrite(topMotor,0);
        analogWrite(bottomMotor,0);
      }

      //do vertical - up,down
      if (verticalinst != 0) {

        timeTaken = millis(); // time started

        // #################### CHECK ANGLES ######################
        leftServopos = turn(leftServo,leftServopos,90);
        rightServopos = turn(rightServo,rightServopos,270);
        if(verticalinst > 0) {
          digitalWrite(leftDir,HIGH);
          digitalWrite(rightDir,HIGH);
        }else{
          verticalinst = verticalinst * -1;
          digitalWrite(leftDir,LOW);
          digitalWrite(rightDir,LOW);
        }
        analogWrite(leftMotor, inputscale*verticalinst);
        analogWrite(rightMotor, inputscale*verticalinst);

        timeTaken = millis() - timeTaken; // (current time) - (time started)
        if (timeTaken < delayBetweenInst) {
          delay(delayBetweenInst - timeTaken);
        }
      }else{ //vertical instruction is 0, turn off motors
        analogWrite(leftMotor,0);
        analogWrite(rightMotor,0);
      }

      //do lateral - left,right
      if (lateralinst != 0) {

        timeTaken = millis(); // time started

        // #################### CHECK ANGLES ######################
        topServopos = turn(topServo,topServopos,180);
        bottomServopos = turn(bottomServo, bottomServopos,0);
        if(lateralinst > 0) {
          digitalWrite(topDir,HIGH);
          digitalWrite(bottomDir,HIGH);
        }else{
          lateralinst = lateralinst * -1;
          digitalWrite(topDir,LOW);
          digitalWrite(bottomDir,LOW);
        }
        analogWrite(topMotor, inputscale*lateralinst);
        analogWrite(bottomMotor, inputscale*lateralinst);

        timeTaken = millis() - timeTaken; // (current time) - (time started)
        if (timeTaken < delayBetweenInst) {
          delay(delayBetweenInst - timeTaken);
        }
      }else{ //lateral instruction is 0, turn off motors
        analogWrite(topMotor,0);
        analogWrite(bottomMotor,0);
      }

      //do yaw swoosh right,left
      if (yawrateinst != 0) {

        timeTaken = millis(); // time started

        // #################### CHECK ANGLES ######################
        leftServopos = turn(leftServo, leftServopos,90);
        rightServopos = turn(rightServo, rightServopos,90);
        if(yawrateinst > 0) {
          digitalWrite(leftDir,LOW);
          digitalWrite(rightDir,HIGH);
        }else{
          yawrateinst = yawrateinst * -1;
          digitalWrite(leftDir,HIGH);
          digitalWrite(rightDir,LOW);
        }
        analogWrite(leftMotor, inputscale*yawrateinst);
        analogWrite(rightMotor, inputscale*yawrateinst);

        timeTaken = millis() - timeTaken; // (current time) - (time started)
        if (timeTaken < delayBetweenInst) {
          delay(delayBetweenInst - timeTaken);
        }
      }else{ //yawrateinstruction is 0
        analogWrite(leftMotor,0);
        analogWrite(rightMotor,0);
      }

      //do roll cw/ccw
      if (rollrateinst != 0) {

        timeTaken = millis(); // time started

        if(verticalinst == 0) {
          // #################### CHECK ANGLES ######################
          leftServopos = turn(leftServo, leftServopos,0);
          rightServopos = turn(rightServo, rightServopos,180);
          if(rollrateinst > 0) {
            digitalWrite(leftDir,LOW);
            digitalWrite(rightDir,HIGH);
          }else{
            rollrateinst = rollrateinst * -1;
            digitalWrite(leftDir,LOW);
            digitalWrite(rightDir,HIGH);
          }
          analogWrite(leftMotor, inputscale*rollrateinst);
          analogWrite(rightMotor, inputscale*rollrateinst);
        }else{
          if (lateralinst != 0) {
            delay(rollDelay);
          }
          // #################### CHECK ANGLES ######################
          topServopos = turn(topServo, topServopos,0);
          bottomServopos = turn(bottomServo, bottomServopos,180);
          if(rollrateinst > 0) {
            digitalWrite(topDir,HIGH);
            digitalWrite(bottomDir,LOW);
          }else{
            rollrateinst = rollrateinst * -1;
            digitalWrite(topDir,LOW);
            digitalWrite(bottomDir,HIGH);
          }
          analogWrite(topMotor, inputscale*rollrateinst);
          analogWrite(bottomMotor, inputscale*rollrateinst);
        }

        timeTaken = millis() - timeTaken; // (current time) - (time started)
        if (timeTaken < delayBetweenInst) {
          delay(delayBetweenInst - timeTaken);
        }
      }

      //do foward
      if (fowardinst > 0) {
        analogWrite(mainThruster, inputscale*fowardinst );
      }else{

        timeTaken = millis(); // time started

        analogWrite(mainThruster, 0);

        delay(backwardDelay);
        fowardinst = fowardinst*-1;
        // #################### CHECK ANGLES ######################
        topServopos = turn(topServo, topServopos,0);
        bottomServopos = turn(bottomServo, bottomServopos,0);
        leftServopos = turn(leftServo, leftServopos,0);
        rightServopos = turn(rightServo, rightServopos,0);
        digitalWrite(topDir,LOW);
        digitalWrite(bottomDir,LOW);
        digitalWrite(leftDir,LOW);
        digitalWrite(rightDir,LOW);
        analogWrite(topMotor,inputscale*fowardinst);
        analogWrite(bottomMotor,inputscale*fowardinst);
        analogWrite(leftMotor,inputscale*fowardinst);
        analogWrite(rightMotor,inputscale*fowardinst);

        timeTaken = millis() - timeTaken; // (current time) - (time started)
        if (timeTaken < delayBetweenInst) {
          delay(delayBetweenInst - timeTaken);
        }
      }
    }
    else
    {
      //ERROR
      Serial.println("ERROR");
    }
  }

  if (clearSerialAfterLoop == true)
  {
    Serial.flush();
  }

}