#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"

const char *msg = "Testing 123 test why is this not on air";

#define TAG "LoRa"

uint32_t packetIndex = 0;

TelemetryPacket telemetryPacket;
uint8_t packetReadiness = 0;

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 = 0;
    frequencyInHz = 869525000;
    ESP_LOGW(TAG, "Enable TCXO");
    float tcxoVoltage = 2.2;     // use TCXO
    bool useRegulatorLDO = true; // use DCDC + LDO

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

    uint8_t spreadingFactor = 7;
    uint8_t bandwidth = SX126X_LORA_BW_250_0;
    uint8_t codingRate = SX126X_LORA_CR_4_8;
    uint16_t preambleLength = 8;
    uint8_t payloadLen = 0;
    bool crcOn = true;
    bool invertIrq = false;
    LoRaConfig(spreadingFactor, bandwidth, codingRate, preambleLength, payloadLen, crcOn, invertIrq);
    uint8_t bufIn[256];  // Maximum Payload size of SX1261/62/68 is 255
    uint8_t bufOut[256]; // Maximum Payload size of SX1261/62/68 is 255

    DownBoundPacket downboundPacket;
    UplinkPacket uplinkPacket;
    SystemControlPacket systemControlPacket;

    while (1)
    {
        start_time = esp_timer_get_time(); // µs since boot
        if (packetReadiness == 1)
        {
            uint8_t downPacketSize = 0;
            memset(bufOut, 0, sizeof(bufOut));
            downboundPacket.missionTimer = start_time;
            downboundPacket.packetIndex = packetIndex++;
            downboundPacket.packetType = 1;

            memcpy(bufOut, &downboundPacket, sizeof(downboundPacket));
            downPacketSize += sizeof(downboundPacket);
            memcpy(((uint8_t *)bufOut) + downPacketSize, &telemetryPacket, sizeof(telemetryPacket));
            downPacketSize += sizeof(telemetryPacket);

            ESP_LOGI(pcTaskGetName(NULL), "%d byte packet sent...", downPacketSize);

            // Wait for transmission to complete
            if (LoRaSend(bufOut, downPacketSize, SX126x_TXMODE_SYNC) == false)
            {
                ESP_LOGE(pcTaskGetName(NULL), "LoRaSend fail");
            } else {
                packetReadiness = 0;
            }
        }

        uint8_t rxLen = LoRaReceive(bufIn, sizeof(bufIn));
        if (rxLen > 0)
        {
            ESP_LOGI(pcTaskGetName(NULL), "%d byte packet received:[%.*s]", rxLen, rxLen, bufIn);

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

        int lost = GetPacketLost();
        if (lost != 0)
        {
            ESP_LOGW(pcTaskGetName(NULL), "%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));
        }
    }
}