/*
 This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU Affero General Public License as
    published by the Free Software Foundation, either version 3 of the
    License, or (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU Affero General Public License for more details.

    You should have received a copy of the GNU Affero General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
// This program implements table top challenge level ###?### --
// The robot follows a flat vertical surface at a distance of a couple inches.

// Set *debug* to 1 to enable debugging statements:
int debug = 1;

// Set *stop_at_edge* to 1 to have robot stop at the edge;
// otherwise, it will back away from edge:
int stop_at_edge = 0;

// Set *table_top_threshold* to a value that captures the
// tabletop distance for *your* robot:
int table_top_threshold = 600;

// Set *sonar_timeout* to a value a 200-300 uSec longer
// that your table top threshold:

int sonar_timeout = 1600;

// Set *sonar_set_point* to a value that specifies the approximate
// follow distance:

int sonar_follow_distance = 800;

// Pin definitions:
int led = 13;
int left_motor_enable = 3;
int left_motor_a = 4;
int left_motor_b = 5;
int right_motor_enable = 10;
int right_motor_a = 9;
int right_motor_b = 8;
int right_sonar_trigger = 7;
int right_sonar_echo = 6;
int left_sonar_trigger = 12;
int left_sonar_echo = 11;

// the setup routine runs once when you press reset:
void setup() {
  // If debugging is enabled,
  if (debug) {
    // 115200 is the fastest "standard" baud rate for debugging.
    // Be sure to set the baud Tools=>Serial Monitor to 115200:
    Serial.begin(9600);
  }

  // Set LED output pin:
  pinMode(led, OUTPUT);

  // Set H-bridge control pins:
  pinMode(left_motor_enable, OUTPUT);
  pinMode(left_motor_a, OUTPUT);
  pinMode(left_motor_b, OUTPUT);
  pinMode(right_motor_enable, OUTPUT);
  pinMode(right_motor_a, OUTPUT);
  pinMode(right_motor_b, OUTPUT);

  // Set control pins for both left and right sonars:
  pinMode(left_sonar_trigger, OUTPUT);
  pinMode(left_sonar_echo, INPUT);
  pinMode(right_sonar_trigger, OUTPUT);
  pinMode(right_sonar_echo, INPUT);

  // Set all the pin values to predefined values:
  digitalWrite(led, HIGH);
  digitalWrite(left_motor_enable, LOW);
  digitalWrite(left_motor_a, LOW);
  digitalWrite(left_motor_b, LOW);
  digitalWrite(right_motor_enable, LOW);
  digitalWrite(right_motor_a, LOW);
  digitalWrite(right_motor_b, LOW);
  digitalWrite(left_sonar_trigger, LOW);
  digitalWrite(right_sonar_trigger, LOW);
  digitalWrite(left_sonar_trigger, LOW);
  digitalWrite(right_sonar_trigger, LOW);
}

void motors_run(int left, int right, int ms_delay) {
  // Set left motor direction:
  if (left > 0) {
    // Set left motor to go forward:
    digitalWrite(left_motor_a, HIGH);
    digitalWrite(left_motor_b, LOW);
  } else {
    // Set left motor to go backward:
    digitalWrite(left_motor_a, LOW);
    digitalWrite(left_motor_b, HIGH);
    left = -left;                   // Make left a positive value:
  }
  // Make sure we cap the speed at 255:
  if (left > 255) {
    left = 255;
  }
  analogWrite(left_motor_enable, left); // Start motor in right direction

  // Set right motor direction:
  if (right > 0) {
    // Set right motor to go forward:
    digitalWrite(right_motor_a, HIGH);
    digitalWrite(right_motor_b, LOW);
  } else {
    // Set right motor to go backward:
    digitalWrite(right_motor_a, LOW);
    digitalWrite(right_motor_b, HIGH);
    right = -right;                 // Make right a positive value:
  }
  // Make sure we cap the speed at 255:
  if (right > 255) {
    right = 255;
  }
  analogWrite(right_motor_enable, right); // Start motor in right direction

  delay(ms_delay);                  // Wait the specified amount of time
}

// This routine will trigger a HC-SR04 sonar that has its
// trigger pin attached to *sonar_trigger* and its echo
// pin attached to *sonar_echo*.  If the echo response
// takes longer than *timeout* microseconds, *timeout* is
// returned.
int ping(int sonar_trigger, int sonar_echo, int timeout) {
  int result = timeout;
  if (!digitalRead(sonar_echo)) {
    // *sonar_echo* is LOW, so we can safely trigger a 12uS
    // trigger pulse:
    digitalWrite(sonar_trigger, HIGH);
    delayMicroseconds(12);
    digitalWrite(sonar_trigger, LOW);

    // Time the resulting echo pulse.  If the resulting echo pulse
    // is longer than *timeout*, *pulseIn* returns 0.
    result = pulseIn(sonar_echo, HIGH, timeout);

    if (result == 0) {
      // We timed out, so return *timeout*:
      result = timeout;
    }
  }
  // else *sonar_echo* is still high from the previous trigger;
  // return *timeout* when this occurs.
  return result;
}

// the loop routine runs over and over again forever:
void loop() {
  // Get the left and right sonar values:
  int left_ping = ping(left_sonar_trigger, left_sonar_echo, sonar_timeout);
  int right_ping = ping(right_sonar_trigger, right_sonar_echo, sonar_timeout);

  // Set the left and right motor speed:
  int left_speed = left_ping - sonar_follow_distance;
  int right_speed = right_ping - sonar_follow_distance;

  // Debugging code:
  if (debug) {
    // Print some debugging information:
    Serial.print("lp=");
    Serial.print(left_ping);
    Serial.print(" rp=");
    Serial.print(right_ping);
    Serial.print(" ls=");
    Serial.print(left_speed);
    Serial.print(" rs=");
    Serial.println(right_speed);

    // Delay a little so that the serial port does not get
    // swamped with debugging information:
    delay(100);
  }

  // Set the motor speeds:
  motors_run(left_speed, right_speed, 0);
}