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

const int numberOfServos=2;
Servo servo[numberOfServos];
int servoTargetAngle[numberOfServos];
int servoTargetTime[numberOfServos];
//int servoLastUpdateTime[numberOfServos]; Currently not used

const int servoSpeed = 0.15; // 60 degrees in 0.4 seconds (400 milliseconds)
const int minMovementAmmount = 1;
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) {
  //debug("Setting servo",0);
  servoTargetAngle[servoNum] = angle;
  servoTargetTime[servoNum] = time + millis();
}

void moveSectionTo(byte sectionNum, int xAngle, int yAngle, int time) {
  //debug("Setting section " + sectionNum + " to move to " + xAngle + " x and " + yAngle + " y in " + time " milliseconds");
  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));
    }
  }
}

// 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);
  while (millis() < endTime) regulator();
}

void regulator() {
  for (byte servoNum=0; servoNum<=(numberOfServos-1); servoNum++) { // servoNum<=8 needs correcting
    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);

      int delayAmount = (servoTargetTime[servoNum] - millis()) - (abs(servo[servoNum].read() - servoTargetAngle[servoNum])/ servoSpeed);
      debug("Delay Ammount",3);
      debugln(delayAmount,3);
      if (delayAmount > 0) {
        if (abs(remainingMovement) < minMovementAmmount) {
          debug("Remaining movement less than min ammount",3);
          delay(delayAmount);
          servo[servoNum].write(remainingMovement);
        } 
        else {
          debug("Remaining movement greater than min ammount delaying",3);
          debugln(delayAmount/(abs(remainingMovement)/minMovementAmmount),3);
          delay(delayAmount/(abs(remainingMovement)/minMovementAmmount));
          if (remainingMovement < 0) {
            servo[servoNum].write(servo[servoNum].read() - minMovementAmmount);
          } 
          else {
            servo[servoNum].write(servo[servoNum].read() + minMovementAmmount);
          }
        }
      } 
      else {
        servo[servoNum].write(servoTargetAngle[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);
    //}
  }
}