#include <msp430.h>
#include <stdint.h>
#include <stdbool.h>


static volatile bool cs_initialized = false;
static volatile uint8_t count_DCO_fail = 0;

#ifdef XT1HFFREQ_3
#define HAL_CS_MAX_DCO_KHZ  24000U
#else
#define HAL_CS_MAX_DCO_KHZ  16000U
#endif

#define HAL_CS_FRAM_WS1_THRESHOLD_KHZ   8000U

#define HAL_CS_MIN_DCO_KHZ  500U

//------------------------------------------------------FRAM FLAG ----------------------------------------------------------------

// This flag tells init not to attempt to source XT1 again if it was already declared dead.
#pragma PERSISTENT(cs_xt1_faulty) // Variable name must be in parentheses
static volatile bool cs_xt1_faulty = false;

bool msp430fr2xx_4xx_hal_cs_is_xt1_faulty(void)
{
    return cs_xt1_faulty;
}

//----------------------------------------------------------FOR ISR - FLL AND DCO ---------------------------------------------------------------//

#define XT1_LF_FREQ_HZ      32768UL     // standard 32.768 kHz watch crystal 
#define FLL_REF_FREQ_HZ     32768UL     // FLL reference FLLREFDIV=1 // I am assuming LF XT1

// XT1 recalibration using the hw counter threshold
#define XT1_HW_RETRY_MAX        3

// DCO recalibration retries per NMI entry 
#define DCO_RECAL_RETRY_MAX     3

// Threshold for the amount of DCO fault flag tries
#define DCO_POR_THRESHOLD       10

//Variable that stores the csctl4 register
static uint16_t xt1_saved_csctl4 = 0;
//this one tells is if we saved the contents in the csctl4 and switched the ref clock to refo
static bool     xt1_csctl4_saved = false;

//This is the amount of time we wil be waiting for the xt1 to recver by itself using its HW counter
static uint32_t xt1_hw_wait_cycles(void)
{
    uint32_t mclk = CS_getMCLK();
    uint16_t csctl6 = HWREG16(CS_BASE + OFS_CSCTL6);

    if (!(csctl6 & XTS)) {
        /* LF mode: 8192 ticks / 32768 Hz = 0.25 s
         * cycles = 0.25 × MCLK = (8192 × MCLK) / 32768            */
        return (8192UL * mclk) / XT1_LF_FREQ_HZ;
    }

    // This is what chat gave me -  Bypass mode also uses 8192 like we need some sort of retrn type to keep the compiler happy uk?
    return (8192UL * mclk) / XT1_LF_FREQ_HZ;
}

//-------------------------------------------------------------- DCO--------------------------------------------------------------------------
static bool CS_recalibrateDCO_ISR(void)
{
    /* Steps 11–12: track the configuration closest to midrange (256) */
    uint16_t bestCsCtl0 = 0;
    uint16_t bestCsCtl1 = 0;
    uint16_t bestDelta = 0xFFFFu;

    uint16_t dcoftrim = (HWREG16(CS_BASE + OFS_CSCTL1) >> 4) & 0x07u;
    uint8_t  attempts = 0;
    bool     fllLocked = false;

    // Step 1: Disable the FLL
    __bis_SR_register(SCG0);

    // Step 2: I did made like a global variable for the ref clock - in our application we will only be using the XT1 in LF mode as the ref clock for FLL

    // Step 3: Enable DCOFTRIMEN so the trim takes effect.
    HWREG16(CS_BASE + OFS_CSCTL1) |= DCOFTRIMEN;

    // Step 5: Three NOPs BEFORE re-enabling the FLL.
    __no_operation();
    __no_operation();
    __no_operation();

    // Step 6: Re-enable FLL. 
    __bic_SR_register(SCG0);

    // Steps 7–14 :  trim search loop. 
    while (attempts < DCO_RECAL_RETRY_MAX)
    {
        // Step 7: Preset DCO tap = 256 (midrange)
        HWREG16(CS_BASE + OFS_CSCTL0) = DCO8;

        // Step 8: Clear DCOFFG BEFORE the wait so the hardware
        HWREG8(CS_BASE + OFS_CSCTL7_L) &= ~DCOFFG;
        HWREG8(SFR_BASE + OFS_SFRIFG1) &= ~OFIFG;

        // Step 9: Wait ≥ 24 / fFLLREF for FLLUNLOCK to stabilise. - (24 × MCLK) / FLLREF.
        __delay_cycles((24UL * CS_getMCLK()) / FLL_REF_FREQ_HZ);

        // Step 10: Poll - spin until locked OR DCO fault re-fires.   
        while ((HWREG16(CS_BASE + OFS_CSCTL7) & (FLLUNLOCK0 | FLLUNLOCK1)) &&
            !(HWREG16(CS_BASE + OFS_CSCTL7) & DCOFFG));

        // Step 11: Read settled tap, compute delta from midrange.    
        uint16_t tap = HWREG16(CS_BASE + OFS_CSCTL0) & 0x01FFu;
        uint16_t delta = (tap >= 256u) ? (tap - 256u) : (256u - tap);

        // Step 12: Record best (closest to 256) CSCTL0/CSCTL1.      
        if (delta < bestDelta) {
            bestDelta = delta;
            bestCsCtl0 = HWREG16(CS_BASE + OFS_CSCTL0);
            bestCsCtl1 = HWREG16(CS_BASE + OFS_CSCTL1);
        }

        // Check if FLL locked cleanly (DCOFFG did not re-fire).      
        if (!(HWREG8(CS_BASE + OFS_CSCTL7_L) & DCOFFG)) {
            fllLocked = true;
            break;
        }

        // Step 13: Adjust DCOFTRIM toward midrange.
        if (tap < 256u && dcoftrim > 0u) {
            dcoftrim--;
        }
        else if (tap >= 256u && dcoftrim < 7u) {
            dcoftrim++;
        }
        else {
            break;   // already at boundary, cannot adjust further 
        }

        // Write updated DCOFTRIM, preserving all other CSCTL1 bits.  
        HWREG16(CS_BASE + OFS_CSCTL1) =
            (HWREG16(CS_BASE + OFS_CSCTL1) & ~(0x07u << 4)) |
            ((uint16_t)dcoftrim << 4);

        // Step 14: Repeat from step 7. 
        attempts++;
    }

    // Step 15 — Reload best CSCTL0/CSCTL1 recorded across iterations. 
    if (bestDelta < 0xFFFFu) {
        HWREG16(CS_BASE + OFS_CSCTL0) = bestCsCtl0;
        HWREG16(CS_BASE + OFS_CSCTL1) = bestCsCtl1;
    }

    return fllLocked;
}

