//
// Created by bruno on 25.2.2025.
//

#include "fskmodem.h"
//#include "messages.h"
#include "driver/uart.h"
#include "../ui/messages.h"
#include "ui/ui.h"

uint16_t TONE2_FREQ;

DataPacket dataPacket;

DataPacket inBoundPacket;

uint8_t *dataPTR = dataPacket.data;

SMSEnteringState gEnteringSMS = SMS_NOT_ENTERING;

SMSResponseState gSMSResponseState = SMS_RESPONSE_IDLE;

bool gGotACK = false;

uint8_t SMSResponseCounter = 0;

typedef enum {
    Receiving,
    Ready
} RXState;

RXState rxState = Ready;

uint8_t SMSResponseCounterTarget = 6;

void FSKModem_TimeSlice500ms(void) {
    if (SMSResponseCounter && FUNCTION_IsRx() && SMSResponseCounter++ > SMSResponseCounterTarget) {
        switch (gSMSResponseState) {
            case SMS_RESPONSE_ACK:
                MSG_FSKSendData(&inBoundPacket);
                gSMSResponseState = SMS_RESPONSE_IDLE;
                SMSResponseCounter = 0;
                break;
            case SMS_RESPONSE_RETRANSMIT:
                MSG_FSKSendData(&inBoundPacket);
                gSMSResponseState = SMS_RESPONSE_IDLE;
                SMSResponseCounter = 0;
                break;
            case SMS_RESPONSE_RESEND:
                MSG_FSKSendData(&dataPacket);
                gSMSResponseState = SMS_RESPONSE_IDLE;
                SMSResponseCounter = 0;
                break;
            case SMS_RESPONSE_ACK_LIGHT:
                BK4819_ToggleGpioOut(BK4819_GPIO5_PIN1_RED, false);
                BK4819_ToggleGpioOut(BK4819_GPIO6_PIN2_GREEN, false);
                gSMSResponseState = SMS_RESPONSE_IDLE;
                SMSResponseCounter = 0;
                break;
            default:
                break;
        }
    }
}

