#include <Servo.h>

Servo left_servo;              // Define left servo
Servo right_servo;             // Define right servo

#include <IRremote.h>

//TV remote
#define input 2640
#define power 2704
#define syncMenu 3416
#define stbMenu 18723
#define one 16
#define two 2064
#define three 1040
#define four 3088
#define five 528
#define six 2576
#define seven 1552
#define eight 3600
#define nine 272
#define zero 2320
#define dot 23785
#define display 1488
#define googlePlay 12579
#define netflix 8024
#define yellow 29417
#define blue 4841
#define red 21225
#define green 13033
#define actionMenu 26915
#define guide 27941
#define apps 10787
#define back 25321
#define home 112
#define up 752
#define down 2800
#define left 720
#define right 3280
#define enter 2672
#define volPlus 1168
#define volMinus 3216
#define jump 3536
#define mute 656
#define chPlus 144
#define chMinus 2192
#define audio 3728
#define fastRewind 27881
#define play 11497
#define fastForward 7401
#define preSearch 7913
#define pause 19689
#define nextSearch 24297
#define stop 3305
#define subtitle 2793
#define help 22819
#define wide 24101
#define picOff 2000


int buzzer_red = 11;    //  Red wire = Positive
int buzzer_black = 12;  //  Black wire = negaetive
int RECV_PIN = 5; //  Infrared signal pin

const byte sm_idling       = 0;
const byte sm_go_forward   = 1;
const byte sm_go_backwards = 2;
const byte sm_spin_right   = 3;
const byte sm_spin_left    = 4;
const byte sm_wait_spin_finished = 5;

byte myState;

// names for making it easy to understand which state is what
char myStateNames[][24] = {
  "sm_idling",
  "sm_go_forward",
  "sm_go_backwards",
  "sm_spin_right",
  "sm_spin_left",
  "sm_wait_spin_finished",
};

const unsigned long driveBackwardsPeriod = 1000;
const unsigned long spinPeriod  = 400;

// just a SINGLE variable for non-blocking timing
unsigned long waitTimer = 0;


IRrecv irrecv(RECV_PIN);
decode_results results;
volatile int active_left = LOW;
volatile int active_right = LOW;
boolean started = false;

void onStateChangePrint() {
  static byte lastState;

  if (lastState != myState) {
    Serial.print("state changed from ");
    Serial.print(myStateNames[lastState]);
    Serial.print(" to ");
    Serial.println(myStateNames[myState]);
    lastState = myState;
  }
}


void myBummpThenTurnStateMachine() {

  switch (myState) {

    case sm_idling:
      if (started) {
        myState = sm_go_forward;
      }
      break; // IMMIDIATELY jump down to END-OF-SWITCH

    case sm_go_forward:
      go_forward();

      if (active_left == HIGH) {           // If left bumper hit
        waitTimer = millis();
        go_backwards(); // START driving backwards
        myState = sm_spin_right;
      }
      
      if (active_right == HIGH) {           // If left bumper hit
        waitTimer = millis();
        go_backwards(); // START driving backwards
        myState = sm_spin_left;
      }
      break; // IMMIDIATELY jump down to END-OF-SWITCH

    case sm_spin_right:
      // driving backwards is going on until driveBackwardsPeriod is over (1000 ms)
      if ( TimePeriodIsOver(waitTimer, driveBackwardsPeriod) ) {
        // when 1000 ms have REALLY passed by
        spin_right();
        waitTimer = millis();
        myState = sm_wait_spin_finished;
      }
      break; // IMMIDIATELY jump down to END-OF-SWITCH

    case sm_spin_left:
      // driving backwards is going on until driveBackwardsPeriod is over (1000 ms)
      if ( TimePeriodIsOver(waitTimer, driveBackwardsPeriod) ) {
        // when 1000 ms have REALLY passed by
        spin_left();
        waitTimer = millis();
        myState = sm_wait_spin_finished;
      }
      break; // IMMIDIATELY jump down to END-OF-SWITCH

    case sm_wait_spin_finished:
      // check if spinPeriod (400 ms) is over
      if ( TimePeriodIsOver(waitTimer, spinPeriod) ) {
        // when 400 ms have REALLY passed by
        active_left  = LOW;
        active_right = LOW;
        myState = sm_go_forward;
      }
      break; // IMMIDIATELY jump down to END-OF-SWITCH

  } // END-OF-SWITCH
}

