#include <ESP32Servo.h>

const int PWM_channel_A = 2;     // On choisit le canal 0 pour le moteur A
const int PWM_channel_B = 4;    // On choisit le canal 2 pour le moteur B
const int PWM_channel_C = 6;    // On choisit le canal 2 pour le moteur C

const int resolution = 8;           // Résolution de 8 bits, dans notre cas ça n'a pas ou peu d'importance

int frequence_max = 1000;          // Nous initialisons la fréquence PWM du moteur de gauche à 1 KHz.
                                    // Notre moteur fera donc 5 tours par seconde

const int en_pin = 4;         // Constante contenant le numéro de broche sur laquelle est branchée l'entrée ENABLE du driver
const int ms1_pin = 21;
const int ms2_pin = 22;
const int ms3_int = 23; 


const int dir_pin_A = 15;        // Constante contenant le numéro de broche sur laquelle est branchée l'entrée DIR du driver
const int step_pin_A = 2;        // Constante contenant le numéro de broche sur laquelle est branchée l'entrée STEP du driver
                                    
const int dir_pin_B = 16;        // Constante contenant le numéro de broche sur laquelle est branchée l'entrée DIR du driver
const int step_pin_B = 17;       // Constante contenant le numéro de broche sur laquelle est branchée l'entrée STEP du driver

const int dir_pin_C = 18;        // Constante contenant le numéro de broche sur laquelle est branchée l'entrée DIR du driver
const int step_pin_C = 19;       // Constante contenant le numéro de broche sur laquelle est branchée l'entrée STEP du driver



// Only for debug purposes
const int yl_pin = 36;
const int xl_pin = 39;
const int sl_pin = 32;
const int yr_pin = 34;
const int xr_pin = 35;
const int sr_pin = 33; 

// Permet de récupérer les données structurées transmises par la télécommande
typedef struct{
  int xl;
  int yl;
  bool sl;
  int xr;
  int yr;
  bool sr;
}t_joyData;

// Déclaration de la variable de type t_joyData 
t_joyData joysticks;

bool button_etat = false;
long last_buttons_checking = 0;
bool ouvrir_pince=false;

long temps_moteur=0;
int pos=0;
int dir=0;

Servo myServo;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.println("Hello, ESP32!");

  // Nous paramétrons les broches permettant de piloter le moteur de gauche
  // On paramètre notre broche "DIR" en sortie
  pinMode(dir_pin_A,OUTPUT);
  pinMode(dir_pin_B,OUTPUT);
  pinMode(dir_pin_C,OUTPUT);

  // On paramètre notre broche "EN" en sortie
  pinMode(en_pin,OUTPUT);
  // On désactive l'alimentation de notre moteur (souvenez-vous l'entrée Enable du driver est en logique inverse)
  digitalWrite(en_pin, LOW);

  // On paramètre la sortie PWM qui pilote la broche "STEP" du moteur "A"
  ledcSetup(PWM_channel_A, frequence_max, resolution);
  // On lie la sortie PWM à la broche "STEP" du moteur "A"
  ledcAttachPin(step_pin_A, PWM_channel_A);

  // On paramètre la sortie PWM qui pilote la broche "STEP" du moteur "B"
  ledcSetup(PWM_channel_B, frequence_max, resolution);
  // On lie la sortie PWM à la broche "STEP" du moteur "B"
  ledcAttachPin(step_pin_B, PWM_channel_B);

  // On paramètre la sortie PWM qui pilote la broche "STEP" du moteur "C"
  ledcSetup(PWM_channel_C, frequence_max, resolution);
  // On lie la sortie PWM à la broche "STEP" du moteur "C"
  ledcAttachPin(step_pin_C, PWM_channel_C);


  // Only to debug purposes
  pinMode(yl_pin, INPUT);
  pinMode(xl_pin, INPUT);
  pinMode(sl_pin, INPUT);
  pinMode(yr_pin, INPUT);
  pinMode(xr_pin, INPUT);
  pinMode(sr_pin, INPUT);

  myServo.attach(12);
}

