#include "bsp.h"

int timerGlobal;
state_bateau_t state_bateau;
state_radio_bateau_t state_radio_bateau;
state_voiture_t state_voiture;
state_radio_voiture_t state_radio_voiture;
esp_mqtt_client_handle_t clientMqtt;
int dataReceived,gazReceived,dirReceived;
bool voitureGO;
bool bateauGO;

// WIFI_PS_NONE WIFI_PS_MIN_MODEM
void setup_bateau(void){
wifi_init_sta(networkType);
    esp_mqtt_client_config_t mqtt_cfg = {
        .broker.address.uri = CONFIG_BROKER_URL_BATEAU,
    };
    clientMqtt = esp_mqtt_client_init(&mqtt_cfg);
    /* The last argument may be used to pass data to the event handler, in this example mqtt_event_handler */
    esp_mqtt_client_register_event(clientMqtt, ESP_EVENT_ANY_ID, mqtt_event_handler_bateau, NULL);
    esp_mqtt_client_start(clientMqtt);

}
void setup_radio_bateau(void){
wifi_init_sta(networkType);
    esp_mqtt_client_config_t mqtt_cfg = {
        .broker.address.uri = CONFIG_BROKER_URL_BATEAU,
    };
    clientMqtt = esp_mqtt_client_init(&mqtt_cfg);
    /* The last argument may be used to pass data to the event handler, in this example mqtt_event_handler */
    esp_mqtt_client_register_event(clientMqtt, ESP_EVENT_ANY_ID, mqtt_event_handler_radio_bateau, NULL);
    esp_mqtt_client_start(clientMqtt);

}

void setup_starter_bateau(void){
wifi_init_sta(networkType);
    esp_mqtt_client_config_t mqtt_cfg = {
        .broker.address.uri = CONFIG_BROKER_URL_BATEAU,
    };
     clientMqtt = esp_mqtt_client_init(&mqtt_cfg);
    /* The last argument may be used to pass data to the event handler, in this example mqtt_event_handler */
    esp_mqtt_client_register_event(clientMqtt, ESP_EVENT_ANY_ID, mqtt_event_handler_starter_bateau, NULL);
    esp_mqtt_client_start(clientMqtt);
    bateauGO=false;
}
void setup_voiture(void){
/*
   static const char *TAG = "BATEAU";
    int i;
    while(1)
    {
        for(i=0;i<1001;i+=100)
        {
        setMotor(i,false,i,true);
        ESP_LOGI(TAG, "speed=%d", i);
        vTaskDelay(2000 / portTICK_PERIOD_MS);
        }
        for(i=0;i<1001;i+=100)
        {
        setMotor(i,true,i,false);
        ESP_LOGI(TAG, "speed=%d", i);
        vTaskDelay(2000 / portTICK_PERIOD_MS);
        }
    }//*/
wifi_init_sta(networkType);
    esp_mqtt_client_config_t mqtt_cfg = {
        .broker.address.uri = CONFIG_BROKER_URL_VOITURE,
    };
     clientMqtt = esp_mqtt_client_init(&mqtt_cfg);
    /* The last argument may be used to pass data to the event handler, in this example mqtt_event_handler */
    esp_mqtt_client_register_event(clientMqtt, ESP_EVENT_ANY_ID, mqtt_event_handler_voiture, NULL);
    esp_mqtt_client_start(clientMqtt);
    voitureGO=false;
   // setMotor(6000,false,6000,true);;
}
void setup_radio_voiture(void){
wifi_init_sta(networkType);
    esp_mqtt_client_config_t mqtt_cfg = {
        .broker.address.uri = CONFIG_BROKER_URL_VOITURE,
    };
     clientMqtt = esp_mqtt_client_init(&mqtt_cfg);
    /* The last argument may be used to pass data to the event handler, in this example mqtt_event_handler */
    esp_mqtt_client_register_event(clientMqtt, ESP_EVENT_ANY_ID, mqtt_event_handler_radio_voiture, NULL);
    esp_mqtt_client_start(clientMqtt);
    voitureGO=false;
}

