// Funzioni comandi F,R,B,L
// Ognuno di questi comandi ha uno stato vero o falso
// Si definiscono 2 valori di tensione per cui i motori si movimentano "bene" e "meglio"
float VH = 3.3; //V
float VM = 2.7; //V
float VL = 0.0; //V
// Se tutti i comandi sono falsi in input, allora il rovere si ferma
// Se F>0 va avanti
// se F>0 AND R>0 allora M== (gira verso destra)


//----------------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------------

int PIN_M1Rr = 17; //GPIO pin Motore 1 destro
int PIN_M1Rb = 16; //GPIO pin Motore 1 destro
int PIN_M2Rr =  2; //...GPIO pin Motore 2 destro
int PIN_M2Rb = 16; //...GPIO pin Motore 2 destro

int PIN_M1Lr = 25; //GPIO pin Motore 1 sinistro
int PIN_M1Lb = 26; //GPIO pin Motore 1 sinistro
int PIN_M2Lr = 27; //...GPIO pin Motore 2 sinistro
int PIN_M2Lb = 14; //...GPIO pin Motore 2 sinistro


// Definizione dei parametri PWM
const int PWM_FREQ      = 5000; // Hz, Arduino Uno is ~490 Hz. Official ESP32 example uses 5kHz
const int PWM_RES       =    8; // Risoluzione PWM a 8 bit, PWM resolution (1 - 16 bits) 
const int MAX_DUTY_CYCLE = (0x01 << PWM_RES) - 1; 
//const int dutyCycleMax  =  255; // 3.3V
const int dutyCycleMax  =  MAX_DUTY_CYCLE; // 3.3V
//const int dutyCycleMild =  209; // 2.7V calcolato come (2.7/3.3) * 255
const int dutyCycleMild = (int)(MAX_DUTY_CYCLE * (VM / VH));; //(int)(255.0 * (2.7 / 3.3)); // Converti 2.7V in duty cycle
const int PWM_DELAY_MS  =   10; //5; //80;  // delay between increments in ms

// Canali LEDC per ESP32
const int channelRightRed   = 0; // 16 canali, da 0 a 15
const int channelRightBlack = 1; // 16 canali, da 0 a 15
const int channelLeftRed    = 2; // 16 canali, da 0 a 15
const int channelLeftBlack  = 3; // 16 canali, da 0 a 15


void moveRobot(bool varF, bool varR, bool varB, bool varL) {
    // Escludiamo combinazioni opposte
    if (varF && varB) varF = varB = false;
    if (varR && varL) varR = varL = false;
    int pwmRightRed = 0, pwmRightBlack = 0;
    int pwmLeftRed  = 0, pwmLeftBlack  = 0;

    if (varF && varR) {
        pwmRightRed   = dutyCycleMild;
        pwmRightBlack = 0;
        pwmLeftRed    = dutyCycleMax;
        pwmLeftBlack  = 0;
    } else if (varF && varL) {
        pwmRightRed   = dutyCycleMax;
        pwmRightBlack = 0;
        pwmLeftRed    = dutyCycleMild;
        pwmLeftBlack  = 0;
    } else if (varB && varR) {
        pwmRightRed   = dutyCycleMild;
        pwmRightBlack = 0;
        pwmLeftRed    = 0;
        pwmLeftBlack  = dutyCycleMax;
    } else if (varB && varL) {
        pwmRightRed   = 0;
        pwmRightBlack = dutyCycleMax;
        pwmLeftRed    = dutyCycleMild;
        pwmLeftBlack  = 0;
    } else if (varF) { //stato singolo
        pwmRightRed   = dutyCycleMax;
        pwmRightBlack = 0;
        pwmLeftRed    = dutyCycleMax;
        pwmLeftBlack  = 0;
    } else if (varB) { //stato singolo
        pwmRightRed   = 0;
        pwmRightBlack = dutyCycleMax;
        pwmLeftRed    = 0;
        pwmLeftBlack  = dutyCycleMax;
    } else if (varR) { //stato singolo
        pwmRightRed   = 0;
        pwmRightBlack = dutyCycleMild;
        pwmLeftRed    = dutyCycleMild;
        pwmLeftBlack  = 0;
    } else if (varL) { //stato singolo
        pwmRightRed   = dutyCycleMild;
        pwmRightBlack = 0;
        pwmLeftRed    = 0;
        pwmLeftBlack  = dutyCycleMild;
    }

    // Scrittura dei valori PWM
    ledcWrite(channelRightRed,   pwmRightRed);
    ledcWrite(channelRightBlack, pwmRightBlack);
    ledcWrite(channelLeftRed,    pwmLeftRed);
    ledcWrite(channelLeftBlack,  pwmLeftBlack);

    //Introdurre il bloccaggio dei motori con tenendoli in stallo !


    delay(PWM_DELAY_MS);
}








void setup() {



  ledcSetup(channelRightRed, PWM_FREQ, PWM_RES);
  ledcAttachPin(PIN_M1Rr, channelRightRed);
  ledcSetup(channelRightBlack, PWM_FREQ, PWM_RES);
  ledcAttachPin(PIN_M1Rb, channelRightBlack);
  ledcSetup(channelLeftRed, PWM_FREQ, PWM_RES);
  ledcAttachPin(PIN_M1Lr, channelLeftRed);
  ledcSetup(channelLeftBlack, PWM_FREQ, PWM_RES);
  ledcAttachPin(PIN_M1Lb, channelLeftBlack);



}

void loop() {


  // Simulazione lettura dei pulsanti (da sostituire con digitalRead dai pulsanti reali)
  bool varF = false, varR = false, varB = false, varL = false;
  moveRobot(varF, varR, varB, varL); // Chiamata della funzione di controllo motori
  delay(1000);

  varF = true, varR = false, varB = false, varL = false;
  moveRobot(varF, varR, varB, varL);
  delay(4000);

  varF = true, varR = true, varB = false, varL = false;
  moveRobot(varF, varR, varB, varL);
  delay(4000);

  varF = true, varR = true, varB = true, varL = false;
  moveRobot(varF, varR, varB, varL);
  delay(4000);

  varF = true, varR = true, varB = true, varL = true;
  moveRobot(varF, varR, varB, varL);
  delay(4000);

  delay(DELAY_MS);


}