/** This example uses the Zumo's line sensors to detect the black
border around a sumo ring.  When the border is detected, it
backs up and turns. */

const int c = 261;
const int d = 294;
const int e = 329;
const int f = 349;
const int g = 391;
const int gS = 415;
const int a = 440;
const int aS = 455;
const int b = 466;
const int cH = 523;
const int cSH = 554;
const int dH = 587;
const int dSH = 622;
const int eH = 659;
const int fH = 698;
const int fSH = 740;
const int gH = 784;
const int gSH = 830;
const int aH = 880;


#include <Wire.h>
#include <Zumo32U4.h>

// This might need to be tuned for different lighting conditions,
// surfaces, etc.
#define QTR_THRESHOLD     700  // microseconds

// These might need to be tuned for different motor types.
#define REVERSE_SPEED     200  // 0 is stopped, 400 is full speed
#define TURN_SPEED        200
#define FORWARD_SPEED     200
#define REVERSE_DURATION  200  // ms
#define TURN_DURATION     300  // ms

int left, right, centerL, centerR;

Zumo32U4LCD lcd;
Zumo32U4ButtonB buttonB;
Zumo32U4Buzzer buzzer;
Zumo32U4Motors motors;
Zumo32U4LineSensors lineSensors;
Zumo32U4ProximitySensors proxSensors;

const char sound_effect[] PROGMEM = "O4 T100 V15 L4 MS g12>c12>e12>G6>E12 ML>G2";

#define NUM_SENSORS 3
unsigned int lineSensorValues[NUM_SENSORS];
void firstSection();
void waitForButtonAndCountDown();


void setup()
{
  Serial.begin(9600);
  lineSensors.initThreeSensors();
  proxSensors.initThreeSensors();
  waitForButtonAndCountDown();
}

void loop()
{
  if (buttonB.isPressed())
  {
    // If button is pressed, stop and wait for another press to
    // go again.
    motors.setSpeeds(0, 0);
    buttonB.waitForRelease();
    waitForButtonAndCountDown();
  }

  /* 6. Read line sensor values */
  lineSensors.read(lineSensorValues);
  proxSensors.read();
  left=proxSensors.countsLeftWithLeftLeds();
  right=proxSensors.countsRightWithRightLeds();
  centerL=proxSensors.countsFrontWithLeftLeds();
  centerR=proxSensors.countsFrontWithRightLeds();

  //Serial.println("left");
  //Serial.println(left);
  //Serial.println("right");
  //Serial.println(right);

  //Serial.println(centerL);
  //Serial.println(centerR);
  //int sensorValue = lineSensorValues[0];


  //Serial.println(sensorValue);




  if (lineSensorValues[0] < QTR_THRESHOLD) //flip aligator sign if you want to detect white edges on a black board instead of black border on a white board.
  {
    // If leftmost sensor detects line, reverse and turn to the
    // right.
    motors.setSpeeds(-REVERSE_SPEED, -REVERSE_SPEED);
    delay(REVERSE_DURATION);

    /* 7. Add three code lines for setting turn speed to turn right (+/- TURN_SPEED),
     *  add turn duration delay and then continuing forward.
     */
     motors.setSpeeds(TURN_SPEED , -TURN_SPEED );
     delay(TURN_DURATION);


  }
  else if (lineSensorValues[NUM_SENSORS - 1] < QTR_THRESHOLD) //flip aligator sign if you want to detect white edges on a black board instead of black border on a white board.
  {
    /* 8.  If rightmost sensor detects line, reverse and turn to the
     left. */
     motors.setSpeeds(-REVERSE_SPEED, -REVERSE_SPEED);
    delay(REVERSE_DURATION);

    /* 7. Add three code lines for setting turn speed to turn right (+/- TURN_SPEED),
     *  add turn duration delay and then continuing forward.
     */
     motors.setSpeeds(-TURN_SPEED , TURN_SPEED );
     delay(TURN_DURATION);

  }
  else if(left>2){
    motors.setSpeeds(-TURN_SPEED , TURN_SPEED );
     delay(TURN_DURATION);

    motors.setSpeeds(FORWARD_SPEED , FORWARD_SPEED );
    delay(100);
  }
  else if(right>2){
    motors.setSpeeds(TURN_SPEED , -TURN_SPEED );
     delay(TURN_DURATION);


    motors.setSpeeds(FORWARD_SPEED , FORWARD_SPEED );
    delay(100);

  }
  else if(centerL>2){
    motors.setSpeeds(FORWARD_SPEED , FORWARD_SPEED );
    delay(100);
  }
  else if(centerR>2){
    motors.setSpeeds(FORWARD_SPEED , FORWARD_SPEED );
    delay(100);
  }
  else
  {
    /* 9. Otherwise, go straight.*/
    motors.setSpeeds(FORWARD_SPEED , FORWARD_SPEED );

  }

}

void waitForButtonAndCountDown()
{
  /* 1. See setup() */
  /* 2. Turn on yellow LED, clear LCD and add "Press B" text to LCD */
  ledYellow(1);
  lcd.print(F("Press B"));


  /* 3. Wait for button B to be pressed */
  buttonB.waitForButton();


  /* 4. Turn off Yellow LED and clear LCD */
  ledYellow(0);

  // Play audible countdown.
  delay(100);
  buzzer.playNote(NOTE_A(4), 500, 15);
  delay(500);
  buzzer.playNote(NOTE_A(4), 500, 15);
  delay(500);
  buzzer.playNote(NOTE_A(4), 500, 15);
  delay(500);
  buzzer.playNote(NOTE_F(4), 350, 15);
  delay(500);
  buzzer.playNote(NOTE_C(4), 150, 15);

}

void firstSection()
{
  buzzer.playFrequency(a, 3000, 15);
  delay(100);
  buzzer.playFrequency(f, 3000, 15);
  delay(100);
  buzzer.playFrequency(a, 3000, 15);
  delay(100);
  buzzer.playFrequency(f, 210, 15);
  buzzer.playFrequency(cH, 90, 15);
  buzzer.playFrequency(a, 300, 15);
  buzzer.playFrequency(f, 210, 15);
  buzzer.playFrequency(cH, 90, 15);
  buzzer.playFrequency(a, 390, 15);
  //3650

  delay(3000);

  buzzer.playFrequency(eH, 300, 15);
  buzzer.playFrequency(eH, 300, 15);
  buzzer.playFrequency(eH, 300, 15);
  buzzer.playFrequency(fH, 210, 15);
  buzzer.playFrequency(cH, 90, 15);
  buzzer.playFrequency(gS, 300, 15);
  buzzer.playFrequency(f, 210, 15);
  buzzer.playFrequency(cH, 90, 15);
  buzzer.playFrequency(a, 390, 15);

  delay(320);
}