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

uint32_t temps_cycle_min = UINT32_MAX;
uint32_t temps_cycle_max=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(">DebugVar:%ld:%d\n", temps, debug_var);
        printf(">DebugVarf:%ld:%f\n", temps, debug_varf);
        struct position_t position = Localisation_get();
        printf(">pos_x:%ld:%f\n", temps, position.x_mm);
        printf(">pos_y:%ld:%f\n", temps, position.y_mm);
        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;
}