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

#include "bot_with_leds_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(LED_BUILTIN, OUTPUT);

  pinMode(button_pin, INPUT_PULLUP);

  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);}

enum class Stay_on_table_state {
  going, start_backing, backing, start_turning, turning};
static Stay_on_table_state stay_on_table_state =
  Stay_on_table_state::going;

void going() {
  Serial.print("going ");
  int forward_speed = 250;
  int stop_speed = 0;

  int left_speed;
  int right_speed;

  // adjust this number as necessary for your robot.
  // it represents how far the table is from your sonar sensor.
  // larger values mean larger distance.  default is 800
  const int right_max_ping_time_over_table = 800;
  const int left_max_ping_time_over_table = 800;

  const int left_ping_time =
    ping(left_trigger_pin, left_echo_pin);
  const int right_ping_time =
    ping(right_trigger_pin, right_echo_pin);

  if (left_ping_time <= left_max_ping_time_over_table
      || right_ping_time <= right_max_ping_time_over_table) {
    if(left_ping_time <= left_max_ping_time_over_table) {
      left_speed = forward_speed; }
    else {
      left_speed = stop_speed; }
    if(right_ping_time <= right_max_ping_time_over_table) {
      right_speed = forward_speed; }
    else {
      right_speed = stop_speed; } }
  else { // both ping times were above max acceptable ping time
    left_speed = right_speed = 0;
    stay_on_table_state = Stay_on_table_state::start_backing; }

  go(left_speed, right_speed); }

void backing(unsigned long start_backing) {
  Serial.print("backing ");
  static const unsigned int allowed_backup_duration = 500;
  unsigned long now = millis();
  unsigned long backup_duration = now - start_backing;

  go(-250, -250);

  if(backup_duration > allowed_backup_duration) {
    stay_on_table_state = Stay_on_table_state::start_turning; } }

void turning(unsigned long start_turning_time) {
  Serial.print("turning ");
  // the exact amount of time for turning around might need
  // twerking for your robot.  the default value is 3200
  static const unsigned int allowed_turning_duration = 2500;
  unsigned long now = millis();
  unsigned long turning_duration = now - start_turning_time;

  go(-250, 250);

  if(turning_duration > allowed_turning_duration) {
    stay_on_table_state = Stay_on_table_state::going; } }

void stay_on_table(){
  Serial.print("stay on table ");
  static unsigned long start_backing_time = 0;
  static unsigned long start_turning_time = 0;


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

  switch(stay_on_table_state) {
  case Stay_on_table_state::going: going(); break;
  case Stay_on_table_state::start_backing:
    Serial.print("start backing ");
    start_backing_time = millis();
    stay_on_table_state = Stay_on_table_state::backing;
  case Stay_on_table_state::backing:
    backing(start_backing_time);
    break;
  case Stay_on_table_state::start_turning:
    Serial.print("start turning ");
    start_turning_time = millis();
    stay_on_table_state = Stay_on_table_state::turning;
  case Stay_on_table_state::turning:
    turning(start_turning_time);
    break; } }

void follow() {
  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);

  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);

  go(left_speed, right_speed); }

enum class Behavior {stay_on_table, follow};
Behavior behavior = Behavior::stay_on_table;

enum class Button_state {up, down};

Button_state prior_button_state = Button_state::up;

void loop() {
  static int count = 0;
  Serial.print(count);
  Serial.print(" ");
  count++;

  Button_state button_state =
    (digitalRead(button_pin) == HIGH)
      ? Button_state::up
      : Button_state::down;
  switch(button_state) {
  case Button_state::up: Serial.print("up "); break;
  case Button_state::down: Serial.print("down "); break; }

  // if button was just pressed
  if (prior_button_state == Button_state::up
      &&    button_state == Button_state::down) {
    // indicate button press recognized
    on(LED_BUILTIN);
    // turn off motors, to allow robot to be set down
    go(0, 0);
    delay(1000);
    switch(behavior) {
    case Behavior::stay_on_table: behavior = Behavior::follow; break;
    case Behavior::follow: behavior = Behavior::stay_on_table; break; }
    off(LED_BUILTIN); }

  switch(behavior) {
  case Behavior::stay_on_table: stay_on_table(); break;
  case Behavior::       follow:        follow(); break; }

  prior_button_state = button_state;

  Serial.println(); }

void loop_asdf() {
  go(250, 250);
}