#include "mbed.h"
#include <cstdint>
#define W_MSA 10 //duree de chaque periode
PwmOut mot1Hor(PA_15); 
PwmOut mot1Trig(PA_10);
PwmOut mot2Hor(PB_13);
PwmOut mot2Trig(PB_14);
PwmOut mot3Hor(PC_9);
PwmOut mot3Trig(PB_8);
void destections(void); //fonction de detection
AnalogIn Capteur1(PC_2);
AnalogIn Capteur2(PC_3);
AnalogIn Capteur3(PC_0);
DigitalOut Ledalacon(LED1);//vestige deboggage
DigitalOut EnaAutorisation(PB_3); //pin sur l'appareil qui permet d'utiliser les moteurs
double maxcool(double*L,int N);
Ticker detection;
double significatif=0.4;
double rcR=0.2;
double rcM=0.3; //initialement il n'y avait qu'un seul rc mais le robot prenait trop de temps a capter
double seuil=0.25; //desuet
void Marche(void); //pour marcher
bool superieur(double a1,double a2); //desuet
void Horiii(void);//desuet
void Trigoooo(void);
Ticker teraterm;//desuet mais utile pour debogger
int main()
{
    EnaAutorisation=1;//indispensable pour rouler
    mot1Hor.period_ms(W_MSA);
    mot1Trig.period_ms(W_MSA);
    mot2Hor.period_ms(W_MSA);
    mot2Trig.period_ms(W_MSA);
    mot3Hor.period_ms(W_MSA);
    mot3Trig.period_ms(W_MSA);
    mot1Hor.write(rcR);
    mot1Trig.write(0);
    mot2Hor.write(rcR);
    mot2Trig.write(0);
    mot3Hor.write(rcR);
    mot3Trig.write(0); //le robot tourne initialement dans le sens horaire, mais plus jamais, c'est une sorte de "logo" de demarrage
    detection.attach(&destections,1); //destections est appelee toutes les millisecondes
    while (true)
    {
    }
}

double maxcool(double*L,int N){
    double M=*L;
    for (int i=1;i<N;i++){
        if (M<*(L+i)){
            M=*(L+i);
        }
    }
    return M;
}

void Marche(int i){
    double a=rcM; //la variable a sert a copier coller plus facilement les codes
    if (i==0){ //va dans le sens du capteur 1
    mot2Hor.write(0);
    mot2Trig.write(a);
    mot3Trig.write(0);
    mot3Hor.write(a);
    mot1Trig.write(0);
    mot1Hor.write(0);
    }
    else if (i==1){ //va dans le sens du capteur 2
    mot3Hor.write(0);
    mot3Trig.write(a);
    mot1Trig.write(0);
    mot1Hor.write(a);
    mot2Trig.write(0);
    mot2Hor.write(0);
}
    else if (i==2){  //va dans le sens du capteur 3
    mot1Hor.write(0);
    mot1Trig.write(a);
    mot2Trig.write(0);
    mot2Hor.write(a);
    mot3Trig.write(0);
    mot3Hor.write(0);
}
}

void destections(void){
    double u[3]={Capteur1,Capteur2,Capteur3};
    int k=0;
    double MI=Capteur1;
    for (int i=1;i<3;i++){//malheureusement la fonction maxcool ne donne pas d'indice
        if (MI<u[i]){
                k=i;
                MI=u[k];
            }
        }
    if(maxcool(u,3)>significatif){ //seuil de marche
            Marche(k); //se dirige vers le capteur qui capte le plus de lumiere
        }
    else{
        Trigoooo(); //recherche de lumiere, cela permet de s'aligner a la source de lumiere
    }
}


void Trigoooo(void){
    double a=rcR;
    mot1Trig.write(a);
    mot2Trig.write(a);
    mot3Trig.write(a);
    mot1Hor.write(0);
    mot2Hor.write(0);
    mot3Hor.write(0);
}

void Horiii(void){
    double a=rcR;
    mot1Trig.write(0);
    mot2Trig.write(0);
    mot3Trig.write(0);
    mot1Hor.write(a);
    mot2Hor.write(a);
    mot3Hor.write(a);
}

bool superieur(double T1,double T2){
    if (T1-seuil>T2){ //superiorite un peu plus stricte
        return 1;
    }
    return 0;
}