#include <Wire.h>
#include <HardwareSerial.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <freertos/queue.h>

// Define GPS UART
HardwareSerial GPSSerial(1);

// Define IMU addresses and variables
const int MPU = 0x68; // I2C address of the MPU-6050
float ax, ay, az, gx, gy, gz;

// Define a structure for GPS data
typedef struct {
    float latitude;
    float longitude;
} GPSData;

// Define a structure for IMU data
typedef struct {
    float accelX;
    float accelY;
    float accelZ;
    float gyroX;
    float gyroY;
    float gyroZ;
} IMUData;

// Queues for sensor data
QueueHandle_t imuQueue;
QueueHandle_t gpsQueue;

// Threshold for distance change
const float DISTANCE_THRESHOLD = 100.0; // in meters

// Function declarations
void gpsTask(void *pvParameters);
void imuTask(void *pvParameters);
void distanceCheckTask(void *pvParameters);
float calculateDistance(float lat1, float lon1, float lat2, float lon2);

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

    // Initialize GPS UART
    GPSSerial.begin(9600, SERIAL_8N1,16, 17); // GPS RX, TX pins

    // Initialize I2C
    Wire.begin( 21, 22);//);
    Wire.beginTransmission(MPU);
    Wire.write(0x6B); // PWR_MGMT_1 register
    Wire.write(0);    // set to zero (wakes up the MPU-6050)
    Wire.endTransmission(true);

    // Create queues
    imuQueue = xQueueCreate(10, sizeof(IMUData));
    gpsQueue = xQueueCreate(10, sizeof(GPSData));

    // Create tasks
    xTaskCreate(gpsTask, "GPSTask", 2048, NULL, 1, NULL);
    xTaskCreate(imuTask, "IMUTask", 2048, NULL, 1, NULL);
    xTaskCreate(distanceCheckTask, "DistanceCheckTask", 2048, NULL, 1, NULL);
}

void loop() {
    // Nothing to do in the loop as tasks handle everything
}

void gpsTask(void *pvParameters) {
    GPSData gpsData;
    char gpsBuffer[100];
    int index = 0;
    while (true) {
        if (GPSSerial.available()) {
            char c = GPSSerial.read();
            if (c == '\n') {
                gpsBuffer[index] = '\0';
                // Parse the GPS data from the buffer (Assuming NMEA format)
                // Extract latitude and longitude from $GPGGA or $GPRMC sentences
                // For simplicity, let's assume we have a function `parseGPS` that does this
                if (parseGPS(gpsBuffer, &gpsData.latitude, &gpsData.longitude)) {
                    xQueueSend(gpsQueue, &gpsData, portMAX_DELAY);
                }
                index = 0;
            } else {
                gpsBuffer[index++] = c;
            }
        }
        vTaskDelay(10 / portTICK_PERIOD_MS);
    }
}

bool parseGPS(char *buffer, float *latitude, float *longitude) {
    // Implement parsing logic here
    // Return true if parsing is successful, false otherwise
    // This is a placeholder for actual parsing logic
    return true;
}

void imuTask(void *pvParameters) {
    IMUData imuData;
    while (true) {
        Wire.beginTransmission(MPU);
        Wire.write(0x3B); // Starting with register 0x3B (ACCEL_XOUT_H)
        Wire.endTransmission(false);
        Wire.requestFrom(MPU, 14, true); // Request a total of 14 registers

        imuData.accelX = Wire.read() << 8 | Wire.read();
        imuData.accelY = Wire.read() << 8 | Wire.read();
        imuData.accelZ = Wire.read() << 8 | Wire.read();
        Wire.read(); Wire.read(); // Ignore temperature data
        imuData.gyroX = Wire.read() << 8 | Wire.read();
        imuData.gyroY = Wire.read() << 8 | Wire.read();
        imuData.gyroZ = Wire.read() << 8 | Wire.read();

        xQueueSend(imuQueue, &imuData, portMAX_DELAY);
        vTaskDelay(100 / portTICK_PERIOD_MS);
    }
}

void distanceCheckTask(void *pvParameters) {
    GPSData currentGPSData, previousGPSData;
    IMUData imuData;
    bool firstGPSDataReceived = false;

    while (true) {
        if (xQueueReceive(gpsQueue, &currentGPSData, portMAX_DELAY)) {
            if (firstGPSDataReceived) {
                float distance = calculateDistance(previousGPSData.latitude, previousGPSData.longitude,
                                                   currentGPSData.latitude, currentGPSData.longitude);
                if (distance > DISTANCE_THRESHOLD) {
                    // Print GPS coordinates and IMU data
                    Serial.print("New Coordinates: ");
                    Serial.print("Lat: "); Serial.print(currentGPSData.latitude, 6);
                    Serial.print(", Lon: "); Serial.println(currentGPSData.longitude, 6);

                    if (xQueueReceive(imuQueue, &imuData, 0)) {
                        Serial.print("IMU Data - Accel: ");
                        Serial.print(imuData.accelX); Serial.print(", ");
                        Serial.print(imuData.accelY); Serial.print(", ");
                        Serial.print(imuData.accelZ);
                        Serial.print(" Gyro: ");
                        Serial.print(imuData.gyroX); Serial.print(", ");
                        Serial.print(imuData.gyroY); Serial.print(", ");
                        Serial.println(imuData.gyroZ);
                    }
                }
            } else {
                firstGPSDataReceived = true;
            }
            previousGPSData = currentGPSData;
        }
    }
}

float calculateDistance(float lat1, float lon1, float lat2, float lon2) {
    // Implement the Haversine formula to calculate distance between two coordinates
    // This is a placeholder for actual distance calculation logic
    return 101.0; // For simplicity, always return a distance greater than 100 meters
}