//--------------------------------------Functions using TI DriverLib ------------------------------------------------------------//

#if defined(__TI_COMPILER_VERSION__) || defined(__IAR_SYSTEMS_ICC__)
#pragma vector=UNMI_VECTOR
__interrupt void UNMI_ISR(void)
#elif defined(__GNUC__)
void __attribute__((interrupt(UNMI_VECTOR))) UNMI_ISR(void)
#else
#error Compiler not supported!
#endif
{
    switch (__even_in_range(HWREG16(__MSP430_BASEADDRESS_SYS__ + OFS_SYSUNIV), SYSUNIV__OFIFG)) {

    case SYSUNIV__NONE: //OFFSET 0
        break;

    case SYSUNIV__NMIIFG: //OFFSET 2
        break;

    case SYSUNIV__OFIFG: {  //OFFSET 4

        uint8_t csctl7 = HWREG8(CS_BASE + OFS_CSCTL7_L);
        if (csctl7 & DCOFFG) {

            bool recal_ok = CS_recalibrateDCO_ISR();

            if (recal_ok) {
                count_DCO_fail = 0;     /* recovery succeeded */
            }
            else {
                count_DCO_fail++;

                /* Last-resort: clear flags and fall through.
                 * If this NMI fires again immediately, the
                 * fault is persistent.                        */
                HWREG8(CS_BASE + OFS_CSCTL7_L) &= ~DCOFFG;
                HWREG8(SFR_BASE + OFS_SFRIFG1) &= ~OFIFG;

                if (count_DCO_fail >= DCO_POR_THRESHOLD) {
                    /* 10 consecutive irrecoverable faults →
                     * force a Power-On Reset.                */
                    HWREG16(PMM_BASE + OFS_PMMCTL0) =
                        PMMPW | PMMSWPOR;
                }
            }
        }


        else if (csctl7 & XT1OFFG)
        {
            /* §3.2.7 CSCTL7: ensure start-fault counter runs. */
            HWREG8(CS_BASE + OFS_CSCTL7_L) |= ENSTFCNT1;

            // Save CSCTL4 before we change SELMS/SELA. So once the xt1 recovers we use the old settings
            if (!xt1_csctl4_saved) {
                xt1_saved_csctl4 = HWREG16(CS_BASE + OFS_CSCTL4);
                xt1_csctl4_saved = true;
            }

            bool    xt1_recovered = false;
            uint8_t hw_retry = 0;

            while (hw_retry < XT1_HW_RETRY_MAX)
            {
                HWREG8(CS_BASE + OFS_CSCTL7_L) &= ~XT1OFFG;
                HWREG8(SFR_BASE + OFS_SFRIFG1) &= ~OFIFG;

                __delay_cycles(xt1_hw_wait_cycles());

                if (!(HWREG8(CS_BASE + OFS_CSCTL7_L) & XT1OFFG)) {
                    xt1_recovered = true;
                    break;
                }
                hw_retry++;
            }

            if (xt1_recovered)
            {
                // we check if the csclt4 contents were saved
                if (xt1_csctl4_saved) {
                    HWREG16(CS_BASE + OFS_CSCTL4) = xt1_saved_csctl4;
                    xt1_csctl4_saved = false;
                    cs_xt1_faulty = false;
                }

                HWREG8(CS_BASE + OFS_CSCTL7_L) &= ~XT1OFFG;
                HWREG8(SFR_BASE + OFS_SFRIFG1) &= ~OFIFG;
            }
            else
            {
                /* XT1 did not recover — commit REFO fallback.   */


                if ((HWREG16(CS_BASE + OFS_CSCTL4) & SELMS) == CS_XT1CLK_SELECT) {
                    HWREG16(CS_BASE + OFS_CSCTL4) &= ~SELMS;
                    HWREG16(CS_BASE + OFS_CSCTL4) |= CS_REFOCLK_SELECT;
                }
                if ((HWREG16(CS_BASE + OFS_CSCTL4) & SELA) == SELA__XT1CLK) {
                    HWREG16(CS_BASE + OFS_CSCTL4) &= ~SELA;
                    HWREG16(CS_BASE + OFS_CSCTL4) |= SELA__REFOCLK;
                }

                /* FRAM: survives POR — blocks XT1 on next boot. */
                cs_xt1_faulty = true;

                HWREG8(CS_BASE + OFS_CSCTL7_L) &= ~XT1OFFG;
                HWREG8(SFR_BASE + OFS_SFRIFG1) &= ~OFIFG;
            }
        }
        break;
    }



    default:
        break;
    }
}