void setup_starter_voiture(void){
wifi_init_sta(networkType);
    esp_mqtt_client_config_t mqtt_cfg = {
        .broker.address.uri = CONFIG_BROKER_URL_VOITURE,
    };
     clientMqtt = esp_mqtt_client_init(&mqtt_cfg);
    /* The last argument may be used to pass data to the event handler, in this example mqtt_event_handler */
    esp_mqtt_client_register_event(clientMqtt, ESP_EVENT_ANY_ID, mqtt_event_handler_starter_voiture, NULL);
    esp_mqtt_client_start(clientMqtt);
    voitureGO=false;
}

void loop_bateau(void ){


    static const char *TAG = "BATEAU";
    char battStr[5],topicBatt[SIZE_TOPIC];
    char data[2];
    int dir,thr,batt;
    int M1,M2;
    uint32_t speedM1, speedM2;
    bool dirM1,dirM2;
    uint32_t timer=0;
    int msg_id;

    dataReceived=0;
  sprintf(topicBatt,"/B/%02d/%03d/Batt/rx",idGroup,idModele);
      
    while(1){
        switch(state_bateau){
            case BATEAU_RECEIVING:
                if(dataReceived){
                    dir=dirReceived;
                    thr=gazReceived;
                    dataReceived=0;
                    M1=(-thr+dir)*5;
                    if(M1>MOTOR_MAX_VALUE)M1=MOTOR_MAX_VALUE;
                    if(M1<-MOTOR_MAX_VALUE)M1=-MOTOR_MAX_VALUE;
                    M2=-(-thr-dir)*5;
                    if(M2>MOTOR_MAX_VALUE)M2=MOTOR_MAX_VALUE;
                    if(M2<-MOTOR_MAX_VALUE)M2=-MOTOR_MAX_VALUE;
                    if(M1>0){
                        speedM1=M1;
                        dirM1=true;
                    }
                    else{
                        speedM1=-M1;
                        dirM1=false;
                    }
                    if(M2>0){
                        speedM2=M2;
                        dirM2=true;
                    }
                    else{
                        speedM2=-M2;
                        dirM2=false;
                    }
                    setMotor(speedM1,dirM1,speedM2,dirM2);
                    ESP_LOGI(TAG, "DIR=%d\tTHR=%d\tM1=%d\tM2=%dspeedM1=%lu\tdirM1=%s\tspeedM2=%lu\tdirM2=%s\t", dir,thr,M1,M2,speedM1,dirM1 ? "true" : "false",speedM2,dirM2 ? "true" : "false");
                    timer=0;
                }
                else{
                    if(timer++>10){// timeout de 1 seconde
                    esp_wifi_set_ps(WIFI_PS_MIN_MODEM);//minimise la conso, augmente la latence
                    state_bateau=BATEAU_SLEEP;
                    setMotor(0,dirM1,0,dirM2);
                    vTaskDelay(TRANSMIT_PERIOD_INTERVAL / portTICK_PERIOD_MS);
                    getBatt(&batt);
                    sprintf(battStr,"%04d",batt);
                    msg_id = esp_mqtt_client_publish(clientMqtt, topicBatt, battStr, 0, 1, 1);
                    }
                    vTaskDelay(TRANSMIT_PERIOD_INTERVAL / portTICK_PERIOD_MS);
                    vTaskDelay(TRANSMIT_PERIOD_INTERVAL / portTICK_PERIOD_MS);
                }
                break;
            case BATEAU_SLEEP: if(dataReceived){
                esp_wifi_set_ps(WIFI_PS_NONE);
                state_bateau=BATEAU_RECEIVING;
                }
                vTaskDelay(TRANSMIT_PERIOD_INTERVAL / portTICK_PERIOD_MS);// necessaire pour éviter les pb de wathdog
                break;
            default:esp_wifi_set_ps(WIFI_PS_MIN_MODEM);//minimise la conso, augmente la latence
                    state_bateau=BATEAU_SLEEP;
                break;
        }

    }
}

