#include <SPI.h>
#include <SD.h>
#include <Adafruit_GPS.h>
#include <TimeLib.h>

// Déclaration de la broche CS
const int chipSelect = 5; // GPIO 5 pour le CS sur un ESP32-E

// Paramètres pour le module GPS
HardwareSerial mySerial(1);
Adafruit_GPS GPS(&mySerial);

// Broche analogique pour la girouette
const int windVanePin = 35;

// Variables pour stocker la position et le temps précédents pour le calcul de la vitesse
float prevLat = 0.0;
float prevLon = 0.0;
uint32_t prevTime = 0;

File dataFile;

void setup() {
    Serial.begin(115200);
    Serial.println("Initialisation du système...");

    // Initialisation du GPS
    GPS.begin(9600);
    GPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA);
    GPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
    GPS.sendCommand(PGCMD_ANTENNA);
    Serial.println("GPS initialisé");

    // Initialisation de la carte SD
    if (!SD.begin(chipSelect)) {
        Serial.println("Échec de l'initialisation de la carte SD !");
        return;
    }
    Serial.println("Carte SD initialisée");

    // Création du fichier texte avec la date et l'heure actuelles comme nom
    uint32_t timestamp = getGPSTime();
    String filename = String(timestamp) + ".txt";
    dataFile = SD.open(filename.c_str(), FILE_WRITE);
    if (dataFile) {
        Serial.println("Fichier créé : " + filename);
    } else {
        Serial.println("Erreur lors de la création du fichier !");
    }
}

void loop() {
    // Récupération de la date et de l'heure en format 32 bits
    uint32_t timestamp = getGPSTime();

    // Récupération des coordonnées GPS en degrés
    float latitude = GPS.latitudeDegrees;
    float longitude = GPS.longitudeDegrees;

    // Affichage des coordonnées et de l'heure GPS
    Serial.print("Heure GPS : ");
    Serial.println(timestamp);
    Serial.print("Latitude : ");
    Serial.println(latitude, 6);
    Serial.print("Longitude : ");
    Serial.println(longitude, 6);

    // Récupération du cap en ° selon le GPS
    float gpsHeading = GPS.angle;
    Serial.print("Cap GPS : ");
    Serial.print(gpsHeading);
    Serial.println("°");

    // Récupération de la direction du vent en °
    float windDirection = analogRead(windVanePin) * (360.0 / 4095.0);
    Serial.print("Direction du vent : ");
    Serial.print(windDirection);
    Serial.println("°");

    // Calcul de la vitesse en noeuds par rapport au dernier point GPS
    float speed = calculateSpeed(latitude, longitude, timestamp);
    Serial.print("Vitesse : ");
    Serial.print(speed);
    Serial.println("nd");

    // Écriture dans le fichier texte
    if (dataFile) {
        uint32_t timestampBytes = htonl(timestamp);
        uint16_t latBytes = htons((uint16_t)(latitude * 100));
        uint16_t lonBytes = htons((uint16_t)(longitude * 100));
        uint16_t gpsHeadingBytes = htons((uint16_t)(gpsHeading * 10));
        uint16_t windDirectionBytes = htons((uint16_t)(windDirection * 10));
        uint16_t speedBytes = htons((uint16_t)(speed * 100));
        uint8_t checksum = 0;

        checksum += writeAndChecksum(dataFile, (uint8_t*)&timestampBytes, 4);
        checksum += writeAndChecksum(dataFile, (uint8_t*)&latBytes, 2);
        checksum += writeAndChecksum(dataFile, (uint8_t*)&lonBytes, 2);
        checksum += writeAndChecksum(dataFile, (uint8_t*)&gpsHeadingBytes, 2);
        checksum += writeAndChecksum(dataFile, (uint8_t*)&windDirectionBytes, 2);
        checksum += writeAndChecksum(dataFile, (uint8_t*)&speedBytes, 2);

        dataFile.write(checksum);
        dataFile.flush(); // Ensure data is written to the file
        Serial.println("Données enregistrées dans le fichier");
    } else {
        Serial.println("Erreur lors de l'ouverture du fichier !");
    }

    // Délai entre deux points
    delay(1000);
}

uint8_t writeAndChecksum(File &dataFile, uint8_t *data, size_t length) {
    uint8_t checksum = 0;
    for (size_t i = 0; i < length; i++) {
        dataFile.write(data[i]);
        checksum += data[i];
    }
    return checksum;
}

float calculateSpeed(float lat, float lon, uint32_t timestamp) {
    if (prevTime == 0) {
        prevLat = lat;
        prevLon = lon;
        prevTime = timestamp;
        return 0.0;
    }

    float distance = haversineDistance(prevLat, prevLon, lat, lon);
    float timeElapsed = (timestamp - prevTime) / 1000.0; // Conversion en secondes
    float speed = (distance / timeElapsed) * 3600.0 / 1852.0; // Conversion m/s en noeuds

    prevLat = lat;
    prevLon = lon;
    prevTime = timestamp;

    return speed;
}

float haversineDistance(float lat1, float lon1, float lat2, float lon2) {
    const float R = 6371000; // Rayon de la Terre en mètres
    float dLat = radians(lat2 - lat1);
    float dLon = radians(lon2 - lon1);
    float a = sin(dLat / 2) * sin(dLat / 2) +
              cos(radians(lat1)) * cos(radians(lat2)) *
              sin(dLon / 2) * sin(dLon / 2);
    float c = 2 * atan2(sqrt(a), sqrt(1 - a));
    return R * c;
}

uint32_t getGPSTime() {
    // Combine les données de l'heure et de la date du GPS en un format 32 bits
    tmElements_t tm;
    tm.Second = GPS.seconds;
    tm.Minute = GPS.minute;
    tm.Hour = GPS.hour;
    tm.Day = GPS.day;
    tm.Month = GPS.month;
    tm.Year = GPS.year - 1970; // TimeLib.h utilise 1970 comme année de référence

    return makeTime(tm);
}