#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "../hw/sx1262.h"
#include "radio.h"

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

void lora_comms_task(void *pvParameters)
{
    sx1262_init();
    sx1262_resetStats();
    ESP_LOGI(TAG_RADIO, "Setting standby RC");
    sx1262_setStandby(SX1262_STANDBY_RC);

    ESP_LOGI(TAG_RADIO, "Getting version");
    unsigned char versDat[16];
    sx1262_readRegister(0x0320, versDat, 16);
    ESP_LOGI(TAG_RADIO, "Version: %s", versDat);

    sx1262_get_status();

    // Read status
    sx1262_get_status();

    ESP_LOGI(TAG_RADIO, "Setting TCXO");
    sx1262_setDIO3AsTCXOCtrl(SX1262_TCXO_VOLTAGE22dV, 320);

    sx1262_get_status();

    ESP_LOGI(TAG_RADIO, "Setting Buffer base address");
    sx1262_setBufferBaseAddress(0, 0);
    sx1262_get_status();

    ESP_LOGI(TAG_RADIO, "Setting packet Type");
    sx1262_setPacketType(SX1262_PACKET_TYPE_LORA);
    sx1262_get_status();

    ESP_LOGI(TAG_RADIO, "Setting RXTX fallback mode");
    sx1262_setRxTXFallbackMode(SX1262_FALLBACK_RC);
    sx1262_get_status();

    ESP_LOGI(TAG_RADIO, "Setting CAD params");
    sx1262_setCadParams(0x03, 0x16, 0x0A, 0, 0);
    sx1262_get_status();

    ESP_LOGI(TAG_RADIO, "Clearing IRQ status");
    sx1262_clearIrqStatus(0x43FF);
    sx1262_get_status();

    ESP_LOGI(TAG_RADIO, "Setting Image rejection");
    sx1262_calibrateImage(0xD7, 0xDB);

    sx1262_get_status();

    ESP_LOGI(TAG_RADIO, "Calibrating Image rejection");
    sx1262_calibrate(SX1262_CALIBRATION_ALL);
    sx1262_get_status();

    ESP_LOGI(TAG_RADIO, "Setting regulator");
    sx1262_setRegulatorMode(SX1262_REGULATOR_DC_DC_LDO);
    sx1262_get_status();

    uint8_t modType;
    modType = sx1262_getPacketType();
    sx1262_get_status();
    ESP_LOGI(TAG_RADIO, "ModType %d", modType);

    sx1262_LoRaModulationParams_t loraModParams;

    loraModParams.bandwidth = SX1262_LORA_BW_125;
    loraModParams.codingRate = SX1262_LORA_CR_4_5;
    loraModParams.lowDataRateOpt = 1;
    loraModParams.spreadingFactor = SX1262_LORA_SF7;

    ESP_LOGI(TAG_RADIO, "Setting modulation params");
    sx1262_setLoRaModulationParams(&loraModParams);

    sx1262_get_status();

    ESP_LOGI(TAG_RADIO, "Setting Sync word");
    uint8_t syncWord[2] = {0x14, 0x24};

    sx1262_writeRegister(SX1262_LORA_SYNC_WORD_MSB, syncWord, 2);
    sx1262_get_status();

    uint8_t ocpBuf[1] = {SX126X_OCP_LEVEL_SX1262};

    ESP_LOGI(TAG_RADIO, "Setting overcurrent protection");
    sx1262_writeRegister(SX126X_OCP_CONFIGURATION, ocpBuf, 1);

    sx1262_get_status();

    ESP_LOGI(TAG_RADIO, "Setting RF Switch out");
    sx1262_setDIO2AsRfSwitchCtrl(1);

    sx1262_get_status();

    sx1262_LoRaPacketParams_t loraPacketParams;
    loraPacketParams.preambleLength = 10;
    loraPacketParams.crcType = SX1262_CRC_ON;
    loraPacketParams.headerType = SX1262_HEADER_TYPE_VARIABLE;
    loraPacketParams.invertIQ = SX1262_STANDARD_IQ;
    loraPacketParams.payloadLength = sizeof(msg);

    ESP_LOGI(TAG_RADIO, "Setting packet params");
    sx1262_setLoRaPacketParams(&loraPacketParams);

    sx1262_get_status();

    ESP_LOGI(TAG_RADIO, "Setting frequency");
    sx1262_setFrequency(869525000);

    uint8_t clampConfig[1] = {0xDE};

    ESP_LOGI(TAG_RADIO, "Setting TX clamp");
    sx1262_writeRegister(SX126X_TX_CLAMP_CONFIG, clampConfig, 1);
    sx1262_get_status();

    ESP_LOGI(TAG_RADIO, "Setting TX power");
    sx1262_configure_tx_power(0x04, 0x07, 0x01, LORA_TX_POWER, SX1262_Ramp_200U);

    sx1262_get_status();

    sx1262_get_status();

    ESP_LOGI(TAG_RADIO, "Setting standby XOSC");
    sx1262_setStandby(SX1262_STANDBY_XOSC);

    sx1262_get_status();

    sx1262_setDioIrqParams(SX1262_IRQ_ALL, SX1262_IRQ_TXDone, 0, 0);

    sx1262_get_status();

    ESP_LOGI(TAG_RADIO, "Setting buffer base address");
    sx1262_setBufferBaseAddress(0, 0);

    sx1262_get_status();

    ESP_LOGI(TAG_RADIO, "Writing message");
    sx1262_writeBuffer(0, (uint8_t *)msg, sizeof(msg));

    ESP_LOGI(TAG_RADIO, "Clearing IRQs");
    sx1262_clearIrqStatus(SX1262_IRQ_ALL);

    // sx1262_setTxContinuousWave();
    sx1262_get_status();

    ESP_LOGI(TAG_RADIO, "Getting modulation type...");
    modType = sx1262_getPacketType();
    ESP_LOGI(TAG_RADIO, "ModType %d", modType);
    sx1262_get_status();

    // ESP_LOGI(TAG_RADIO, "Enabling CW...");
    // sx1262_setTxContinuousWave();
    // sx1262_get_status();

    uint8_t txModulation[1] = {0x04};

    ESP_LOGI(TAG_RADIO, "Setting modulation thing");
    sx1262_writeRegister(SX126X_TX_MODULATION, txModulation, 1);
    sx1262_get_status();

    ESP_LOGI(TAG_RADIO, "Clearing IRQs");
    sx1262_clearIrqStatus(SX1262_IRQ_ALL);
    sx1262_get_status();

    ESP_LOGI(TAG_RADIO, "Enabling TX...");
    sx1262_setMode(SX1262_TRANSMIT_MODE, 0);
    sx1262_get_status();

    ESP_LOGI(TAG_RADIO, "Waiting for TX finish");
    while (1)
    {
        if (gpio_get_level(LORA_DIO1))
        {
            ESP_LOGI(TAG_RADIO, "Got IRQin");
            uint16_t irqS = sx1262_getIrqStatus();
            if (irqS & SX1262_IRQ_TXDone)
            {
                break;
            }
        }
        vTaskDelay(pdMS_TO_TICKS(10));
    }

    // vTaskDelay(pdMS_TO_TICKS(2000));
    // ESP_LOGI(TAG_RADIO, "Returning back to sleep");
    // sx1262_setStandby(SX1262_STANDBY_XOSC);

    // sx1262_get_status();

    ESP_LOGI(TAG_RADIO, "Clearing IRQs");
    sx1262_clearIrqStatus(SX1262_IRQ_TXDone);

    sx1262_get_status();
    while (1)
    {
        vTaskDelay(pdMS_TO_TICKS(1000));
    }

    // Set frequency to 868 MHz

    // Set TX power to 22 dBm

    // sx1262_setTxContinuousWave();
}