#include "PowerManagerFsm.hpp"

// FaultState实现
void FaultState::entry() {
    PowerManagerFsm::entry();
    if(pm->m_missionState.type == TaskType::THROW_LOAD)
    {
        LOG_F(INFO, "检测到抛载");
        pm->m_systemData->setSystemInfo("ERROR:检测到抛载");
        //1 解除启闭机构
        unsigned char cmd = 0;
        pm->m_lowerCommManager->operate_HVA_actuatorCircuit_Breaker(0);
        MOOSPause(3);
        pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowCover1, (unsigned char)cmd);
        pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowCover2, (unsigned char)cmd);
        pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowCover3, (unsigned char)cmd);
        pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowCover4, (unsigned char)cmd);
        pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowCover5, (unsigned char)cmd);
        pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowCover6, (unsigned char)cmd);
        pm->m_upperCommManager->processPeriodicTasks();
        //2 解除敞水机构
        pm->m_lowerCommManager->operate_HVA_openWaterCoverStartCylinderCircuit_Breaker(0);
        MOOSPause(3);
        pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowWaterInet1, (unsigned char)cmd);
        pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowWaterInet2, (unsigned char)cmd);
        pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowWaterInet3, (unsigned char)cmd);
        pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowWaterInet4, (unsigned char)cmd);
        pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowWaterInet5, (unsigned char)cmd);
        pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowWaterInet6, (unsigned char)cmd);
        pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowWaterInet7, (unsigned char)cmd);
        pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowWaterInet8, (unsigned char)cmd);
        pm->m_upperCommManager->processPeriodicTasks();
        //3 解除艏部舵机
        pm->m_lowerCommManager->operate_HVA_bowRudderControlBoxCircuit_Breaker(0);
        MOOSPause(3);
        pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowRudderDeploy, (unsigned char)cmd);
        pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::bowRudderServo1, (unsigned char)cmd);
        pm->m_upperCommManager->processPeriodicTasks();
        //4 解除艉部舵机
        pm->m_lowerCommManager->operate_HVA_mastSteeringGearControlBoxCircuit_Breaker(0);
        MOOSPause(3);
        pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::mastLiftingServo, (unsigned char)cmd);
        pm->m_upperCommManager->processPeriodicTasks();
        //关闭推进电机
        pm->m_lowerCommManager->operate_HV_propulsionMotor_Breaker(0);
        //检测电机去使能状态，5秒超时
        auto startTime = std::chrono::steady_clock::now();
        while(pm->m_thrustEnable == 1)
        {
            // 检查是否超时（5秒）
            auto currentTime = std::chrono::steady_clock::now();
            auto elapsed = std::chrono::duration_cast<std::chrono::seconds>(currentTime - startTime);
            if(elapsed.count() >= 20)
            {
                LOG_F(WARNING, "电机去使能超时（20秒）");
                pm->m_systemData->setSystemInfo("ERROR:电机去使能超时（20秒）");
                return;
            }
            MOOSPause(100);
        }
    }
}

void FaultState::react(FaultEvent const & e) {
    LOG_F(INFO, "FaultState 处理故障事件");
}

void FaultState::react(MasterCommandEvent const &) {
    PowerManagerFsm::handelPowerCmd();
    PowerManagerFsm::handleModCmd();
    PowerManagerFsm::handleWorkCmd();
}