#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "sx1262.h"
#include "radio.h"
#include "esp_log.h"
#include "string.h"
#include "esp_timer.h"
#include "packets.h"
#include "esp_rom_crc.h"


#define TAG "LoRaGS"

uint32_t uplinkPacketIndex = 0;

TelemetryPacket telemetryPacket;
uint8_t packetReadiness = 0;

void send_ack(uint32_t packetIndex, uint32_t crc)
{
    ACKPacket ack;
    ack.packetIndex = packetIndex;
    ack.crc32Checksum = crc;

    UplinkPacket uplinkHeader = {0};
    memcpy(uplinkHeader.syncPhrase, UplinkSync, sizeof(UplinkSync));
    uplinkHeader.packetIndex = uplinkPacketIndex++;
    uplinkHeader.packetType = UplinkPacketType_ACK;

    uint8_t txBuf[256] = {0};
    int offset = 0;

    memcpy(txBuf + offset, &uplinkHeader, sizeof(uplinkHeader));
    offset += sizeof(uplinkHeader);

    memcpy(txBuf + offset, &ack, sizeof(ack));
    offset += sizeof(ack);

    ESP_LOGI(TAG, "Sending ACK for packet %lu...", packetIndex);

    if (!LoRaSend(txBuf, offset, SX126x_TXMODE_SYNC))
    {
        ESP_LOGE(TAG, "ACK Send Failed");
    }
}

#define MAG_SCALE (4912.0f / 32760.0f)

