#include "safety_manager.h"

#include "cy_project.h"
#include "cy_device_headers.h"
#include "motor_control.h"
#include "seat_motor_manager.h"
#include "seat_move_config.h"

#if defined(CoreDebug) && defined(CoreDebug_DHCSR_C_DEBUGEN_Msk)
#define SAFETY_DEBUG_DETACH_SUPPORTED (1u)
#else
#define SAFETY_DEBUG_DETACH_SUPPORTED (0u)
#endif

static volatile safety_fault_t latchedFault = SAFETY_FAULT_NONE;
static bool watchdogReset = false;
static bool watchdogInitialized = false;
static bool hilWatchdogResetArmed = false;
static bool hilWatchdogRecovered = false;
#if (SAFETY_DEBUG_DETACH_SUPPORTED == 1u)
static bool debuggerWasAttached = false;
#endif

static bool SafetyManager_DebuggerAttached(void)
{
#if (SAFETY_DEBUG_DETACH_SUPPORTED == 1u)
    return ((CoreDebug->DHCSR & CoreDebug_DHCSR_C_DEBUGEN_Msk) != 0ul);
#else
    return false;
#endif
}

static uint32_t SafetyManager_ReadDebugMarker(void)
{
    return BACKUP->unBREG[SEAT_MOVE_DEBUG_SESSION_BREG_INDEX].u32Register;
}

static bool SafetyManager_WriteDebugMarker(uint32_t marker)
{
    BACKUP->unBREG[SEAT_MOVE_DEBUG_SESSION_BREG_INDEX].u32Register = marker;
    __DSB();
    return (SafetyManager_ReadDebugMarker() == marker);
}

static bool SafetyManager_WaitForWatchdogState(bool enabled)
{
    uint32_t remaining = 200ul;
    uint32_t expected = enabled ? 1ul : 0ul;

    /* WDT ENABLE is an asynchronous request bit; ENABLED is the synchronized
     * LF-clock state.  Configuration writes made before disable completes are
     * ignored by the hardware. */
    while((WDT->unCTL.stcField.u1ENABLED != expected) && (remaining > 0ul))
    {
        Cy_SysTick_DelayInUs(1u);
        remaining--;
    }
    return (WDT->unCTL.stcField.u1ENABLED == expected);
}

static void SafetyManager_ClearHilWatchdogCredential(void)
{
    BACKUP->unBREG[SEAT_MOVE_HIL_WDT_MAGIC_BREG_INDEX].u32Register = 0u;
    BACKUP->unBREG[SEAT_MOVE_HIL_WDT_TOKEN_BREG_INDEX].u32Register = 0u;
    BACKUP->unBREG[SEAT_MOVE_HIL_WDT_CHECK_BREG_INDEX].u32Register = 0u;
    __DSB();
}

