/*  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=1;
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 int sectionLength = 10; // Length of snake section in centimeters

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

void loop() {
  debug("----------- Program started ----------------",1);
  //boolean state = digitalRead(button);
  //while (state != HIGH) {
  //  debug("Putton not pressed delaying",0);
  //  delay(10);
  //}
  debug("Button pressed starting routine",1);
  moveServoTo(0,0,0); // Reset servo to zero
  debug("Made first movement",1);
  snakeDelay(2000);
  debug("Finished first movement",1);
  moveServoTo(0,180,2000);
  debug("Made second movement",1);
  snakeDelay(2000);
  debug("Finished second movement",1);
  moveServoTo(0,0,2000);
  debug("Made third movement",1);
  snakeDelay(2000);
  debug("End of loop going round again",1);
}

/*
 - 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)-1,xAngle,time);
  moveServoTo((sectionNum*2),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);
  } 
}

/*
 - startAngle/endAngle = right=0 working round anticlockwise
 
 */
void snakeDrawCircle(byte firstSectionNum, byte lastSectionNum, int arcRadius, int startAngle, int endAngle, boolean reverseDirection, int time) {
  int arcAngle = asin(arcRadius/(sectionLength*(lastSectionNum-firstSectionNum)));
  if (!reverseDirection) {
    for (int angle=startAngle; angle<endAngle; angle++) {
      snakeBend(firstSectionNum, lastSectionNum, cos(angle)*arcAngle, sin(angle)*arcAngle, time/(endAngle-startAngle));
    }
  } 
  else {
    for (int angle=startAngle; angle<endAngle; angle--) {
      snakeBend(firstSectionNum, lastSectionNum, cos(angle)*arcAngle, sin(angle)*arcAngle, time/(endAngle-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<=0; servoNum++) { // servoNum<=8 needs correcting
    debug("Regulating ",2);
    debugln(servoNum, 2);
    int remainingMovement = servoTargetAngle[servoNum] - servo[servoNum].read();
    if (remainingMovement != 0) {
      debug("Remaining movement ",2);
      debugln(remainingMovement,2);

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

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