/*  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);*/
   
   // Snake Servo Test - Move each servo one by one through its full movement
   for (int servoNum=0; servoNum<numberOfServos; numberOfServos++) {
     moveServoTo(servoNum, 180, 0);
     snakeDelay(2000);
     moveServoTo(servoNum, 0, 0);
     snakeDelay(2000);
     moveServoTo(servoNum, 90, 0);
     snakeDelay(2000);
   }  
}

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

/*
 - This will begin at the first section and end at the last, this means that it the snake wants
   to begin coiling at the 7th section and end at the 2nd firstSectionNum=7 lastSectionNum=2
 - 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 { // TODO This is NOT finished
    int timeToMoveSection = time - (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++) {

      } 
    }
  }
}

void snakeStraighten(byte firstSectionNum, byte lastSectionNum, int time, int graduation) {
  int timeToMoveSection = time - (abs(graduation) * abs(firstSectionNum-lastSectionNum));
  
}

// 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);
    if ((servoNextUpdateTime[servoNum] <= millis()) && (servoNextUpdateTime[servoNum] != -1)) {
      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) + delayAmount;
          if (remainingMovement < 0) {
            movement = servo[servoNum].read() - 1 + servoOffset[servoNum];
          }
          else {
            movement = servo[servoNum].read() + 1 + servoOffset[servoNum];
          }
        }
        else {
          movement = servoTargetAngle[servoNum];
          servoNextUpdateTime[servoNum] = -1 // basicaly dont update until told otherwise (by the servoMoveTo method setting the value to 0)
        }

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

// 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);
    //}
  }
}