/*  Copyright 2011 Christopher Baines <cbaines8@gmail.com>
 Copyright 2011 Adam "Insert other details here if wished"
 
 This program is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.
 
 This program is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details.
 
 You should have received a copy of the GNU General Public License
 along with this program.  If not, see <http://www.gnu.org/licenses/>.*/

#include <math.h>
#include <Servo.h>

const int debugLevel = 2; // 0 = No debug output 1 = main debug output 2 = low level movement debug output 3 = regulator output

const int numberOfServos=2;
const int servoSidewaysMovementLimit=85;
Servo servo[numberOfServos];
int servoTargetAngle[numberOfServos];
int servoOffset[numberOfServos];
int servoTargetTime[numberOfServos];
int servoNextUpdateTime[numberOfServos];

const int servoSpeed = 0.15; // 60 degrees in 0.4 seconds (400 milliseconds)
const float sectionLength = 10; // Length of snake section in centimeters

void setup() {
  Serial.begin(9600);
  debugln("Setup started",1);
  //debug("Where in!", 2);
  // Setup Servos
  servo[0].attach(2);
  servo[1].attach(3);
  //debug("Done attaching servos",2);
  pinMode(31, INPUT);
  debugln("Finish setup",1);
}

void loop() {
  debugln("----------- Program started ----------------",1);
  while (digitalRead(31) != HIGH) {
    delay(10);
  }
  debugln("Button pressed starting routine",1);
  moveServoTo(0,90,0); // Reset servos to zero
  moveServoTo(1,90,0);
  snakeDelay(3000);
  debugln("Finished reseting servos",1);
  debugln("Starting circle",1);
  //snakeBend(1,1, 90,0,0);
  //snakeDelay(2000);
  //snakeBend(1 ,1, 0, 90, 0);
  //snakeDelay(2000);
  snakeDrawCircle(1,1,8,0,359,true,5000);
  snakeDelay(5000);
  debugln("Circle ended",1);
  /*debugln("Moving both to 180",1);
   moveServoTo(0,180,0);
   moveServoTo(1,180,0);
   snakeDelay(3000);
   debugln("Finished second movement",1);
   moveServoTo(0,0,0);
   moveServoTo(1,0,0);
   snakeDelay(3000);
   debugln("Made third movement",1);
   debugln("End of loop going round again",1);
   moveServoTo(0,90,0); // Reset servos to zero
   moveServoTo(1,90,0);
   snakeDelay(3000);*/
}

/*
 - 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, int angle, int time) {
  servoTargetAngle[servoNum] = angle;
  servoTargetTime[servoNum] = time + millis();
  servoNextUpdateTime[servoNum] = 0; // Set update time to 0 to force regulator to pay attention 
}

void moveSectionTo(byte sectionNum, int xAngle, int yAngle, int time) {
  moveServoTo((sectionNum*2)-2,xAngle,time);
  moveServoTo((sectionNum*2)-1,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) {
  debug("Snake bend ",2);
  debug(xAngle,2);
  debug(" x ",2);
  debug(yAngle,2);
  debugln(" y",2);
  int xAnglePerSection;
  int yAnglePerSection;
  if (lastSectionNum != firstSectionNum) {
    xAnglePerSection = xAngle/(lastSectionNum - lastSectionNum);
    yAnglePerSection = yAngle/(lastSectionNum - firstSectionNum);
  } 
  else {
    xAnglePerSection = xAngle;
    yAnglePerSection = yAngle;
  }

  for (int i=firstSectionNum; i<=lastSectionNum; i++) {
    moveSectionTo(i,xAnglePerSection,yAnglePerSection, time);
  } 
}

/*
 - startAngle/endAngle = right=0 working round anticlockwise
 
 */
void snakeDrawCircle(byte firstSectionNum, byte lastSectionNum, float arcRadius, int startAngle, int endAngle, boolean reverseDirection, int time) {
  float arcAngle;
  int endTime = millis() + time;
  if (lastSectionNum != firstSectionNum) {
    arcAngle = asin(arcRadius/(sectionLength*(lastSectionNum-firstSectionNum)));
  } 
  else {
    debug("Arc Radius ",2);
    debugln(arcRadius,2);
    debug("Section Length ",2);
    debugln(sectionLength,2);
    float radiusOverLength = (float)arcRadius/(float)sectionLength;
    debug("One over the other ",2);
    debugln(radiusOverLength,2);
    arcAngle = (180.0/3.14)*asin(radiusOverLength);
  }
  debug("Arc angle ",2);
  debugln(arcAngle,2);
  if (!reverseDirection) {
    for (int angle=startAngle; angle<endAngle; angle=angle+5) {
      debug("Angle ",2);
      debugln(angle,2);
      debug("Snake bend ",2);
      debug(cos(angle*(3.14/180.0))*arcAngle,2);
      debug(" x ",2);
      debug(sin(angle*(3.14/180.0))*arcAngle,2);
      debugln(" y",2);
      snakeBend(firstSectionNum, lastSectionNum, 90.0+cos(angle*(3.14/180.0))*arcAngle, 90.0+sin(angle*(3.14/180.0))*arcAngle, time/(endAngle-startAngle));
      debug("Delay ",2);
      debugln((endTime-millis())/(endAngle-angle),2);
      snakeDelay((endTime-millis())/(endAngle-angle));
    }
  } 
  else {
    for (int angle=endAngle; angle>startAngle; angle=angle-5) { // Start Angle and End Angle are reversed in this loop as it is moving in reverse
      debug("Angle ",2);
      debugln(angle,2);
      debug("Snake bend ",2);
      debug(cos(angle*(3.14/180.0))*arcAngle,2);
      debug(" x ",2);
      debug(sin(angle*(3.14/180.0))*arcAngle,2);
      debugln(" y",2);
      snakeBend(firstSectionNum, lastSectionNum, 90.0+cos(angle*(3.14/180.0))*arcAngle, 90.0+sin(angle*(3.14/180.0))*arcAngle, time/(endAngle-startAngle));
      debug("Delay ",2);
      debugln((endTime-millis())/(angle-startAngle),2);
      snakeDelay((endTime-millis())/(angle-startAngle));
    }
  }
}

