#include <QTRSensors.h>
#include <ZumoReflectanceSensorArray.h>
#include <ZumoMotors.h>
#include <ZumoBuzzer.h>
#include <Pushbutton.h>

ZumoBuzzer buzzer;
ZumoReflectanceSensorArray reflectanceSensors;
ZumoMotors motors;
Pushbutton button(ZUMO_BUTTON);
int lastError = 0;

const int MAX_SPEED = 380;  //maximum motor speed
const int buttonPin = 27; //Flinduino boot button

int buttonPushCounter = 0; // counter for button presses
int buttonState = 0; // button state detector
int lastButtonState = 0; // previous state of button


void setup() {
  // put your setup code here, to run once:
  pinMode(buttonPin, INPUT); //int for bootbutton imput
  Serial.begin(9600);

  buzzer.play(">g32>>c32");
  reflectanceSensors.init();
  button.waitForButton();

  pinMode(13, OUTPUT);
  digitalWrite(13, HIGH);
  delay(1000);
  int i;
  for(i = 0; i < 80; i++)
  {
    if ((i > 10 && i <= 30) || (i > 50 && i <= 70))
      motors.setSpeeds(-200, 200);
    else
      motors.setSpeeds(200, -200);
    reflectanceSensors.calibrate();

    // Since our counter runs to 80, the total delay will be
    // 80*20 = 1600 ms.
    delay(20);
  }
  motors.setSpeeds(0,0);
  digitalWrite(13, LOW);
  buzzer.play(">g32>>c32");
  delay(5500);
  buttonState = digitalRead(buttonPin);

  if (buttonState != latButtonState) {
    if (buttonState == HIGH) {
      buttonPushCounter++;
      Serial.println("Button Pushed");
      Serial.println("number of button pushes: ");
      Serial.println(buttonPushCounter);
    } else {
      Serial.println("off");
    }
    delay(5)
  }
  lastButtonState = buttonState;

 if (buttonPushCounter % 4 == 0) {
  motors.setLeftSpeed(200);
 } else {
  motors.setRightSpeed(200);}
  delay (250);
  motors.setSpeed(0,0);
  delay (1000);


}

void loop() {
   unsigned int sensors[6];

  int position = reflectanceSensors.readLine(sensors);

  int error = position - 2500;

  int speedDifference = error / 3 + 5 * (error - lastError);

  lastError = error;

  int m1Speed = MAX_SPEED + speedDifference;
  int m2Speed = MAX_SPEED - speedDifference;


  if (m1Speed < 0)
    m1Speed = 0;
  if (m2Speed < 0)
    m2Speed = 0;
  if (m1Speed > MAX_SPEED)
    m1Speed = MAX_SPEED;
  if (m2Speed > MAX_SPEED)
    m2Speed = MAX_SPEED;

  motors.setSpeeds(m1Speed, m2Speed);

}