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

typedef struct {
    float x;
    float y;
    float theta;
} State;

typedef struct {
    State state;  // Estimation de l'état
    float P[3][3]; // Covariance de l'estimation
    float Q[3][3]; // Bruit de processus
    float R[2][2]; // Bruit de mesure
} KalmanFilter;

// Fonction pour initialiser la matrice à l'identité
void init_identity_matrix(float matrix[3][3]) {
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            if (i == j)
                matrix[i][j] = 1.0;
            else
                matrix[i][j] = 0.0;
        }
    }
}

// Fonction pour initialiser le filtre de Kalman
void kalman_init(KalmanFilter *kf) {
    kf->state.x = 0.0;
    kf->state.y = 0.0;
    kf->state.theta = 0.0;

    // Initialiser la matrice de covariance P
    init_identity_matrix(kf->P);

    // Initialiser le bruit de processus Q (vous pouvez ajuster ces valeurs)
    kf->Q[0][0] = 0.1;
    kf->Q[1][1] = 0.1;
    kf->Q[2][2] = 0.01;

    // Initialiser le bruit de mesure R (par exemple, pour les capteurs)
    kf->R[0][0] = 0.5;  // Bruit sur la mesure de x
    kf->R[1][1] = 0.5;  // Bruit sur la mesure de y
}

// Fonction pour prédire l'état en utilisant le modèle de mouvement
void kalman_predict(KalmanFilter *kf, float v, float omega, float dt) {
    // Prédire la position et l'orientation
    float theta = kf->state.theta;

    kf->state.x += v * cos(theta) * dt;
    kf->state.y += v * sin(theta) * dt;
    kf->state.theta += omega * dt;

    // Mise à jour simplifiée de la covariance
    kf->P[0][0] += kf->Q[0][0];
    kf->P[1][1] += kf->Q[1][1];
    kf->P[2][2] += kf->Q[2][2];
}

// Fonction de mise à jour avec des mesures z (x, y)
void kalman_update(KalmanFilter *kf, float z_x, float z_y) {
    // Gain de Kalman K = P * H^T * (H * P * H^T + R)^{-1}
    // Dans ce cas, H est l'identité puisque nous mesurons directement x et y

    float K[3][2];
    float S[2][2] = { {kf->P[0][0] + kf->R[0][0], 0},
                      {0, kf->P[1][1] + kf->R[1][1]} };

    // Calcul du gain de Kalman
    K[0][0] = kf->P[0][0] / S[0][0];
    K[1][0] = kf->P[1][1] / S[1][1];
    K[2][0] = kf->P[2][2] / (kf->P[2][2] + 1);  // Simplification

    // Mise à jour de l'état avec les mesures
    float y_x = z_x - kf->state.x;
    float y_y = z_y - kf->state.y;

    kf->state.x += K[0][0] * y_x;
    kf->state.y += K[1][0] * y_y;

    // Mise à jour de la covariance P = (I - K * H) * P
    kf->P[0][0] *= (1 - K[0][0]);
    kf->P[1][1] *= (1 - K[1][0]);
}

// Fonction pour afficher l'état actuel
void print_state(KalmanFilter *kf) {
  //Serial.print("Pos : ");
  //Serial.print(" x : "); 
    Serial.print(kf->state.x); Serial.print("\t");
  //Serial.print(" y : "); 
    Serial.print(kf->state.y); Serial.print("\t");
  //Serial.print(" t : "); 
    Serial.print(kf->state.theta);

  Serial.println();
}

void setup() {
  Serial.begin(115200);
  delay(1000);
    KalmanFilter kf;
    kalman_init(&kf);

    // Simuler un mouvement avec vitesse et rotation
    float v = 1.0;     // m/s
    float omega = 0; // rad/s
    float dt = 1;    // Intervalle de temps

    for (int i = 0; i < 100; i++) {
        // Prédiction du filtre
        kalman_predict(&kf, v, omega, dt);

        // Simulation d'une mesure bruitée
        float z_x = kf.state.x + random(-0.5,0.5);
        float z_y = kf.state.y + random(-0.5,0.5);

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

        // Affichage de l'état estimé
        print_state(&kf);
        delay(100);
    }
}

void loop(){
  while(1);
};