#include <DRV8833.h>
#include <Wire.h>

DRV8833 driver = DRV8833();
// Create an instance of the DRV8833:
//const int inputA1 = 3, inputA2 = 6;
const int motorSpeed = 128;
// spectrum shield band arrays

const int LEDPIN=13; // light up when audio detected
const int PIN_STROBE=4; // spectrum shield
const int PIN_RESET=5; // spectrum shield
const int PIN_MOTOR_L=3; //PWM to motor open mouth
const int PIN_MOTOR_R=6; //PWM to motor close mouth
const int PIN_MOTOR_SLEEP=7; //sleep fxn on motor board
const int PIN_MOTOR_STALL=8; //stall warning on motor board

//analog pins
const int PIN_LEFT=0; // L analog from spectrum shield
const int PIN_RIGHT=1; // R analog from spectrum shield
const int PIN_MOTOR_POT=3; //analog potentiometer on motor


int left[7];
int right[7];
int minRotation = 400;  //approximate reading when mouth closed
int maxRotation =  600;  //approximate reading when mouth open
int targetL = 721;//measure the position
int targetH = 166;
int motor1Pin_O;
int motor1Pin_C;
int currentSpeed;
int y=0;

void setup() {
 Wire.begin();
                                            //Wire.onReceive(receiveEvent);
// driver.attachMotorA(inputA1, inputA2);

  pinMode(LEDPIN, OUTPUT); // LED
  //initialize spectrum board related pins
  pinMode(PIN_RESET, OUTPUT); // reset
  pinMode(PIN_STROBE, OUTPUT); // strobe
  digitalWrite(PIN_RESET,LOW); // reset low
  digitalWrite(PIN_STROBE,HIGH); //pin 5 is RESET on the shield

  Serial.begin(9600);
  Serial.println("Ready!");

}


void loop() {

   if(Wire.available()){
    Wire.beginTransmission(3);
    Wire.write(readMSGEQ7());
    Wire.endTransmission(3);
   }

}

int readMSGEQ7(){
  //reset the data
  digitalWrite(PIN_RESET, HIGH);
  digitalWrite(PIN_RESET, LOW);

  //loop thru all 7 bands
        int sumRight = 0;
        int sumLeft = 0;

  for(int band=0; band < 7; band++) {
       digitalWrite(PIN_STROBE,LOW); // go to the next band
       delayMicroseconds(20); //gather some data
       left[band] = analogRead(PIN_LEFT); // store left band reading

       //right[band] = left[band]; //use this only for MONO!
             right[band] = analogRead(PIN_RIGHT); // store right band reading

       digitalWrite(PIN_STROBE,HIGH); // reset the strobe pin
             sumRight = sumRight+right[band]; // get the sum from all bands
             sumLeft = sumLeft+left[band]; // get the sum from all bands

             if(left[band] > 100 || right[band] > 100) {
               digitalWrite(LEDPIN,HIGH); // sound detected
             }
             else {
               digitalWrite(LEDPIN,LOW); // reset low
             }
          }

               //Serial.println(sumLeft);

             if(sumLeft > 2000)  {sumLeft = 2000;}
             x = map(sumLeft, 0, 2000, 0, 300);
             //Serial.println(x);
             //targetH = target_mouthPosition + posError;
             //targetL = target_mouthPosition - posError;

             return x;
}