void printTelemetryPacket(const TelemetryPacket *packet)
{
    ESP_LOGI(TAG, "Telemetry Packet:");

    float accel_f[3], gyro_f[3], temp_f, magnet_f[3];

    accel_f[0] = packet->accelerationX / 16384.0; // Accel X in g
    accel_f[1] = packet->accelerationY / 16384.0; // Accel Y in g
    accel_f[2] = packet->accelerationZ / 16384.0; // Accel Z in g

    gyro_f[0] = packet->gyroX / 131.0; // Gyro X in deg/s
    gyro_f[1] = packet->gyroY / 131.0; // Gyro Y in deg/s
    gyro_f[2] = packet->gyroZ / 131.0; // Gyro Z in deg/s

    magnet_f[0] = packet->magnetX * MAG_SCALE; // Gyro X in deg/s
    magnet_f[1] = packet->magnetY * MAG_SCALE; // Gyro Y in deg/s
    magnet_f[2] = packet->magnetZ * MAG_SCALE; // Gyro Z in deg/s

    temp_f = (packet->accelerometer_temperature / 333.87) + 21.0; // Temperature in °C

    // MPU Data
    ESP_LOGI(TAG, "  MPU:");
    ESP_LOGI(TAG, "    Acceleration [X: %d, Y: %d, Z: %d]", packet->accelerationX, packet->accelerationY, packet->accelerationZ);
    ESP_LOGI(TAG, "    Gyroscope [X: %d, Y: %d, Z: %d]", packet->gyroX, packet->gyroY, packet->gyroZ);
    ESP_LOGI(TAG, "    Magnetometer [X: %d, Y: %d, Z: %d]", packet->magnetX, packet->magnetY, packet->magnetZ);
    ESP_LOGI(TAG, "    Accelerometer Temp: %d", packet->accelerometer_temperature);

    ESP_LOGI(TAG, "  MPU (Processed Readings):");

    ESP_LOGI(TAG, "    Acceleration     [X: %.3f g, Y: %.3f g, Z: %.3f g]", accel_f[0], accel_f[1], accel_f[2]);
    ESP_LOGI(TAG, "    Gyroscope      [X: %.3f deg/s, Y: %.3f deg/s, Z: %.3f deg/s]", gyro_f[0], gyro_f[1], gyro_f[2]);
    ESP_LOGI(TAG, "    Magnetometer    [X: %.3f uT, Y: %.3f uT, Z: %.3f uT]", magnet_f[0], magnet_f[1], magnet_f[2]);
    ESP_LOGI(TAG, "    Accelerometer Temp: %f °C", temp_f);

    // CCS Data
    ESP_LOGI(TAG, "  CCS:");
    ESP_LOGI(TAG, "    eCO2: %u ppm", packet->eCO2);
    ESP_LOGI(TAG, "    TVOC: %u ppb", packet->tvoc);
    ESP_LOGI(TAG, "    Current CCS: %u", packet->currentCCS);
    ESP_LOGI(TAG, "    Raw CCS Data: %u", packet->rawCCSData);

    float miliVolts = packet->volts * 1.25;
    float miliAmps = packet->current * 1.25;
    float power = packet->power * 10;

    // INA Data
    ESP_LOGI(TAG, "  INA:");
    ESP_LOGI(TAG, "    Voltage: %f mV", miliVolts);
    ESP_LOGI(TAG, "    Current: %f mA", miliAmps);
    ESP_LOGI(TAG, "    Power: %f mW", power);

    // BME Data
    ESP_LOGI(TAG, "  BME:");
    ESP_LOGI(TAG, "    Temperature: %.2f °C", packet->temperature / 100.0f);
    ESP_LOGI(TAG, "    Humidity: %.2f %%", packet->humidity / 100.0f);
    ESP_LOGI(TAG, "    Pressure: %.2f hPa", packet->pressure / 100.0f);
    ESP_LOGI(TAG, "    Gas Resistance: %u Ohms", packet->gas);
    ESP_LOGI(TAG, "    Gas Valid: %s", packet->gas_valid ? "Yes" : "No");
    ESP_LOGI(TAG, "    Heater Stable: %s", packet->heater_stable ? "Yes" : "No");
    ESP_LOGI(TAG, "    Gas Range: %u", packet->gas_range);
    ESP_LOGI(TAG, "    Gas Index: %u", packet->gas_index);

    // BME Processed Data
    ESP_LOGI(TAG, "  BME (Processed / Compensated Readings):");
    ESP_LOGI(TAG, "    Air Temperature: %.2f °C", packet->air_temperature);
    ESP_LOGI(TAG, "    Relative Humidity: %.2f %%", packet->relative_humidity);
    ESP_LOGI(TAG, "    Barometric Pressure: %.2f hPa", packet->barometric_pressure);
    ESP_LOGI(TAG, "    Gas Resistance: %.2f Ohms", packet->gas_resistance);
    ESP_LOGI(TAG, "    IAQ Score: %u", packet->iaq_score);
    ESP_LOGI(TAG, "    Temperature Score: %.2f", packet->temperature_score);
    ESP_LOGI(TAG, "    Humidity Score: %.2f", packet->humidity_score);
    ESP_LOGI(TAG, "    Gas Score: %.2f", packet->gas_score);

    // GPS Data
    ESP_LOGI(TAG, "  GPS:");
    ESP_LOGI(TAG, "    Time (seconds): %u", packet->time_seconds);
    ESP_LOGI(TAG, "    Latitude: %.4f°", packet->latitude_centi_degrees / 10000.0f);
    ESP_LOGI(TAG, "    Longitude: %.4f°", packet->longitude_centi_degrees / 10000.0f);
    ESP_LOGI(TAG, "    Altitude: %.2f m", packet->altitude_centi_meters / 100.0f);
    ESP_LOGI(TAG, "    Fix Quality: %u", packet->fix_quality);
    ESP_LOGI(TAG, "    Satellites: %u", packet->num_satellites);
    ESP_LOGI(TAG, "    Date (YYDDMM): %06u", packet->date_yyddmm);
    ESP_LOGI(TAG, "    Speed: %.2f knots", packet->speed_centi_knots / 100.0f);

    ESP_LOGI(TAG, "  GPS Prediction:");
    ESP_LOGI(TAG, "    Predicted Latitude: %.4f°", packet->predicted_latitude_centi_degrees / 10000.0f);
    ESP_LOGI(TAG, "    Predicted Longitude: %.4f°", packet->predicted_longitude_centi_degrees / 10000.0f);
    ESP_LOGI(TAG, "    Predicted Altitude: %.2f m", packet->predicted_altitude_centi_meters / 100.0f);

    // ADC Data
    ESP_LOGI(TAG, "  ADC Sensors:");
    ESP_LOGI(TAG, "    NH3: %d", packet->NH3);
    ESP_LOGI(TAG, "    CO: %d", packet->CO);
    ESP_LOGI(TAG, "    NO2: %d", packet->NO2);
    ESP_LOGI(TAG, "    UVC: %d", packet->UVC);

    // Servo Data
    ESP_LOGI(TAG, "  Servos:");
    ESP_LOGI(TAG, "    Servo A: Current = %d, Target = %d", packet->currentServoA, packet->targetServoA);
    ESP_LOGI(TAG, "    Servo B: Current = %d, Target = %d", packet->currentServoB, packet->targetServoB);
}