void SafetyManager_Init(uint32_t resetReason)
{
    const uint32_t watchdogMask = CY_SYSRESET_WDT |
                                  CY_SYSRESET_MCWDT0 |
                                  CY_SYSRESET_MCWDT1 |
                                  CY_SYSRESET_MCWDT2 |
                                  CY_SYSRESET_MCWDT3;
    bool watchdogStopped;
    bool debuggerAttached;
    uint32_t marker;
    uint32_t hilMagic;
    uint32_t hilToken;
    uint32_t hilCheck;
    bool authorizedHilReset;
    bool markerOk = true;

    /* A watchdog instance is retained across its own reset on this device.
     * Stop it before reading credentials or initializing peripherals; the
     * 100 ms bench timeout otherwise permits a reset loop. */
    Cy_WDT_Disable();
    watchdogStopped = SafetyManager_WaitForWatchdogState(false);

    debuggerAttached = SafetyManager_DebuggerAttached();
    marker = SafetyManager_ReadDebugMarker();
    hilMagic = BACKUP->unBREG[
        SEAT_MOVE_HIL_WDT_MAGIC_BREG_INDEX].u32Register;
    hilToken = BACKUP->unBREG[
        SEAT_MOVE_HIL_WDT_TOKEN_BREG_INDEX].u32Register;
    hilCheck = BACKUP->unBREG[
        SEAT_MOVE_HIL_WDT_CHECK_BREG_INDEX].u32Register;
    authorizedHilReset =
        (hilMagic == SEAT_MOVE_HIL_WDT_MARKER) &&
        (hilCheck == (hilMagic ^ hilToken ^ 0xFFFFFFFFu)) &&
        (((resetReason & watchdogMask) != 0ul) ||
         ((resetReason & CY_SYSRESET_SOFT) != 0ul));

    latchedFault = SAFETY_FAULT_NONE;
    watchdogInitialized = false;
    hilWatchdogResetArmed = false;
    watchdogReset = ((resetReason & watchdogMask) != 0ul) ||
                    authorizedHilReset;
    hilWatchdogRecovered = authorizedHilReset;
    if((resetReason & CY_SYSRESET_PORVDDD) != 0ul)
    {
        markerOk = SafetyManager_WriteDebugMarker(0u);
        marker = SafetyManager_ReadDebugMarker();
        SafetyManager_ClearHilWatchdogCredential();
        hilWatchdogRecovered = false;
    }
    else if(hilWatchdogRecovered)
    {
        /* Consume before CAN comes online: transport retries cannot turn one
         * authenticated reset into a second physical reset. */
        SafetyManager_ClearHilWatchdogCredential();
    }
    else if(hilMagic == SEAT_MOVE_HIL_WDT_MARKER)
    {
        /* XRES/programming without a valid reset reason is not completion. */
        SafetyManager_ClearHilWatchdogCredential();
    }
#if (SAFETY_DEBUG_DETACH_SUPPORTED == 1u)
    debuggerWasAttached = debuggerAttached;
#endif

    if(!markerOk || (marker == SEAT_MOVE_DEBUG_EXIT_MAGIC) ||
       ((marker == SEAT_MOVE_DEBUG_ACTIVE_MAGIC) && !debuggerAttached))
    {
        latchedFault = SAFETY_FAULT_DEBUG_DETACH;
    }
    else if(watchdogReset && !hilWatchdogRecovered)
    {
        latchedFault = SAFETY_FAULT_BOOT_RECOVERY;
    }
    else if(debuggerAttached &&
            !SafetyManager_WriteDebugMarker(SEAT_MOVE_DEBUG_ACTIVE_MAGIC))
    {
        latchedFault = SAFETY_FAULT_DEBUG_DETACH;
    }

    /* The system-WDT configuration register is retained across some reset
     * paths and AUTO_SERVICE is not reset by Cy_WDT_Init().  If that bit is
     * left enabled, stopping the two software service paths can never prove a
     * real watchdog reset because hardware services the counter at WARN_LIMIT.
     * Configure only while stopped, and make software the sole WDT owner. */
    if(!watchdogStopped)
    {
        latchedFault = SAFETY_FAULT_DRIVER;
        return;
    }
    Cy_WDT_Init();
    Cy_WDT_Unlock();
    Cy_WDT_SetUpperLimit(SEAT_MOVE_WDT_TIMEOUT_TICKS);
    Cy_WDT_SetUpperAction(CY_WDT_LOW_UPP_ACTION_RESET);
    Cy_WDT_SetWarnAction(CY_WDT_WARN_ACTION_NONE);
    Cy_WDT_SetAutoService(CY_WDT_DISABLE);
    Cy_WDT_SetDebugRun(CY_WDT_ENABLE);
    Cy_WDT_Lock();
    if((WDT->unUPPER_LIMIT.stcField.u32UPPER_LIMIT !=
        SEAT_MOVE_WDT_TIMEOUT_TICKS) ||
       (WDT->unCONFIG.stcField.u1UPPER_ACTION !=
        (uint32_t)CY_WDT_LOW_UPP_ACTION_RESET) ||
       (WDT->unCONFIG.stcField.u1AUTO_SERVICE !=
        (uint32_t)CY_WDT_DISABLE))
    {
        latchedFault = SAFETY_FAULT_DRIVER;
        return;
    }
    Cy_WDT_Enable();
    if(!SafetyManager_WaitForWatchdogState(true))
    {
        latchedFault = SAFETY_FAULT_DRIVER;
        return;
    }
    Cy_WDT_ClearWatchdog();
    Cy_SysTick_DelayInUs(1000u);
    if(Cy_WDT_GetCount() == 0ul)
    {
        latchedFault = SAFETY_FAULT_DRIVER;
        return;
    }
    watchdogInitialized = true;
    SafetyManager_KickWatchdog();
}

void SafetyManager_KickWatchdog(void)
{
    if((false != watchdogInitialized) && !hilWatchdogResetArmed)
    {
        Cy_WDT_ClearWatchdog();
    }
}

