#include <ESP32Servo.h> //Library for the servo
#include "BluetoothSerial.h" //library for the bluetooth device

String device_name = "ESP32-BT";

BluetoothSerial SerialBT;

//The basic esp32 gives 3.3V in its VCC pin.
//The idea for the project is using two ESP32 devices, one for the
//joysticks and another one for the control of the dc motor and the
//servo motor of the direction.
//In this script we are going to program the joysticks and send the
//necessary data to the other ESP32 via bluetooth.
int pin32 = 32; //pin used for the right-left joystick
int pin33 = 33; //pin used for the forward-backward joystick

//The input pins have 12bit resolution (0-4095).
int rljoy = 0; //variables to store the value read for the joysticks
int fbjoy = 0; //

int num1 = 0; //variables for counting the digits of the data to send
int num2 = 0;
int rest1 = 1;
int rest2 = 1;

//Variables for servo
int pinServo = 16;
int freqServo = 50; //50Hz
int channelServo; //We choose channel 0 for servo
float range = 180;
int pos = 0; //Position of the servo in degrees
Servo servo;

//Variables for the control of the ESC
int freqESC = 1000; //1KHz for operation
int channelESC = 1; //Channels for PWM are from 0 to 15. We choose 0 for the ESC
int res = 8; //Resolution 8 bits => Duty is 0-255
int pinESC = 17;
int vel = 0;


void setup() {
  //We set them as inputs because they have to read the value
  pinMode(pin33,INPUT);
  pinMode(pin32,INPUT);

  //Variables for the ESC
  pinMode(pinESC, OUTPUT);
  ledcSetup(channelESC, freqESC, res);
  ledcAttachPin(pinESC,channelESC);

  //Variables for the servo
  servo.setPeriodHertz(freqServo); //50 Hz standard servo
  channelServo = servo.attach(pinServo); //This function assigns the next free channel for the pwm
  

  Serial.begin(115200);
  //SerialBT.begin(device_name); //Bluetooth device name
  //Serial.printf("The device is started. Now you can pair it with Bluetooth!\n");



}

void loop() {
  rljoy = analogRead(pin32);
  fbjoy = analogRead(pin33);
  
  //PWM pins go from 0 to 255 (because we choose 8 bits resol), so we have to remap the output
  pos = map(rljoy,0,4095,range,0);
  //vel = map(fbjoy,0,4095,-100,100);

  //printf("LR: %d \n",rlmap);
  //printf("FB: %d \n",fbmap);

  //The idea is to send these values via bluetooth to the other ESP32
  //However, first we are going to try to control the servo using the
  //same ESP32. Omitting the delivery of data. However, this is the
  //code that we should write in the other ESP32
  //In the real ESP32 we don't have the 5V supply. Instead we will use
  //an external power supply, but here we can't do that so we will
  //use the 5V supply form the ESP32 (although it is not available irl)
  //We have to consider that the servo used in simulation is 0-180º, but
  //the servo used irl is 0-202º according to datasheet
  
  servo.write(pos);
  //The only thing that we have to do is to write the value of position mapped
  //from the value obtained from the joystick.

  rest1 = rljoy;
  rest2 = fbjoy;
  //The following loops are used to count the number of digits that each data has
  if(rest1 == 0){
    num1 = 1;
  }
  else{
    while(rest1>0){ 
      rest1 = rest1/10;
      num1++;
  }
  }
  
  if(rest2 == 0){
    num2 = 1;
  }
  else{
    while(rest2>0){
     rest2 = rest2/10;
     num2++;
  }
  }

  //Now we are going to do the delivery of data via bluetooth.
  //To debug, we are going to send the data to a mobile app to check if
  //everything is going well.
 //The info that will be delivered is the lectures from both joysticks.
  Serial.print(";");
  Serial.print(num1);
  Serial.print(rljoy);
  Serial.print(num2);
  Serial.print(fbjoy);
  
  //Serial.print(pos);
  Serial.print("\n"); //This line is only for debugging!!!

  num1 = 0;
  num2 = 0;

  delay(10); // this speeds up the simulation
}