#include "Sx1262.h"

#include <cstring>
#include "hal/gpio_hal.h"

constexpr const char* TAG = "Sx1262";

void IRAM_ATTR dio1handler(void* context) {
    ((Sx1262*)context)->dio1Event();
}

bool Sx1262::configure(const Parameter parameter, const float value) {
    using enum Parameter;

    switch (parameter) {
        case Power:
            power = value;
            TT_LOG_I(TAG, "Configure %s=%d", toString(parameter), power);
            return true;
        case Frequency:
            frequency = value;
            TT_LOG_I(TAG, "Configure %s=%f", toString(parameter), frequency);
            return true;
        case Bandwidth:
            bandwidth = value;
            TT_LOG_I(TAG, "Configure %s=%f", toString(parameter), bandwidth);
            return true;
        case SpreadFactor:
            spreadFactor = value;
            TT_LOG_I(TAG, "Configure %s=%d", toString(parameter), spreadFactor);
            return true;
        case CodingRate:
            codingRate = value;
            TT_LOG_I(TAG, "Configure %s=%d", toString(parameter), codingRate);
            return true;
        case SyncWord:
            syncWord = value;
            TT_LOG_I(TAG, "Configure %s=%X", toString(parameter), syncWord);
            return true;
        case PreambleLength:
            preambleLength = value;
            TT_LOG_I(TAG, "Configure %s=%d", toString(parameter), preambleLength);
            return true;
        case DataRate:
            bitRate = value;
            return true;
        case FrequencyDeviation:
            frequencyDeviation = value;
            return true;
        default:
            break;
    }

    return false;
}

void Sx1262::registerDio1Isr() {
    hal.pinMode(GPIO_NUM_9, GPIO_MODE_OUTPUT);
    gpio_set_level(GPIO_NUM_9, 0);

    gpio_hal_context_t gpiohal;
    gpiohal.dev = GPIO_LL_GET_HW(GPIO_PORT_0);

    gpio_config_t conf = {
        .pin_bit_mask = (1ULL<<configuration.irqPin),
        .mode = (gpio_mode_t)GPIO_MODE_INPUT,
        .pull_up_en = GPIO_PULLUP_DISABLE,
        .pull_down_en = GPIO_PULLDOWN_ENABLE,
        .intr_type = (gpio_int_type_t)gpiohal.dev->pin[configuration.irqPin].int_type,
    };
    gpio_config(&conf);

    // We cannot use the RadioLib API to register this action,
    // as it does not have the capability to pass an instance pointer via context.
    // A trampoline has been tried, but is not linkable to be in IRAM_ATTR (dangerous relocation).
    gpio_install_isr_service((int)ESP_INTR_FLAG_IRAM);
    gpio_set_intr_type(configuration.irqPin, GPIO_INTR_POSEDGE);
    gpio_isr_handler_add(configuration.irqPin, dio1handler, this);
}

void Sx1262::unregisterDio1Isr() {
    gpio_isr_handler_remove(configuration.irqPin);
    gpio_wakeup_disable(configuration.irqPin);
    gpio_set_intr_type(configuration.irqPin, GPIO_INTR_DISABLE);
}
/*
void IRAM_ATTR Sx1262::dio1Event() {
    static const auto DRAM_ATTR rxbit = RADIO_RECEIVED_BIT;
    static const auto DRAM_ATTR txbit = RADIO_TRANSMITTED_BIT;

    switch (exchangeState) {
        case ExchangeState::Receive:
            getEventFlag().set(rxbit);
            break;
        case ExchangeState::TransmitWaiting:
            getEventFlag().set(txbit);
            break;
        default:
            break;
    }

    gpio_set_level(GPIO_NUM_9, 1);
}*/


void IRAM_ATTR Sx1262::dio1Event() {
    static const auto DRAM_ATTR bit = SX1262_DIO1_EVENT_BIT;
    getEventFlag().set(bit);

    gpio_set_level(GPIO_NUM_9, 1);
}