void MSG_ConfigureFSK(bool rx) {

    // quick helper
    bool isAFSK = (gEeprom.FSKMode == MOD_AFSK_1200 ||
                   gEeprom.FSKMode == MOD_AFSK_2400 ||
                   gEeprom.FSKMode == MOD_NOAA_SAME);


    // Base tone config
    uint16_t tone1 = 0, tone2 = 0;
#define BK4819_FREQ_WORD(freq_hz) ((uint16_t)((freq_hz) * 10.32444f))

    if (isAFSK) {
        // Bell-202 style tones for AFSK/NOAA
        switch (gEeprom.FSKMode) {
            case MOD_AFSK_1200:
                tone1 = BK4819_FREQ_WORD(1200.0f);  // mark
                tone2 = BK4819_FREQ_WORD(2200.0f);  // space
                break;
            case MOD_AFSK_2400:
                tone1 = BK4819_FREQ_WORD(1200.0f);
                tone2 = BK4819_FREQ_WORD(2400.0f);
                break;
            case MOD_NOAA_SAME:
                tone1 = BK4819_FREQ_WORD(1562.5f);  // typical mark
                tone2 = BK4819_FREQ_WORD(2083.3f);  // typical space
                break;
            default:
                tone1 = BK4819_FREQ_WORD(1200.0f);
                tone2 = BK4819_FREQ_WORD(2200.0f);
                break;
        }

        BK4819_WriteRegister(BK4819_REG_71, tone1);
        BK4819_WriteRegister(BK4819_REG_72, tone2);
        // enable both tones
        BK4819_WriteRegister(BK4819_REG_70,
                             TONE1_ENABLE_BIT | TONE2_ENABLE_BIT |
                             (96U << 8) | (96U << 0));
    } else {
        // plain FSK path: only TONE2 used (frequency for binary shift display/debug)
        switch (gEeprom.FSKMode) {
            case MOD_FSK_450:
                tone2 = BK4819_FREQ_WORD(450.0f);
                break;
            case MOD_FSK_700:
                tone2 = BK4819_FREQ_WORD(700.0f);
                break;
            case MOD_FSK_1200:
                tone2 = BK4819_FREQ_WORD(1200.0f);
                break;
            case MOD_FSK_2400:
                tone2 = BK4819_FREQ_WORD(2400.0f);
                break;
            default:
                tone2 = BK4819_FREQ_WORD(1200.0f);
                break;
        }
        BK4819_WriteRegister(BK4819_REG_72, tone2);
        BK4819_WriteRegister(BK4819_REG_70, TONE2_ENABLE_BIT | (96U << 0));
    }

    // Base FSK config
    uint16_t fskConfig = FSK_ENABLE_BIT | FSK_PREAMBLE_TYPE_AA;

    // Compose the correct fields into REG_58:
    // - TX mode bits are at <15:13> (use your FSK_TX_MODE_* macros which are <<13)
    // - RX mode bits are at <12:10> (use your FSK_RX_MODE_* macros which are <<10)
    // - RX BW bits are at <3:1> (use your FSK_RX_BW_* macros which are <<1)
    switch (gEeprom.FSKMode) {
        case MOD_AFSK_1200:
            fskConfig |= FSK_TX_MODE_FFSK_1200_1800      /*<<13 in macro*/
                         | FSK_RX_MODE_FFSK_1200_1800      /*<<10 in macro*/
                         | FSK_RX_BW_FFSK_1200_1800;      /*<<1  in macro*/
            break;

        case MOD_AFSK_2400:
            fskConfig |= FSK_TX_MODE_FFSK_1200_2400
                         | FSK_RX_MODE_FFSK_1200_2400
                         | FSK_RX_BW_2_4K_FFSK_1200_2400;
            break;

        case MOD_NOAA_SAME:
            fskConfig |= FSK_TX_MODE_NOAA_SAME
                         | FSK_RX_MODE_FSK_1_2K_2_4K_NOAA
                         | FSK_RX_BW_NOAA_SAME;
            break;

        case MOD_FSK_2400:
            // binary FSK 1.2k/2.4k - use default FSK TX/RX modes and 1.2k BW
            fskConfig |= FSK_TX_MODE_FSK_1_2K_2_4K
                         | FSK_RX_MODE_FSK_1_2K_2_4K_NOAA
                         | FSK_RX_BW_2_4K_FFSK_1200_2400;
            break;

        case MOD_FSK_700:
        case MOD_FSK_450:
        case MOD_FSK_1200:
        default:
            // binary FSK 1.2k/2.4k - use default FSK TX/RX modes and 1.2k BW
            fskConfig |= FSK_TX_MODE_FSK_1_2K_2_4K
                         | FSK_RX_MODE_FSK_1_2K_2_4K_NOAA
                         | FSK_RX_BW_1_2K;
            break;
    }


    BK4819_WriteRegister(BK4819_REG_58, fskConfig);

    BK4819_WriteRegister(BK4819_REG_5A, FSK_SYNC_BYTE0 << 8 | FSK_SYNC_BYTE1);
    BK4819_WriteRegister(BK4819_REG_5B, FSK_SYNC_BYTE2 << 8 | FSK_SYNC_BYTE3);
    BK4819_WriteRegister(BK4819_REG_5C, FSK_CRC_ON);

    if (rx) {
        uint8_t threshold;
        switch (gEeprom.FSKMode) {
            case MOD_AFSK_1200:
            case MOD_NOAA_SAME:
                threshold = 1U;
                break;
            case MOD_AFSK_2400:
                threshold = 2U;   // Slightly higher; 2400 baud can fill FIFO faster
                break;
            default:
                threshold = 4U;   // FSK modes
                break;
        }
        BK4819_WriteRegister(BK4819_REG_5E, (64U << 3) | threshold);
    }


    // Data length (REG_5D<15:8>)
    size_t size = sizeof(dataPacket);
    if (rx) size = (((size + 1) / 2) * 2) + 2;
    BK4819_WriteRegister(BK4819_REG_5D, (size << 8));

    BK4819_FskClearFifo();

    // REG_59: clear flags, preamble len, sync len etc.
    //uint16_t fskParams = FSK_SYNC_LEN_BIT | ((rx ? 0U : 15U) << 4);
    uint16_t preamble_len = (gEeprom.FSKMode == MOD_AFSK_1200 || gEeprom.FSKMode == MOD_AFSK_2400 || gEeprom.FSKMode == MOD_NOAA_SAME) ? 10U : (rx ? 0U
                                                                                                              : 15U);
    uint16_t fskParams = FSK_SYNC_LEN_BIT | (preamble_len << 4);
    if (gCurrentVfo->SCRAMBLING_TYPE > 0) fskParams |= FSK_SCRAMBLE_ENABLE;
    BK4819_WriteRegister(BK4819_REG_59, fskParams);


    // Clear interrupt flags
    BK4819_WriteRegister(BK4819_REG_02, 0);

    // Enable AF path filters for tone demodulation in AFSK/NOAA modes
    if (gEeprom.FSKMode == MOD_AFSK_1200 ||
        gEeprom.FSKMode == MOD_AFSK_2400 ||
        gEeprom.FSKMode == MOD_NOAA_SAME) {

        // Keep DC cancel, disable only deemphasis
        BK4819_WriteRegister(BK4819_REG_2B,
                             (1U << 15) | (1U << 14) | (1U << 8));
    } else {

        //BK4819_WriteRegister(BK4819_REG_2B, (1u << 2) | (1u << 0));
    }


    // set the FM deviation level
    const uint16_t dev_val = BK4819_ReadRegister(BK4819_REG_40);

    uint16_t deviation;
    switch (gEeprom.FSKMode) {
        case MOD_AFSK_1200:
        case MOD_AFSK_2400:
        case MOD_NOAA_SAME:
            deviation = 650;   // 0.65 kHz for audio-based tones (linear region)
            break;

        case MOD_FSK_450:
            deviation = 225;   // ≈ half of 450 Hz separation
            break;

        case MOD_FSK_700:
            deviation = 350;   // ≈ half of 700 Hz separation
            break;

        case MOD_FSK_1200:
            deviation = 600;   // halfway between 1200 and 2400 = 1.8 kHz spacing → ±900 Hz dev
            break;

        case MOD_FSK_2400:
            deviation = 1200;   // halfway between 1200 and 2400 = 1.8 kHz spacing → ±900 Hz dev
            break;

        default:
            deviation = 750;   // safe fallback
            break;
    }
    if (gCurrentVfo->CHANNEL_BANDWIDTH == BK4819_FILTER_BW_FSK_OFFLOAD) {
        BK4819_WriteRegister(BK4819_REG_40, (dev_val & 0xf000) | (deviation & 0x0fff));
    }

    // restore FM deviation level
    //BK4819_WriteRegister(BK4819_REG_40, dev_val);

}