void handle_downlink_packet(uint8_t *buf, uint8_t rxLen)
{
    if (rxLen < sizeof(DownBoundPacket))
    {
        ESP_LOGW(TAG, "Received packet too small to be valid.");
        return;
    }

    DownBoundPacket down;
    memcpy(&down, buf, sizeof(DownBoundPacket));

    ESP_LOGI(TAG, "Downlink packet index: %lu, type: %u", down.packetIndex, down.packetType);

    // Verify sync phrase
    if (strncmp(down.syncPhrase, DownlinkSync, strlen(DownlinkSync)) != 0)
    {
        ESP_LOGW(TAG, "Invalid sync phrase, ignoring packet.");
        return;
    }

    uint8_t *payload = buf + sizeof(DownBoundPacket);
    uint32_t payloadSize = rxLen - sizeof(DownBoundPacket);

    switch (down.packetType)
    {
    case DownlinkPacketType_Telemetry:
        if (payloadSize >= sizeof(TelemetryPacket))
        {
            memcpy(&telemetryPacket, payload, sizeof(TelemetryPacket));
            ESP_LOGI(TAG, "Telemetry packet received!");
            printTelemetryPacket(&telemetryPacket);
            // Here you would actually do something with the telemetry, like save it or display it
        }
        else
        {
            ESP_LOGW(TAG, "Telemetry packet too small (%u bytes)", payloadSize);
        }
        break;
    case DownlinkPacketType_Ping:
        ESP_LOGI(TAG, "Ping packet received!");
        // Optionally respond with pong here
        break;
    case DownlinkPacketType_ACK:
        ESP_LOGI(TAG, "ACK packet received!");
        break;
    default:
        ESP_LOGW(TAG, "Unknown packet type: %u", down.packetType);
        break;
    }

    uint32_t crc = esp_rom_crc32_le(0, &(buf[0]) + sizeof(DownBoundPacket), payloadSize);
    

    send_ack(down.packetIndex, crc);
}

void lora_comms_task(void *pvParameters)
{
    const int64_t interval_us = 100000; // 100 ms
    int64_t start_time, end_time, elapsed;

    LoRaInit();
    int8_t txPowerInDbm = 20;
    uint32_t frequencyInHz = 869525000;

    ESP_LOGW(TAG, "Enable TCXO");
    float tcxoVoltage = 2.2;
    bool useRegulatorLDO = true;

    LoRaDebugPrint(false);
    if (LoRaBegin(frequencyInHz, txPowerInDbm, tcxoVoltage, useRegulatorLDO) != 0)
    {
        ESP_LOGE(TAG, "Does not recognize the module");
        while (1)
        {
            vTaskDelay(1);
        }
    }

    uint8_t spreadingFactor = 6;
    uint8_t bandwidth = SX126X_LORA_BW_250_0;
    uint8_t codingRate = SX126X_LORA_CR_4_8;
    uint16_t preambleLength = 8;
    bool crcOn = true;
    bool invertIrq = false;

    LoRaConfig(spreadingFactor, bandwidth, codingRate, preambleLength, 0, crcOn, invertIrq);

    uint8_t bufIn[256];

    while (1)
    {
        start_time = esp_timer_get_time();

        uint8_t rxLen = LoRaReceive(bufIn, sizeof(bufIn));
        if (rxLen > 0)
        {
            ESP_LOGI(TAG, "%d byte packet received", rxLen);

            int8_t rssi, snr;
            GetPacketStatus(&rssi, &snr);
            ESP_LOGI(TAG, "rssi=%d[dBm] snr=%d[dB]", rssi, snr);

            handle_downlink_packet(bufIn, rxLen);
        }

        int lost = GetPacketLost();
        if (lost != 0)
        {
            ESP_LOGW(TAG, "%d packets lost", lost);
        }

        end_time = esp_timer_get_time();
        elapsed = end_time - start_time;

        if (elapsed < interval_us)
        {
            vTaskDelay(pdMS_TO_TICKS((interval_us - elapsed) / 1000));
        }
    }
}