#include <TimedAction.h>

TimedAction servo1Regulator = TimedAction((1/servoSpeed),regulator(1));
TimedAction servo2Regulator = TimedAction((1/servoSpeed),regulator(2));
TimedAction servo3Regulator = TimedAction((1/servoSpeed),regulator(3));
TimedAction servo4Regulator = TimedAction((1/servoSpeed),regulator(4));

/* TODO:
 - Communication bewteen the other chips,
 - Finalise the number of servos on this chip and correct the servo code accordingly
 - Ammend the servo handeling functions to deal with the remote servos
 - Work out if manual speed regulation of the servos is feasible
 - Write the comm code for the slaves
 - Add debug statements to the program using the debug function
*/

/*
Snake Sections  Snake Servos

Head
Section 1       Servo 1
                Servo 2
Section 2       Servo 3
                Servo 4
Section 3       Servo 5 - On Slave Board One
                Servo 6 - On Slave Board Two
*/

// Regulator Settings:
#define minMovementAmmount 1;

#define debug 2; // 0 = No debug output 1 = minimal debug output 2 = maximum debug output

Servo servo1; int servo1TargetAngle; long servo1TargetTime; 
Servo servo2; int servo2TargetAngle; long servo2TargetTime; 
Servo servo3; int servo3TargetAngle; long servo3TargetTime; 
Servo servo4; int servo4TargetAngle; long servo4TargetTime; 
 
#define servoSpeed = 0.15 // 60 degrees in 0.4 seconds (400 milliseconds)
#define sectionLength 10; // Length of snake section in centimeters
#define firstServoInSectionInX  true // Assuming the first servo in each section is in the horizontal plane

void debug(String message, byte debugLevel) {
  if (debug >= debugLevel) Serial.println(message);
}

void moveServoTo(byte servoNum, int angle) {
  switch (servoNum) {
    case 1:
      servo1.write(angle);
      servo1.
      break:
    case 2:
      servo2.write(angle);
      break:
}

int getServoAngle(byte servoNum) {
    switch (servoNum) {
    case 1:
      return servo1.read();
    case 2:
      return servo2.read();
    }
}

int getServoTargetAngle(byte servoNum) {
    switch (servoNum) {
    case 1:
      return servo1TargetAngle;
    case 2:
      return servo2TargetAngle;
    }
}

int getServoTargetTime(byte servoNum) {
    switch (servoNum) {
    case 1:
      return servo1TargetTime;
    case 2:
      return servo2TargetTime;
    }
}

/*
 - servo          The servo to be moved
 - smoothMovement Use smooth movement?
 - angle          The angle to move the servo to 90 = centre
 - time           The time to spend moving in milliseconds
*/
void moveServoTo(byte servoNum, boolean smoothMovement, int angle, int time) {
  int startPos = servo.read();
  int moveAngle = abs(angle - startPos);
  int delayAmount = time - (abs(angle - startPos)/ servoSpeed); // The time the servo must waste if it is to get to the angle on time
  // WARNING, if this is negitive the servo cant make the movement in time 

  if (delayAmount <= 0) { // If cant make the movement just try anyway, else move in increments of 1 degree while taking breaks
     moveServoTo(servoNum,angle);
  } else if (smoothMovement) {
    // TODO
  } else {
    int wasteTimePerDegree = moveAngle/delayAmount;
      if (angle > startPos) {
        for (int i=0; i<moveAngle; i++) {
          moveServoTo(servoNum,(startPos + i));
          delay(1/servoSpeed); // Wait for the time taken to move one degree
          delay(wasteTimePerDegree);
        }
      } else {
         for (int i=0; i<moveAngle; i++) {
           moveServoTo(servoNum,(startPos - i));
          delay(1/servoSpeed); // Wait for the time taken to move one degree
          delay(wasteTimePerDegree);
        }
      }
  }
}

void moveSectionTo(byte sectionNum, int xAngle, int yAngle, int time) {
  moveServoTo((sectionNum*2)-1,false,xAngle,time);
  moveServoTo((sectionNum*2),false,yAngle,time);
}

/*
 - xArcRad 

*/
void snakeArc(byte firstSectionNum, byte lastSectionNum, int xArcRadius, int yArcRadius, int time) {
  snakeBend(firstSectionNum,lastSectionNum, ((lastSectionNum-firstSectionNum)*sectionLength)/xArcRadius, ((lastSectionNum-firstSectionNum)*sectionLength)/yArcRadius, time)
}

/*
 - xArcRad 

*/
void snakeBend(byte firstSectionNum, byte lastSectionNum, int xAngle, int yAngle, int time) {
  int xAnglePerSection = xAngle/(lastSectionNum - firstSectionNum);
  int yAnglePerSection = yAngle/(lastSectionNum - firstSectionNum);
  
  for (int i=firstSectionNum; i<=lastSectionNum; i++) {
    moveSectionTo(i,xAnglePerSection,yAnglePerSection, time)
  }
  
}

void regulator(byte servoNum) {
  if (getServoAngle(servoNum) != getServoTargetAngle(servoNum)) {
    int delayAmount = (getServoTargetTime(servoNum) - millis()) - (abs(angle - startPos)/ servoSpeed);
      moveServoTo
      
  } else {
    
  }
}