#include <ESP32Servo.h>
#include "BluetoothSerial.h"

double timer;
double manual;
String s_bluetooth;
Servo servo_17;
BluetoothSerial bt_serial;
unsigned long time_timer=millis();

void frenar() {
	digitalWrite(32,LOW);
	digitalWrite(33,LOW);
	analogWrite(4,(uint16_t)255);
	digitalWrite(26,LOW);
	digitalWrite(27,LOW);
	analogWrite(4,(uint16_t)255);
}
void acelerar() {
	digitalWrite(32,HIGH);
	digitalWrite(33,LOW);
	analogWrite(4,(uint16_t)255);
	digitalWrite(26,HIGH);
	digitalWrite(27,LOW);
	analogWrite(4,(uint16_t)255);
}
void reversa() {
	digitalWrite(32,LOW);
	digitalWrite(33,HIGH);
	analogWrite(4,(uint16_t)255);
	digitalWrite(26,LOW);
	digitalWrite(27,HIGH);
	analogWrite(4,(uint16_t)255);
}
void izquierda() {
	digitalWrite(32,HIGH);
	digitalWrite(33,LOW);
	analogWrite(4,(uint16_t)255);
	digitalWrite(26,LOW);
	digitalWrite(27,HIGH);
	analogWrite(4,(uint16_t)255);
}
void derecha() {
	digitalWrite(32,LOW);
	digitalWrite(33,HIGH);
	analogWrite(4,(uint16_t)255);
	digitalWrite(26,HIGH);
	digitalWrite(27,LOW);
	analogWrite(4,(uint16_t)255);
}

void setup()
{
  	servo_17.attach(17);
	pinMode(32, OUTPUT);
	pinMode(33, OUTPUT);
	pinMode(4, OUTPUT);
	pinMode(26, OUTPUT);
	pinMode(27, OUTPUT);
	pinMode(16, INPUT);

	//bluetooth classic slave
	bt_serial.begin(String("AgroBit").c_str());

	Serial.begin(115200);
	Serial.flush();
	while(Serial.available()>0)Serial.read();

	servo_17.write(90);
	digitalWrite(32,LOW);
	digitalWrite(33,LOW);
	analogWrite(4,(uint16_t)255);
	digitalWrite(26,LOW);
	digitalWrite(27,LOW);
	analogWrite(4,(uint16_t)255);
	manual = 1;
	timer = 30;
	s_bluetooth = String("S");

}


void loop()
{
	yield();

  	if (((Serial.available()>0) == true)) {
  		s_bluetooth = Serial.readString();
  		timer = ((millis()-time_timer)/1000);
  		manual = 1;
  		Serial.println(String("Comando Recibido: ")+String(s_bluetooth));
  	}

  	if (((bt_serial.available()>0) == true)) {
  		s_bluetooth = bt_serial.readString();
  		timer = ((millis()-time_timer)/1000);
  		manual = 1;
  		Serial.println(String("Comando Recibido: ")+String(s_bluetooth));
  	}

  	if ((manual == 1)) {
  		if (String(s_bluetooth).equals(String("S"))) {
  			frenar();
  		}

  		if (String(s_bluetooth).equals(String("F"))) {
  			acelerar();
  		}

  		if (String(s_bluetooth).equals(String("B"))) {
  			reversa();
  		}

  		if (String(s_bluetooth).equals(String("L"))) {
  			izquierda();
  		}

  		if (String(s_bluetooth).equals(String("R"))) {
  			derecha();
  		}

  		if (String(s_bluetooth).equals(String("V"))) {
  			servo_17.write(145);
  		}

  		if (String(s_bluetooth).equals(String("v"))) {
  			servo_17.write(90);
  		}

  	}

  	if ((manual == 0)) {
  		servo_17.write(145);
  		acelerar();
  		if ((digitalRead(16) == true)) {
  			servo_17.write(90);
  			frenar();
  		}

  	}

  	if (((timer + 30) < ((millis()-time_timer)/1000))) {
  		manual = 0;
  	}

}