void loop_radio_bateau(void ){
    static const char *TAG = "RADIO_BATEAU";
    char topicTx[SIZE_TOPIC],topicBatt[SIZE_TOPIC],battStr[5];
    char data[10];
    int dir,thr,batt;
    int msg_id;
    
    sprintf(topicTx,"/B/%02d/%03d/com/gazdir",idGroup,idModele);
    sprintf(topicBatt,"/B/%02d/%03d/Batt/radio",idGroup,idModele);
    
    while(1){
        switch(state_radio_bateau){
            case RADIO_BATEAU_TRANSMITTING:
                if(bateauGO){
                    getDir(&dir);
                    getThr(&thr);
                    sprintf(data,"%03d;%03d;",dir,thr);
                    msg_id = esp_mqtt_client_publish(clientMqtt, topicTx, data, 0, 0, 0);
                    //ESP_LOGI(TAG, "DIR=%d\tTHR=%d\tBATT=%d", dir,thr,batt);
                    vTaskDelay(TRANSMIT_PERIOD_INTERVAL / portTICK_PERIOD_MS);
                }
                else{
                    esp_wifi_set_ps(WIFI_PS_MIN_MODEM);
                    state_radio_bateau=RADIO_BATEAU_SLEEP;
                    getBatt(&batt);
                    sprintf(battStr,"%04d",batt);
                    msg_id = esp_mqtt_client_publish(clientMqtt, topicBatt, battStr, 0, 1, 1);
                }
                break;
            case RADIO_BATEAU_SLEEP: if(bateauGO){
                esp_wifi_set_ps(WIFI_PS_NONE);//minimise la conso, augmente la latence
                state_radio_bateau=RADIO_BATEAU_TRANSMITTING;
                }
                vTaskDelay(TRANSMIT_PERIOD_INTERVAL / portTICK_PERIOD_MS);// necessaire pour éviter les pb de wathdog
                break;
            default:esp_wifi_set_ps(WIFI_PS_MIN_MODEM);
                    state_radio_bateau=RADIO_BATEAU_SLEEP;
                break;
        }

    }
}

void loop_starter_bateau(void){
    static const char *TAG = "STARTER_BATEAU";
    char topicGo[SIZE_TOPIC],topicBatt[SIZE_TOPIC],battStr[5];
    char data='0';
    int batt;
    uint32_t timer=0;
    int msg_id;
    
    sprintf(topicGo,"/B/%02d/go",idGroup);
    sprintf(topicBatt,"/B/%02d/%03d/Batt/tx",idGroup,idModele);
    
    while(1){
        switch(state_radio_bateau){
            case RADIO_BATEAU_TRANSMITTING:
                if(timer>0 ){                    
                    vTaskDelay(TRANSMIT_PERIOD_INTERVAL / portTICK_PERIOD_MS);
                    timer--;
                }
                else{
                    //esp_wifi_set_ps(WIFI_PS_NONE);
                    data='0';
                    msg_id = esp_mqtt_client_publish(clientMqtt, topicGo, &data, 0, 1, 1);
                    state_radio_bateau=RADIO_BATEAU_SLEEP;
                    getBatt(&batt);
                    sprintf(battStr,"%04d",batt);
                    msg_id = esp_mqtt_client_publish(clientMqtt, topicBatt, battStr, 0, 1, 1);
                }
                break;
            case RADIO_BATEAU_SLEEP: if(gpio_get_level(IN1)==0){
                //esp_wifi_set_ps(WIFI_PS_MIN_MODEM);//minimise la conso, augmente la latence
                data='1';
                msg_id = esp_mqtt_client_publish(clientMqtt, topicGo, &data, 0, 1, 1);
                state_radio_bateau=RADIO_BATEAU_TRANSMITTING;
                timer=timerGlobal;
                }
                vTaskDelay(TRANSMIT_PERIOD_INTERVAL / portTICK_PERIOD_MS);// necessaire pour éviter les pb de wathdog
                break;
            default:esp_wifi_set_ps(WIFI_PS_MIN_MODEM);
                    state_radio_bateau=RADIO_BATEAU_SLEEP;
                break;
        }

    }

}

