#include "PowerManagerFsm.hpp"

// ShoreBasedReady实现
void ShoreBasedReady::react(MasterCommandEvent const &) {
    PowerManagerFsm::handelPowerCmd();
    PowerManagerFsm::handleModCmd();
    PowerManagerFsm::handleWorkCmd();
}

void ShoreBasedReady::entry() {
    PowerManagerFsm::entry();
    pm->m_driver.getSHORE_BASED_READYable(&pm->m_subDisSysCmd);
    pm->workCondition = WorkCondition::CHANING;
    
    //依次接入执行机构动力电设备
    //1. 开启动力电
    //1.1 动力锂电池上电
    {
        std::lock_guard<std::mutex> lock(pm->m_stateMutex);
        pm->m_CcuColCmd.powerBatCmd = BatteryCommand::Start;
    }
    // 等待动力锂电池上电完成
    int count=0;
    while (!(pm->m_systemData->getDynVoltageLink() > 500.0))
    {
        LOG_F(INFO,"动力锂电池上电中...");
        MOOSPause(1000);
        if(count>10)
        {
            LOG_F(INFO, "动力锂电池无上电反馈"); 
            pm->m_systemData->setSystemInfo("ERROR:动力锂电池无上电反馈");
            return;
        }
        count++;
    }
    //1.2 闭合动力电断路器
    pm->m_lowerCommManager->operate_HV_powerLithiumBattery_Breaker(1);
    MOOSPause(100);
    pm->m_lowerCommManager->operate_HV_dcDc5Module_Breaker(1);
    MOOSPause(100);
    pm->m_lowerCommManager->operate_HV_dcDc5_Breaker(1);
    pm->m_lowerCommManager->operate_HV_coolingSystem_Breaker(1);
    MOOSPause(100);
    pm->m_lowerCommManager->operate_HV_bowHighVoltageBox_Breaker(1);
    MOOSPause(100);
    //2.  启动动力电设备 
    unsigned char cmd = 1;
    //2.0 艉舵
    pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::sternRudderServo1, (unsigned char)cmd);
    pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::sternRudderServo2, (unsigned char)cmd);
    pm->m_upperCommManager->processPeriodicTasks();
    pm->m_lowerCommManager->operate_HV_sternRudder1_Breaker(1);
    pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::sternRudderServo3, (unsigned char)cmd);
    pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::sternRudderServo4, (unsigned char)cmd);
    pm->m_upperCommManager->processPeriodicTasks();
    pm->m_lowerCommManager->operate_HV_sternRudder2_Breaker(1);
    //2.1 启闭机构
    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();
    MOOSPause(100);
    pm->m_lowerCommManager->operate_HVA_actuatorCircuit_Breaker(1);
    //2.2 敞水电缸
    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();
    MOOSPause(100);
    pm->m_lowerCommManager->operate_HVA_openWaterCoverStartCylinderCircuit_Breaker(1);
    //2.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();
    MOOSPause(100);
    pm->m_lowerCommManager->operate_HVA_bowRudderControlBoxCircuit_Breaker(1);
    //2.4 桅杆舵机
    pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::mastLiftingServo, (unsigned char)cmd);
    pm->m_upperCommManager->processPeriodicTasks();
    MOOSPause(100);
    pm->m_lowerCommManager->operate_HVA_mastSteeringGearControlBoxCircuit_Breaker(1);
    //2.5 艏部浮调机构
    pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::buoyancyAdjust1, (unsigned char)cmd);
    pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::buoyancyAdjust2, (unsigned char)cmd);
    pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::buoyancyAdjust3, (unsigned char)cmd);    
    pm->m_upperCommManager->processPeriodicTasks();            
    MOOSPause(100);
    pm->m_lowerCommManager->operate_HVA_bowFTDevice123Circuit_Breaker(1);
    //2.6 艉部浮调机构
    pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::buoyancyAdjust4, (unsigned char)cmd);
    pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::buoyancyAdjust5, (unsigned char)cmd);
    pm->m_upperCommManager->processPeriodicTasks();
    MOOSPause(100);
    pm->m_lowerCommManager->operate_HV_sternFt45_Breaker(1);
    //2.7 推进电机
    pm->m_driver.setDeviceState(pm->m_subDisSysCmd, DeviceId::thruster, (unsigned char)cmd);
    pm->m_upperCommManager->processPeriodicTasks();
    MOOSPause(100);
    pm->m_lowerCommManager->operate_HV_propulsionMotor_Breaker(1);
    pm->workCondition = WorkCondition::SHORE_BASED_READY;
    LOG_F(INFO, "进入岸基备航状态");
    pm->m_systemData->setSystemInfo("INFO:已经进入岸基备航");
}