/*static FILE uartout = {0} ;

static int uart_putchar (char c, FILE *stream)
{
  Serial.write(c) ;
  return 0 ;
}*/

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

// Définition des constantes
#define DT 1 // Période d'échantillonnage (en secondes)
#define STATE_SIZE 3
#define MEAS_SIZE 3

// Définition des matrices (taille fixe pour simplifier)
typedef struct {
    double x;  // Position en x
    double y;  // Position en y
    double theta; // Orientation
} State_t;

// Matrices globales
double P[STATE_SIZE][STATE_SIZE] = { {0.1, 0.0, 0.0}, {0.0, 0.1, 0.0}, {0.0, 0.0, 0.1} }; // Covariance
double Q[STATE_SIZE][STATE_SIZE] = { {0.01, 0.0, 0.0}, {0.0, 0.01, 0.0}, {0.0, 0.0, 0.01} }; // Bruit de processus
double R[MEAS_SIZE][MEAS_SIZE] = { {0.1, 0.0, 0.0}, {0.0, 0.1, 0.0}, {0.0, 0.0, 0.1} }; // Bruit de mesure

// Fonctions auxiliaires pour la matrice
void mat_mult(double A[STATE_SIZE][STATE_SIZE], double B[STATE_SIZE][STATE_SIZE], double C[STATE_SIZE][STATE_SIZE]) {
    for (int i = 0; i < STATE_SIZE; i++) {
        for (int j = 0; j < STATE_SIZE; j++) {
            C[i][j] = 0.0;
            for (int k = 0; k < STATE_SIZE; k++) {
                C[i][j] += A[i][k] * B[k][j];
            }
        }
    }
}

void mat_add(double A[STATE_SIZE][STATE_SIZE], double B[STATE_SIZE][STATE_SIZE], double C[STATE_SIZE][STATE_SIZE]) {
    for (int i = 0; i < STATE_SIZE; i++) {
        for (int j = 0; j < STATE_SIZE; j++) {
            C[i][j] = A[i][j] + B[i][j];
        }
    }
}

// Étape de prédiction
void ekfpredict(State_t* state, double dx, double dy, double dtheta) {
    // Mise à jour de l'état en utilisant un modèle de mouvement
    state->x += dx * cos(state->theta) - dy * sin(state->theta);
    state->y += dx * sin(state->theta) + dy * cos(state->theta);
    state->theta += dtheta;

    // Normalisation de theta
    state->theta = fmod(state->theta + M_PI, 2 * M_PI) - M_PI;

    // Mise à jour de la covariance (simplifiée ici)
    double F[STATE_SIZE][STATE_SIZE] = {
        {1, 0, -dx * sin(state->theta) - dy * cos(state->theta)},
        {0, 1,  dx * cos(state->theta) - dy * sin(state->theta)},
        {0, 0, 1}
    };

    double F_P[STATE_SIZE][STATE_SIZE], F_T[STATE_SIZE][STATE_SIZE], temp[STATE_SIZE][STATE_SIZE];
    mat_mult(F, P, F_P);
    mat_mult(F_P, F, F_T); // F^T
    mat_add(F_T, Q, P);
}

// Étape de mise à jour
void ekfupdate(State_t *state, double z[MEAS_SIZE]) {
    // Calcul de l'innovation (résidu de mesure)
    double y[MEAS_SIZE] = {
        z[0] - state->x,
        z[1] - state->y,
        z[2] - state->theta
    };

    // Calcul du gain de Kalman (simplifié)
    double K[STATE_SIZE][MEAS_SIZE] = {
        {0.1, 0.0, 0.0},
        {0.0, 0.1, 0.0},
        {0.0, 0.0, 0.1}
    };

    // Mise à jour de l'état
    state->x += K[0][0] * y[0] + K[0][1] * y[1] + K[0][2] * y[2];
    state->y += K[1][0] * y[0] + K[1][1] * y[1] + K[1][2] * y[2];
    state->theta += K[2][0] * y[0] + K[2][1] * y[1] + K[2][2] * y[2];

    // Normalisation de theta
    state->theta = fmod(state->theta + M_PI, 2 * M_PI) - M_PI;

    // Mise à jour de la covariance (non implémentée pour simplicité)
}

void setup() {
  Serial.begin(115200);
  //fdev_setup_stream (&uartout, uart_putchar, NULL, _FDEV_SETUP_WRITE);
  //stdout = &uartout ;

    // Initialisation
    State_t robot = {0.0, 0.0, 0.0}; // Position initiale
    double measurements[MEAS_SIZE] = {50, 0, 0}; // Exemples de mesures


for (int i = 0; i < 100; i++){
    // Étape de prédiction
    ekfpredict(&robot, measurements[0], measurements[1], measurements[2]);

    // Mise à jour avec les mesures
   ekfupdate(&robot, measurements);

   
    // Affichage de l'état estimé
    //Serial.print("Pos : ");
    //Serial.print(" x : "); 
    Serial.print(robot.x); Serial.print("\t");
    //Serial.print(" y : "); 
    Serial.print(robot.y); Serial.print("\t");
    //Serial.print(" t : "); 
    Serial.print(robot.theta);
    Serial.println();

}
    return 0;
}

void loop(){
  while(true);
}