#include <ESP32Servo.h>

const int PWM_channel_left = 2;     // On choisit le canal 0 pour le moteur de gauche
const int PWM_channel_right = 4;    // On choisit le canal 2 pour le moteur de droite
const int resolution = 8;           // Résolution de 8 bits, dans notre cas ça n'a pas ou peu d'importance

int frequence_left = 1000;          // Nous initialisons la fréquence PWM du moteur de gauche à 1 KHz.
                                    
int frequence_right = 1000;         // Nous initialisons la fréquence PWM du moteur de droite à 1 KHz 

const int dir_pin_left = 21;        // Constante contenant le numéro de broche sur laquelle est branchée l'entrée DIR du driver
const int step_pin_left = 22;       // Constante contenant le numéro de broche sur laquelle est branchée l'entrée STEP du driver
const int en_pin_left = 23;         // Constante contenant le numéro de broche sur laquelle est branchée l'entrée ENABLE du driver

const int dir_pin_right = 15;        // Constante contenant le numéro de broche sur laquelle est branchée l'entrée DIR du driver
const int step_pin_right = 2;        // Constante contenant le numéro de broche sur laquelle est branchée l'entrée STEP du driver
const int en_pin_right = 26;         // Constante contenant le numéro de broche sur laquelle est branchée l'entrée ENABLE du driver

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

const int L0=0.125;
int omega = 0; 

#define VERT_PIN 14
#define HORZ_PIN 27
#define SEL_PIN 33

const int servoPin=13;

const int sl_pin = 32;
bool button_etat = false;
long last_buttons_checking = 0;
bool ouvrir_pince=false;

Servo myServo;

void setup(){
    // On initialise la communication série (baudrate = 115200)
    Serial.begin(115200);

    pinMode(VERT_PIN, INPUT);
    pinMode(HORZ_PIN, INPUT);
    pinMode(SEL_PIN, INPUT_PULLUP);

    // Nous paramétrons les broches permettant de piloter le moteur de gauche
    // On paramètre notre broche "DIR" en sortie
    pinMode(dir_pin_left,OUTPUT);
    // On paramètre notre broche "STEP" en sortie
//    pinMode(step_pin_left,OUTPUT);    
    // On paramètre notre broche "EN" en sortie
    pinMode(en_pin_left,OUTPUT);
    // On désactive l'alimentation de notre moteur (souvenez-vous l'entrée Enable du driver est en logique inverse)
    digitalWrite(en_pin_left, HIGH);

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

    // On paramètre la sortie PWM qui pilote la broche "STEP" du moteur "left"
    ledcSetup(PWM_channel_left, frequence_left, resolution);
    // On lie la sortie PWM à la broche "STEP" du moteur "left"
    ledcAttachPin(step_pin_left, PWM_channel_left);

    ledcSetup(PWM_channel_right, frequence_right, resolution);
    // On lie la sortie PWM à la broche "STEP" du moteur "left"
    ledcAttachPin(step_pin_right, PWM_channel_right);

   pinMode(sl_pin, INPUT_PULLUP);
    
    myServo.attach(servoPin);
}

void loop(){
  int horz = map(analogRead(HORZ_PIN), 0, 4095, -100, 100);
  int vert = map(analogRead(VERT_PIN), 0, 4095, -100, 100);
  // Serial.println(vert);
  // vert = map
    // Nous vérifions si le bouton correspondant à l'action souhaitée est pressé
    // Si c'est le cas nous exécutons les instructions contenues entres les accolades de notre condition
    if(vert>70){
        if(horz<-70){
        ledcWriteTone(PWM_channel_left,frequence_left*0.6);
        ledcWriteTone(PWM_channel_right,frequence_right);
        }
        else if(horz>70){
        ledcWriteTone(PWM_channel_right,frequence_right*0.6);
        ledcWriteTone(PWM_channel_left,frequence_left);
        }
        else{
        ledcWriteTone(PWM_channel_left,frequence_left);
        ledcWriteTone(PWM_channel_right,frequence_right);
        }

        // Nous activons l'alimentation électrique du moteur "left"
        digitalWrite(en_pin_left,LOW);
        digitalWrite(en_pin_right,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_left,HIGH);
        digitalWrite(dir_pin_right,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 (vert<-70){
      if(horz<-70){
        ledcWriteTone(PWM_channel_left,frequence_left*0.6);
        ledcWriteTone(PWM_channel_right,frequence_right);
        }
        else if(horz>70){
        ledcWriteTone(PWM_channel_right,frequence_right*0.6);
        ledcWriteTone(PWM_channel_left,frequence_left);        
        }
        else{
        ledcWriteTone(PWM_channel_left,frequence_left);
        ledcWriteTone(PWM_channel_right,frequence_right);
        }

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

        digitalWrite(dir_pin_left,LOW);
        digitalWrite(dir_pin_right,HIGH);

        delay(50);
    }
    else if (horz<-70){
        // Nous modifions la fréquence de la broche PWM correspondant au moteur "left"
        ledcWriteTone(PWM_channel_left,frequence_left);
        ledcWriteTone(PWM_channel_right,frequence_right);

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

        digitalWrite(dir_pin_left,HIGH);
        digitalWrite(dir_pin_right,HIGH);

        delay(50);
    }
    else if (horz>70){
        // Nous modifions la fréquence de la broche PWM correspondant au moteur "left"
        ledcWriteTone(PWM_channel_left,frequence_left);
        ledcWriteTone(PWM_channel_right,frequence_right);
        // Nous activons l'alimentation électrique du moteur "left"
        digitalWrite(en_pin_left,LOW);
        digitalWrite(en_pin_right,LOW);

        digitalWrite(dir_pin_left,LOW);
        digitalWrite(dir_pin_right,LOW);

        delay(50);
    }
    else{
        digitalWrite(en_pin_left,HIGH);
        digitalWrite(en_pin_right,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(digitalRead(sl_pin) == 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(digitalRead(sl_pin) == 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();
  }
}