/*
 * Demo line-following code for the Pololu Zumo Robot
 *
 * This code will follow a black line on a white background, using a
 * PID-based algorithm.  It works decently on courses with smooth, 6"
 * radius curves and has been tested with Zumos using 30:1 HP and
 * 75:1 HP motors.  Modifications might be required for it to work
 * well on different courses or with different motors.
 *
 * https://www.pololu.com/catalog/product/2506
 * https://www.pololu.com
 * https://forum.pololu.com
 */

//Inlcuding the nessesary librarys and defining classes
#include <Zumo32U4.h>
#include <Wire.h>
#include <ZumoShield.h>

Zumo32U4LineSensors lineSensor;
Zumo32U4LCD lcd;
Zumo32U4Motors motors;
Zumo32U4ButtonA buttonA;

ZumoBuzzer buzzer;
ZumoReflectanceSensorArray reflectanceSensors;
ZumoMotors motors;
Pushbutton button(ZUMO_BUTTON);

int lastError = 0;

//Arrays
unsigned int lineSensorValues[5];

//Defining global constants
const int motor_speeds = 200;

//Millis() varibles
unsigned long time_now;
const int terskel = 2000;


// This is the maximum speed the motors will be allowed to turn.
// (400 lets the motors go at top speed; decrease to impose a speed limit)
const int MAX_SPEED = 400;

void lcd_startup(){
  lcd.clear();
  lcd.gotoXY(0,0);
  lcd.print("A to");
  lcd.gotoXY(0,1);
  lcd.print("calibr8");
}


void setup()
{

// Play a little welcome song
  buzzer.play(">g32>>c32");

//calibration start
  lineSensor.initFiveSensors();
  lcd_startup();
  buttonA.waitForButton();
  calibrate_IR();

// Initialize the reflectance sensors module
  reflectanceSensors.init();

  // Play music and wait for it to finish before we start driving.
  buzzer.play("L16 cdegreg4");
  while(buzzer.isPlaying());
}

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

  // Get the position of the line.  Note that we *must* provide the "sensors"
  // argument to readLine() here, even though we are not interested in the
  // individual sensor readings
  int position = reflectanceSensors.readLine(sensors);

  // Our "error" is how far we are away from the center of the line, which
  // corresponds to position 2500.
  int error = position - 2500;

  // Get motor speed difference using proportional and derivative PID terms
  // (the integral term is generally not very useful for line following).
  // Here we are using a proportional constant of 1/4 and a derivative
  // constant of 6, which should work decently for many Zumo motor choices.
  // You probably want to use trial and error to tune these constants for
  // your particular Zumo and line course.
  int speedDifference = error / 4 + 6 * (error - lastError);

  lastError = error;

  // Get individual motor speeds.  The sign of speedDifference
  // determines if the robot turns left or right.
  int m1Speed = MAX_SPEED + speedDifference;
  int m2Speed = MAX_SPEED - speedDifference;

  // Here we constrain our motor speeds to be between 0 and MAX_SPEED.
  // Generally speaking, one motor will always be turning at MAX_SPEED
  // and the other will be at MAX_SPEED-|speedDifference| if that is positive,
  // else it will be stationary.  For some applications, you might want to
  // allow the motor speed to go negative so that it can spin in reverse.
  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);
}



void calibrate_IR(){                                                                                      //Function that makes car spin and calibrates the IR sensors
  delay(1000);                                                                                            //Delay, so that the car doesn't begin calibrating right away
  motors.setSpeeds(motor_speeds, -motor_speeds);                                                          //Makes car spin
  time_now = millis();                                                                                    //Takes timestamp

  for(int i = 0; i < 180; i++){
    if(i > 30 && i < 120){
      motors.setSpeeds(motor_speeds,-motor_speeds);
      lineSensor.calibrate();
    }
    else{
      motors.setSpeeds(-motor_speeds,motor_speeds);
      lineSensor.calibrate();
    }
  }
  motors.setSpeeds(0,0);
  buzzer.play(">g32>>c32");
}

void lcd_startup(){
  lcd.clear();                                                                                            //Clears lcd screen
  lcd.gotoXY(0,0);                                                                                        //Sets cursor to 0,0
  lcd.print("A to");                                                                                      //Writes text
  lcd.gotoXY(0,1);                                                                                        //Sets crisor to 1,0
  lcd.print("calibr8");                                                                                   //Prints text
}