#include <stdio.h>
#include "pico/error.h"
#include "Geometrie.h"
#include "Strategie_2024_pots.h"

int test_calcul_position_pot(void);

int test_strategie_2024(){
    printf("A - Position groupes pot.\n");
    
    int lettre;
    do{
        lettre = getchar_timeout_us(0);
    }while(lettre == PICO_ERROR_TIMEOUT || lettre == 0);
    switch(lettre){
        case 'a':
        case 'A':
            while(test_calcul_position_pot());
            break;
    }
}

void print_position(struct position_t position){
    printf("x_mm: %.2f, y_mm: %.2f, angle: %.2f\n", position.x_mm, position.y_mm, position.angle_radian/DEGRE_EN_RADIAN);
}

int test_calcul_position_pot(){
    printf("\ngroupe: GROUPE_POT_B1, pot: 5 \n");
    print_position(groupe_pot_get_pot(GROUPE_POT_B1, POT_5));

    printf("\ngroupe: GROUPE_POT_B2, pot: 1 \n");
    print_position(groupe_pot_get_pot(GROUPE_POT_B2, POT_1));

    printf("\ngroupe: GROUPE_POT_L1, pot: 1 \n");
    print_position(groupe_pot_get_pot(GROUPE_POT_L1, POT_1));

    printf("\ngroupe: GROUPE_POT_L2, pot: 1 \n");
    print_position(groupe_pot_get_pot(GROUPE_POT_L2, POT_1));

    printf("\ngroupe: GROUPE_POT_R1, pot: 1 \n");
    print_position(groupe_pot_get_pot(GROUPE_POT_R1, POT_1));

    printf("\ngroupe: GROUPE_POT_R2, pot: 1 \n");
    print_position(groupe_pot_get_pot(GROUPE_POT_R2, POT_1));

    printf("\ngroupe: GROUPE_POT_L1, pot: 1 \n");
    print_position(groupe_pot_get_pot(GROUPE_POT_L1, POT_1));

    printf("\ngroupe: GROUPE_POT_L1, pot: 2 \n");
    print_position(groupe_pot_get_pot(GROUPE_POT_L1, POT_2));

    printf("\ngroupe: GROUPE_POT_L1, pot: 3 \n");
    print_position(groupe_pot_get_pot(GROUPE_POT_L1, POT_3));

    printf("\ngroupe: GROUPE_POT_L1, pot: 4 \n");
    print_position(groupe_pot_get_pot(GROUPE_POT_L1, POT_4));

    printf("\ngroupe: GROUPE_POT_L1, pot: 5 \n");
    print_position(groupe_pot_get_pot(GROUPE_POT_L1, POT_5));

    return 0;

}