int32_t Sx1262::threadMain(const Modulation modulation) {
    using enum ExchangeState;

    uint16_t rc = RADIOLIB_ERR_NONE;

    if (modulation == Modulation::LoRa) {
        rc = radio.begin(
            frequency,
            bandwidth,
            spreadFactor,
            codingRate,
            syncWord,
            power,
            preambleLength,
            configuration.tcxoVoltage,
            configuration.useRegulatorLdo
        );
        /*
        radio.forceLDRO(false);
        radio.setCRC(true);
        radio.invertIQ(false);
        radio.setWhitening(true, 0x00FF);
        radio.explicitHeader();*/

    } else if (modulation == Modulation::Fsk) {
        rc = radio.beginFSK(
            frequency,
            bitRate,
            frequencyDeviation,
            bandwidth,
            power,
            preambleLength,
            configuration.tcxoVoltage,
            configuration.useRegulatorLdo
        );
    } else {
        TT_LOG_E(TAG, "SX1262 not capable of modulation \"%s\"", toString(modulation));
        setState(State::Error);
        return -1;
    }

    if (rc != RADIOLIB_ERR_NONE) {
        TT_LOG_E(TAG, "Radiolib init failed with code %hi", rc);
        setState(State::Error);
        return -1;
    }

    registerDio1Isr();
    setState(State::On);

    TT_LOG_I(TAG, "SX1262 device ready to receive!");
    exchangeState = Receive;

    while (!isThreadInterrupted()) {
        radio.startReceive();
        TT_LOG_I(TAG, "WAIT FLAG");
        auto eventFlags = getEventFlag().wait(RADIO_TERMINATE_BIT | SX1262_DIO1_EVENT_BIT | RADIO_TRANSMIT_QUEUED_BIT);
        TT_LOG_W(TAG, "Event, flag=%X", eventFlags);

        // Thread might've been interrupted in the meanwhile
        if (isThreadInterrupted()) {
            break;
        }

        if ((eventFlags & RADIO_TRANSMIT_QUEUED_BIT) && (getTxQueueSize() > 0)) {
            currentTx = popNextQueuedTx();
            radio.standby();
            uint16_t rc = radio.startTransmit(currentTx.data.data(), currentTx.data.size());
            if (rc == RADIOLIB_ERR_NONE) {
                exchangeState = TransmitWaiting;
                currentTx.callback(currentTx.id, TransmissionState::PendingTransmit);

                TT_LOG_I(TAG, "WAIT TX FLAG");
                auto txEventFlags = getEventFlag().wait(RADIO_TERMINATE_BIT | SX1262_DIO1_EVENT_BIT, tt::EventFlag::WaitAny, pdMS_TO_TICKS(2000));
                TT_LOG_W(TAG, "Event, flag=%X", txEventFlags);

                // Thread might've been interrupted in the meanwhile
                if (isThreadInterrupted()) {
                    break;
                }
                if (txEventFlags & SX1262_DIO1_EVENT_BIT) {
                    currentTx.callback(currentTx.id, TransmissionState::Transmitted);
                } else {
                    currentTx.callback(currentTx.id, TransmissionState::Timeout);
                }
                exchangeState = Receive;

            } else {
                TT_LOG_E(TAG, "Error transmitting id=%d, rc=%hi", currentTx.id, rc);
                currentTx.callback(currentTx.id, TransmissionState::Error);
                exchangeState = Receive;
            }
        } else if (eventFlags & SX1262_DIO1_EVENT_BIT) {
            uint16_t rxSize = radio.getPacketLength(true);
            uint8_t *dataBuffer = new uint8_t[rxSize];
            uint16_t rc = radio.readData(dataBuffer, rxSize);
            if (rc != RADIOLIB_ERR_NONE) {
                TT_LOG_E(TAG, "Error receiving data, RadioLib returned %hi", rc);
            } else if(rxSize == 0) {
                //TT_LOG_W(TAG, "Received data length 0");
            } else {
                float rssi = radio.getRSSI();
                float snr = radio.getSNR();
                TT_LOG_I(TAG, "LoRa RX size=%d RSSI=%f SNR=%f", rxSize, rssi, snr);
                std::string message((char*)dataBuffer, rxSize);
                TT_LOG_I(TAG, "msg=%s", message.c_str());
                auto rxPacket = tt::hal::radio::RxPacket {
                    .data = dataBuffer,
                    .size = rxSize,
                    .rssi = rssi,
                    .snr = snr
                };

                publishRx(rxPacket);
            }

            delete[] dataBuffer;

            // A delay is needed before a new command
            vTaskDelay(pdMS_TO_TICKS(100));
            gpio_set_level(GPIO_NUM_9, 0);
        } else {
            TT_LOG_W(TAG, "Unhandled event, flag=%X", eventFlags);
        }
    }

    unregisterDio1Isr();
    return 0;
}