uint16_t calculateCRC(uint8_t *data, size_t length) {
    uint16_t crc = 0xFFFF; // Example CRC-16 initialization
    for (size_t i = 0; i < length; i++) {
        crc ^= data[i];
        for (uint8_t j = 0; j < 8; j++) {
            if (crc & 1) {
                crc = (crc >> 1) ^ 0xA001; // Example polynomial (CRC-16-IBM)
            } else {
                crc >>= 1;
            }
        }
    }
    return crc;
}


void processReceivedPacket(DataPacket *packet) {
    char String[18];
    char numBuf[11];  // Enough for any 64-bit unsigned integer
    const unsigned int vfo = (gEeprom.CROSS_BAND_RX_TX == CROSS_BAND_OFF) ? gEeprom.RX_VFO
                                                                          : gEeprom.TX_VFO;

    UART_Send(packet, sizeof(DataPacket));
    if (packet->dest == gEeprom.FSKSRCAddress) {
        if ((packet->flags & 0x80)) {
            if (packet->flags & 0x40) {
                uint16_t crcSent = calculateCRC(dataPacket.data, 20);
                uint16_t crcGot = inBoundPacket.data[0] | (inBoundPacket.data[1] << 8);
                if (crcSent == crcGot) {
                    gGotACK = true;
                    BK4819_ToggleGpioOut(BK4819_GPIO5_PIN1_RED, true);
                    BK4819_ToggleGpioOut(BK4819_GPIO6_PIN2_GREEN, true);
                    SMSResponseCounter = 1;
                    gSMSResponseState = SMS_RESPONSE_ACK_LIGHT;
                } else {
                    if (--dataPacket.ttl) {
                        gSMSResponseState = SMS_RESPONSE_RESEND;
                        SMSResponseCounter = 1;
                    }
                }
            } else {
                AUDIO_PlayBeep(BEEP_500HZ_60MS_DOUBLE_BEEP);
                BK4819_PlaySingleTone(1000, 250, 127, true);
                strcpy(String, "SMS by ");
                itoa(packet->src, numBuf);  // Convert number to string
                strcat(String, numBuf);
                MESSAGES_SAVE();
                gRequestDisplayScreen = DISPLAY_MESSAGES;
                GUI_SelectNextDisplay(gRequestDisplayScreen);
                gActiveMessageBank = MESSAGES_COUNT / 3 - 1;
                UI_DisplayPopup(String);
                inBoundPacket.flags |= 0x40;
                inBoundPacket.dest = inBoundPacket.src;
                inBoundPacket.src = gEeprom.FSKSRCAddress;
                uint16_t crcTest = calculateCRC(inBoundPacket.data, 20);
                memset(inBoundPacket.data, 0, DataPacketDataSize);
                inBoundPacket.data[0] = crcTest & 0xFF;
                inBoundPacket.data[1] = (crcTest >> 8) & 0xFF;
                gSMSResponseState = SMS_RESPONSE_ACK;
                SMSResponseCounter = 1;
                UI_PrintStringSmallNormal(String, 2, 0, 4);
            }
        }

    } else if (VfoState[vfo] == VFO_STATE_NORMAL && !TX_freq_check(gCurrentVfo->freq_config_TX.Frequency)) {
        if (packet->ttl--) {
            SMSResponseCounter = 1;
            gSMSResponseState = SMS_RESPONSE_RETRANSMIT;
        }
    }
}

