#include "pitches.h"
/*
  Code for driving remote controlled tank
 Includes code for:
 external interrupt 0 on pin 2
 midi sounds through speaker using tone() on pin 8
 H-bridge for driving motors
 Flashing LED that doesn't use a delay
 Serial port communication
 */

//Number of melodies to allocate for the speaker
#define NUM_SONGS 3

//Phase Definitions
#define STOPPED 0
#define FORWARD 1
#define RIGHT 2
#define LEFT 3
#define REVERSE 4

//H-Bridge Pin assignments
const unsigned char PIN_HBRIDGE_1A = 5;
const unsigned char PIN_HBRIDGE_2A = 3;
const unsigned char PIN_HBRIDGE_3A = 6;
const unsigned char PIN_HBRIDGE_4A = 9;

const unsigned char ledPin = 13; // LED that will flash

const unsigned char speakerPin = 8; // Speaker for melodies

const unsigned char buttonPin = 2; // the number of the pushbutton pin
const unsigned char int0 = 0; // pin 2 is attached to interrupt 0

//LED settings
long ledInterval = 500; // interval at which to blink (milliseconds)
int ledState = LOW;  // start LED turned off
long ledPreviousMillis = 0; // keep track of the last flash of the LED

//Serial port settings
unsigned char prompt = 1;  // should we prompt for a keystroke?
int inByte = 0;         // incoming serial byte

//Determines the tanks current task
unsigned int phase = STOPPED;

//Melody containers
int melodyLength[NUM_SONGS];
int* melody[NUM_SONGS];
int* noteDurations[NUM_SONGS];

//Melody 0
int melodyLength0 = 8;
int melody0[] = {
  NOTE_C4, NOTE_G3,NOTE_G3, NOTE_A3, NOTE_G3,0, NOTE_B3, NOTE_C4};
int noteDurations0[] = {
  4, 8, 8, 4,4,4,4,4 };

//Melody 1 - Beep
int melodyLength1 = 1;
int melody1[] = {
  NOTE_C4};
int noteDurations1[] = {
  2};

//Melody 2 - Lord of the Rings theme
int melodyLength2 = 16;
int melody2[] = {
  NOTE_A4,NOTE_G4,NOTE_G4,NOTE_G4,NOTE_A4,
  NOTE_D5,NOTE_E5,NOTE_F5,NOTE_E5,NOTE_D5,NOTE_C5,NOTE_D5,NOTE_E5,
  NOTE_D5,NOTE_C5,NOTE_B4};
int noteDurations2[] = {
  1,1,8,8,1,8,8,1,8,8,1,8,8,1,2,2};


void setup() {
  //Attach pins
  pinMode(PIN_HBRIDGE_1A, OUTPUT);
  pinMode(PIN_HBRIDGE_2A, OUTPUT);
  pinMode(PIN_HBRIDGE_3A, OUTPUT);
  pinMode(PIN_HBRIDGE_4A, OUTPUT);
  pinMode(ledPin, OUTPUT);
  pinMode(buttonPin, INPUT);

  //Setup interrupts
  attachInterrupt(int0, ButtonInterrupt, RISING);

  // start serial port at 9600 bps:
  Serial.begin(9600);

  //Setup melodies and play startup sound
  CreateMelodies();
  PlayMelody(0);

  Serial.println("Starting tank...");
}

//Interrupt handler for button press
void ButtonInterrupt() {
  //Debounce button press
  static unsigned long lastInterruptTime = 0;
  unsigned long interruptTime = millis();
  // If interrupts come faster than 200ms, assume it's a bounce and ignore
  if(interruptTime - lastInterruptTime > 200) {
    //Button interrupt handler
    //Increment phase, rollover after 4
    Serial.println("Interrupt!!");
    if (++phase == 5)
      phase = STOPPED;
  }
  lastInterruptTime = interruptTime;
}

