#define DEBUG 0

//L293D
//Motor A (left)
#define MOTOR_PIN1 9 // Pin 2 of L293
#define MOTOR_PIN2 10 // Pin 7 of L293
//Motor B (right)
#define MOTOR_PIN3 11 // Pin  10 of L293
#define MOTOR_PIN4 12 // Pin  15 of L293

//IR Remote
#include <IRremote.h>
#define IR_PIN 8
IRrecv irrecv(IR_PIN);
decode_results ir_results;

//Ultrasonic sensor
#define US_TRIG_PIN 7
#define US_ECHO_PIN 6
#define EVADE_DISTANCE 6

enum movements{
  UNDEFINED,
  STOP,
  FORWARD,
  BACKWARD,
  LEFT_ROTATE,
  RIGHT_ROTATE,
  LEFT_CIRCLE,
  RIGHT_CIRCLE
};

enum movements current_mode = STOP;
enum movements next_mode = STOP;
enum movements last_mode = STOP;
boolean evading = false;
boolean moving = false;

#define TIME_WAIT 3600

void setup() {
  randomSeed(analogRead(0));
  //Set pins as outputs
  pinMode(MOTOR_PIN1, OUTPUT);
  pinMode(MOTOR_PIN2, OUTPUT);
  pinMode(MOTOR_PIN3, OUTPUT);
  pinMode(MOTOR_PIN4, OUTPUT);
  motor_a_stop();
  motor_b_stop();
  pinMode(US_TRIG_PIN, OUTPUT);
  digitalWrite(US_TRIG_PIN, LOW);
  pinMode(US_ECHO_PIN, INPUT);
#if DEBUG > 0
  Serial.begin(9600);
  Serial.println("IR Receiver Raw Data + Button Decode Test");
#endif
  irrecv.enableIRIn(); // Start the receiver
}

void motor_a_stop() {
  digitalWrite(MOTOR_PIN1, LOW);
  digitalWrite(MOTOR_PIN2, LOW);
}

void motor_a_forward() {
  digitalWrite(MOTOR_PIN1, LOW);
  digitalWrite(MOTOR_PIN2, HIGH);
}

void motor_a_backward() {
  digitalWrite(MOTOR_PIN1, HIGH);
  digitalWrite(MOTOR_PIN2, LOW);
}

void motor_b_stop() {
  digitalWrite(MOTOR_PIN3, LOW);
  digitalWrite(MOTOR_PIN4, LOW);
}

void motor_b_forward() {
  digitalWrite(MOTOR_PIN3, LOW);
  digitalWrite(MOTOR_PIN4, HIGH);
}

void motor_b_backward() {
  digitalWrite(MOTOR_PIN3, HIGH);
  digitalWrite(MOTOR_PIN4, LOW);
}

void left_stop() {
  motor_a_stop();
}

void right_stop() {
  motor_b_stop();
}

void stop() {
  left_stop();
  right_stop();
  moving = false;
}

void left_forward() {
  motor_a_backward();
  moving = true;
}

void left_backward() {
  motor_a_forward();
  moving = true;
}

void right_forward() {
  motor_b_forward();
  moving = true;
}

void right_backward() {
  motor_b_backward();
  moving = true;
}

void forward() {
  left_forward();
  right_forward();
}

void backward() {
  left_backward();
  right_backward();
}

void left_rotate() {
  left_backward();
  right_forward();
}

void right_rotate() {
  left_forward();
  right_backward();
}

void left_circle() {
  left_stop();
  right_forward();
}

void right_circle() {
  right_stop();
  left_forward();
}

void wait() {
  delay(TIME_WAIT);
  stop();
}

void test_movements(){
  forward();
  wait();
  backward();
  wait();
  left_rotate();
  wait();
  right_rotate();
  wait();
  left_circle();
  wait();
  right_circle();
  wait();
  stop();
  wait();
}

