#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "../hw/sx1262.h"
#include "radio.h"
#include "esp_log.h"
#include "string.h"
#include "esp_timer.h"
#include "esp_rom_crc.h"
#include <hw/buscfg.h>
#include "sensors.h"

#define TAG "LoRa"

#define ACK_TIMEOUT_MS 250 // Wait 300ms for ACK
#define MAX_RETRIES 1

uint32_t packetIndexTX = 0;
uint32_t packetIndexRX = 0;

TelemetryPacket telemetryPacket;
uint8_t packetReadiness = 0;

volatile bool ackReceived = false;
uint32_t lastAckIndex = 0;

SemaphoreHandle_t loraRadioMutex;

void setup_lora(void)
{
    LoRaInit();

    int8_t txPowerInDbm = 20;
    uint32_t frequencyInHz = 869525000;
    float tcxoVoltage = 2.2;
    bool useRegulatorLDO = true;

    ESP_LOGW(TAG, "Enable TCXO");
    LoRaDebugPrint(false);

    if (LoRaBegin(frequencyInHz, txPowerInDbm, tcxoVoltage, useRegulatorLDO) != 0)
    {
        ESP_LOGE(TAG, "LoRa module not recognized. Halting.");
        while (1)
        {
            vTaskDelay(1);
        }
    }

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

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

static void prepare_downbound_packet(DownBoundPacket *packet, uint8_t type, uint64_t missionTimer, uint32_t crc)
{
    memset(packet, 0, sizeof(DownBoundPacket));
    packet->missionTimer = missionTimer;
    packet->packetIndex = packetIndexTX++;
    ESP_LOGI(TAG_RADIO, "Sending downbound packet with index %ld", packetIndexTX - 1);
    packet->packetType = type;
    packet->CRCCheck = crc;
    strcpy(packet->syncPhrase, "PlechDole");
}

static void send_packet_without_retries(uint8_t *data, uint16_t size)
{
        if (xSemaphoreTake(loraRadioMutex, portMAX_DELAY) == pdTRUE)
        {
            if (!LoRaSend(data, size, SX126x_TXMODE_SYNC))
            {
                ESP_LOGW(TAG, "LoRaSend failed");
                xSemaphoreGive(loraRadioMutex);
            }
            else
            {
                ESP_LOGI(TAG, "%d byte packet sent", size);
                xSemaphoreGive(loraRadioMutex);
            }
        }
}

static void send_packet_with_retries(uint8_t *data, uint16_t size)
{
    for (int retry = 0; retry <= MAX_RETRIES; retry++)
    {
        if (xSemaphoreTake(loraRadioMutex, portMAX_DELAY) == pdTRUE)
        {
            if (!LoRaSend(data, size, SX126x_TXMODE_SYNC))
            {
                ESP_LOGW(TAG, "LoRaSend failed, retry %d", retry);
                xSemaphoreGive(loraRadioMutex);
            }
            else
            {
                ESP_LOGI(TAG, "%d byte packet sent (attempt %d)", size, retry + 1);
                xSemaphoreGive(loraRadioMutex);

                // Wait for ACK
                ackReceived = false;
                uint64_t start_wait = esp_timer_get_time();
                while ((esp_timer_get_time() - start_wait) < (ACK_TIMEOUT_MS * 1000))
                {
                    if (ackReceived)
                    {
                        ESP_LOGI(TAG, "ACK received for packet.");
                        return;
                    }
                    vTaskDelay(pdMS_TO_TICKS(10));
                }

                ESP_LOGW(TAG, "ACK timeout, retrying...");
            }
        }
    }

    ESP_LOGE(TAG, "Failed to send packet after %d retries", MAX_RETRIES);
}

void prepare_and_send_telemetry(uint64_t missionTimer)
{
    uint8_t bufOut[256] = {0};

    DownBoundPacket downboundPacket;
    uint32_t crc = esp_rom_crc32_le(0,(uint8_t *) &telemetryPacket, sizeof(telemetryPacket));
    prepare_downbound_packet(&downboundPacket, DownlinkPacketType_Telemetry, missionTimer, crc);

    uint16_t offset = 0;
    memcpy(bufOut + offset, &downboundPacket, sizeof(downboundPacket));
    offset += sizeof(downboundPacket);
    memcpy(bufOut + offset, &telemetryPacket, sizeof(telemetryPacket));
    offset += sizeof(telemetryPacket);

    send_packet_without_retries(bufOut, offset);
    packetReadiness = 0;
}

static void build_and_send_ack(uint32_t ackIndex, uint32_t crc32Checksum, uint64_t missionTimer)
{
    uint8_t bufOut[256] = {0};

    
    ACKPacket ackPacket = {
        .packetIndex = ackIndex,
        .crc32Checksum = crc32Checksum,
    };

    uint32_t crc = esp_rom_crc32_le(0, (uint8_t *) &ackPacket, sizeof(ackPacket));

    DownBoundPacket downboundPacket;
    prepare_downbound_packet(&downboundPacket, DownlinkPacketType_ACK, missionTimer, crc);

    uint16_t offset = 0;
    memcpy(bufOut + offset, &downboundPacket, sizeof(downboundPacket));
    offset += sizeof(downboundPacket);
    memcpy(bufOut + offset, &ackPacket, sizeof(ackPacket));
    offset += sizeof(ackPacket);

    if (xSemaphoreTake(loraRadioMutex, portMAX_DELAY) == pdTRUE)
    {
        if (!LoRaSend(bufOut, offset, SX126x_TXMODE_SYNC))
        {
            ESP_LOGE(TAG, "Failed to send ACK");
        }
        else
        {
            ESP_LOGI(TAG, "%d byte ACK sent", offset);
        }
        xSemaphoreGive(loraRadioMutex);
    }
}

void process_uplink_packet(uint8_t *data, uint8_t len, uint64_t missionTimer)
{
    if (len < sizeof(UplinkPacket))
    {
        ESP_LOGW(TAG, "Uplink packet too small: %d bytes", len);
        return;
    }

    UplinkPacket uplinkPacket;
    memcpy(&uplinkPacket, data, sizeof(UplinkPacket));

    if (strcmp(UplinkSync, uplinkPacket.syncPhrase) != 0)
    {
        ESP_LOGW(TAG, "Invalid sync phrase");
        return;
    }

    ESP_LOGI(TAG, "Got uplink packet of type %d, index %ld", uplinkPacket.packetType, uplinkPacket.packetIndex);

    uint8_t *payload = data + sizeof(UplinkPacket);

    uint8_t payloadRXLen = len - sizeof(UplinkPacket);

    uint32_t crc = esp_rom_crc32_le(0, payload, payloadRXLen);

    if (crc != uplinkPacket.CRCCheck) {
        ESP_LOGE(TAG, "Received BAD CRC for packet %ld, crc is %ld, should be %ld", uplinkPacket.packetIndex, crc, uplinkPacket.CRCCheck);
        return;
    }

    if (uplinkPacket.packetType == UplinkPacketType_ACK)
    {
        ESP_LOGI(TAG, "Received ACK for packet %ld", uplinkPacket.packetIndex);
        ackReceived = true;
        lastAckIndex = uplinkPacket.packetIndex;
        return;
    }

    if (uplinkPacket.packetIndex == packetIndexRX + 1)
    {
        ESP_LOGI(TAG, "Packet arrived in correct order");
        packetIndexRX = uplinkPacket.packetIndex;

        uint32_t crc = esp_rom_crc32_le(0, data + sizeof(UplinkPacket), payloadRXLen);
        build_and_send_ack(packetIndexRX, crc, missionTimer);

        switch (uplinkPacket.packetType)
        {
        case UplinkPacketType_SystemControl:
            if (payloadRXLen == sizeof(SystemControlPacket))
            {
                SystemControlPacket sysCtrl;
                memcpy(&sysCtrl, data + sizeof(UplinkPacket), sizeof(SystemControlPacket));
                setPowerMode(sysCtrl.powerMode);
                // TODO: Process sysCtrl
            }
            else
            {
                ESP_LOGW(TAG, "SystemControlPacket size mismatch");
            }
            break;

        case UplinkPacketType_Ping:
            // TODO: handle Ping
            break;

        default:
            ESP_LOGW(TAG, "Unknown uplink packet type %d", uplinkPacket.packetType);
            break;
        }
    }
    else if (uplinkPacket.packetIndex > packetIndexRX + 1)
    {
        ESP_LOGW(TAG, "Skipped %ld packets", uplinkPacket.packetIndex - (packetIndexRX + 1));
        packetIndexRX = uplinkPacket.packetIndex;
    }
    else
    {
        ESP_LOGW(TAG, "Duplicate packet: %ld", (packetIndexRX + 1) - uplinkPacket.packetIndex);
    }
}

void lora_receive_task(void *pvParameters)
{
    ESP_LOGI(TAG, "lora_receive_task started");
    uint8_t bufIn[256];
    while (1)
    {
        // Wait to take the semaphore before accessing LoRa
        if (xSemaphoreTake(loraRadioMutex, 0) == pdTRUE)
        {
            uint8_t rxLen = LoRaReceive(bufIn, sizeof(bufIn));
            if (rxLen > 0)
            {
                ESP_LOGI(TAG, "%d byte packet received", rxLen);
                process_uplink_packet(bufIn, rxLen, esp_timer_get_time());
                int8_t rssi, snr;
                GetPacketStatus(&rssi, &snr);
                ESP_LOGI(TAG, "rssi=%d[dBm], snr=%d[dB]", rssi, snr);
            }

            // Release the semaphore when done with LoRa RX
            xSemaphoreGive(loraRadioMutex);
        }
        vTaskDelay(pdMS_TO_TICKS(10)); // Delay to prevent busy-waiting
    }
}

void lora_comms_task(void *pvParameters)
{



    ESP_LOGI(TAG, "lora_comms_task started");
    
    // while (foundDevices[0] != 2) {
    //     vTaskDelay(10);
    // }
    
    // Initialize the semaphore for radio access (binary semaphore, 1 = available)
    loraRadioMutex = xSemaphoreCreateMutex();
    xSemaphoreGive(loraRadioMutex); // Set semaphore as available

    setup_lora();
    xTaskCreate(
        lora_receive_task,
        "LoraReceiveTask",
        8192,
        NULL,
        (tskIDLE_PRIORITY + 2),
        NULL);
    while (1)
    {
        int64_t start_time = esp_timer_get_time();

        if (packetReadiness)
        {
            ESP_LOGI(TAG, "Preparing telemetry");

            prepare_and_send_telemetry(start_time);
        }

        const int64_t interval_us = ((powerMode == HIGH_POWER_MODE) ? 10000 : 10000000); // 10 ms or 10 000 ms

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

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