void setup() {                
  // initialize the digital pins.
  pinMode(11, OUTPUT);//LED1 / PB3
  pinMode(8, OUTPUT);//LED2 / PB0
  pinMode(12, OUTPUT);//LED3 / PB4
  pinMode(6, OUTPUT);//DIR1 / PD6
  //pinMode(7, OUTPUT);//DIR1A / PD7
  pinMode(2, OUTPUT);//DIR2 / PD2
  //pinMode(3, OUTPUT);//DIR2A / PD3
  pinMode(9, OUTPUT);//EN1 PB1
  pinMode(10, OUTPUT);//EN2 PB2
  pinMode(4, INPUT);//Button 1 / PD4
  digitalWrite(4, HIGH);//internal pull-up on PD4
  pinMode(5, INPUT);//Button 2 / PD5
  digitalWrite(5, HIGH);//internal pull-up on PD5
  
  #define led1ON digitalWrite(11, HIGH);
  #define led1OFF digitalWrite(11, LOW);
  #define led2ON digitalWrite(8, HIGH);
  #define led2OFF digitalWrite(8, LOW);
  #define led3ON digitalWrite(12, HIGH);
  #define led3OFF digitalWrite(12, LOW);
  #define led4ON digitalWrite(13, HIGH);
  #define led4OFF digitalWrite(13, LOW);

  
  #define button1 !digitalRead(4)//If pressed, returns TRUE
  #define button2 !digitalRead(5)
  
  //Line sensors
//  #define SEN_BACK !digitalRead(A5)//White returns TRUE, black returns FALSE
//  #define SEN_LEFT !digitalRead(A4)
//  #define SEN_RIGHT !digitalRead(A3)
  #define SEN_BACK analogRead(A5)<50//White returns TRUE, black returns FALSE
  #define SEN_LEFT analogRead(A4)<50
  #define SEN_RIGHT analogRead(A3)<50
  
  

  //Motor speeds 0-255
  #define MaxSpeed 255
  #define MediumSpeed 100
  #define LowSpeed 50
randomSeed(analogRead(A2));    
  //UART inicialize for debugging
  //Serial.begin(19200);
}

void Forward() {analogWrite(9,MaxSpeed);analogWrite(10,MaxSpeed);digitalWrite(6, HIGH);digitalWrite(2, LOW);}
void Backward() {analogWrite(9,MaxSpeed);analogWrite(10,MaxSpeed);digitalWrite(6, LOW);digitalWrite(2, HIGH);}
void Right() {analogWrite(9,MaxSpeed);analogWrite(10,MaxSpeed);digitalWrite(6, LOW);digitalWrite(2, LOW);}
void Left() {analogWrite(9,MaxSpeed);analogWrite(10,MaxSpeed);digitalWrite(6, HIGH);digitalWrite(2, HIGH);}
void ForwardSLOW() {analogWrite(9,MediumSpeed);analogWrite(10,MediumSpeed);digitalWrite(6, HIGH);digitalWrite(2, LOW);}
void BackwardSLOW() {analogWrite(9,MediumSpeed);analogWrite(10,MediumSpeed);digitalWrite(6, LOW);digitalWrite(2, HIGH);}
void RightSLOW() {analogWrite(9,MediumSpeed);analogWrite(10,MediumSpeed);digitalWrite(6, LOW);digitalWrite(2, LOW);}
void LeftSLOW() {analogWrite(9,MediumSpeed);analogWrite(10,MediumSpeed);digitalWrite(6, HIGH);digitalWrite(2, HIGH);}
void RightSLOWradius() {analogWrite(9,LowSpeed);analogWrite(10,MediumSpeed);digitalWrite(6, HIGH);digitalWrite(2, LOW);}
void LeftSLOWradius() {analogWrite(9,MediumSpeed);analogWrite(10,LowSpeed);digitalWrite(6, HIGH);digitalWrite(2, LOW);}
void RightSLOWradiusBack() {analogWrite(9,LowSpeed);analogWrite(10,MediumSpeed);digitalWrite(6, LOW);digitalWrite(2, HIGH);}
void LeftSLOWradiusBack() {analogWrite(9,MediumSpeed);analogWrite(10,LowSpeed);digitalWrite(6, LOW);digitalWrite(2, HIGH);}
void RightradiusBack() {analogWrite(9,MediumSpeed);analogWrite(10,MaxSpeed);digitalWrite(6, LOW);digitalWrite(2, HIGH);}
void LeftradiusBack() {analogWrite(9,MaxSpeed);analogWrite(10,MediumSpeed);digitalWrite(6, LOW);digitalWrite(2, HIGH);}
void Rightradius() {analogWrite(9,MediumSpeed);analogWrite(10,MaxSpeed);digitalWrite(6, HIGH);digitalWrite(2, LOW);}
void Leftradius() {analogWrite(9,MaxSpeed);analogWrite(10,MediumSpeed);digitalWrite(6, HIGH);digitalWrite(2, LOW);}
void Stop() {analogWrite(9,0);analogWrite(10,0);}

int sensorValue,TestType=0;
long startTime,startTime2;                    
long elapsedTime,elapsedTime2;
int SHARPreadcount=0;
int Sens=0;
int State=0,OldState=0;
int SHARPon=1;

/*Suggestion:
Folowing variables could be used fot counting time, 
how much have elapsed from 1 moment to another:

long startTime ;                    
long elapsedTime ;

Function millis() returns time how long device 
is turned on in milliseconds. 1000 milisec = 1 sec.

for example:
User press, button 1, and you want to know after how much time
user will press button 2, then code will look something like this:

if(button1){//If user have pressed button 1
  startTime=millis();//Save time when user have pressed button 1
  while(!button2){}//While user havn`t pressed button 2, do nothing
  elapsedTime=millis()-startTime;//Calculate how much time has gone, since user pressed button 1
}
*/

void loop() {
  led1OFF; led2OFF; led3OFF;
  startTime=millis();  
while(button1){
  elapsedTime=millis()-startTime;
  if(elapsedTime>1000) {TestType=1;led1ON;}
  if(elapsedTime>2000) {TestType=2;led2ON;}
  if(elapsedTime>3000) {TestType=3;led3ON;}
}  
if(TestType)
{
  led1OFF;
  delay(500);
  led2OFF;
  delay(500);
  led3OFF;
  delay(500);
}
  switch (TestType) {
    case 1:
      while(1)
      {
        if(analogRead(A1)>100){led1ON;}else{led1OFF;}
     }
      break;
    case 2:
    while(1){
      if(SEN_BACK) {led3ON;} else {led3OFF;}
      if(SEN_LEFT) {led2ON;} else {led2OFF;}
      if(SEN_RIGHT) {led1ON;} else {led1OFF;}
    }
      break;
    case 3:
      led1ON;delay(300);led2ON;delay(300);led3ON;
      ForwardSLOW();delay(600);BackwardSLOW();delay(600);RightSLOW();delay(600);LeftSLOW();delay(600);Stop();delay(600);
      Forward();delay(600);Backward();delay(600);Right();delay(600);Left();delay(600);Stop();delay(600);
      RightSLOWradius();delay(600);RightSLOWradiusBack();delay(600);LeftSLOWradius();delay(600);LeftSLOWradiusBack();delay(600);Stop();
      Rightradius();delay(600);RightradiusBack();delay(600);Leftradius();delay(600);LeftradiusBack();delay(600);Stop();
      break;
  }
if (button2)
{
//Start sumo code
}

  
}