#include <TimedAction.h>



TimedAction regulateServos = TimedAction(2,regulator);

/*
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
*/

Servo servo1; int servo1RequiredAngle;
Servo servo2; int servo2RequiredAngle;
Servo servo3; int servo3RequiredAngle;
Servo servo4; int servo4RequiredAngle;

#define servoSpeed = 60/0.4
int sectionLength = 10; // Length of snake section in centimeters
int firstServoInSectionInX = true // Assuming the first servo in each section is in the horizontal plane

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

/*
 - 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() {
  
  
}