int trigPin1 = 11;    // Trigger
int echoPin1 = 12;    // Echo
int trigPin2 = 3;    // Trigger
int echoPin2 = 8;    // Echo
long duration, cm;
long beepTimer;
int shortBeepTime = 30;
int beepPause = 120;
bool beeperIsBeepin;
bool ArduinoInStandBy;
bool firstBoot = true;
int CurrentSpeed = 255;
int LeftRight;
int theDistance;
float changeDirectionDistance = 40;
bool isTurning = false;
float turningTime = 230;
long turningTimer;
float pauseBeforeStateChange = 100;



void setup() {
  //Serial Port begin
  Serial.begin (9600);
  //Inputs and outputs for Distance Meter
  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);

  //Motor Pins
  pinMode(13, OUTPUT);
  pinMode(4, OUTPUT);
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(7, OUTPUT);

  //PWM
  //LEFT WHEEL
  pinMode(9, OUTPUT);
  //RIGHT WHEEL
  pinMode(10, OUTPUT);

  //beeper Pin
  pinMode(2, OUTPUT);

}

void setAllToLOW()  //Set all motor pins to LOW just in case
{
  digitalWrite(4, LOW);
  digitalWrite(5, LOW);
  digitalWrite(6, LOW);
  digitalWrite(7, LOW);
  digitalWrite(13, LOW);
  CurrentSpeed = 255;
}

void shortBeep()
{
  if (beeperIsBeepin == false)
    {
      beeperIsBeepin = true;
      beepTimer = millis();
      digitalWrite(2, HIGH);
    }

  if ((millis() >= beepTimer + shortBeepTime) && (beeperIsBeepin = true) )
    {
      digitalWrite(2, LOW);
    }
  if ((millis() >= beepTimer + shortBeepTime + beepPause) && (beeperIsBeepin = true))
    {
          beeperIsBeepin = false;
    }

}

float getDistance1() //right ultraconic distance sensor
{
  digitalWrite(trigPin1, LOW);
  delayMicroseconds(5);
  digitalWrite(trigPin1, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin1, LOW);

  pinMode(echoPin1, INPUT);
  duration = pulseIn(echoPin1, HIGH);

  cm = (duration/2) / 29.1;     // Divide by 29.1 or multiply by 0.0343
  delay(5);
  return cm;
}

float getDistance2() //left ultraconic distance sensor
{
  digitalWrite(trigPin2, LOW);
  delayMicroseconds(5);
  digitalWrite(trigPin2, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin2, LOW);

  pinMode(echoPin2, INPUT);
  duration = pulseIn(echoPin2, HIGH);

  cm = (duration/2) / 29.1;     // Divide by 29.1 or multiply by 0.0343
  delay(5);
  return cm;
}

float getDistance() // take the lowest mesured distance and run with it
{
  float Distance1;
  float Distance2;
  float Distance3;
  float Distance4;
  float firstWinner;
  float secondWinner;
  float finalWinner;
  float AllMeasurments[] = {Distance1, Distance2, Distance3, Distance4};

  Distance1 = getDistance1();
  Distance2 = getDistance2();
  Distance3 = getDistance1();
  Distance4 = getDistance2();

  AllMeasurments[0] = Distance1;
  AllMeasurments[1] = Distance2;
  AllMeasurments[2] = Distance3;
  AllMeasurments[3] = Distance4;

  firstWinner = min(Distance1, Distance2);
  secondWinner = min(Distance3, Distance4);
  finalWinner = min(firstWinner, secondWinner);


  Serial.println(finalWinner);
  beepPause = map(finalWinner, 30, 300, 1, 2000); //the closer to obstacle the faster it beeps
  return finalWinner; //temp


}


void leftWhellBack()
{
  digitalWrite(4, HIGH);
  digitalWrite(5, LOW);
}

void leftWhellFrw()
{
  digitalWrite(4, LOW);
  digitalWrite(5, HIGH);
}

void rightWhellFrw()
{
  digitalWrite(6, HIGH);
  digitalWrite(7, LOW);
}

void rightWhellBack()
{
  digitalWrite(6, LOW);
  digitalWrite(7, HIGH);
}

void SoftStart(int desiredSpeed)
{
  analogWrite(9, CurrentSpeed-2);
  analogWrite(10, CurrentSpeed);

  if (CurrentSpeed == 255)
    {
      CurrentSpeed = 70;
    }
  if (CurrentSpeed < desiredSpeed)
    {
      ++CurrentSpeed;
    }
  else if (CurrentSpeed > desiredSpeed)
    {
      --CurrentSpeed;
    }


}

void DriveForward()
{
  int forwardSpeed;

  forwardSpeed = map(theDistance, 30, 120, 120, 255); //the closer to obstacle the slower it goes
  SoftStart(forwardSpeed);
  rightWhellFrw();
  leftWhellFrw();
}

void DriveBack()
{
  SoftStart(150);
  rightWhellBack();
  leftWhellBack();
}

void TurnLeft()
{
  SoftStart(125);
  rightWhellFrw();
  leftWhellBack();
}

void TurnRight()
{
  SoftStart(125);
  rightWhellBack();
  leftWhellFrw();
}

void FirtBoot()
{
  if (firstBoot == true)
    {
      firstBoot = false;
      delay(50);
      digitalWrite(2, HIGH);
      delay(200);
      digitalWrite(2, LOW);
      delay(2000);
      digitalWrite(2, HIGH);
      delay(50);
      digitalWrite(2, LOW);
      delay(50);
      digitalWrite(2, HIGH);
      delay(50);
      digitalWrite(2, LOW);
      delay(50);
      digitalWrite(2, HIGH);
      delay(50);
      digitalWrite(2, LOW);
      delay(50);
      digitalWrite(2, HIGH);
      delay(50);
      digitalWrite(2, LOW);
      delay(50);
      digitalWrite(2, HIGH);
      delay(50);
      digitalWrite(2, LOW);
      delay(50);
      digitalWrite(2, HIGH);
      delay(50);
      digitalWrite(2, LOW);
      delay(2000);
      setAllToLOW();
    }

}


//Main_logic

void loop()
{

  FirtBoot();
  theDistance = getDistance();
  shortBeep();
  if ((theDistance <= changeDirectionDistance) && (isTurning == false))
    {
      setAllToLOW();
      LeftRight = random(1, 3);
      turningTimer = millis();
      isTurning = true;
    }

  if (isTurning == false)
    {
      DriveForward();
    }
  if (isTurning == true)
    {
      if (LeftRight == 1)
        {
          TurnLeft();
        }
       else
        {
          TurnRight();
        }
      if (millis() >= turningTimer + turningTime)
        {
          setAllToLOW();
          isTurning = false;
        }
    }



}