#include <Wire.h>
#include "Adafruit_MPU6050.h"
#include "Adafruit_Sensor.h"
#include <ArduinoJson.h>
#include <WebSocketsServer_Generic.h>
#include "secrets.h"
#include <WiFi.h>

#define WS_PORT 8080

// Identifiants WiFi
int NB_NETWORKS_PERSO = 4;
const char* ssid[3];      // Tableau pour stocker jusqu'à 10 noms de réseau
const char* password[3];  // Tableau pour stocker jusqu'à 10 mots de passe
int numNetworks = 0;      // Nombre de réseaux trouvés
int currentNetwork = 0;   // Réseau actuellement connecté

WebSocketsServer webSocket = WebSocketsServer(WS_PORT);

// Gestion du temps
unsigned long lastUpdateTime = 0;    // Dernière mise à jour du calcul
unsigned long lastSendTime = 0;      // Dernier envoi de données
unsigned long updateInterval = 100;  // Interval de mise à jour en millisecondes (ex: mise à jour toutes les 100ms)
unsigned long sendInterval = 1000;   // Interval d'envoi en millisecondes (ex: envoyer des données toutes les 1000ms)

// Définition des broches PWM
const int motorPin1 = 1;  // Remplacer par une broche valide pour PWM
const int motorPin2 = 2;  // Remplacer par une broche valide pour PWM

// Paramètres PID
double setpoint = 0, input, output, kp = 0.1, ki = 0.0, kd = 0.0;

// Variables de Kalman
double angle_kalman, bias, rate;
double P[2][2], K[2], y, S;
double Q_angle = 0.001, Q_bias = 0.003, R_measure = 0.03;

// Variables système
int power = 0;

double errSum = 0, lastErr = 0;
int motorThreshold = 60;  // Seuil minimal de PWM pour démarrer le moteur

char buffer[100];  // Pour le débugage en sprintf

// Paramètres i2c et IMU
Adafruit_MPU6050 mpu;
#define SCL_PIN 44
#define SDA_PIN 43

void setup() {
  Serial.begin(115200);

  // Scanner les réseaux Wifi
  connectToWiFi();

  // Démarrage du serveur WebSocket
  webSocket.begin();
  webSocket.onEvent(webSocketEvent);

  // Démarrage de l'i2C pour accéder au MPU
  Wire.begin(SDA_PIN, SCL_PIN);

  // Activation du MPU6050
  while (!mpu.begin()) {
    delay(1000);
  }

  // Initialisation du moteur
  setupMotor();

  // Initialisation du filtre de Kalman
  initKalman();
}

void loop() {
  unsigned long currentTime = millis();

  if (currentTime - lastUpdateTime >= updateInterval) {
    sensors_event_t a, g, temp;
    mpu.getEvent(&a, &g, &temp);

    double dt = (currentTime - lastUpdateTime) / 1000.0;  // Temps en secondes depuis la dernière mise à jour
    lastUpdateTime = currentTime;                         // Mettre à jour le dernier temps de mise à jour

    double rate = g.gyro.x - bias;
    double angle_acc = atan2(a.acceleration.y, a.acceleration.z) * 180 / PI;
    angle_kalman += dt * rate;

    updateKalmanFilter(a, g, dt);

    double error = setpoint - angle_kalman;
    double pidOutput = computePID(error, dt);  // Assurez-vous que dt est passé ici
    int duty = controlMotor(pidOutput);

    if (currentTime - lastSendTime >= sendInterval) {
      sendPIDData(angle_acc, error, pidOutput, duty);
      lastSendTime = currentTime;
    }
  }

  webSocket.loop();  // Traitement des messages WebSocket, à faire à chaque itération
}

void setupMotor() {
  ledcSetup(0, 5000, 8);        // Configuration du canal PWM
  ledcSetup(1, 5000, 8);        // Configuration d'un autre canal PWM
  ledcAttachPin(motorPin1, 0);  // Attacher le canal PWM à motorPin1
  ledcAttachPin(motorPin2, 1);  // Attacher l'autre canal PWM à motorPin2
}

void initKalman() {
  angle_kalman = 0.0;  // Angle initial estimé
  bias = 0.0;          // Bias gyro initial estimé
  P[0][0] = 0;
  P[0][1] = 0;
  P[1][0] = 0;
  P[1][1] = 0;  // Erreur de covariance initiale
}

double computePID(double error, double dt) {
  // Accumuler l'erreur sur le temps pour le terme intégral
  errSum += error * dt;

  // Calculer la différence d'erreur pour le terme dérivatif
  double dErr = (error - lastErr) / dt;

  // Calculer la sortie du PID
  double output = kp * error + ki * errSum + kd * dErr;

  // Mettre à jour lastErr pour la prochaine itération
  lastErr = error;

  return output;
}

