#include "pico/stdlib.h"
#include "pico/multicore.h"
#include "gyro.h"
#include "gyro_ADXRS453.h"
#include "Log.h"
#include <stdio.h>


#define TEST_TIMEOUT_US 10000000

int test_gyro_vitesse_brute(void);

// Test du gyroscope
// 1 - lecture continue de la valeur brute de l'angle

int test_gyro(){
    int lettre;

    while(1){

        do{
            printf("A : Lecture brute\n");
            printf("Q : Quitter\n");
            
            lettre = getchar_timeout_us(TEST_TIMEOUT_US);
            stdio_flush();
        }while(lettre == PICO_ERROR_TIMEOUT ||lettre == 0);

        switch(lettre){

            case 'A':
            case 'a':
                while(test_gyro_vitesse_brute());
                break;

            case 'Q':
            case 'q':
                return 0;
        }
    }
}

void affichage(){
    while(1){
        Log_gestion();
    }
}

int test_gyro_vitesse_brute(void){
    printf("Lecture vitesse brute\n");
    uint16_t tampon_envoi[4];
    uint8_t tampon_reception[4];
    char message[LOG_MAX_MESSAGE_SIZE];
    Log_init();
    Gyro_init_spi();
    Gyro_init_config();

    struct t_angle_gyro angle_gyro;
    multicore_launch_core1(affichage);
    printf("Debut acquisition\n");
    while(1){
        gyro_get_vitesse_brute(&angle_gyro, NULL);
        if(time_us_32() % 100000){
            sprintf(message,">angle:%d\n", angle_gyro.rot_z);
            Log_message(message, TELEPLOT);
        }
    }

}