// Serial output constants
#define FORWARD             ">F<"   // Robot platform moving forward
#define RIGHT               ">R<"   // Platform rotates RIGHT in-place ~90 deg
#define LEFT                ">L<"   // Platform rotates LEFT in-place ~90 deg
#define SCAN_FRONT_CLEAR    ">SF0<"
#define SCAN_FRONT_OBSTACLE ">SF1<"
#define SCAN_RIGHT_CLEAR    ">SR0<"
#define SCAN_RIGHT_OBSTACLE ">SR1<"
#define SCAN_LEFT_CLEAR     ">SL0<"
#define SCAN_LEFT_OBSTACLE  ">SL1<"

#define TRIG_PIN 13
#define ECHO_PIN 12

#include <Servo.h>

const int left_motor_fwd = 7;
const int right_motor_fwd = 4;
const int left_motor_back = 8;
const int right_motor_back = 3;
const int enA = 6;
const int enB = 5;

long distance, duration;
int time;
bool collision = false;
bool keep_turning = false;
int motor_speed = 150;

Servo servo;
int servo_pos = 0;

int turning_direction = 0;
bool right_clear = true;
bool front_clear = true;
bool left_clear = true;


bool checkCollision()
{
  //Signala izsutisana no Trig pin
  digitalWrite(TRIG_PIN, LOW); // Pirms impulsa sutisanas, parliecinamies, ka Trig pin ir izslegts
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  //Atbildes impulsa salasisana no Sensora
  duration = pulseIn(ECHO_PIN, HIGH);

  //Attaluma noteiksana
  //Izmantojam skanas atrumu 343 metri sekunde
  distance = (duration / 2) * 0.0343;

  return distance < 35;
}


void motorsOff()
{
  digitalWrite(right_motor_fwd, LOW);
  digitalWrite(left_motor_fwd, LOW);
  digitalWrite(right_motor_back, LOW);
  digitalWrite(left_motor_back, LOW);
}


void forward() {
  Serial.println(FORWARD);
  digitalWrite(left_motor_back, HIGH);
  digitalWrite(right_motor_back, HIGH);
}


int turn(int direction, bool keep_turning)
{
  motorsOff();
  if (direction == 0) {
    Serial.println(LEFT);
    digitalWrite(left_motor_back, HIGH);
    digitalWrite(right_motor_fwd, HIGH);
    if (keep_turning == false) {
      direction = 180;
    }
  } else if (direction == 180) {
    Serial.println(RIGHT);
    digitalWrite(right_motor_back, HIGH);
    digitalWrite(left_motor_fwd, HIGH);
    if (keep_turning == false) {
      direction = 0;
    }
  }

  delay(360);
  motorsOff();
  delay(100);
  return direction;
}


void scanSurroundings()
{
  motorsOff();

  // left collision check
  servo.write(0);
  delay(1000);

  if (checkCollision() == false)
  {
    left_clear = true;
    Serial.println(SCAN_LEFT_CLEAR);
  }
  else
  {
    left_clear = false;
    Serial.println(SCAN_LEFT_OBSTACLE);
  }

  // front collision check
  servo.write(90);
  delay(1000);

  if (checkCollision() == false)
  {
    front_clear = true;
    Serial.println(SCAN_FRONT_CLEAR);
  }
  else
  {
    front_clear = false;
    Serial.println(SCAN_FRONT_OBSTACLE);
  }

  // right collision check
  servo.write(180);
  delay(1000);
  if (checkCollision() == false)
  {
    right_clear = true;
    Serial.println(SCAN_RIGHT_CLEAR);
  }
  else
  {
    right_clear = false;
    Serial.println(SCAN_RIGHT_OBSTACLE);
  }

  servo.write(90);

}

void collisionHandling()
{
  motorsOff();
  servo.write(90);
  delay(1000);
  keep_turning = true;
  turning_direction = turn(turning_direction, keep_turning);
  servo.write(turning_direction);
  delay(1000);
  motorsOff();
  delay(200);
}


void setup()
{
  pinMode(LED_BUILTIN, OUTPUT);
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  pinMode(left_motor_fwd, OUTPUT);
  pinMode(right_motor_fwd, OUTPUT);
  pinMode(left_motor_back, OUTPUT);
  pinMode(right_motor_back, OUTPUT);
  pinMode(enA, OUTPUT);
  pinMode(enB, OUTPUT);

  Serial.begin(9600);
  servo.attach(10);
  analogWrite(enA, motor_speed);
  analogWrite(enB, motor_speed + 5);
}

void loop()
{
  scanSurroundings();

  if (front_clear == false)
  {
    collisionHandling();
  }
  else
  {
    forward();
    delay(800);
    if (keep_turning == true)
    {
      motorsOff();
      delay(200);
      keep_turning = false;
      turning_direction = turn(turning_direction, keep_turning);
    }
  }

  motorsOff();
}