void MSG_EnableRX(const bool enable) {

    if (enable) {
        MSG_ConfigureFSK(true);

        //if(gEeprom.MESSENGER_CONFIG.data.receive)
        BK4819_FskEnableRx();
    } else {
        BK4819_WriteRegister(BK4819_REG_70, 0);
        BK4819_WriteRegister(BK4819_REG_58, 0);
    }
}

void MSG_FSKSendData(DataPacket *dataPacketIn) {
    const unsigned int vfo = (gEeprom.CROSS_BAND_RX_TX == CROSS_BAND_OFF) ? gEeprom.RX_VFO
                                                                          : gEeprom.TX_VFO;
    if (VfoState[vfo] == VFO_STATE_NORMAL &&
        !TX_freq_check(gCurrentVfo->freq_config_TX.Frequency)) {

        RADIO_PrepareTX();
        BK4819_SetScramble(gCurrentVfo->SCRAMBLING_TYPE);
        BK4819_ToggleGpioOut(BK4819_GPIO5_PIN1_RED, true);

        bool isAudioOn = gEnableSpeaker;
        if (gEnableSpeaker) {
            AUDIO_AudioPathOff();
            BK4819_EnterTxMute();
            gEnableSpeaker = false;
        }
        // turn off CTCSS/CDCSS during FFSK
        const uint16_t css_val = BK4819_ReadRegister(BK4819_REG_51);
        BK4819_WriteRegister(BK4819_REG_51, 0);



        // REG_2B   0
        //
        // <15> 1 Enable CTCSS/CDCSS DC cancellation after FM Demodulation   1 = enable 0 = disable
        // <14> 1 Enable AF DC cancellation after FM Demodulation            1 = enable 0 = disable
        // <10> 0 AF RX HPF 300Hz filter     0 = enable 1 = disable
        // <9>  0 AF RX LPF 3kHz filter      0 = enable 1 = disable
        // <8>  0 AF RX de-emphasis filter   0 = enable 1 = disable
        // <2>  0 AF TX HPF 300Hz filter     0 = enable 1 = disable
        // <1>  0 AF TX LPF filter           0 = enable 1 = disable
        // <0>  0 AF TX pre-emphasis filter  0 = enable 1 = disable
        //
        // disable the 300Hz HPF and FM pre-emphasis filter
        //

        const uint16_t filt_val = BK4819_ReadRegister(BK4819_REG_2B);
        // Enable AF path filters for tone demodulation in AFSK/NOAA modes
        if (gEeprom.FSKMode == MOD_AFSK_1200 ||
            gEeprom.FSKMode == MOD_AFSK_2400 ||
            gEeprom.FSKMode == MOD_NOAA_SAME) {

            // Keep DC cancel, disable only deemphasis
            BK4819_WriteRegister(BK4819_REG_2B,
                                 (1U << 15) | (1U << 14) | (1U << 8));
        } else {

            BK4819_WriteRegister(BK4819_REG_2B, (1u << 2) | (1u << 0));
        }

        MSG_ConfigureFSK(false);


        SYSTEM_DelayMs(100);

        {    // load the entire packet data into the TX FIFO buffer
            uint16_t *ptr = (uint16_t *) dataPacketIn;
            size_t wordCount = sizeof(*dataPacketIn) / sizeof(uint16_t);

            for (size_t i = 0; i < wordCount; i++) {
                BK4819_WriteRegister(BK4819_REG_5F, ptr[i]);
            }
        }

        // enable FSK TX
        BK4819_FskEnableTx();

        {
            // allow up to 310ms for the TX to complete
            // if it takes any longer then somethings gone wrong, we shut the TX down
            unsigned int timeout = 1000 / 5;

            while (timeout-- > 0) {
                SYSTEM_DelayMs(5);
                if (BK4819_ReadRegister(BK4819_REG_0C) & (1u << 0)) {    // we have interrupt flags
                    BK4819_WriteRegister(BK4819_REG_02, 0);
                    if (BK4819_ReadRegister(BK4819_REG_02) & BK4819_REG_02_FSK_TX_FINISHED)
                        timeout = 0;       // TX is complete
                }
            }
        }
        //BK4819_WriteRegister(BK4819_REG_02, 0);

        SYSTEM_DelayMs(100);

        // disable TX
        MSG_ConfigureFSK(true);

        // restore TX/RX filtering
        BK4819_WriteRegister(BK4819_REG_2B, filt_val);

        // restore the CTCSS/CDCSS setting
        BK4819_WriteRegister(BK4819_REG_51, css_val);

        SYSTEM_DelayMs(50);
        APP_EndTransmission();
        FUNCTION_Select(FUNCTION_FOREGROUND);
        gUpdateStatus = true;
        gUpdateDisplay = true;
        gFlagEndTransmission = false;

        if (isAudioOn) {
            AUDIO_AudioPathOn();
            gEnableSpeaker = true;
            BK4819_ExitTxMute();
        }

        //if (!(dataPacketIn->flags & 0x64)) {
        if (!(dataPacketIn->flags & 0x40)) {
            gGotACK = false;
        }
        BK4819_ToggleGpioOut(BK4819_GPIO5_PIN1_RED, false);
        MSG_EnableRX(true);
        gVfoConfigureMode = VFO_CONFIGURE;
        dataPTR = dataPacket.data;

    }
}

