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

const uint16_t maxSpeed = 400;   //Max speed is 400.

Zumo32U4ButtonA buttonA;
Zumo32U4LineSensors lineSensors;
Zumo32U4Motors motors;
Zumo32U4LCD LCD;
#define NUM_SENSORS 5    //We will be using all five sensors, not three.
unsigned int lineSensorValues[NUM_SENSORS];


void calibrateSensors() {
  delay(1000);
  for(uint16_t i = 0; i < 120; i++)   //the car rotates to the left and right in order to cross the
  {         //black tape and calibrate the sensor values. The car stops
    if (i > 30 && i <= 90)      //when the calibration is finished.
    {
      motors.setSpeeds(-200, 200);
    }
    else
    {
      motors.setSpeeds(200, -200);
    }

    lineSensors.calibrate();
  }
  motors.setSpeeds(0, 0);
}


void setup() {
  buttonA.waitForButton();
  lineSensors.initFiveSensors();
  calibrateSensors();
}

void lcdWrite(String line1, String line2){
  LCD.clear();
  LCD.gotoXY(0, 0);
  LCD.print(line1);
  LCD.gotoXY(0, 1);
  LCD.print(line2);
}


void LineFollow_noPID() {
  uint16_t position = lineSensors.readLine(lineSensorValues,true);  //Determines where the tape is.
  lcdWrite("",String(position));

  while(1950 < position && position < 2050){
    motors.setSpeeds(100,100);
    position = lineSensors.readLine(lineSensorValues);
  }

  while(2050 <= position && position <= 4000){
    motors.setSpeeds(100,50);
    position = lineSensors.readLine(lineSensorValues);
  }

  while(0 <= position && position <= 1950){
    motors.setSpeeds(50,100);
    position = lineSensors.readLine(lineSensorValues);
  }
}

void loop() {
  LineFollow_noPID();
}