/*
 * Project: QT
 * Description: A bunny that responds to its master
 * Author: Nicholas Stone
 * Date: 1/29/2017
 * Version: 2
 */
// H-bridge
const int motor1Pin = D3;    // leg 1
const int motor2Pin = D4;    // leg 2
const int enablePin = D5;    // enable pin
// Photoresistor
const int photoPin = A0;
int photoSampleRate = 100;
int light = 0;
// Servo Motor for the head
Servo headServo;
const int headServoPin = D0;
int headServoPos = 10; // avoid the 0/180 poles
//Number of DC cycles per 180 degrees
const int bodyLoopCounter = 10;
const int photoLightThres = 2000;
void setup() {
    Serial.begin(9600);
    //H-bridge
    pinMode(motor1Pin, OUTPUT);
    pinMode(motor2Pin, OUTPUT);
    pinMode(enablePin, OUTPUT);
    //set headServo initial angle
    headServo.attach(headServoPin);
    headServo.write(10);
    // allow motor to turn on if needed:
    digitalWrite(enablePin, HIGH);
  }
void loop() {
    int i = 0;
    light = analogRead(photoPin);
    //loop when no light, ie veggie present
    while(light<photoLightThres) {
      //change code state of motor
      if(digitalRead(enablePin)==LOW)) {
        digitalWrite(enablePin, HIGH);
        //dcMotorStatus = 1;
      }
    while(i<bodyLoopCounter && light<photoLightThres) {
      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
      }
      i++;
      light = analogRead(photoPin);
      delay(photoSampleRate);
      Serial.print("CW Counter Value: ");
      Serial.println(i);
    }
    while(i<2*bodyLoopCounter && light<photoLightThres) {
      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
      }
      // turn head aroudn
      if(i>=bodyLoopCounter*1.5 && i>=(bodyLoopCounter*1.5)+1) {
        headServoPos = 170;
        headServo.write(headServoPos);
      }
    i++;
    light = analogRead(photoPin);
    delay(photoSampleRate);
    Serial.print("CCW Counter Value: ");
    Serial.println(i);
  }
      if(i==2*bodyLoopCounter && light<photoLightThres){
          i = 0;
          headServoPos = 25;
          headServo.write(headServoPos);
          delay(1000);
          headServoPos = 10;
          headServo.write(headServoPos);
        }
}
if(digitalRead(enablePin)==HIGH)
{
  digitalWrite(enablePin, LOW);
}
Serial.print("Light Value: ");
Serial.println(analogRead(photoPin));
delay(photoSampleRate);
  }