void prepareDataPacket() {
    dataPacket.src = gEeprom.FSKSRCAddress;
    uint8_t Data[8];
    EEPROM_ReadBuffer(SEQParameterEEPROM, Data, 8);
    dataPacket.seq = Data[0];
    Data[0]++;
    EEPROM_WriteBuffer(SEQParameterEEPROM, Data);
    dataPacket.ttl = 20;
}

void FSK_HANDLE_IRQ(unsigned short irq) {
    //const uint16_t rx_sync_flags   = BK4819_ReadRegister(BK4819_REG_0B);

    const bool rx_sync = (irq & BK4819_REG_02_FSK_RX_SYNC) != 0;
    const bool rx_fifo_almost_full = (irq & BK4819_REG_02_FSK_FIFO_ALMOST_FULL) != 0;
    const bool rx_finished = (irq & BK4819_REG_02_FSK_RX_FINISHED) != 0;

    //UART_printf("\nMSG : S%i, F%i, E%i | %i", rx_sync, rx_fifo_almost_full, rx_finished, irq);

    if (rx_sync) {
        // prevent listening to fsk data and squelch (kamilsss655)
        // CTCSS codes seem to false trigger the rx_sync
        if (gCurrentCodeType == CODE_TYPE_OFF)
            AUDIO_AudioPathOff();
        gFSKWriteIndex = 0;
        memset(&inBoundPacket, 0, sizeof(DataPacket));
        rxState = Receiving;
    }

    if (rx_fifo_almost_full && rxState == Receiving) {
        const uint16_t count = BK4819_ReadRegister(BK4819_REG_5E) & (7u << 0);  // Almost full threshold
        uint16_t *ptr = (uint16_t *) &inBoundPacket;
        size_t wordCount = sizeof(inBoundPacket) / sizeof(uint16_t);

        for (uint16_t i = 0; i < count && gFSKWriteIndex < wordCount; i++) {
            ptr[gFSKWriteIndex++] = BK4819_ReadRegister(BK4819_REG_5F);
            UART_Send((const void *) &ptr[gFSKWriteIndex - 1], 2);
        }

        SYSTEM_DelayMs(2);
    }

    if (rx_finished) {
        // Turn off green LED
        BK4819_FskClearFifo();

        rxState = Ready;

        if (gFSKWriteIndex > 2) {
            // Validate checksum (assuming last 2 bytes are CRC-16)
            //size_t dataLength = sizeof(inBoundPacket.data) - 2;
            //uint16_t receivedCRC = (inBoundPacket.data[dataLength] << 8) | inBoundPacket.data[dataLength + 1];
            //uint16_t calculatedCRC = calculateCRC(inBoundPacket.data, dataLength);

            //if (receivedCRC == calculatedCRC) {
            processReceivedPacket(&inBoundPacket);
            //}
        }

        APP_EndTransmission();
        FUNCTION_Select(FUNCTION_FOREGROUND);
        gUpdateStatus = true;
        gUpdateDisplay = true;
        gFlagEndTransmission = false;

        gFSKWriteIndex = 0;
    }
}