#include <stdio.h>
#include <math.h>

typedef struct {
    float x;       // Position en x
    float y;       // Position en y
    float theta;   // Orientation
    float v;       // Vitesse de translation
    float omega;   // Vitesse angulaire
} State;

typedef struct {
    State state;    // État estimé
    float P[5][5];  // Covariance
    float Q[5][5];  // Bruit de processus
    float R[3][3];  // Bruit de mesure (x, y, theta)
} KalmanFilter;

KalmanFilter kf = {0};

void kalman_init(KalmanFilter *kf) {
    kf->state.x = 0.0;
    kf->state.y = 0.0;
    kf->state.theta = 0.0;
    kf->state.v = 0.0;
    kf->state.omega = 0.0;

    // Initialisation des matrices de covariance et bruit
    for (int i = 0; i < 5; i++) {
        for (int j = 0; j < 5; j++) {
            kf->P[i][j] = (i == j) ? 1.0 : 0.0;
            kf->Q[i][j] = (i == j) ? 0.1 : 0.0;
        }
    }

    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            kf->R[i][j] = (i == j) ? 0.5 : 0.0;
        }
    }
}

void kalman_predict(KalmanFilter *kf, float dt) {
    State *s = &kf->state;

    // Mise à jour de la position seulement si une vitesse est mesurée
    if (s->v != 0 || s->omega != 0) {
        float dx = s->v * cos(s->theta) * dt;
        float dy = s->v * sin(s->theta) * dt;
        float dtheta = s->omega * dt;

        s->x += dx;
        s->y += dy;
        s->theta += dtheta;
    }

    // Réduction explicite des vitesses pour éviter la dérive
    s->v *= 1;
    s->omega *= 1;

    // Mise à jour de la covariance
    for (int i = 0; i < 5; i++) {
        for (int j = 0; j < 5; j++) {
            kf->P[i][j] += kf->Q[i][j];
        }
    }
}



void kalman_update(KalmanFilter *kf, float z_x, float z_y, float z_theta, float dt) {
    State *s = &kf->state;

    // Innovations (différences entre mesures et état prédit)
    float y_x = z_x - s->x;
    float y_y = z_y - s->y;
    float y_theta = z_theta - s->theta;

    // Mise à jour des vitesses si les mesures indiquent un arrêt
    if (fabs(y_x) < 1e-3 && fabs(y_y) < 1e-3 && fabs(y_theta) < 1e-3) {
        s->v = 0.0; // Réinitialisation explicite
        s->omega = 0.0;
    } else {
        // Estimation des vitesses à partir des innovations
        s->v = (s->v + sqrt((y_x / dt) * (y_x / dt) + (y_y / dt) * (y_y / dt))) / 2.0;
        s->omega = (s->omega + y_theta / dt) / 2.0;
    }

    // Mise à jour des états
    s->x += y_x * 0.5;
    s->y += y_y * 0.5;
    s->theta += y_theta * 0.5;

    // Mise à jour de la covariance
    for (int i = 0; i < 5; i++) {
        for (int j = 0; j < 5; j++) {
            kf->P[i][j] *= 0.95;
        }
    }
}


void setup() {
    
    kalman_init(&kf);

    float dt = 1; // Intervalle de temps

    Serial.begin(115200);

    // Simuler un mouvement avec des mesures bruitées
    for (int i = 0; i < 100; i++) {
        // Prédiction
        kalman_predict(&kf, dt);

        // Simuler des mesures
        float z_x = kf.state.x ;//+ random(-5,5);
        float z_y = kf.state.y +5;//+ random(-5,5);
        float z_theta = kf.state.theta;//+ random(-PI/20, PI/20);

        // Mise à jour avec les mesures
        kalman_update(&kf, z_x, z_y, z_theta, dt);

        // Afficher l'état estimé
        Serial.print(" x "); Serial.print(kf.state.x);
        Serial.print(" y "); Serial.print(kf.state.y);
        Serial.print(" th "); Serial.print(kf.state.theta);
        Serial.print(" v "); Serial.print(kf.state.v);
        Serial.print(" om "); Serial.print(kf.state.omega);
        Serial.println();
    }
}

void loop(){
  while(true);
}