void loop_voiture(void ){
    static const char *TAG = "VOITURE";
    char battStr[5],topicBatt[SIZE_TOPIC];
    char data[2];
    int dir,thr,batt;
    int M1,M2;
    uint32_t speedM1, speedM2;
    bool dirM1,dirM2;
    uint32_t timer=0;
    int msg_id;

    dataReceived=0;
    sprintf(topicBatt,"/V/%02d/%03d/Batt/rx",idGroup,idModele);
    
    while(1){
        switch(state_voiture){
            case VOITURE_RECEIVING:
                if(dataReceived){
                    dir=dirReceived;
                    thr=gazReceived;
                    dataReceived=0;
                    M1=(-thr+dir)*5;
                    if(M1>MOTOR_MAX_VALUE)M1=MOTOR_MAX_VALUE;
                    if(M1<-MOTOR_MAX_VALUE)M1=-MOTOR_MAX_VALUE;
                    M2=-(-thr-dir)*5;
                    if(M2>MOTOR_MAX_VALUE)M2=MOTOR_MAX_VALUE;
                    if(M2<-MOTOR_MAX_VALUE)M2=-MOTOR_MAX_VALUE;
                    if(M1>0){
                        speedM1=M1;
                        dirM1=true;
                    }
                    else{
                        speedM1=-M1;
                        dirM1=false;
                    }
                    if(M2>0){
                        speedM2=M2;
                        dirM2=true;
                    }
                    else{
                        speedM2=-M2;
                        dirM2=false;
                    }
                    setMotor(speedM1,dirM1,speedM2,dirM2);
                    //ESP_LOGI(TAG, "DIR=%d\tTHR=%d\tM1=%d\tM2=%dspeedM1=%lu\tdirM1=%s\tspeedM2=%lu\tdirM2=%s\t", dir,thr,M1,M2,speedM1,dirM1 ? "true" : "false",speedM2,dirM2 ? "true" : "false");
                    timer=0;
                }
                else{
                    if(timer++>10){// timeout de 1 seconde
                    esp_wifi_set_ps(WIFI_PS_MIN_MODEM);//minimise la conso, augmente la latence
                    state_voiture=VOITURE_SLEEP;
                    setMotor(0,dirM1,0,dirM2);
                    getBatt(&batt);
                    sprintf(battStr,"%04d",batt);
                    ESP_LOGI(TAG, "battStr=%s batt=%d\t", battStr,batt);
                    msg_id = esp_mqtt_client_publish(clientMqtt, topicBatt, battStr, 0, 1, 1);
                    }
                    vTaskDelay(TRANSMIT_PERIOD_INTERVAL / portTICK_PERIOD_MS);
                }
                break;
            case VOITURE_SLEEP: if(dataReceived){
                esp_wifi_set_ps(WIFI_PS_NONE);
                state_voiture=VOITURE_RECEIVING;
                }
                vTaskDelay(TRANSMIT_PERIOD_INTERVAL / portTICK_PERIOD_MS);// necessaire pour éviter les pb de wathdog
                break;
            default:esp_wifi_set_ps(WIFI_PS_MIN_MODEM);//minimise la conso, augmente la latence
                    state_voiture=VOITURE_SLEEP;
                break;
        }

    }
    
}
void loop_radio_voiture(void ){
    static const char *TAG = "RADIO_VOITURE";
    char topicTx[SIZE_TOPIC],topicBatt[SIZE_TOPIC],battStr[5];
    char data[10];
    int dir,thr,batt;
    int msg_id;
    
    sprintf(topicTx,"/V/%02d/%03d/com/gazdir",idGroup,idModele);
    sprintf(topicBatt,"/V/%02d/%03d/Batt/radio",idGroup,idModele);
    
    while(1){
        switch(state_radio_voiture){
            case RADIO_VOITURE_TRANSMITTING:
                if(voitureGO){                    
                    getDir(&dir);
                    getThr(&thr);
                    sprintf(data,"%03d;%03d;",dir,thr);
                    msg_id = esp_mqtt_client_publish(clientMqtt, topicTx, data, 0, 0, 0);
                    //ESP_LOGI(TAG, "DIR=%d\tTHR=%d\tBATT=%d", dir,thr,batt);
                    vTaskDelay(TRANSMIT_PERIOD_INTERVAL / portTICK_PERIOD_MS);
                }
                else{
                    esp_wifi_set_ps(WIFI_PS_MIN_MODEM);//minimise la conso, augmente la latence
                    state_radio_voiture=RADIO_VOITURE_SLEEP;
                    getBatt(&batt);
                    sprintf(battStr,"%04d",batt);
                    msg_id = esp_mqtt_client_publish(clientMqtt, topicBatt, battStr, 0, 1, 1);
                }
                break;
            case RADIO_VOITURE_SLEEP: if(voitureGO){
                esp_wifi_set_ps(WIFI_PS_NONE);
                state_radio_voiture=RADIO_VOITURE_TRANSMITTING;
                }
                vTaskDelay(TRANSMIT_PERIOD_INTERVAL / portTICK_PERIOD_MS);// necessaire pour éviter les pb de wathdog
                break;
            default:esp_wifi_set_ps(WIFI_PS_MIN_MODEM);//minimise la conso, augmente la latence
                    state_radio_voiture=RADIO_VOITURE_SLEEP;
                break;
        }

    }

}
void loop_starter_voiture(void){
    static const char *TAG = "STARTER_VOITURE";
    char topicGo[SIZE_TOPIC],topicBatt[SIZE_TOPIC],battStr[5];
    char data='0';
    int batt;
    uint32_t timer=0;
    int msg_id;
    
    sprintf(topicGo,"/V/%02d/go",idGroup);
    sprintf(topicBatt,"/V/%02d/%03d/Batt/tx",idGroup,idModele);
    
    while(1){
        switch(state_radio_voiture){
            case RADIO_VOITURE_TRANSMITTING:
                if(timer>0 ){                    
                    vTaskDelay(TRANSMIT_PERIOD_INTERVAL / portTICK_PERIOD_MS);
                    timer--;
                }
                else{
                    //esp_wifi_set_ps(WIFI_PS_NONE);
                    data='0';
                    msg_id = esp_mqtt_client_publish(clientMqtt, topicGo, &data, 0, 1, 1);
                    state_radio_voiture=RADIO_VOITURE_SLEEP;
                    getBatt(&batt);
                    sprintf(battStr,"%04d",batt);
                    msg_id = esp_mqtt_client_publish(clientMqtt, topicBatt, battStr, 0, 1, 1);
                }
                break;
            case RADIO_VOITURE_SLEEP: if(gpio_get_level(IN1)==0){
                //esp_wifi_set_ps(WIFI_PS_MIN_MODEM);//minimise la conso, augmente la latence
                data='1';
                msg_id = esp_mqtt_client_publish(clientMqtt, topicGo, &data, 0, 1, 1);
                state_radio_voiture=RADIO_VOITURE_TRANSMITTING;
                timer=timerGlobal;
                }
                vTaskDelay(TRANSMIT_PERIOD_INTERVAL / portTICK_PERIOD_MS);// necessaire pour éviter les pb de wathdog
                break;
            default:esp_wifi_set_ps(WIFI_PS_MIN_MODEM);
                    state_radio_voiture=RADIO_VOITURE_SLEEP;
                break;
        }

    }

}

void setupBoard(void){
        switch  (QUISUISJE){
            case BATEAU:
                setup_bateau();
                break;
            case RADIO_BATEAU:
                setup_radio_bateau();
                break;
            case STARTER_BATEAU:
                setup_starter_bateau();
                break;
            case VOITURE:
                setup_voiture();
                break;
            case RADIO_VOITURE:
                setup_radio_voiture();
                break;
            case STARTER_VOITURE:
                setup_starter_voiture();
                break;
            default:
                break;
        }

}
void loop_board(void){
        switch  (QUISUISJE){
            case BATEAU:
                loop_bateau();
                break;
            case RADIO_BATEAU:
                loop_radio_bateau();
                break;
            case STARTER_BATEAU:
                loop_starter_bateau();
                break;
            case VOITURE:
                loop_voiture();
                break;
            case RADIO_VOITURE:
                loop_radio_voiture();
                break;
            case STARTER_VOITURE:
                loop_starter_voiture();
                break;
            default:
                break;
        }

}