#include "simpletools.h"    // Include simple tools (pause, pwm_set, ect...)
#include "ping.h"                      

int main()        // Main function
{
  pause(250);
  //Turns off the backwards(direction) channels of motor controller 
  set_outputs(5, 2, 0b0000);  
  set_directions(5, 2, 0b1111);
  pwm_start(1000); 
  int counter = 0;
  
  while(1)
  {
    int leftSensor = ping_cm(7);
    int rightSensor = ping_cm(6);
    
    if (leftSensor > 30) {
      if (counter == 0) {    //if the first turn has NOT been made yet, turn for 475 millisceonds
        pwm_set(4, 0, 620);
        pwm_set(3, 1, 180);  
        pause(475);
        pwm_set(4, 0, 600);   //small straight amount after the turn
        pwm_set(3, 1, 600);
        pause(130);
        counter ++;
      }
      else if (counter == 1) { //if the first turn HAS been made, turn for 500 milliseconds
        pwm_set(4, 0, 600);
        pwm_set(3, 1, 180);
        pause(500);
        pwm_set(4, 0, 600);  //small straight amount after the turn
        pwm_set(3, 1, 600);
        pause(100);
        pwm_set(4, 0, 600);  //Then, make the second 90 degree turn of the 180 degree sequence
        pwm_set(3, 1, 180);
        pause(500);
        pwm_set(4, 0, 600);  //small straight amount after the turn
        pwm_set(3, 1, 600);
        pause(75);
        counter = counter + 2;
      }
      else if (counter == 7) {  //if the 7th turn HAS been made, turn for 500 milliseconds
        pwm_set(4, 0, 600);  
        pwm_set(3, 1, 180);
        pause(500);
        pwm_set(4, 0, 600);  //small straight amount after the turn
        pwm_set(3, 1, 600);
        pause(50);
        pwm_set(4, 0, 600);  //Then, make the second 90 degree turn of the 180 degree sequence
        pwm_set(3, 1, 180);
        pause(515);
        pwm_set(4, 0, 600);  //small straight amount after the turn
        pwm_set(3, 1, 600);
        pause(150);
        counter = counter + 2;
      }
      else if (counter == 9) {  //if the 9th turn HAS been made, turn for 450 milliseconds 
        pwm_set(4, 0, 600);
        pwm_set(3, 1, 180);
        pause(500);
        pwm_set(4, 0, 600);  //small straight amount after the turn
        pwm_set(3, 1, 600);
        pause(100);
        pwm_set(4, 0, 600);  //Then, make the second 90 degree turn of the 180 degree sequence
        pwm_set(3, 1, 180);
        pause(500);
        pwm_set(4, 0, 600);  //small straight amount after the turn
        pwm_set(3, 1, 600);
        pause(50);
        counter = counter + 2; 
      }                              
    }
    if (rightSensor > 30) {pause(1);
      if (rightSensor > 30) {
        if (counter == 13) {  //if the 13th turn HAS been made, then turn for 505 milliseconds
          pwm_set(4, 0, 150);
          pwm_set(3, 1, 600);  
          pause(495);
          pwm_set(4, 0, 600);  //small straight amount after the turn
          pwm_set(3, 1, 600);
          pause(120);
          counter ++;
        }
        else if (counter == 3) {  //if the 3rd turn HAS been made, turn for 500 milliseconds
          pwm_set(4, 0, 180);
          pwm_set(3, 1, 600);
          pause(500);
          pwm_set(4, 0, 600);  //small straight amount after the turn
          pwm_set(3, 1, 600);
          pause(75);
          pwm_set(4, 0, 180);  //Then, make the second 90 degree turn of the 180 degree sequence
          pwm_set(3, 1, 600);
          pause(495);
          pwm_set(4, 0, 600);  //small straight amount after the turn
          pwm_set(3, 1, 600);
          pause(223); //160-170 works
          counter = counter + 2;
        }
        else if (counter == 5) {  //if the 5th turn HAS been made, turn for 500 milliseconds
          pwm_set(4, 0, 180);
          pwm_set(3, 1, 600);
          pause(485);
          pwm_set(4, 0, 600);  //small straight amount after the turn
          pwm_set(3, 1, 600);
          pause(100);
          pwm_set(4, 0, 180);  //Then, make the second 90 degree turn of the 180 degree sequence
          pwm_set(3, 1, 600);
          pause(485);
          pwm_set(4, 0, 600);  //small straight amount after the turn
          pwm_set(3, 1, 600);
          pause(100);
          counter = counter + 2;
        }
        else if (counter == 11) {  //if the 11th turn HAS been made, turn for 500 milliseconds
          pwm_set(4, 0, 180);
          pwm_set(3, 1, 600);
          pause(485);
          pwm_set(4, 0, 600);  //small straight amount after the turn
          pwm_set(3, 1, 600);
          pause(50);
          pwm_set(4, 0, 180);  //Then, make the second 90 degree turn of the 180 degree sequence
          pwm_set(3, 1, 600);
          pause(480);
          pwm_set(4, 0, 600);  //small straight amount after the turn
          pwm_set(3, 1, 600);
          pause(500); 
          counter = counter + 2;
        }    
      }
    }       
    //Straight Corrections Sequence   
    else if (leftSensor == 14 || rightSensor == 14) {  //First, check if the robot is in the center of the maze
      pwm_set(4, 0, 600);
      pwm_set(3, 1, 600);
      pause(20); 
    }                                   
    else if (leftSensor > 14 || rightSensor < 14){  //If robot is closer to right wall, then drift to the right slightly         "||" means "or"
      pwm_set(4, 0, 600);
      pwm_set(3, 1, 534);
      pause(20);
    }
    else if (rightSensor > 14 || leftSensor < 14) {  //If robot is closer to left wall, then drift to the left slightly         "||" means "or"
     pwm_set(4, 0, 534);
      pwm_set(3, 1, 600);
      pause(20);
    }                 
  }
}