/*##### Motor Shield (L298N) #####*/
/*
* Arduino Pin --> L298N
* 5 --> ENA
* 6 --> ENB
* 2 --> IN1
* 3 --> IN2
* 4 --> IN3
* 7 --> IN4
*/
const int ENA = 5;
const int ENB = 6; 
/*
* IN1: HIGH; IN2: LOW --> Direction 1 
* IN1: LOW; IN2: HIGH --> Direction 2
* IN3: HIGH; IN4: LOW --> Direction 1
* IN3: LOW; IN4: HIGH --> Direction2
*/
const int IN1 = 11; 
const int IN2 = 10;
const int IN3 = 9;
const int IN4 = 8;

const int leftTrigger=0;
const int leftEcho=1;

const int forTrigger =2;
const int forEcho=3;

const int rightTopTrigger = 4;
const int rightTopEcho =5;

const int rightBottomTrigger = 7;
const int rightBottomEcho = 13;


#include <NewPing.h>
 
#define TRIGGER_PIN 13
#define ECHO_PIN 12
#define MAX_DISTANCE 200

NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);

void setup()
{
 pinMode(ENA, OUTPUT);
 pinMode(ENB, OUTPUT);
 pinMode(IN1, OUTPUT);
 pinMode(IN2, OUTPUT);
 pinMode(IN3, OUTPUT);
 pinMode(IN4, OUTPUT);
 pinMode(leftTrigger, OUTPUT);
 pinMode(leftEcho, INPUT);
 pinMode(forTrigger, OUTPUT);
 pinMode(forEcho, INPUT);
 pinMode(rightTopTrigger, OUTPUT);
 pinMode(rightTopEcho, INPUT);
  pinMode(rightBottomTrigger, OUTPUT);
 pinMode(rightBottomEcho, INPUT);
 // Enable Motor A, Motor B: Constant Speed
 digitalWrite(ENA, HIGH);
 digitalWrite(ENB, HIGH);
 // Serial communication
 Serial.begin(9600);
}
 
void loop()
{
  
  getForDistance();  //**************************************************************************************************loop TODO- getFunction that gets what sonar to run
}

 long microsecondsToCentimeters(long microseconds)
{
  return microseconds / 29 / 2;
}

void MotorAB_Direction1(int milliseconds)
{
  analogWrite(IN1,200);
   analogWrite(IN2, 100);
   analogWrite(IN3, 200);
   analogWrite(IN4,100);
 if (milliseconds > 0)
 delay(milliseconds);
}
 
void MotorAB_Direction2(int milliseconds)
{
  analogWrite(IN1,100);
   analogWrite(IN2, 200);
   analogWrite(IN3, 100);
   analogWrite(IN4,200);
 if(milliseconds > 0)
 delay(milliseconds);
}
 
void MotorAB_Brake(int milliseconds)
{
  analogWrite(IN1,100);
   analogWrite(IN2, 100);
   analogWrite(IN3, 100);
   analogWrite(IN4,100);
 if(milliseconds > 0)
 delay(milliseconds); 
}

void turnLeft(int milliSeconds)
{
   analogWrite(IN1,200);
   analogWrite(IN2, 100);
   analogWrite(IN3, 100);
   analogWrite(IN4,200);
   if(milliSeconds > 0)
     delay(milliSeconds);   
}

void turnRight(int milliSeconds)
{
   analogWrite(IN1, 100);
   analogWrite(IN2, 200);
   analogWrite(IN3, 200);
   analogWrite(IN4, 100);
   if(milliSeconds > 0)
     delay(milliSeconds);   
}

int getForDistance()
 {
  delay(50);
   unsigned int uS = sonar.ping_cm();
   Serial.print(uS);
   Serial.println("cm");
 }

int getRightTopDistance() 
 {
  delay(50);
   unsigned int uS = sonar.ping_cm();
   Serial.print(uS);
   Serial.println("cm");
 }

 
int getRightBottomDistance() 
 {
  delay(50);
   unsigned int uS = sonar.ping_cm();
   Serial.print(uS);
   Serial.println("cm");
 }
 

void stickToRight()
{
  while((getRightTopDistance()<30)&&(getRightBottomDistance()<30))
  {
    if(getRightTopDistance()<30)
  {
    turnLeft(100);
  }
  if(getRightBottomDistance()<30)
  {
    turnRight(100);
  } 
  }
 
}