// license info at bottom of file
// tl;dr AGPLv3+

#include "bot_map.h"

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

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

void set_motor(int speed_pin,
               int forward_pin,
               int backward_pin,
               int speed){
  if(speed > 0){
    off(backward_pin);
    on(forward_pin);}
  else if(speed < 0){
    off(forward_pin);
    on(backward_pin);
    speed = -speed;}
  else{ // speed is 0
    off(forward_pin);
    off(backward_pin);}
  // since speed has been set positive, no need to check if speed < -255.
  if(speed > 255){
    speed = 255;}
  analogWrite(speed_pin, speed);}

void go(int left_motor_speed, int right_motor_speed){
  set_motor(left_motor_speed_pin,
            left_motor_forward_pin,
            left_motor_backward_pin,
            left_motor_speed);
  set_motor(right_motor_speed_pin,
            right_motor_forward_pin,
            right_motor_backward_pin,
            right_motor_speed);}

int ping(int trigger, int echo){
  int ping_time = 0;
  // turn off trigger
  off(trigger);
  delayMicroseconds(2);
  // turn on the trigger and leave it on long enough for the
  // sonar sensor to notice
  on(trigger);
  delayMicroseconds(10);
  off(trigger);
  ping_time = pulseIn(echo, HIGH);
  if(ping_time <= 0){
    ping_time = 3000;}
  // sonar needs some time to recover before pinging again,
  // so make sure it gets enough sleep right here.  50 milliseconds
  delay(50);
  return ping_time;}

void setup(){
  Serial.begin(9600);

  pinMode(right_motor_speed_pin, OUTPUT);
  pinMode(right_motor_forward_pin, OUTPUT);
  pinMode(right_motor_backward_pin, OUTPUT);

  pinMode(right_echo_pin, INPUT);
  pinMode(right_trigger_pin, OUTPUT);

  pinMode(left_motor_speed_pin, OUTPUT);
  pinMode(left_motor_forward_pin, OUTPUT);
  pinMode(left_motor_backward_pin, OUTPUT);

  pinMode(left_echo_pin, INPUT);
  pinMode(left_trigger_pin, OUTPUT);

  off(left_motor_speed_pin);
  off(left_motor_forward_pin);
  off(left_motor_backward_pin);
  off(left_trigger_pin);

  off(right_motor_speed_pin);
  off(right_motor_forward_pin);
  off(right_motor_backward_pin);
  off(right_trigger_pin); }

void loop() {
  int left_speed;
  int right_speed;

  // you'll need to adjust these based on your sonar sensor's behavior
  int desired_right_ping_time = 800;
  int desired_left_ping_time = 800;

  unsigned int actual_left_ping_time = ping(left_trigger_pin, left_echo_pin);
  unsigned int actual_right_ping_time = ping(right_trigger_pin, right_echo_pin);

  /* int left_led_value = (int)(actual_left_ping_time / 16.0); */
  /* int right_led_value = (int)(actual_right_ping_time / 16.0); */

  /* analogWrite(left_led_pin, left_led_value); */
  /* analogWrite(right_led_pin, right_led_value); */

  /* Serial.print("left led value = "); */
  /* Serial.print(left_led_value); */
  /* Serial.print(", right led value = "); */
  /* Serial.print(right_led_value); */

  left_speed = actual_left_ping_time - desired_left_ping_time;
  right_speed = actual_right_ping_time - desired_right_ping_time;

  Serial.print(", left: ping = ");
  Serial.print(actual_left_ping_time);
  Serial.print(" speed = ");
  Serial.print(left_speed);
  Serial.print(" right: ping = ");
  Serial.print(actual_right_ping_time);
  Serial.print(" speed = ");
  Serial.print(right_speed);
  Serial.println();

  go(left_speed, right_speed); }

/*
 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/>.
 */