/*
 - coilGraduation delay between sections moving means start at the top (in millis)
 - reverseDirection false = coil clockwise true = coil anti-clockwise
 */

void snakeCoil(byte firstSectionNum, byte lastSectionNum, float coilDiameter, float coilHeight, boolean reverseDirection, int time, int coilGraduation) {
  if (coilGraduation == 0) {
    if (firstSectionNum > lastSectionNum) {
      snakeArc(lastSectionNum, firstSectionNum, coilDiameter, coilHeight, time);
    } 
    else  {
      snakeArc(firstSectionNum, lastSectionNum, coilDiameter, coilHeight, time);
    }
  } 
  else {
    int timeToMoveSection = time - (abs(coilGraduation) * abs(firstSectionNum-lastSectionNum));
    if (firstSectionNum > lastSectionNum) {
      for (int section=lastSectionNum; section>=firstSectionNum; section--) {
        moveSectionTo(section, xAngle, int yAngle, int time)
        } 
      } 
    else {
      for (int section=firstSectionNum; section<=lastSectionNum; section++) {

      } 
    }
  }
}

// Regulator and Delay Stuff

// This MUST be used instead of delay() or the regulator will not work!
void snakeDelay(int time) {
  int endTime = (millis() + time);
  regulator();
  while (millis() < endTime) regulator();
}

void regulator() {
  for (byte servoNum=0; servoNum<=(numberOfServos-1); servoNum++) {
    debug("Regulating ",3);
    debugln(servoNum, 3);
    int remainingMovement = servoTargetAngle[servoNum] - servo[servoNum].read();
    if (remainingMovement != 0) {
      debug("Remaining movement ",3);
      debugln(remainingMovement,3);

      debug("Servo angle ",3);
      debug(servo[servoNum].read(),3);
      debug(" Servo target angle ",3);
      debugln(servoTargetAngle[servoNum],3);

      // The delay ammount per one degree of movement
      int delayAmount = ((servoTargetTime[servoNum] - millis()) - (abs(remainingMovement)/servoSpeed))/abs(remainingMovement);
      debug("Delay Ammount",3);
      debugln(delayAmount,3);

      int movement;

      if (delayAmount > 0) {
        // Next update time = time for one degree of movement + delay ammount per one degree of movement ((time left - min time needed)/movem
        // nts remaining)
        servoNextUpdateTime[servoNum] = (1/servoSpeed) + ((servoTargetTime[servoNum] - millis()) - (abs(remainingMovement)/servoSpeed))/abs(remainingMovement);
        if (remainingMovement < 0) {
          movement = servo[servoNum].read() - 1 + servoOffset[servoNum];
        }
        else {
          movement = servo[servoNum].read() + 1 + servoOffset[servoNum];
        }
      }
      else {
        movement = servoTargetAngle[servoNum];
      }

      if (movement > servoSidewaysMovementLimit) {
        servo[servoNum].write(servoSidewaysMovementLimit);
      } 
      else if (movement < servoSidewaysMovementLimit) {
        servo[servoNum].write(-servoSidewaysMovementLimit);
      } 
      else {
        servo[servoNum].write(movement);
      }

      servoNextUpdateTime[servoNum] = 
    }
  }   
}

// Debug Stuff

void debugln(String message, int messageDebugLevel) {
  if (messageDebugLevel <= debugLevel) {
    /*if (messageDebugLevel == 1) {
     Serial.println("  "+message);
     } 
     else if (messageDebugLevel == 2) {
     Serial.println("    "+message);
     } 
     else {*/
    Serial.println(message);
    //}
  }
}

/*void debugln(int message, int messageDebugLevel) {
 if (messageDebugLevel <= debugLevel) {
/*if (messageDebugLevel == 1) {
 Serial.println("  "+message);
 } 
 else if (messageDebugLevel == 2) {
 Serial.println("    "+message);
 } 
 else {
 Serial.println(message);
 //}
 }
 }*/

void debugln(float message, int messageDebugLevel) {
  if (messageDebugLevel <= debugLevel) {
    /*if (messageDebugLevel == 1) {
     Serial.println("  "+message);
     } 
     else if (messageDebugLevel == 2) {
     Serial.println("    "+message);
     } 
     else {*/
    Serial.println(message);
    //}
  }
}

void debug(String message, int messageDebugLevel) {
  if (messageDebugLevel <= debugLevel) {
    /*if (messageDebugLevel == 1) {
     Serial.print("  "+message);
     } 
     else if (messageDebugLevel == 2) {
     Serial.print("    "+message);
     } 
     else {*/
    Serial.print(message);
    //}
  }
}

/*void debug(int message, int messageDebugLevel) {
 if (messageDebugLevel <= debugLevel) {
/*if (messageDebugLevel == 1) {
 Serial.print("  "+message);
 } 
 else if (messageDebugLevel == 2) {
 Serial.print("    "+message);
 } 
 else {
 Serial.print(message);
 //}
 }
 }*/

void debug(float message, int messageDebugLevel) {
  if (messageDebugLevel <= debugLevel) {
    /*if (messageDebugLevel == 1) {
     Serial.print("  "+message);
     } 
     else if (messageDebugLevel == 2) {
     Serial.print("    "+message);
     } 
     else {*/
    Serial.print(message);
    //}
  }
}