/*
 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/>.
 */
/*
  Motor.
  Runs both motors back and forth.
n
  This example code is in the public domain.
 */

int motor1_enable = 10;
int motor1a = 9;
int motor1b = 8;
int motor2_enable = 3;
int motor2a = 4;
int motor2b = 5;

// ping sensors
int left_trigger = 12;
int left_echo = 11;
int right_trigger = 7;
int right_echo = 6;

// the setup routine runs once when you press reset:
void setup() {
  Serial.begin(9600);

  // initialize the digital pin as an output.
  pinMode(motor1_enable, OUTPUT);
  pinMode(motor1a, OUTPUT);
  pinMode(motor1b, OUTPUT);
  pinMode(motor2_enable, OUTPUT);
  pinMode(motor2a, OUTPUT);
  pinMode(motor2b, OUTPUT);

  // ping sensors
  pinMode(left_trigger, OUTPUT);
  pinMode(left_echo, INPUT);
  pinMode(right_trigger, OUTPUT);
  pinMode(right_echo, INPUT);

  digitalWrite(motor1_enable, LOW);
  digitalWrite(motor1a, LOW);
  digitalWrite(motor1b, LOW);
  digitalWrite(motor2_enable, LOW);
  digitalWrite(motor2a, LOW);
  digitalWrite(motor2b, LOW);
}

void
on(int pin)
{
  digitalWrite (pin, HIGH);
}

void
off(int pin)
{
  digitalWrite (pin, LOW);
}

int
ping(int trigger, int echo)
{
  int ping_time;
  on(trigger);
  delayMicroseconds(12);
  off(trigger);
  ping_time = pulseIn (echo, HIGH);
  if (ping_time <= 0)
    ping_time = 3000;
  return ping_time;
}

void motorsRun(int left, int right, int ms_delay) {
  // Set left motor direction:
  if (left > 0) {
    // Set left motor to go forward:
    digitalWrite(motor1a, HIGH);
    digitalWrite(motor1b, LOW);
  } else {
    // Set left motor to go backward:
    digitalWrite(motor1a, LOW);
    digitalWrite(motor1b, HIGH);
    left = -left;                   // Make left a positive value:
  }
  analogWrite(motor1_enable, left); // Start motor in right direction

  // Set left motor direction:
  if (right > 0) {
    // Set right motor to go forward:
    digitalWrite(motor2a, HIGH);
    digitalWrite(motor2b, LOW);
  } else {
    // Set right motor to go backward:
    digitalWrite(motor2a, LOW);
    digitalWrite(motor2b, HIGH);
    right = -right;                 // Make right a positive value:
  }
  analogWrite(motor2_enable, right); // Start motor in right direction

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

// the loop routine runs over and over again forever:
void
loop()
{
  int left_speed = 0;
  int right_speed = 0;

  int left_ping = ping (left_trigger, left_echo);
  int right_ping = ping (right_trigger, right_echo);

  /*
  Serial.print ("left ping = ");
  Serial.print (left_ping);

  Serial.print ("  right ping = ");
  Serial.println (right_ping);
  */

  if (left_ping < 400)
    {
      left_speed = 250;
    }

  if (right_ping < 400)
    {
      right_speed = 250;
    }

  if (left_speed == 0 && right_speed == 0)
    {
      // backup
      motorsRun(-250, -250, 2000);
      // turn around
      motorsRun(-250, 250, 3200);
    }
  else
    {
      motorsRun(left_speed, right_speed, 0);
    }
}