import fido
import pyb

fido.fidolog.debug("Running checkSeal patch")

# Seconds to wait after pump off before the baseline vacuum sample (avoids pump noise).
_POST_PUMP_SETTLE_SEC = 2

# --- Field MSC hotfix: checkSeal on frozen 65729cb firmware; tunables from flash via fido.configurations.
# Rollback: restore stock main.py from commit 65729cb04e015cf5ee382453c50e398984c72ffd and prior configurations.py.

checkSeal_orig = fido.checkSeal

def _checkSeal_msc(breakVacuum):
    if not fido.clamp.atClose():
        raise fido.exceptions.FidoError("checkSeal, clamp must be closed before checking seal")
    fido.fidolog.debug("Confirming closed clamp seal")
    fido.vacuumPump.on()
    fido.actuator.stateSet(fido.VV + fido.VPV)
    fido.time.sleep(fido.configurations.CHECKSEAL_PUMP_TIME)
    fido.vacuumPump.off()

    fido.fidolog.debug(
        f"checkSeal post-pump settle {_POST_PUMP_SETTLE_SEC}s before baseline read"
    )
    fido.time.sleep(_POST_PUMP_SETTLE_SEC)
    baseline_vacuum = fido.signals.vacuumraw()
    fido.fidolog.debug(
        f"checkSeal baseline vacuum (after settle) {baseline_vacuum}, setpoint {fido.configurations.GOODVACUUM}"
    )
    if baseline_vacuum >= fido.configurations.GOODVACUUM:
        fido.vacuumPump.off()
        fido.actuator.stateSet(fido.VCV)
        fido.time.sleep(1)
        fido.actuator.stateSet(fido.NONE)
        raise fido.exceptions.FidoError("checkSeal, Closed Clamp failed to reach vacuum")

    fido.fidolog.debug("Waiting leak-check duration before second vacuum read")
    fido.time.sleep(fido.configurations.CHECKSEAL_HOLD_TIME)
    leak_check_vacuum = fido.signals.vacuumraw()
    delta = leak_check_vacuum - baseline_vacuum
    fido.fidolog.debug(
        f"checkSeal leak-check vacuum {leak_check_vacuum}, delta {delta}, delta setpoint {fido.configurations.CHECKSEAL_MAX_HOLD_DELTA}"
    )
    if delta > fido.configurations.CHECKSEAL_MAX_HOLD_DELTA:
        fido.vacuumPump.off()
        fido.actuator.stateSet(fido.VCV)
        fido.time.sleep(1)
        fido.actuator.stateSet(fido.NONE)
        raise fido.exceptions.FidoError("checkSeal, Clamp failed to hold vacuum")
    if breakVacuum:
        fido.actuator.stateSet(fido.VCV)
        fido.actuator.stateSet(fido.SBV | fido.VV | fido.SV | fido.VCV)
        fido.time.sleep(1)
        fido.vacuumPump.off()
        fido.actuator.stateSet(fido.NONE)
    fido.fidolog.debug("Seal confirmed")


fido.checkSeal = _checkSeal_msc

pyb.usb_mode(None)
pyb.usb_mode("MSC")

if fido.init()[0] != "ERROR":
    if fido.backupRAM.recoverState() == fido.backupRAM.BACKUP_RAM_RECOVER_SAFE:
        if fido.safe()[0] != "ERROR":
            fido.check()
        else:
            fido.fidolog.error("FIDO failed to safe, may need human interaction")
    else:
        fido.fidolog.error("FIDO recovery state not safe, run fido.safe()")
        fido.LEDindicators.on1()
else:
    fido.fidolog.error("FIDO failed to intialize")