void loop() {
  // Appel de la fonction getRemoteData(). Cette fonction sert à "simuler" la réception des données de la télécommande
  getRemoteData();


    if(joysticks.yr>70){
        if(joysticks.xr<-70){
        ledcWriteTone(PWM_channel_B,frequence_max*0.6);
        ledcWriteTone(PWM_channel_A,frequence_max);
        }
        else if(joysticks.xr>70){
        ledcWriteTone(PWM_channel_A,frequence_max*0.6);
        ledcWriteTone(PWM_channel_B,frequence_max);
        }
        else{
        ledcWriteTone(PWM_channel_B,frequence_max);
        ledcWriteTone(PWM_channel_A,frequence_max);
        }

        // Nous activons l'alimentation électrique du moteur "left"
        digitalWrite(en_pin,LOW);
        digitalWrite(en_pin,LOW);

        // Conformément à ce qui est indiqué dans le diagramme qui vous est fourni, nous passons l'état de la broche
        // gérant le sens de rotation à "HIGH"
        digitalWrite(dir_pin_B,HIGH);
        digitalWrite(dir_pin_A,LOW);
        // Nous utilisons un délai afin de permettre le bon fonctionnement du programme
        // Sans ce-dernier celui-ci ne fonctionne pas convenablement
        delay(50);
    }
    else if (joysticks.yr<-70){
      if(joysticks.xr<-70){
        ledcWriteTone(PWM_channel_B,frequence_max*0.6);
        ledcWriteTone(PWM_channel_A,frequence_max);
        }
        else if(joysticks.xr>70){
        ledcWriteTone(PWM_channel_A,frequence_max*0.6);
        ledcWriteTone(PWM_channel_B,frequence_max);        
        }
        else{
        ledcWriteTone(PWM_channel_B,frequence_max);
        ledcWriteTone(PWM_channel_A,frequence_max);
        }

        // Nous activons l'alimentation électrique du moteur "left"
        digitalWrite(en_pin,LOW);
        digitalWrite(en_pin,LOW);

        digitalWrite(dir_pin_B,LOW);
        digitalWrite(dir_pin_A,HIGH);

        delay(50);
    }
    else if (joysticks.xr<-70){
        // Nous modifions la fréquence de la broche PWM correspondant au moteur "left"
        ledcWriteTone(PWM_channel_B,frequence_max);
        ledcWriteTone(PWM_channel_A,frequence_max);

        // Nous activons l'alimentation électrique du moteur "left"
        digitalWrite(en_pin,LOW);
        digitalWrite(en_pin,LOW);

        digitalWrite(dir_pin_B,HIGH);
        digitalWrite(dir_pin_A,HIGH);

        delay(50);
    }
    else if (joysticks.xr>70){
        // Nous modifions la fréquence de la broche PWM correspondant au moteur "left"
        ledcWriteTone(PWM_channel_B,frequence_max);
        ledcWriteTone(PWM_channel_A,frequence_max);
        // Nous activons l'alimentation électrique du moteur "left"
        digitalWrite(en_pin,LOW);
        digitalWrite(en_pin,LOW);

        digitalWrite(dir_pin_B,LOW);
        digitalWrite(dir_pin_A,LOW);

        delay(50);
    }
    else{
        digitalWrite(en_pin,HIGH);
        digitalWrite(en_pin,HIGH);
        // Nous utilisons un délai afin de permettre le bon fonctionnement du programme
        // Sans ce-dernier celui-ci ne fonctionne pas convenablement
        delay(50);
    }



    if(millis()-last_buttons_checking >= 50){
        if(joysticks.sl == LOW && button_etat == false){
            // Nous stockons le nouvel état du bouton, cela nous servira à n'incrémenter qu'une seule fois
            button_etat = true;
            ouvrir_pince=!ouvrir_pince;
        }
        else if(joysticks.sl == HIGH && button_etat == true){
            button_etat= false;
        }
        last_buttons_checking = millis();
    }

    if(millis() - temps_moteur >= 5){
    if(pos<=180 and ouvrir_pince==true){
      pos+=2;
      dir=0;
    }
    else if (pos>=0 and ouvrir_pince==false){
      pos-=2;
      dir=1;
    }
    myServo.write(pos);
    temps_moteur=millis();
  }

  
  
}



void getRemoteData(){
  //t_joyData buf;
  joysticks.yl = map(analogRead(yl_pin),0,4095,-100,100);
  joysticks.xl = map(analogRead(xl_pin),0,4095,-100,100);

  joysticks.yr = map(analogRead(yr_pin),0,4095,-100,100);
  joysticks.xr = map(analogRead(xr_pin),0,4095,-100,100);
  
  joysticks.sl = digitalRead(sl_pin);
  joysticks.sr = digitalRead(sr_pin);

  //return buf;
}