/*
 * Project: QT
 * Description: A bunny that responds to its master
 * Author: Nicholas Stone
 * Date: 1/29/2017
 * Version: 4
 */
int veggiePresent = 0;
//LED
const int ledOnePin = D2;
const int ledTwoPin = D3;
int brightness = 0;
int fadeAmount = 20;
// H-bridge
// const int motor1Pin = D3;    // leg 1
// const int motor2Pin = D4;    // leg 2
//const int enablePin = D5;    // enable pin
int motionStatus = 0;
// Servo Motor for the head
int servoDelay = 100;
Servo headServo;
const int headServoPin = D0;
int headServoPos = 10; // avoid the 0/180 poles
// Servo body for driving the body
Servo bodyServo;
const int bodyServoPin = D1;
//value of continuous servo that causes it to remain static
int bodyServoStatic = 93;
//Number of DC cycles per 180 degrees
const int bodyLoopCounter = 50;
int i = 0;
static const int bodyForwardSpeedArray[] = {
180,180,180,180,180,180,180,180,180,180,180,172,164,157,149,141,134,126,118,110,103,95,95,95,95,95,
95,95,96,96,97,98,103,107,116,130,130,128,125,121,113,102,95,95,95,45,45,45,135,135,135
};
static const int bodyBackwardSpeedArray[] = {
0,0,0,0,0,0,0,0,0,0,0,8,16,23,31,39,46,54,62,70,77,85,85,85,85,85,85,85,84,84,83,82,77,73,64,50,50,
52,55,59,67,78,85,85,85,135,135,135,45,45,45
};
void setup() {
    Serial.begin(9600);
    // Thermo Subscription
    Particle.subscribe("diotQTcarrot", foodStatus);
    //H-bridge
    // pinMode(motor1Pin, OUTPUT);
    // pinMode(motor2Pin, OUTPUT);
    //pinMode(enablePin, OUTPUT);
    // allow motor to turn on if needed:
    // digitalWrite(enablePin, HIGH);
    //LED
    pinMode(ledOnePin,OUTPUT);
    pinMode(ledTwoPin,OUTPUT);
    //set Servos initial angle
    headServo.attach(headServoPin);
    headServo.write(10);
    bodyServo.attach(bodyServoPin);
  }
void loop() {
    //int i = 0;
    //loop when veggie present
    while(veggiePresent==1) {
      //change code state of motor
      if(motionStatus==0) {
        motionStatus = 1;
      }
    while(i<bodyLoopCounter && veggiePresent==1) {
      /*if(i==0){
        digitalWrite(motor1Pin, LOW);   // set leg 1 of the H-bridge low
        digitalWrite(motor2Pin, HIGH);  // set leg 2 of the H-bridge high
      }*/
      bodyServo.write(bodyForwardSpeedArray[i]);
      delay(servoDelay);
      Serial.print("CW Counter Value: ");
      Serial.println(i);
      glowLed();
      i++;
    }
    while(i<2*bodyLoopCounter && veggiePresent == 1) {
      /*if(i==bodyLoopCounter){
        digitalWrite(motor1Pin, HIGH);  // set leg 1 of the H-bridge high
        digitalWrite(motor2Pin, LOW);   // set leg 2 of the H-bridge low
        //bodyServo.write(0);
      }*/
      // turn head around
      if(i>=bodyLoopCounter*1.5 && i>=(bodyLoopCounter*1.5)+1) {
        headServoPos = 170;
        headServo.write(headServoPos);
      }
    bodyServo.write(bodyBackwardSpeedArray[i-bodyLoopCounter]);
    delay(servoDelay);
    Serial.print("CCW Counter Value: ");
    Serial.println(i);
    glowLed();
    i++;
    }
      if(i==2*bodyLoopCounter && veggiePresent==1){
          //end of loop
          i = 0;
          headServoPos = 25;
          headServo.write(headServoPos);
          delay(servoDelay*10);
          headServoPos = 10;
          headServo.write(headServoPos);
        }
    }
  if(motionStatus == 1)
  {
    motionStatus == 0;
    bodyServo.write(bodyServoStatic);
    //turn off LEDs
    brightness = 0;
    analogWrite(ledOnePin, brightness);
    analogWrite(ledTwoPin, brightness);
  }
}
//trigger presence of veggie from event publish by other photon
void foodStatus(const char *event,const char *data) {
  if(strcmp(data, "foundVegetable")){
    Serial.println("foundVegetable");
    veggiePresent = 1;
    return;
  }
  if(strcmp(data, "removeVegetable")) {
      Serial.println("removeVegetable");
      veggiePresent = 0;
      return;
      }
      return;
        }
void glowLed() {
  analogWrite(ledOnePin, brightness);
  analogWrite(ledTwoPin, brightness);
  brightness = brightness + fadeAmount;
  if (brightness <= 0 || brightness >= 255) {
    fadeAmount = -fadeAmount;
    }
}