/*
 * Project: QT
 * Description: A bunny that responds to its master
 * Author: Nicholas Stone
 * Date: 1/29/2017
 * Version: 3
 */
int veggiePresent = 0;
// H-bridge
// const int motor1Pin = D3;    // leg 1
// const int motor2Pin = D4;    // leg 2
// const int enablePin = D5;    // enable pin
// 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 = 92;
//Number of DC cycles per 180 degrees
const int bodyLoopCounter = 20;
// const int photoLightThres = 2000;
static const int bodyForwardSpeedArray[] = {
180,180,180,180,
160,
140,
120,
110,110,110,
120,120,
130,
140,140,140,140,140,140,140
};
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);
    //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(digitalRead(enablePin)==LOW) {
        digitalWrite(enablePin, HIGH);
      }
    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);
      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(0);
    delay(servoDelay);
    Serial.print("CCW Counter Value: ");
    Serial.println(i);
    i++;
    }
      if(i==2*bodyLoopCounter && veggiePresent==1){
          i = 0;
          headServoPos = 25;
          headServo.write(headServoPos);
          delay(servoDelay*10);
          headServoPos = 10;
          headServo.write(headServoPos);
        }
    }
  if(digitalRead(enablePin)==HIGH)
  {
    digitalWrite(enablePin, LOW);
    bodyServo.write(bodyServoStatic);
  }
}
//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;
        }