void updateKalmanFilter(sensors_event_t& a, sensors_event_t& g, double& dt) {
  double rate = g.gyro.x - bias;
  double angle_acc = atan2(a.acceleration.y, a.acceleration.z) * 180 / PI;
  angle_kalman += dt * rate;

  y = angle_acc - angle_kalman;
  S = P[0][0] + R_measure;
  K[0] = P[0][0] / S;
  K[1] = P[1][0] / S;
  angle_kalman += K[0] * y;
  bias += K[1] * y;

  P[0][0] -= K[0] * P[0][0];
  P[0][1] -= K[0] * P[0][1];
  P[1][0] -= K[1] * P[0][0];
  P[1][1] -= K[1] * P[0][1];
}

int controlMotor(double pidOutput) {
  int speed = constrain((int)pidOutput, -255, 255);
  /* Débugage à commenter lorsque fonctionnel
  sprintf(buffer, "Speed : %d%", speed);
  Serial.println(buffer);*/

  // Gérer le deadband
  if (speed > 0 && speed < motorThreshold) {
    speed = motorThreshold;  // Minimum pour tourner dans un sens
  } else if (speed < 0 && speed > -motorThreshold) {
    speed = -motorThreshold;  // Minimum pour tourner dans l'autre sens
  }

  if (speed > 0) {
    ledcWrite(0, speed);
    ledcWrite(1, 0);
  } else if (speed < 0) {
    ledcWrite(0, 0);
    ledcWrite(1, -speed);
  } else {
    ledcWrite(0, 0);
    ledcWrite(1, 0);
  }
  return speed;
}

void sendPIDData(double measuredAngle, double currentError, double pidCorrection, int motorDuty) {
  StaticJsonDocument<200> doc;
  doc["measured_angle"] = measuredAngle;  // Angle mesuré
  doc["estimated_angle"] = angle_kalman;  // Angle estimé par Kalman, utilisé directement car il s'agit d'une variable globale
  doc["error"] = currentError;            // Erreur actuelle
  doc["correction"] = pidCorrection;      // Correction PID
  doc["duty_cycle"] = motorDuty;          // Duty cycle du moteur

  String output;
  serializeJson(doc, output);
  webSocket.broadcastTXT(output);
}

void webSocketEvent(uint8_t num, WStype_t type, uint8_t* payload, size_t length) {
  if (type == WStype_TEXT) {
    Serial.printf("Received: %s\n", payload);

    StaticJsonDocument<200> doc;
    DeserializationError error = deserializeJson(doc, payload);

    if (error) {
      Serial.print("deserializeJson() failed: ");
      Serial.println(error.f_str());
      return;
    }

    // Parcours de toutes les paires clé/valeur reçues
    for (JsonPair kv : doc.as<JsonObject>()) {
      const char* key = kv.key().c_str();
      if (doc[key].is<double>()) {  // Assurez-vous que la valeur peut être traitée comme un double
        double value = doc[key];
        Serial.print("Received ");
        Serial.print(key);
        Serial.print(": ");
        Serial.println(value);

        // Mise à jour de la variable globale correspondante
        if (strcmp(key, "kp") == 0) kp = value;
        else if (strcmp(key, "ki") == 0) ki = value;
        else if (strcmp(key, "kd") == 0) kd = value;
        else if (strcmp(key, "setpoint") == 0) setpoint = value;
        else if (strcmp(key, "setpoint") == 0) motorThreshold = value;
        else if (strcmp(key, "power") == 0) power = (int)value;  // Convertit en int si nécessaire
      }
    }
  }
}

void connectToWiFi() {
  Serial.println("Recherche des réseaux WiFi disponibles...");
  int numNetworks = WiFi.scanNetworks();
  if (numNetworks == 0) {
    Serial.println("Aucun réseau WiFi disponible.");
    return;
  }

  bool isConnected = false;
  for (int i = 0; i < numNetworks && !isConnected; i++) {
    String ssid = WiFi.SSID(i);
    for (int j = 0; j < sizeof(ssidList) / sizeof(ssidList[0]); j++) {
      if (ssid.equals(ssidList[j])) {
        Serial.print("Tentative de connexion à ");
        Serial.println(ssid);
        WiFi.begin(ssidList[j], passwordList[j]);
        delay(1000);  // Attendez un certain temps pour la connexion
        if (WiFi.status() == WL_CONNECTED) {
          Serial.println("Connecté au réseau WiFi avec succès.");
          isConnected = true;
          break;  // Sortir de la boucle après une connexion réussie
        } else {
          Serial.println("Échec de la connexion au réseau WiFi.");
        }
      }
    }
  }
  if (!isConnected) {
    Serial.println("Aucun réseau WiFi correspondant trouvé dans la liste.");
  }
}