//
//	MANGRITON 2WD
//	        L298N		        HC-05      HORN       LIGHTS
//	  D0	                   	X
//	  D1
//  	D2		                                      		X
//pwm	D3		                             	X
//  	D4		                                      		X
//pwm	D5
//pwm	D6
//  	D7	   IN1
//    D8     IN2
//pwm	D9	                                       			X
//pwm	D10    ENA
//pwm	D11	   ENB
//	  D12    IN4
//	  D13	   IN3
//Analog A1:	                                      X

#include <SoftwareSerial.h>
//
// Led HORN --------------------------------------------------
const int pinZumbador = 3;


//  L298N  -----------------
const int pinENA = 10;
const int pinIN1 = 7;
const int pinIN2 = 8;
const int pinIN3 = 13;
const int pinIN4 = 12;
const int pinENB = 11;


const int pinMotorA[3] = { pinENA, pinIN1, pinIN2 };
const int pinMotorB[3] = { pinENB, pinIN3, pinIN4 };


#include <TimerOne.h>


//  
//
// motor 1       ADELANTE  ATRAS   STOP
//  IN1            HIGH     LOW     LOW
//  IN2            LOW      HIGH    LOW
//
// motor 2       ADELANTE  ATRAS   STOP
//  IN3            HIGH     LOW     LOW
//  IN4            LOW      HIGH    LOW
//
// LIGHTS -----------------------------------------------------
//
const int pinFaroDelanteroDerecho = 9;
const int pinFaroDelanteroIzquierdo = 4;
const int pinFaroTraseroDerecho = 2;
const int pinFaroTraseroIzquierdo = A1;
   
//
// MODULE HC-05 BLUETOOTH
// 5V
// GND
// TX in RX
// DISCONNECT TO LOAD PROGRAM TO ARDUINO.
//
SoftwareSerial HC05(0, 1);


char command;
int velocidad = 255;

void setup() {
  Serial.begin(9600);  // Serial monitor
  HC05.begin(9600);    //Set the baud rate
  // L298N
  pinMode(pinIN1, OUTPUT);
  pinMode(pinIN2, OUTPUT);
  pinMode(pinENA, OUTPUT);
  pinMode(pinIN3, OUTPUT);
  pinMode(pinIN4, OUTPUT);
  pinMode(pinENB, OUTPUT);
  // Horn
  pinMode(pinZumbador, OUTPUT);
  // Lights
  Timer1.initialize(666666);  //Set TIMER in 0.66 seconds
  Timer1.stop();
  Timer1.attachInterrupt(LucesEmergencia);  //Set ionterriuption in Timer 1



  // Lights
  pinMode(pinFaroDelanteroDerecho, OUTPUT);
  pinMode(pinFaroDelanteroIzquierdo, OUTPUT);
  pinMode(pinFaroTraseroDerecho, OUTPUT);
  pinMode(pinFaroTraseroIzquierdo, OUTPUT);
}

void loop() {
  if (HC05.available() > 0) {
    command = HC05.read();
    Serial.print(command);

    fullStop(pinMotorA);
    fullStop(pinMotorB);
    switch (command) {
      case 'F':  //
        moveForward(pinMotorA, velocidad);
        moveForward(pinMotorB, velocidad);
        break;
      case 'B':  //
        moveBackward(pinMotorA, velocidad);
        moveBackward(pinMotorB, velocidad);
        break;
      case 'L':  //
        moveForward(pinMotorA, velocidad);
        moveBackward(pinMotorB, velocidad);
        break;
      case 'R':  //
        moveBackward(pinMotorA, velocidad);
        moveForward(pinMotorB, velocidad);
        break;
      case 'I':  //
        fullStop(pinMotorA);
        moveForward(pinMotorB, velocidad);
        break;
      case 'J':  //
        fullStop(pinMotorA);
        moveBackward(pinMotorB, velocidad);
        break;
      case 'G':  //
        fullStop(pinMotorB);
        moveForward(pinMotorA, velocidad);
        break;
      case 'H':  //
        fullStop(pinMotorB);
        moveBackward(pinMotorA, velocidad);
        break;
      case 'q':
        velocidad = 255;
        break;
      case 'W':  // swith on front lights
        digitalWrite(pinFaroDelanteroDerecho, HIGH);
        digitalWrite(pinFaroDelanteroIzquierdo, HIGH);
        break;
      case 'w':  // switch off front lights
        digitalWrite(pinFaroDelanteroDerecho, LOW);
        digitalWrite(pinFaroDelanteroIzquierdo, LOW);
        break;
      case 'V':                  // horn
        tone(pinZumbador, 500);  // Tone 500 Hz
        break;
      case 'v':  // switch horn off
        noTone(pinZumbador);
        break;
      case 'X':  // emergency lights on
        Timer1.start();
        break;
      case 'x':  //    off
        Timer1.stop();
        digitalWrite(pinFaroDelanteroDerecho, LOW);
        digitalWrite(pinFaroDelanteroIzquierdo, LOW);
        digitalWrite(pinFaroTraseroDerecho, LOW);
        digitalWrite(pinFaroTraseroIzquierdo, LOW);
        break;
      case 'U':  // switch back lights on
        digitalWrite(pinFaroTraseroDerecho, HIGH);
        digitalWrite(pinFaroTraseroIzquierdo, HIGH);
        break;
      case 'u':  // switch back lights off
        digitalWrite(pinFaroTraseroDerecho, LOW);
        digitalWrite(pinFaroTraseroIzquierdo, LOW);
        break;
    }

    //if speed change
    if (command >= '1' && command <= '9') {

      int convertido = String(command).toInt();
      velocidad = convertido * 25.5;
    }
    //  
    //}
  }
}

void moveForward(const int pinMotor[3], int speed) {
  digitalWrite(pinMotor[1], HIGH);
  digitalWrite(pinMotor[2], LOW);
  analogWrite(pinMotor[0], speed);
}

void moveBackward(const int pinMotor[3], int speed) {
  digitalWrite(pinMotor[1], LOW);
  digitalWrite(pinMotor[2], HIGH);

  analogWrite(pinMotor[0], speed);
}



void fullStop(const int pinMotor[3]) {
  digitalWrite(pinMotor[1], LOW);
  digitalWrite(pinMotor[2], LOW);
  analogWrite(pinMotor[0], 0);
}

// 
void LucesEmergencia(void) {
  digitalWrite(pinFaroDelanteroDerecho, !digitalRead(pinFaroDelanteroDerecho));
  digitalWrite(pinFaroDelanteroIzquierdo, !digitalRead(pinFaroDelanteroIzquierdo));
  digitalWrite(pinFaroTraseroDerecho, !digitalRead(pinFaroTraseroDerecho));
  digitalWrite(pinFaroTraseroIzquierdo, !digitalRead(pinFaroTraseroIzquierdo));
}