void loop() {
  if (prompt) {
    PromptKeyPress();
  }

  if (Serial.available() > 0) {
    ReadKeyPress();
  }

  //This block of code can be uncommented and adjusted
  //for atonomous dead reckoning of the tank
  /*
  TankForward();
   delay(4000);
   TankRight();
   delay(1500);
   TankForward();
   delay(4000);
   TankLeft();
   delay(1500);
   TankStop();
   delay(2000);
   */

  switch(phase) {
  case STOPPED:
    TankStop();
    Serial.println("Tank stop");
    break;
  case FORWARD:
    TankForward();
    Serial.println("Tank forward");
    break;
  case RIGHT:
    TankRight();
    Serial.println("Tank right");
    break;
  case LEFT:
    TankLeft();
    Serial.println("Tank left");
    break;
  case REVERSE:
    TankReverse();
    Serial.println("Tank reverse");
    break;
  default:
    TankStop();
    Serial.println("Tank stop");
    break;
  }

  FlashLed();
  //  PlayMelody(1);
}

//Sets up melodies
void CreateMelodies() {
  Serial.println("Creating melodies...");
  melodyLength[0] = melodyLength0;
  melody[0] = melody0;
  noteDurations[0] = noteDurations0;

  melodyLength[1] = melodyLength1;
  melody[1] = melody1;
  noteDurations[1] = noteDurations1;

  melodyLength[2] = melodyLength2;
  melody[2] = melody2;
  noteDurations[2] = noteDurations2;
  Serial.println("Done.");
}

//Plays melodies by assigned number
void PlayMelody(int s) {
  Serial.println("Playing melody...");
  // iterate over the notes of the melody:
  for (int thisNote = 0; thisNote < melodyLength[s]; thisNote++) {

    // to calculate the note duration, take one second 
    // divided by the note type.
    //e.g. quarter note = 1000 / 4, eighth note = 1000/8, etc.
    int noteDuration = 1000/noteDurations[s][thisNote];
    tone(speakerPin, melody[s][thisNote],noteDuration);

    // to distinguish the notes, set a minimum time between them.
    // the note's duration + 30% seems to work well:
    int pauseBetweenNotes = noteDuration * 1.30;
    delay(pauseBetweenNotes);
  }
  Serial.println("Done.");
}

void PromptKeyPress() {
  Serial.println("Press a number");
  prompt = 0; 
}

//Read key presses over serial
void ReadKeyPress() {
  inByte = Serial.read();
  switch (inByte)
  {
  case '2':
    phase = REVERSE;
    break;
  case '4':
    phase = LEFT;
    break;
  case '5':
    phase = STOPPED;
    break;
  case '6':
    phase = RIGHT;
    break;
  case '8':
    phase = FORWARD;
    break;
  default:
    Serial.println("Invalid command");
    break;
  }
  prompt = 1;
}

//Flashes onboard LED periodically without using DELAY
void FlashLed() {
  if (millis() - ledPreviousMillis > ledInterval) {
    // save the last time you blinked the LED 
    ledPreviousMillis = millis();   

    // if the LED is off turn it on and vice-versa:
    if (ledState == LOW)
      ledState = HIGH;
    else
      ledState = LOW;

    // set the LED with the ledState of the variable:
    digitalWrite(ledPin, ledState);
  }
}


//These functions control the tank
void TankForward() {
  MotorLeftForward();
  MotorRightForward();
}

void TankStop() {
  MotorLeftStop();
  MotorRightStop(); 
}

void TankReverse() {
  MotorLeftReverse();
  MotorRightReverse();
}

void TankLeft() {
  MotorLeftForward();
  MotorRightReverse();
}

void TankRight() {
  MotorLeftReverse();
  MotorRightForward();
}

//These functions perform the low-level control of H-Bridge
void MotorRightStop() {
  digitalWrite(PIN_HBRIDGE_1A, LOW);
  digitalWrite(PIN_HBRIDGE_2A, LOW);
}

void MotorRightForward() {
  digitalWrite(PIN_HBRIDGE_1A, HIGH);
  digitalWrite(PIN_HBRIDGE_2A, LOW);
}

void MotorRightReverse() {
  digitalWrite(PIN_HBRIDGE_1A, LOW);
  digitalWrite(PIN_HBRIDGE_2A, HIGH);
}

void MotorLeftStop() {
  digitalWrite(PIN_HBRIDGE_3A, LOW);
  digitalWrite(PIN_HBRIDGE_4A, LOW);
}

void MotorLeftForward() {
  digitalWrite(PIN_HBRIDGE_3A, LOW);
  digitalWrite(PIN_HBRIDGE_4A, HIGH);
}

void MotorLeftReverse() {
  digitalWrite(PIN_HBRIDGE_3A, HIGH);
  digitalWrite(PIN_HBRIDGE_4A, LOW);
}