#include <Servo.h>
#include <IRremote.h>


const int pingPin = 5; // Trigger Pin of Ultrasonic Sensor
const int echoPin = 4; // Echo Pin of Ultrasonic Sensor
int back_left_one = 13;
int back_left_second = 12;
int back_right_one = 10;
int back_right_second = 11;
int front_left_one = 7;
int front_left_second = 6;
int front_right_one = 8;
int front_right_second = 9;

//Servo
int servoPin = 3;
Servo servo;
// Infrared Sensor
int ir_sensor = 1;
decode_results results;
IRrecv irrecv(ir_sensor);


// Autonomous driving on/off
bool elon_driving = false;
int pins[] = {back_left_one,back_left_second,back_right_one,back_right_second,front_left_one,front_left_second,front_right_one,front_right_second};
void setup()
{
  pinMode(pingPin, OUTPUT);
  pinMode(echoPin, INPUT);
  Serial.begin(9600);
  for(int i = 0; i < sizeof(pins)/sizeof(pins[0]);i++){
    pinMode(pins[i],OUTPUT);
  }
  servo.attach(servoPin);
  pinMode(ir_sensor,INPUT);
  irrecv.enableIRIn();
  Serial.println(results.value);
  start();
}

void start(){
  //set all pins to full speed
  setMotor(pins[0],pins[1],HIGH,false);
  setMotor(pins[2],pins[3],HIGH,false);
  setMotor(pins[4],pins[5],HIGH,false);
  setMotor(pins[6],pins[7],HIGH,false);
}

void stop(){
  //set all pins to low speed
  	setMotor(pins[0],pins[1],LOW,false);
  	setMotor(pins[2],pins[3],LOW,false);
  	setMotor(pins[4],pins[5],LOW,false);
    setMotor(pins[6],pins[7],LOW,false);
}

void reverse(){
  // reverse the speed on the second pin of the h bridge
  	setMotor(pins[0],pins[1],HIGH,true);
  	setMotor(pins[2],pins[3],HIGH,true);
  	setMotor(pins[4],pins[5],HIGH,true);
    setMotor(pins[6],pins[7],HIGH,true);
}

void driveLeft(){
  	//left
  setMotor(pins[0],pins[1],HIGH,true);
  setMotor(pins[4],pins[5],HIGH,true);
  //Right
  setMotor(pins[2],pins[3],HIGH,false);
  setMotor(pins[6],pins[7],HIGH,false);
}

void driveRight(){
  //left
 	setMotor(pins[0],pins[1],HIGH,false);
  	setMotor(pins[4],pins[5],HIGH,false);
  //Right
  	setMotor(pins[2],pins[3],HIGH,true);
    setMotor(pins[6],pins[7],HIGH,true);
}

long sensor_reading = 0;

void loop()
{

  if (irrecv.decode(&results))
  {
    switch(results.value){
       case 0xFF6897: elon_driving = true;    break;
	    case 0xFF9867: elon_driving = false;    break;
      default: break;
    }
    if(elon_driving == true){
	    autoDrive();
    }else{
      switch(results.value){
      	case 0xFF629D: start(); break;
  		case 0xFF22DD: driveLeft();    break;
  		case 0xFF02FD: stop();    break;
  		case 0xFFC23D: driveRight();   break;
  		case 0xFFA857: reverse(); break;
  		default: break;
      }
    }
    irrecv.resume(); // Receive the next value
  }
  /*
  sensor_reading = readDistance();

  turnServo();
  */
}

void autoDrive(){
  bool found_obstical = false;
	 	for (int pos = 0; pos <= 180; pos += 2) {
        servo.write(pos);
          if (readDistance() >= 20){
            found_obstical = true;
            break;
          }
      }
  		delay(500);
      for (int pos = 180; pos >= 0; pos -= 2) {
          servo.write(pos);
        	if (readDistance() >= 20){
            found_obstical = true;
            break;
          }
      }
  if(found_obstical == false){
    elon_driving = false;
  }else{
    reverse();
    delay(500);
    stop();
    autoDrive();
  }
}

void setMotor(int first_in,int second_in,int speed, boolean reverse)
{
  if(reverse == true){
    digitalWrite(first_in, LOW);
  	digitalWrite(second_in,speed);
  }else{
    digitalWrite(first_in, speed);
    digitalWrite(second_in,LOW);
  }
}

long duration;
long cm;
long readDistance(){
  digitalWrite(pingPin, LOW);
   delayMicroseconds(2);
  digitalWrite(pingPin, HIGH);
   delayMicroseconds(10);
  digitalWrite(pingPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  // the ultrasonic reader is not really accuarte, so multipling by 1.2 seems to get
  // the sensor a lot better when it comes to accuracy.
  cm = microsecondsToCentimeters(duration) * 1.2;
  return cm;
}

long microsecondsToCentimeters(long microseconds) {
   return microseconds / 29 / 2;
}