#include <hcsr04.h>

/* constans */
/* ******** */

/* Define your pins here */
/* Wheels */
#define left_wheel_1  0
#define left_wheel_2  1
#define right_wheel_1 2
#define right_wheel_2 3

/* Ultrasonic sensor */
#define sensor_echo    9
#define sensor_trigger 10

/* Other constans */
#define move_delay 500
#define turn_delay 500

/* disconnected pin for random seed */
#define analog_disconnected 0

/* Initialize sensor class */
HCSR04 hcsr04(sensor_trigger, sensor_echo, 20, 4000);

/* Array bellow contains all the wheels */
int wheels[] = {left_wheel_1, left_wheel_2, right_wheel_1, right_wheel_2};

void setup() {
  /* Setup pins */
  /* wheels */
  for (int i = 0; i < sizeof(wheels) - 1; i++) {
    pinMode(wheels[i], OUTPUT);
  }

  /* Starts serial communication */
  Serial.begin(9600);
  Serial.println("Bot is ready");
}

/* Wheel actions */
/* ************* */

/* Forward */
void move_forward_wheel_left() {
  digitalWrite(left_wheel_1, HIGH);
  digitalWrite(left_wheel_2, LOW);
}

void move_forward_wheel_right() {
  digitalWrite(right_wheel_1, HIGH);
  digitalWrite(right_wheel_2, LOW);
}

void move_forward() {
  move_forward_wheel_left();
  move_forward_wheel_right();
  delay(move_delay);
}

/* Backward */
void move_backward_wheel_left() {
  digitalWrite(left_wheel_1, LOW);
  digitalWrite(left_wheel_2, HIGH);
}

void move_backward_wheel_right() {
  digitalWrite(right_wheel_1, LOW);
  digitalWrite(right_wheel_2, HIGH);
}

void move_backward() {
  move_backward_wheel_left();
  move_backward_wheel_right();
  delay(move_delay);
}

/* Stops */
void stop_all() {
  for (int i = 0; sizeof(wheels) - 1; i++) {
    digitalWrite(wheels[i], LOW);
  }
}

void stop_left() {
  digitalWrite(left_wheel_1, LOW);
  digitalWrite(left_wheel_2, LOW);
}

void stop_right() {
  digitalWrite(right_wheel_1, LOW);
  digitalWrite(right_wheel_2, LOW);
}

/* Turns */
void turn_left(int delayMS) {
  stop_left();
  move_forward_wheel_right();
  delay(delayMS);
}

void turn_right(int delayMS) {
  stop_right();
  move_forward_wheel_left();
  delay(delayMS);
}

void turn_left_or_right(int delayMS) {
  randomSeed(analogRead(analog_disconnected));
  int rand_number = random(1, 99);
  if (rand_number > 50) {
    turn_right(delayMS);
  } else {
    turn_left(delayMS);
  }
}

/* Ultrasonic sensor actions */
/* ************************* */

/* get_distance -- returns distance to an object in centimeters */
int get_distance() {
  int mm = hcsr04.distanceInMillimeters();
  if (mm > 0) {
    return mm / 10;
  }
  return 0;
}

/* Main code */
/* ********* */

/* The idea:

   If robot see an object in distance less than 25cm,
   he must do the following actions:

     * stop the wheels
     * turn on wheels to go backward
     * stop wheels
     * turn left or right
     * then go forward
*/
void loop() {
  if (get_distance() > 25) {
    move_forward();
  } else {
    stop_all();
    move_backward();
    turn_left_or_right(turn_delay);
  }
}