#include "pico/stdlib.h"
#include <stdio.h>
#include "Monitoring.h"
#include "Localisation.h"
#include "Trajet.h"
#include "Asser_Moteurs.h"
#include "i2c_annexe.h"
#include "Evitement.h"

#define LED_PIN_ROUGE 28

uint32_t temps_cycle_min = UINT32_MAX;
uint32_t temps_cycle_max=0;
int erreur_critique = 0;
int lock=0;
uint32_t debug_var=0;
float debug_varf=0;

void temps_cycle_check(){
    static uint32_t temps_old;
    uint32_t temps, temps_cycle;

    temps = time_us_32();
    temps_cycle = temps - temps_old;
    
    if(temps_cycle < temps_cycle_min){
        temps_cycle_min = temps_cycle;
    }

    if(temps_cycle > temps_cycle_max){
        temps_cycle_max = temps_cycle;
    }
    temps_old=time_us_32();
}

void temps_cycle_reset(){
    temps_cycle_min = UINT32_MAX;
    temps_cycle_max=0;
}

void Monitoring_display(){
    while(1){
        uint32_t temps;
        temps = time_us_32()/1000;
        temps_cycle_display();
        printf(">V_bat:%ld:%2.1f\n", temps, (float) (i2c_annexe_get_tension_batterie() / 10.));
        printf(">DebugVar:%ld:%d\n", temps, debug_var);
        printf(">Distance_Obstacle:%ld:%f\n", temps, Trajet_get_obstacle_mm());
        printf(">Etat_Evitement:%ld:%d\n", temps, Evitement_get_statu());
        printf(">DebugVarf(abscisse):%ld:%f\n", temps, debug_varf);
        struct position_t position = Localisation_get();
        printf(">pos_x:%ld:%f\n", temps, position.x_mm);
        printf(">P_con_x:%ld:%f\n", temps, Trajet_get_consigne().x_mm);
        printf(">pos_y:%ld:%f\n", temps, position.y_mm);
        printf(">P_con_y:%ld:%f\n", temps, Trajet_get_consigne().y_mm);
        printf(">pos_angle:%ld:%f\n", temps, position.angle_radian);
        printf(">P_con_angle:%ld:%f\n", temps, Trajet_get_consigne().angle_radian);
        printf(">V_a:%ld:%f\n", temps, AsserMoteur_getVitesse_mm_s(MOTEUR_A, 1));
        printf(">V_b:%ld:%f\n", temps, AsserMoteur_getVitesse_mm_s(MOTEUR_B, 1));
        printf(">V_c:%ld:%f\n", temps, AsserMoteur_getVitesse_mm_s(MOTEUR_C, 1));
        printf(">V_con_a:%ld:%f\n", temps, AsserMoteur_getConsigne_mm_s(MOTEUR_A));
        printf(">V_con_b:%ld:%f\n", temps, AsserMoteur_getConsigne_mm_s(MOTEUR_B));
        printf(">V_con_c:%ld:%f\n", temps, AsserMoteur_getConsigne_mm_s(MOTEUR_C));
    }
}

void temps_cycle_display(){
    uint32_t temps;
    temps = time_us_32()/1000;
    printf(">T_cycle_min(us):%ld:%d\n", temps, temps_cycle_min);
    printf(">T_cycle_max(us):%ld:%d\n", temps, temps_cycle_max);
    temps_cycle_reset();
}

uint32_t temps_cycle_get_min(){
    return temps_cycle_min;
}

uint32_t temps_cycle_get_max(){
    return temps_cycle_max;
}

void set_debug_var(uint32_t variable){
    debug_var = variable;
}

void set_debug_varf(float variable){
    debug_varf = variable;
}

void Monitoring_Error(char * msg){
    gpio_put(LED_PIN_ROUGE, 1);

}

void Monitoring_set_erreur_critique(){
    Monitoring_Error("");
    erreur_critique=1;
}

int Monitoring_get_erreur_critique(){
    return erreur_critique;
}