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

#include "fskmodem.h"

uint16_t TONE2_FREQ;

uint8_t seq = 0;
DataPacket dataPacket;

DataPacket inBoundPacket;

uint8_t *dataPTR = dataPacket.data;

SMSEnteringState gEnteringSMS = SMS_NOT_ENTERING;

void MSG_ConfigureFSK(bool rx) {
    BK4819_WriteRegister(BK4819_REG_70, TONE2_ENABLE_BIT | (96U << 0));

    switch (gEeprom.FSKMode) {
        case MOD_AFSK_2400:
            TONE2_FREQ = 24779U; // Estimated
            break;
        case MOD_AFSK_1200:
            TONE2_FREQ = 12389U;
            break;
        case MOD_FSK_700:
            TONE2_FREQ = 7227U;
            break;
        case MOD_FSK_450:
            TONE2_FREQ = 4646U;
            break;
        case MOD_FSK_1200_2400:
            TONE2_FREQ = 12389U; // Default to 1200Hz, dynamic switching may be required
            break;
        case MOD_NOAA_SAME:
            TONE2_FREQ = 2083U; // NOAA SAME uses 2083.3 Hz for mark and 1562.5 Hz for space
            break;
    }


    BK4819_WriteRegister(BK4819_REG_72, TONE2_FREQ);

//    uint16_t fskConfig = FSK_ENABLE_BIT | FSK_RX_BW_1_2K | FSK_RX_GAIN_DEFAULT | FSK_PREAMBLE_TYPE_AA;
//    if (gEeprom.FSKMode == MOD_AFSK_1200) {
//        fskConfig = FSK_ENABLE_BIT | FSK_TX_MODE_FSK_1_2K_2_4K | FSK_RX_MODE_FSK_1_2K_2_4K_NOAA;
//    }

    uint16_t fskConfig = FSK_ENABLE_BIT | FSK_PREAMBLE_TYPE_AA;

    switch (gEeprom.FSKMode) {
        case MOD_FSK_700:
        case MOD_FSK_450:
        case MOD_FSK_1200_2400:
            fskConfig |= FSK_TX_MODE_FSK_1_2K_2_4K | FSK_RX_MODE_FSK_1_2K_2_4K_NOAA | FSK_RX_BW_1_2K;
            break;
        case MOD_AFSK_1200:
            fskConfig |= FSK_TX_MODE_FFSK_1200_1800 | FSK_RX_MODE_FFSK_1200_1800 | FSK_RX_BW_FFSK_1200_1800;
            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;
        default:
            // Unsupported mode; keep previous setting
            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) {
        BK4819_WriteRegister(BK4819_REG_5E, (64U << 3) | (1U << 0));
    }

    size_t size = sizeof(dataPacket);
    if (rx) {
        size = (((size + 1) / 2) * 2) + 2;
    }
    BK4819_WriteRegister(BK4819_REG_5D, (size << 8));

    BK4819_FskClearFifo();

    uint16_t fskParams = FSK_SYNC_LEN_BIT | ((rx ? 0U : 15U) << 4);
    if (gCurrentVfo->SCRAMBLING_TYPE > 0) {
        fskParams |= FSK_SCRAMBLE_ENABLE;
    }
    BK4819_WriteRegister(BK4819_REG_59, fskParams);

    BK4819_WriteRegister(BK4819_REG_02, 0);
}


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() {

    // turn off CTCSS/CDCSS during FFSK
    const uint16_t css_val = BK4819_ReadRegister(BK4819_REG_51);
    BK4819_WriteRegister(BK4819_REG_51, 0);

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

    {
        uint16_t deviation;
        switch (gEeprom.VfoInfo[gEeprom.TX_VFO].CHANNEL_BANDWIDTH) {
            case BK4819_FILTER_BW_WIDE:
                deviation = 1300;
                break; // 20k // measurements by kamilsss655
            case BK4819_FILTER_BW_NARROW:
                deviation = 1200;
                break; // 10k
                // case BK4819_FILTER_BW_NARROWAVIATION:  deviation =  850; break;  // 5k
                // case BK4819_FILTER_BW_NARROWER:        deviation =  850; break;  // 5k
                // case BK4819_FILTER_BW_NARROWEST:	      deviation =  850; break;  // 5k
            default:
                deviation = 850;
                break;  // 5k
        }

        //BK4819_WriteRegister(0x40, (3u << 12) | (deviation & 0xfff));
        BK4819_WriteRegister(BK4819_REG_40, (dev_val & 0xf000) | (deviation & 0xfff));
    }

    // 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);
    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 *) &dataPacket;
        size_t wordCount = sizeof(dataPacket) / 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 FM deviation level
    BK4819_WriteRegister(BK4819_REG_40, dev_val);

    // 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;

}

void prepareDataPacket() {
    dataPacket.src = gEeprom.FSKSRCAddress;
    dataPacket.seq = seq++;
}