#include "pitches.h"
//Pins
int redLEDPin = 10;
int grnLEDPin = 9;
int speakerPin = 3;
int analogInPin = A0; //analog input pin that the potentiometer is attached to
int sonarPin = A1;
//----
//sensor values
int targetDistance = 0; //potentiometer
int sensorValue = 0; //sonar
int outputValueRed = 0; //Red LED
int outputValueGrn = 0; //Green LED
int distance = 0;

boolean play = false; //to play or not to play the melody

void setup(){
  analogReference(DEFAULT);
  // initialize serial communications at 9600 bps:
  Serial.begin(9600);
  //playMelody();
}

void loop(){
  delay(50);
  play = false;
  // read the analog in value:
  //The three is to make the potentiometer less erratic
  targetDistance = analogRead(analogInPin) / 3;

  //conversion to inch based on experiment and docs
  sensorValue = analogRead(sonarPin)*6/11;

  //Serial.print("ActualDistance: ");
  //Serial.println(sensorValue);
  //Serial.print("SpecifiedDistance: ");
  //Serial.println(targetDistance);

  if(abs(targetDistance - sensorValue) <= 5){
    outputValueGrn = 0;
    outputValueRed = 0;
    distance = 0;
    play = true;

  }else if(targetDistance - sensorValue < -5){//If too far
    //Serial.println("Too Far");
    outputValueGrn = 0;
    distance = sensorValue - targetDistance;
    //Serial.print("Difference: ");
    //Serial.println(distance);
    if(distance <=40){
      outputValueRed = map(distance, 5, 40, 0, 200);
    }else{
      outputValueRed = 255;
    }
  }else{ //if too close
    //Serial.println("Too Close");
    outputValueRed = 0;
    distance = targetDistance - sensorValue;
    //Serial.print("Difference: ");
    //Serial.println(distance);
    if(distance <=40){
      outputValueGrn = map(distance, 5, 40, 0, 200);
    }else{
      outputValueGrn = 255;
    }
  }
  //outputValue = map(0, 0, 1023, 0, 255);
  analogWrite(redLEDPin, outputValueRed);
  analogWrite(grnLEDPin, outputValueGrn);
  //Serial.println("");
  //Serial.print("red: ");
  Serial.println(outputValueRed);
  //Serial.print("grn: ");
  Serial.println(outputValueGrn);
  if(play){
    playTone();
  }
  Serial.println("");
}

//the method to play the tone
void playTone(){
  tone(speakerPin, NOTE_C4 ,1000/4);
  delay(1000/4*1.30);
  noTone(speakerPin);

}