#include <MakeItRobotics.h>
MakeItRobotics line_following;

#include <IRremote.h>
#include <IRremoteInt.h>

#include "pitches.h"
#include <NewTone.h>

//ultra sonic
#include "Arduino.h"

#define MOVE_FORWARD    0xE0E006F9
#define MOVE_RIGHT	0xE0E046B9
#define MOVE_LEFT       0xE0E0A659
#define STOP_MOVE       0xE0E016E9
#define INC_TURN_RATE   0xE0E048B7
#define DEC_TURN_RATE   0xE0E008F7
#define MOVE_BACKWARD   0xE0E08679
#define DANCE           0xE0E0F00F

#define SL_BD_RT        9600
#define RECV_PIN        11
#define RED_LED         5

#define MOVE_COUNT_MAX  1
#define MOVE_INTERVAL   1000
#define DONT_MOVE_COUNT_MAX 3

IRrecv irrecv(RECV_PIN);
decode_results results;

int ledState = LOW;
long previousMillis = 0;
long interval = 250;

int move_count = 0;
int dont_move_count = 0;
long movePreviousMillis = 0;

int ultrasonicCount = 0;
int ultrasonicCountMax = 1000;

bool movingForward = false;
bool movingLeft = false;
bool movingRight = false;
bool movingBackward = false;

int robot_move_speed = 255;
bool freewill = false;
bool findingRoute = false;

int stoppedCount = 0;
int stoppedCountMax = 10000;

int turn_rate = 150;

int mario_theme_melody[] = {zg,za,zb,zy,zg,za,zb,zy,zx,zz,zC,zD,zx,zz,zC,zD,za,zb,zy,zw,za,zb,zy,zw,zz,zC,zD,zE,zz,zC,zD,zE,zb,zy,zw,zF,zC,zD,zE,zq,zy,zw,zF,zG,zD,zE,zq,zi,0,0,0,0,0,0,0,0,za,zz,zb,zC};

int mario_theme_noteDurations[] = {4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4};

int mario_theme_melody2[] = {NOTE_G4, NOTE_G4, NOTE_G4, NOTE_G4, NOTE_DS4,
NOTE_F4, NOTE_G4, NOTE_F4, NOTE_G4};

int mario_theme_noteDurations2[] = {8,8,8,4,4,4,8,8,2};

class Ultrasonic
{
	public:
		Ultrasonic(int pin);
        void DistanceMeasure(void);
		long microsecondsToCentimeters(void);
		long microsecondsToInches(void);
	private:
		int _pin;//pin number of Arduino that is connected with SIG pin of Ultrasonic Ranger.
        long duration;// the Pulse time received;
};
Ultrasonic::Ultrasonic(int pin)
{
	_pin = pin;
}
/*Begin the detection and get the pulse back signal*/
void Ultrasonic::DistanceMeasure(void)
{
    pinMode(_pin, OUTPUT);
	digitalWrite(_pin, LOW);
	delayMicroseconds(2);
	digitalWrite(_pin, HIGH);
	delayMicroseconds(5);
	digitalWrite(_pin,LOW);
	pinMode(_pin,INPUT);
	duration = pulseIn(_pin,HIGH);
}
/*The measured distance from the range 0 to 400 Centimeters*/
long Ultrasonic::microsecondsToCentimeters(void)
{
	return duration/29/2;
}
/*The measured distance from the range 0 to 157 Inches*/
long Ultrasonic::microsecondsToInches(void)
{
	return duration/74/2;
}

Ultrasonic ultrasonic(7);

void setup()
{
  pinMode(RED_LED, OUTPUT);

  delay(500);

 //used for motors
    Serial.begin(10420);
 //used for testing & ultrasonic
    //Serial.begin(SL_BD_RT);

  line_following.line_following_setup();
  line_following.all_stop();
  irrecv.enableIRIn();
}