// easy to use helper-function for non-blocking timing
boolean TimePeriodIsOver (unsigned long &startOfPeriod, unsigned long TimePeriod) {
  unsigned long currentMillis  = millis();
  if ( currentMillis - startOfPeriod >= TimePeriod ) {
    // more time than TimePeriod has elapsed since last time if-condition was true
    startOfPeriod = currentMillis; // a new period starts right here so set new starttime
    return true;
  }
  else return false;            // actual TimePeriod is NOT yet over
}

const byte bumperSwitchRight_Pin = 2;
const byte bumperSwitchleft_Pin  = 3;

void setup() {
  // Set pin modes for switches
  pinMode(buzzer_red, OUTPUT);       //  Buzzer positive output
  pinMode(buzzer_black, OUTPUT);     //  Buzzer negative output
  pinMode(bumperSwitchRight_Pin, INPUT);
  pinMode(bumperSwitchleft_Pin,  INPUT);
  pinMode(4, OUTPUT);
  digitalWrite(bumperSwitchRight_Pin, HIGH);
  digitalWrite(bumperSwitchleft_Pin,  HIGH);
  digitalWrite(4, LOW);     // Serves as ground connection

  pinMode(6, OUTPUT);       // IR power, ground pins
  pinMode(7, OUTPUT);
  digitalWrite(6, LOW);     // IR ground
  digitalWrite(7, HIGH);    // IR power

  right_servo.attach(10);    // Set right servo to digital pin 9
  left_servo.attach(9);    // Set left servo to digital pin 10
  irrecv.enableIRIn();     // Start the receiver

  Serial.begin(9600);

  // Set up interrupts
  attachInterrupt(digitalPinToInterrupt(bumperSwitchRight_Pin), bump_right, FALLING);
  attachInterrupt(digitalPinToInterrupt(bumperSwitchleft_Pin), bump_left, FALLING);

  started = true;
}


void loop() {

  onStateChangePrint();
  myBummpThenTurnStateMachine();
  //onStateChangePrint();
  
  if (irrecv.decode(&results)) {
    Serial.print(results.value, DEC);
    Serial.print("    ");
    Serial.println(results.value);

    switch (results.value) {
      case one:
        Serial.println("1");     // Turn left forward
        left_turn_fwd();
        break;

      case two:
        Serial.println("2");     // Forward
        go_forward();
        break;

      case three:
        Serial.println("3");     // Turn right forward
        right_turn_fwd();
        break;

      case four:
        Serial.println("4");    // Spin left
        spin_left();
        break;

      case five:
        Serial.println("5");    // Stop
        stop_all();
        break;

      case six:
        Serial.println("6");    // Spin right
        spin_right();
        break;

      case seven:
        Serial.println("7");    // Turn left reverse
        left_turn_backwards();
        break;

      case eight:
        Serial.println("8");    // Reverse
        go_backwards();
        break;

      case nine:
        Serial.println("9");    // Turn right reverse
        turn_right_backwards();
        break;
    }
    irrecv.resume(); // Receive the next value
    delay(2);
  }
}

// Routines for forward, reverse, turns, and stop
void go_forward() {
  left_servo.write(1700);
  right_servo.write(1300);
}

void go_backwards() {
  Serial.println("goBackwards");
  left_servo.write(1300);
  right_servo.write(1700);
}

void spin_right() {
  Serial.println("spin_right()");
  left_servo.write(1700);
  right_servo.write(1700);
}

void spin_left() {
  Serial.println("spin_left()");
  left_servo.write(1300);
  right_servo.write(1300);
}

void right_turn_fwd() {
  left_servo.write(1700);
  right_servo.write(1500);
}

void left_turn_fwd() {
  left_servo.write(1500);
  right_servo.write(1300);
}

void left_turn_backwards() {
  left_servo.write(1500);
  right_servo.write(1700);
}

void turn_right_backwards() {
  left_servo.write(1300);
  right_servo.write(1500);
}

void stop_all() {
  left_servo.write(1500);
  right_servo.write(1500);
}

// Interrupt service routines

void bump_left() {
  if (started)
    active_left = HIGH;
}

void bump_right() {
  if (started)
    active_right = HIGH;
}