bool SafetyManager_IsWatchdogInitialized(void)
{
    return watchdogInitialized;
}

void SafetyManager_LatchFault(safety_fault_t fault)
{
    if((fault != SAFETY_FAULT_NONE) &&
       (latchedFault == SAFETY_FAULT_NONE))
    {
        latchedFault = fault;
        Motor_LatchSafetyInhibitFromIsr(0u);
        Motor_StopAll();
        SeatMotorManager_StopAll(0ul);
    }
}

void SafetyManager_Service(void)
{
    bool debuggerAttached = SafetyManager_DebuggerAttached();
    uint32_t marker = SafetyManager_ReadDebugMarker();

    if(marker == SEAT_MOVE_DEBUG_EXIT_MAGIC)
    {
        SafetyManager_LatchFault(SAFETY_FAULT_DEBUG_DETACH);
    }

    if(Motor_IsDriverFaultLatched())
    {
        SafetyManager_LatchFault(SAFETY_FAULT_DRIVER);
    }
    else
    {
        /* No newly observed driver fault. */
    }

#if (SAFETY_DEBUG_DETACH_SUPPORTED == 1u)
    if(debuggerAttached)
    {
        if((marker != SEAT_MOVE_DEBUG_ACTIVE_MAGIC) &&
           !SafetyManager_WriteDebugMarker(SEAT_MOVE_DEBUG_ACTIVE_MAGIC))
        {
            SafetyManager_LatchFault(SAFETY_FAULT_DEBUG_DETACH);
        }
        debuggerWasAttached = true;
    }
    else if(debuggerWasAttached)
    {
        SafetyManager_LatchFault(SAFETY_FAULT_DEBUG_DETACH);
        debuggerWasAttached = false;
    }
#else
    (void)debuggerAttached;
#endif

    SafetyManager_KickWatchdog();
}

bool SafetyManager_IsInhibited(void)
{
    return (latchedFault != SAFETY_FAULT_NONE);
}

safety_fault_t SafetyManager_GetFault(void)
{
    return latchedFault;
}

bool SafetyManager_WasWatchdogReset(void)
{
    return watchdogReset;
}

bool SafetyManager_TriggerHilWatchdogReset(uint16_t token)
{
    uint32_t check;

    if(!watchdogInitialized || hilWatchdogResetArmed)
    {
        return false;
    }
    BACKUP->unBREG[SEAT_MOVE_HIL_WDT_MAGIC_BREG_INDEX].u32Register =
        SEAT_MOVE_HIL_WDT_MARKER;
    BACKUP->unBREG[SEAT_MOVE_HIL_WDT_TOKEN_BREG_INDEX].u32Register = token;
    check = SEAT_MOVE_HIL_WDT_MARKER ^ (uint32_t)token ^ 0xFFFFFFFFu;
    BACKUP->unBREG[SEAT_MOVE_HIL_WDT_CHECK_BREG_INDEX].u32Register = check;
    __DSB();
    if((BACKUP->unBREG[
            SEAT_MOVE_HIL_WDT_MAGIC_BREG_INDEX].u32Register !=
        SEAT_MOVE_HIL_WDT_MARKER) ||
       (BACKUP->unBREG[
            SEAT_MOVE_HIL_WDT_CHECK_BREG_INDEX].u32Register != check))
    {
        SafetyManager_ClearHilWatchdogCredential();
        return false;
    }
    /* Long-running motor calibration/coast loops normally service WDT
     * directly so they remain safe even while the application loop is busy.
     * The armed HIL case must suspend both service paths or a real watchdog
     * reset can never occur.  Startup re-enables this after the reset. */
    Motor_SetWatchdogServiceEnabled(false);
    Cy_WDT_ClearWatchdog();
    hilWatchdogResetArmed = true;
    return true;
}

void SafetyManager_CommitHilWatchdogReset(void)
{
    if(!hilWatchdogResetArmed)
    {
        return;
    }

    /* This bench's connected CMSIS-DAP can suppress the WDT reset output even
     * after C-SPY detaches.  First prove physical WDT expiry, then use the
     * reference project's authenticated processor-reset fallback. */
    while(Cy_WDT_GetCount() < SEAT_MOVE_WDT_TIMEOUT_TICKS)
    {
        __NOP();
    }
    __DSB();
    NVIC_SystemReset();
    for(;;)
    {
        __NOP();
    }
}

bool SafetyManager_WasHilWatchdogRecovered(void)
{
    return hilWatchdogRecovered;
}