/**
 * @file moto_config.c
 * @brief Implémentation des fonctions utilitaires pour système IMU moto
 */

#include "moto_config.h"
#include <math.h>
#include <string.h>
#include <stdio.h>

//------------------------------------------------------------------------------
// Fonctions publiques
//------------------------------------------------------------------------------

void Moto_InitData(MotoData_t *data) {
    memset(data, 0, sizeof(MotoData_t));
    data->state = MOTO_STATE_NORMAL;
    data->is_initializing = true;
    data->last_update_time = 0;
}

void Moto_UpdateState(MotoData_t *data, float roll, float pitch, float yaw,
                      float gyro_x, float gyro_y, float gyro_z) {

    // Mise à jour des angles bruts
    data->roll = roll;
    data->pitch = pitch;
    data->yaw = yaw;

    // Calcul de la magnitude de la vitesse angulaire
    float gyro_magnitude = sqrtf(gyro_x*gyro_x + gyro_y*gyro_y + gyro_z*gyro_z);

    // Détection d'état prioritaire : crash possible
    if (Moto_DetectCrash(data, gyro_magnitude)) {
        data->state = MOTO_STATE_POSSIBLE_CRASH;
        return;
    }

    // Détection de wheelie/stoppie
    if (pitch > MOTO_PITCH_WHEELIE_THRESHOLD) {
        data->state = MOTO_STATE_WHEELIE;
    } else if (pitch < MOTO_PITCH_STOPPIE_THRESHOLD) {
        data->state = MOTO_STATE_STOPPIE;
    }
    // Détection de virage rapide
    else if (fabsf(gyro_z) > MOTO_GYRO_RAPID_TURN_THRESHOLD) {
        data->state = MOTO_STATE_RAPID_TURN;
    }
    // Détection d'inclinaison dangereuse
    else if (fabsf(roll) > MOTO_ROLL_DANGER_THRESHOLD) {
        data->state = MOTO_STATE_DANGER;
    }
    // Détection d'inclinaison d'avertissement
    else if (fabsf(roll) > MOTO_ROLL_WARNING_THRESHOLD) {
        data->state = MOTO_STATE_WARNING;
    }
    // État normal
    else {
        data->state = MOTO_STATE_NORMAL;
        data->crash_detect_counter = 0; // Reset compteur crash
    }
}

void Moto_FilterAngles(MotoData_t *data) {
    // Filtre passe-bas simple pour lisser l'affichage
    data->roll_filtered = MOTO_ANGLE_FILTER_ALPHA * data->roll +
                         (1.0f - MOTO_ANGLE_FILTER_ALPHA) * data->roll_filtered;

    data->pitch_filtered = MOTO_ANGLE_FILTER_ALPHA * data->pitch +
                          (1.0f - MOTO_ANGLE_FILTER_ALPHA) * data->pitch_filtered;

    data->yaw_filtered = MOTO_ANGLE_FILTER_ALPHA * data->yaw +
                        (1.0f - MOTO_ANGLE_FILTER_ALPHA) * data->yaw_filtered;
}

const char* Moto_GetStateString(MotoState_t state) {
    switch (state) {
        case MOTO_STATE_NORMAL:         return "NORMAL        ";
        case MOTO_STATE_WARNING:        return "ATTENTION     ";
        case MOTO_STATE_DANGER:         return "DANGER        ";
        case MOTO_STATE_WHEELIE:        return "WHEELIE       ";
        case MOTO_STATE_STOPPIE:        return "STOPPIE       ";
        case MOTO_STATE_RAPID_TURN:     return "VIRAGE RAPIDE ";
        case MOTO_STATE_POSSIBLE_CRASH: return "CHUTE POSSIBLE";
        default:                        return "INCONNU";
    }
}

