/*
 * Starter code for a sound-following robot that uses
 * two microphones to drive toward a sound source.
 * 
 */

// declare constant variables for motor control pins
const int leftMotorForwardPin = 4;
const int leftMotorBackwardPin = 5;
const int rightMotorForwardPin = 2;
const int rightMotorBackwardPin = 3;
const int leftMotorSpeedPin = 9;
const int rightMotorSpeedPin = 10;

// constant variables for microphone pins
const int mic1pin = A0;
const int mic2pin = A1;

// other variables
const int sample_window = 3000; // amount of time to sample sound in milliseconds
int mic1;      // microphone 1 value from analogRead (0-1023)
int mic2;      // microphone 2 value from analogRead (0-1023)
int mic1_max;  // maximum value recorded by mic1 during sample window
int mic1_min;  // minimum value recorded by mic1 during sample window
int mic2_max;  // maximum value recorded by mic2 during sample window
int mic2_min;  // minimum value recorded by mic2 during sample window
int amp1;      // largest amplitude of mic1 during sample window (max-min)
int amp2;      // largest amplitude of mic2 during sample window (max-min)
int difference;  // difference between the mic amplitudes
unsigned long start_time;  // time in milliseconds at start of sample window (since program started)
unsigned long current_time; // current time in milliseconds (since program started)

void setup() { // setup code that only runs once
  // set motor control pins as outputs
  pinMode(leftMotorForwardPin,OUTPUT);
  pinMode(leftMotorBackwardPin,OUTPUT);
  pinMode(rightMotorForwardPin,OUTPUT);
  pinMode(rightMotorBackwardPin,OUTPUT);
  pinMode(leftMotorSpeedPin,OUTPUT);
  pinMode(rightMotorSpeedPin,OUTPUT);
  
  // set the H-bridge enable pins for speed control HIGH
  // so motors will always run at full speed. You can
  // replace these with analogWrite to make the motors
  // run at different speeds. You can also put analogWrite
  // commands in the various steering functions to make
  // the speed variable.
  digitalWrite(leftMotorSpeedPin,HIGH);
  digitalWrite(rightMotorSpeedPin,HIGH);

  Serial.begin(9600);  // initialize serial communication for debugging
}

void loop() { // code that loops forever

  // recommended: stop driving so motor noise does not 
  // affect sound sample
  stopDriving;
  delay(500);
  
  // sample the microphones for sample_window milliseconds
  // and calculate the maximum amplitude for each mic.
  // you could also put this part of the code inside
  // a function.
  
  // reset max and min values
  mic1_min = 1023;
  mic1_max = 0;
  mic2_min = 1023;
  mic2_max = 0;

  start_time = millis();    // start time for this sample window
  current_time = millis();
  Serial.println("Beginning sample");
  while( millis() - start_time < sample_window){
    // read mic values
    mic1 = analogRead(mic1pin);
    mic2 = analogRead(mic2pin);

    // assign new min and max values
    mic1_min = min(mic1, mic1_min);
    mic1_max = max(mic1, mic1_max);
    mic2_min = min(mic2, mic2_min);
    mic2_max = max(mic2, mic2_max);

    current_time = millis(); // update current time
  }

  // done sampling - calculate max amplitude for each mic
  amp1 = mic1_max - mic1_min;
  amp2 = mic2_max - mic2_min;
  difference = amp1-amp2;

  // Print amplitudes to serial monitor

  Serial.print("Mic 1 amplitude: ");
  Serial.print(amp1);
  Serial.print(" | Mic 2 amplitude: ");
  Serial.print(amp2);
  Serial.print(" | Difference = ");
  Serial.print(difference);

  /*
   *  YOUR ALGORITHM HERE!
   *  What should the robot do based on the microphone
   *  amplitudes and the difference between them?
   *  You can make use of the pre-defined functions below
   *  to control the robot, or write your own.
   */
}

void driveForward(){  // set pins to make robot drive forward
  digitalWrite(leftMotorForwardPin,HIGH);
  digitalWrite(leftMotorBackwardPin,LOW);
  digitalWrite(rightMotorForwardPin,HIGH);
  digitalWrite(rightMotorBackwardPin,LOW);
}

void driveBackward(){  // set pins to make robot drive backward 
  digitalWrite(leftMotorForwardPin,LOW);
  digitalWrite(leftMotorBackwardPin,HIGH);
  digitalWrite(rightMotorForwardPin,LOW);
  digitalWrite(rightMotorBackwardPin,HIGH);
}

void turnRight(){  // set pins to make robot turn right
  digitalWrite(leftMotorForwardPin,HIGH);
  digitalWrite(leftMotorBackwardPin,LOW);
  digitalWrite(rightMotorForwardPin,LOW);
  digitalWrite(rightMotorBackwardPin,HIGH);
}

void turnLeft(){  // set pins to make robot turn left
  digitalWrite(leftMotorForwardPin,LOW);
  digitalWrite(leftMotorBackwardPin,HIGH);
  digitalWrite(rightMotorForwardPin,HIGH);
  digitalWrite(rightMotorBackwardPin,LOW);
}

void stopDriving(){  // set pins to make robot stop
  digitalWrite(leftMotorForwardPin,LOW);
  digitalWrite(leftMotorBackwardPin,LOW);
  digitalWrite(rightMotorForwardPin,LOW);
  digitalWrite(rightMotorBackwardPin,LOW);
}