void loop()
{
  long RangeInInches;
  long RangeInCentimeters;
  ultrasonicCount++;
  if(ultrasonicCount >= ultrasonicCountMax)
  {
    ultrasonicCount = 0;

    ultrasonic.DistanceMeasure();// get the current signal time;
    RangeInInches = ultrasonic.microsecondsToInches();//convert the time to inches;
   /* RangeInCentimeters = ultrasonic.microsecondsToCentimeters();//convert the time to centimeters
	Serial.println("The distance to obstacles in front is: ");
	Serial.print(RangeInInches);//0~157 inches
	Serial.println(" inch");
	Serial.print(RangeInCentimeters);//0~400cm
	Serial.println(" cm");*/

    if(freewill == true)
     {
       if(RangeInInches <= 5)
       {
         if(move_count == 0)
         {
           movingForward = false;
           movingLeft = true;
           movingRight = false;
           movingBackward = false;
         }
         ledState = HIGH;
         digitalWrite(RED_LED, ledState);
       } else {
         if(move_count == 0)
         {
           movingForward = true;
           movingLeft = false;
           movingRight = false;
           movingBackward = false;
         }
         ledState = LOW;
         digitalWrite(RED_LED, ledState);
       }
     }
   }

  unsigned long moveCurrentMillis = millis();
  if(moveCurrentMillis - movePreviousMillis > MOVE_INTERVAL)
  {
    movePreviousMillis = moveCurrentMillis;

    if((movingForward == true || movingBackward == true)||(movingLeft == true || movingRight == true))
    {
       if(move_count >= MOVE_COUNT_MAX)
       {
          line_following.all_stop();
          movingForward = false;
          movingLeft = false;
          movingRight = false;
          movingBackward = false;
          dont_move_count = 0;
       }
       else
       {
         move_count++;
       }


    }
       if(dont_move_count >= DONT_MOVE_COUNT_MAX)
       {
          line_following.all_stop();
          movingForward = false;
          movingLeft = false;
          movingRight = false;
          movingBackward = false;
          move_count = 0;
       }
       else
       {
         dont_move_count++;
       }
   }

    if(movingForward == true)
      line_following.go_backward(robot_move_speed);
    if(movingLeft == true)
      line_following.turn_left(turn_rate);
    if(movingRight == true)
      line_following.turn_right(turn_rate);
    if(movingBackward == true)
      line_following.go_forward(robot_move_speed);



    if (irrecv.decode(&results))
    {
      //Serial.println(results.value, HEX);
      switch(results.value)
      {
      case MOVE_FORWARD:
         movingForward = true;
         movingLeft = false;
         movingRight = false;
         movingBackward = false;
         freewill = false;
        // robot_move_speed = 255;
      break;

      case MOVE_RIGHT:
         movingForward = false;
         movingLeft = false;
         movingRight = true;
         movingBackward = false;
         freewill = false;
        // robot_move_speed = 255;
      break;

      case MOVE_LEFT:
         movingForward = false;
         movingLeft = true;
         movingRight = false;
         movingBackward = false;
         freewill = false;
         //robot_move_speed = 255;
      break;

      case MOVE_BACKWARD:
         movingForward = false;
         movingLeft = false;
         movingRight = false;
         movingBackward = true;
         freewill = false;
         //robot_move_speed = 255;
      break;

      case STOP_MOVE:
         line_following.all_stop();
         movingForward = false;
         movingLeft = false;
         movingRight = false;
         movingBackward = false;
         move_count = 0;
         freewill = false;
         //robot_move_speed = 255;
      break;

     case INC_TURN_RATE:
       if(turn_rate < 250)
       {
         turn_rate += 50;
         freewill = true;
         //robot_move_speed = 100;
       }
     break;

    case DEC_TURN_RATE:
      if(turn_rate > 50)
        turn_rate -= 50;
    break;

    case DANCE:
      play_song_1();
    break;
  }
    //delay(200);  // 1/5 second delay for arbitrary clicks.  Will implement debounce later.
    irrecv.resume();
    }
}

void play_song_1 ()
{
    int size = sizeof(mario_theme_melody) / sizeof(int);
    for (int thisNote = 0; thisNote < size; 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/mario_theme_noteDurations[thisNote];
      NewTone(8, mario_theme_melody[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);
      // stop the tone playing:
      noNewTone(8);
    }
}