void ir_decode(){
  switch(ir_results.value){
    /*
     * 0xFF38C7 => 5 (stop)         => STOP
     * 0xFF18E7 => 2 (forward)      => FORWARD
     * 0xFF4AB5 => 8 (backward)     => BACKWARD
     * 0xFF10EF => 4 (left rotate)  => LEFT_ROTATE
     * 0xFF5AA5 => 6 (right rotate) => RIGHT_ROTATE
     * 0xFF7A85 => 3 (right circle) => LEFT_CIRCLE
     * 0xFF30CF => 1 (left circle)  => RIGHT_CIRCLE
     */
    case 0xFF38C7:
#if DEBUG > 0
      Serial.println("Stop (5)");
#endif
      next_mode = STOP;
      break;

    case 0xFF18E7:
#if DEBUG > 0
      Serial.println("Forward (2)");
#endif
      next_mode = FORWARD;
      break;

    case 0xFF4AB5:
#if DEBUG > 0
      Serial.println("Backward (8)");
#endif
      next_mode = BACKWARD;
      break;

    case 0xFF10EF:
#if DEBUG > 0
      Serial.println("Left rotate (4)");
#endif
      next_mode = LEFT_ROTATE;
      break;

    case 0xFF5AA5:
#if DEBUG > 0
      Serial.println("Right rotate (6)");
#endif
      next_mode = RIGHT_ROTATE;
      break;

    case 0xFF7A85:
#if DEBUG > 0
      Serial.println("Left circle (3)");
#endif
      next_mode = LEFT_CIRCLE;
      break;

    case 0xFF30CF:
#if DEBUG > 0
      Serial.println("Right circle (1)");
#endif
      next_mode = RIGHT_CIRCLE;
      break;

#if DEBUG > 0
    default:
      Serial.print("Unknown button: ");
      Serial.println(ir_results.value, HEX);
#endif
  }
}

void sonic_check(){
  long duration;
  int distance;
  //reset
  digitalWrite(US_TRIG_PIN, LOW);
  delayMicroseconds(2);
  //Set the trigger pin on HIGH state for 10 micro seconds
  digitalWrite(US_TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(US_TRIG_PIN, LOW);
  //Read echo
  duration = pulseIn(US_ECHO_PIN, HIGH);
  //Calculating distance
  distance = duration * 0.034 / 2;
#if DEBUG > 1
  Serial.print("Distance: ");
  Serial.println(distance);
#endif
  if (distance <= EVADE_DISTANCE){
    if (!evading){
      last_mode = int(current_mode);
    }
    switch(current_mode){
      case LEFT_ROTATE:
        next_mode = LEFT_ROTATE;
        break;
      case RIGHT_ROTATE:
        next_mode = RIGHT_ROTATE;
        break;
      case LEFT_CIRCLE:
        next_mode = LEFT_ROTATE;
        break;
      case RIGHT_CIRCLE:
        next_mode = RIGHT_ROTATE;
        break;
      default:
        if (random(10) >= 5){
          next_mode = LEFT_ROTATE;
        } else {
          next_mode = RIGHT_ROTATE;
        }
    }
    evading = true;
  } else if (evading){
    next_mode = int(last_mode);
    evading = false;
  }
}

void mode_check(){
#if DEBUG > 2
  Serial.println("Mode: ");
  Serial.print("# Current: ");
  Serial.println(current_mode);
  Serial.print("# Next: ");
  Serial.println(next_mode);
#endif
  if (next_mode != current_mode) {
    switch(next_mode){
      case STOP:
        stop();
        current_mode = STOP;
        break;
      case FORWARD:
        forward();
        current_mode = FORWARD;
        break;
      case BACKWARD:
        backward();
        current_mode = BACKWARD;
        break;
      case LEFT_ROTATE:
        left_rotate();
        current_mode = LEFT_ROTATE;
        break;
      case RIGHT_ROTATE:
        right_rotate();
        current_mode = RIGHT_ROTATE;
        break;
      case LEFT_CIRCLE:
        left_circle();
        current_mode = LEFT_CIRCLE;
        break;
      case RIGHT_CIRCLE:
        right_circle();
        current_mode = RIGHT_CIRCLE;
        break;
    }
  } else {
    delay(500);
  }
}

void loop() {
  if (irrecv.decode(&ir_results)){
    ir_decode();
    irrecv.resume(); // receive the next value
  }
  if (moving){
    sonic_check();
  }
  mode_check();
#ifdef DEBUG
    //test_movements()
#endif
}