void Moto_UpdateStats(MotoStats_t *stats, const MotoData_t *data,
                      float gyro_x, float gyro_y, float gyro_z) {

    stats->total_samples++;

    // Mise à jour des extremums d'angles
    if (data->roll > stats->max_roll) stats->max_roll = data->roll;
    if (data->roll < stats->min_roll) stats->min_roll = data->roll;
    if (data->pitch > stats->max_pitch) stats->max_pitch = data->pitch;
    if (data->pitch < stats->min_pitch) stats->min_pitch = data->pitch;

    // Mise à jour des extremums de vitesse angulaire
    if (fabsf(gyro_x) > stats->max_gyro_x) stats->max_gyro_x = fabsf(gyro_x);
    if (fabsf(gyro_y) > stats->max_gyro_y) stats->max_gyro_y = fabsf(gyro_y);
    if (fabsf(gyro_z) > stats->max_gyro_z) stats->max_gyro_z = fabsf(gyro_z);

    // Comptage des états d'alerte
    if (data->state == MOTO_STATE_WARNING) stats->warning_count++;
    if (data->state == MOTO_STATE_DANGER ||
        data->state == MOTO_STATE_POSSIBLE_CRASH) stats->danger_count++;
}

void Moto_CalibrateMagnetometer(float *mx, float *my, float *mz) {
    // Application des offsets et facteurs d'échelle
    *mx = (*mx - MOTO_MAG_OFFSET_X) * MOTO_MAG_SCALE_X;
    *my = (*my - MOTO_MAG_OFFSET_Y) * MOTO_MAG_SCALE_Y;
    *mz = (*mz - MOTO_MAG_OFFSET_Z) * MOTO_MAG_SCALE_Z;
}

bool Moto_DetectCrash(MotoData_t *data, float gyro_magnitude) {
    if (gyro_magnitude > MOTO_GYRO_CRASH_THRESHOLD) {
        data->crash_detect_counter++;

        // Confirme la chute si seuil dépassé pendant plusieurs échantillons
        if (data->crash_detect_counter > 5) { // ~50ms à 100Hz
            return true;
        }
    } else {
        // Décrémente progressivement le compteur
        if (data->crash_detect_counter > 0) {
            data->crash_detect_counter--;
        }
    }

    return false;
}

void Moto_FormatDisplay(const MotoData_t *data, int line, char *buffer) {
    switch (line) {
        case 0:
            snprintf(buffer, 21, "R:%5.1f P:%5.1f",
                    data->roll_filtered, data->pitch_filtered);
            break;

        case 1:
            snprintf(buffer, 21, "Yaw: %6.1f deg", data->yaw_filtered);
            break;

        case 2:
            snprintf(buffer, 21, "Etat: %s", Moto_GetStateString(data->state));
            break;

        case 3:
            if (data->is_initializing) {
                snprintf(buffer, 21, "---- INIT EN COURS ----");
            } else {
                switch (data->state) {
                    case MOTO_STATE_NORMAL:
                        snprintf(buffer, 21, "---- EQUILIBRE ----");
                        break;
                    case MOTO_STATE_WARNING:
                        if (data->roll > 0) {
                            snprintf(buffer, 21, "INCLIN. DROITE >>>");
                        } else {
                            snprintf(buffer, 21, "<<< INCLIN. GAUCHE");
                        }
                        break;
                    case MOTO_STATE_DANGER:
                    case MOTO_STATE_POSSIBLE_CRASH:
                        snprintf(buffer, 21, "!!! ATTENTION !!!   ");
                        break;
                    case MOTO_STATE_WHEELIE:
                        snprintf(buffer, 21, "^^^ WHEELIE ^^^     ");
                        break;
                    case MOTO_STATE_STOPPIE:
                        snprintf(buffer, 21, "vvv STOPPIE vvv     ");
                        break;
                    case MOTO_STATE_RAPID_TURN:
                        snprintf(buffer, 21, ">>> VIRAGE <<<      ");
                        break;
                    default:
                        snprintf(buffer, 21, "--- INCONNU ---     ");
                        break;
                }
            }
            break;

        default:
            snprintf(buffer, 21, "Ligne invalide");
            break;
    }
}

//------------------------------------------------------------------------------
// Fonctions utilitaires privées
//------------------------------------------------------------------------------

// Ajouter ici d'autres fonctions utilitaires si nécessaire