bool DEBUG = 1; // 1 = Serial Monitor debug prints; 0 = NO debug prints
int16_t x = 0, y = 0; // counter
bool movefwd;

#include <Servo.h> //Servo motor library - for lookLeft and lookRight
Servo servo_motor; // Create a Servo "look" object to control
Servo steer_motor; // Create a Servo "steer" object to control
#define servoPin 11
#define steerPin 10
#define HOME 90 // servo home

#define trig_pin A1  //analog input 1
#define echo_pin A2  //analog input 2
#define maximum_distance 200
boolean goesForward = false;
int distance = 30;
#include <NewPing.h>  //Ultrasonic sensor function library. You must install this library
NewPing sonar(trig_pin, echo_pin, maximum_distance);  // Create a NewPing object to control

// DC Motor:    = L298N requires:
// LeftForward  = IN1 HIGH, IN2 GND
// LeftReverse  = IN1 GND , IN2 HIGH
// RightForward = IN3 HIGH, IN4 GND
// RightReverse = IN3 GND, IN4 HIGH

// LEFT DC motor with L298N H-bridge control pins
// const int LeftMotorEnable = 8; // ENA or PWM of LEFT motor
const int LeftMotorForward = 7;
const int LeftMotorBackward = 6;

// RIGHT DC motor with L298N H-bridge control pins
// const int LeftMotorEnable = 3; // ENA or PWM of RIGHT motor
const int RightMotorForward = 4;
const int RightMotorBackward = 5;

void setup() {
  Serial.begin(115200);
  randomSeed(analogRead(0)); // random seed to create random "distance" measurements

  // pinMode(LeftMotorEnable, OUTPUT); // pin 8 -
  pinMode(LeftMotorForward, OUTPUT); // pin 7
  pinMode(LeftMotorBackward, OUTPUT); // pin 6
  pinMode(RightMotorBackward, OUTPUT); // pin 5
  pinMode(RightMotorForward, OUTPUT); // pin 4
  // pinMode(RightMotorEnable, OUTPUT); // pin 3 -

  servo_motor.attach(servoPin);  // servo pin
  steer_motor.attach(steerPin);  // steer pin
  servo_motor.write(HOME); // go to HOME
  steer_motor.write(HOME); // go to HOME
}

void loop() {
  int distanceRight = 0;
  int distanceLeft = 0;

  distance = readPing();

  // delay(50);

  if (distance <= 20) {

    if (DEBUG)
      Serial.print("\nObject:");
    if (DEBUG)
      if (distance < 10)
        Serial.print(" ");
    if (DEBUG)
      Serial.print(distance);
    if (DEBUG)
      Serial.print("cm ");

    moveStop();
    delay(400);
    moveBackward();
    delay(400);
    moveStop();
    delay(400);

    distanceLeft = lookLeft();
    if (DEBUG)
    {
      distanceLeft  = random(30);
      Serial.print("Object:");
      if (distanceLeft < 10) Serial.print(" ");
      Serial.print(distanceLeft);
      Serial.print("cm");
    }

    distanceRight = lookRight();
    if (DEBUG)
    {
      distanceRight = random(30);
      Serial.print("Object:");
      if (distanceRight < 10) Serial.print(" ");
      Serial.print(distanceRight);
      Serial.print("cm");
    }

    if (distanceRight >= distanceLeft) {
      turnRight();
      moveStop();
    } else {
      turnLeft();
      moveStop();
    }
  } else {
    moveForward();
  }
}

int lookRight() {
  if (DEBUG)
    Serial.print(" lookRIGHT ");
  servo_motor.write(170); // look right
  delay(500); // wait for device to catch up
  distance = readPing(); // distance is global
  servo_motor.write(HOME); // home
  return distance;
}

int lookLeft() {
  if (DEBUG)
    Serial.print(" lookLEFT ");
  servo_motor.write(10); // look left
  delay(500); // wait for device to catch up
  distance = readPing();
  servo_motor.write(HOME); // homne
  return distance;
}

int readPing() {
  int cm = sonar.ping_cm();
  if (cm == 0) {
    cm = 250; // ?? will this not make the robot roll forward?
  }
  return cm;
}

void moveStop() {
  movefwd = 0; //
  if (DEBUG)
    Serial.print("moveSTOP");
  steer_motor.write(HOME); // steer left
  digitalWrite(RightMotorForward, LOW);
  digitalWrite(LeftMotorForward, LOW);
  digitalWrite(RightMotorBackward, LOW);
  digitalWrite(LeftMotorBackward, LOW);
  delay(500);
}

void moveForward() {
  if (DEBUG)
  {
    if (!movefwd)
    {
      movefwd = 1;
      Serial.print("\nmoveFORWARD ");
    }

    // these counters are for a cleaner serial monitor
    x++;
    if (x > 10)
    {
      Serial.print("> ");
      x = 0;
      y++;
      if (y == 40)
      {
        y = 0;
        movefwd = 0;
      }
    }
  }
  steer_motor.write(HOME); // steer forward
  // delay(500);
  digitalWrite(LeftMotorForward, HIGH);
  digitalWrite(RightMotorForward, HIGH);
  digitalWrite(LeftMotorBackward, LOW);
  digitalWrite(RightMotorBackward, LOW);
  // do not put a delay here to make it reactive
}

void moveBackward() {
  if (DEBUG)
    Serial.print(" moveBACK  ");
  digitalWrite(LeftMotorBackward, HIGH);
  digitalWrite(RightMotorBackward, HIGH);
  digitalWrite(LeftMotorForward, LOW);
  digitalWrite(RightMotorForward, LOW);
  delay(500);
}

void turnRight() {
  if (DEBUG)
    Serial.print(" turnRIGHT ");
  steer_motor.write(170); // steer right
  // delay(500);
  digitalWrite(LeftMotorForward, HIGH);
  digitalWrite(RightMotorBackward, HIGH);
  digitalWrite(LeftMotorBackward, LOW);
  digitalWrite(RightMotorForward, LOW);
  delay(500);
}

void turnLeft() {
  if (DEBUG)
    Serial.print(" turnLEFT  ");
  steer_motor.write(10); // steer left
  // delay(500);
  digitalWrite(LeftMotorBackward, HIGH);
  digitalWrite(RightMotorForward, HIGH);
  digitalWrite(LeftMotorForward, LOW);
  digitalWrite(RightMotorBackward, LOW);
  delay(500);
}