// ;  Pins capteurs de couleur
#define DROITE A0
#define GAUCHE A1




// Pins carte de commande (moteur n°1)
#define PWM_GAUCHE  5
#define SENS_GAUCHE 4
//                        (moteur n°2)
#define PWM_DROIT   6
#define SENS_DROIT  7

#define SIZE 60

unsigned int droite;
unsigned int gauche;
unsigned int cpt;

unsigned int queue_droite[SIZE];
unsigned int queue_gauche[SIZE];

unsigned int c= 0;

void setup() {


    cpt = 0;


    // Capteur
    pinMode(GAUCHE, INPUT);
    pinMode(DROITE, INPUT);


    // Carte Commande
    pinMode(PWM_GAUCHE, OUTPUT);
    pinMode(SENS_GAUCHE, OUTPUT);
    pinMode(PWM_DROIT, OUTPUT);
    pinMode(SENS_DROIT, OUTPUT);


    // Affichage série
    Serial.begin(115200);
}


void loop() {
    c++;
    // Lecture capteur de couleur
    gauche = analogRead(GAUCHE);
    droite = analogRead(DROITE);

    /*  Zone noires, avancer
     *  Le robot est sur la ligne
     */
    if(gauche > 700 && droite > 700)
      {
          vitesseGauche(180);
          vitesseDroit(180);
      }


    /* Capteur gauche --> blanc : tourner à droite
     *  Le robot est à gauche de la ligne
     */
    if(gauche < 200 && droite > 700)
      {
          vitesseGauche(150*0.9);
          vitesseDroit(-70);
      }


    /* Capteur droit --> blanc : tourner à gauche
     *  Le robot est à droite de la ligne
     */
    if(gauche > 700 && droite < 200)
      {
          vitesseGauche(-70);
          vitesseDroit(150);
      }  


    /* Bleu : demi tour et arret=*/
    if(gauche > 380 && gauche < 570 && droite > 380 && droite < 570)
      {
        if (cpt == 0) {
            demiTour();
            cpt = 1;
        } else if (cpt == 1) {
            vitesseDroit(0);
            vitesseGauche(0);
        }
          
      }
}


// Controle du sens et de la vitesse du moteur Gauche
void vitesseGauche(int vitesse) {
    if (vitesse >= 0) {
        digitalWrite(SENS_GAUCHE, HIGH);
        analogWrite(PWM_GAUCHE, vitesse);
    } else {
        digitalWrite(SENS_GAUCHE, LOW);
        analogWrite(PWM_GAUCHE, -vitesse);
    }
}


// Controle du sens et de la vitesse du moteur Droit
void vitesseDroit(int vitesse) {
    if (vitesse >= 0) {
        digitalWrite(SENS_DROIT, HIGH);
        analogWrite(PWM_DROIT, vitesse);
    } else {
        digitalWrite(SENS_DROIT, LOW);
        analogWrite(PWM_DROIT, -vitesse);
    }
}


void demiTour() {
    while(gauche < 700) {
        gauche = analogRead(GAUCHE);
        vitesseDroit(180);
        vitesseGauche(-180);
    }
}