/************************************************************/
/*    NAME:                                               */
/*    ORGN: MIT, Cambridge MA                               */
/*    FILE: PowerManger.cpp                                        */
/*    DATE: December 29th, 1963                             */
/************************************************************/

#include <iterator>
#include "MBUtils.h"
#include "ACTable.h"
#include "PowerManger.h"
#include "logc/loguru.hpp"
#include <memory>

using namespace std;


bool _FsmCB(void * pParam)
{
    PowerManger* pMe = (PowerManger* )pParam;
    return pMe->FsmLoop();
}

bool _CommCB(void * pParam)
{
    PowerManger* pMe = (PowerManger* )pParam;
    return pMe->commLoop();
}

//---------------------------------------------------------
// Constructor()

PowerManger::PowerManger()
{
  workCondition = 0;
  currentMod = 0;
  powerState = 0;
  faultLevel = 0;
  soc = 0;
  sustainableTime = 0;
  m_faultNum = 0;
  m_ticker = 0;
  m_leakage = 0;
  m_thrustRpm = 0;
  m_thrustEnable = 0;
  m_insData = {};
  faultCode.clear();

  m_skew = 10;

  // CCU 地址默认值，可在 moos 文件中用 ccuhost/ccuport 覆盖
  m_ccuHost = CCUHOST;
  m_ccuPort = CCUPORT;

  m_pmCurrentState = {};
  m_currentDisBreakerState = {};
  m_targetDisBreakerState = {};
  m_currentDriverState = {};
  m_MSDriverCmd = {};
  m_subDisSysCmd = {};
  m_subDisSysState = {};
  m_subDisSysFault = {};

  //初始化系统数据
  m_systemData = new SystemData();

  //初始化控制指令
  initControlCmd();

  //初始化下位机通信
  m_lowerCommManager = new LowerCommManager(this,IPORT,m_ccuPort,m_ccuHost);
  m_lowerCommManager->initUdpComm();

  //初始化上位机通信
  m_upperCommManager = new UpperCommManager(this);

  //数据库在 OnStartUp 读取配置后初始化
  m_db = nullptr;

  //初始化http服务器
  m_httpServer = new HttpServer(8090);
  m_httpServer->setPowerManger(this);
  m_httpServer->setLowerCommManager(m_lowerCommManager);
  m_httpServer->setSystemData(m_systemData);
  m_httpServer->start();
}

//---------------------------------------------------------
// Destructor

PowerManger::~PowerManger()
{
  m_commThread.Stop();
  // 必须先停 HTTP 线程（stop() 内部 pthread_join 阻塞等待线程退出），
  // 否则 HTTP 线程每秒仍会读取 m_systemData / m_lowerCommManager / m_upperCommManager，
  // 导致 use-after-free。
  delete m_httpServer;
  delete m_lowerCommManager;  // 停止监听线程，确保其不再调用 DB
  delete m_db;
  delete m_upperCommManager;
  delete m_systemData;        // 最后删除，此时无任何线程引用
}

//---------------------------------------------------------
// Procedure: OnNewMail()

bool PowerManger::OnNewMail(MOOSMSG_LIST &NewMail)
{
  AppCastingMOOSApp::OnNewMail(NewMail);

  MOOSMSG_LIST::iterator p;
  for(p=NewMail.begin(); p!=NewMail.end(); p++) 
  {
    CMOOSMsg &msg = *p;
    string key    = msg.GetKey();
    string strJson = msg.GetString();
    double dfT;
    msg.IsSkewed(m_curr_time, &dfT);
    if(fabs(dfT) > m_skew)
    {
      LOG_F(INFO, "Message from %s at time %f is Skewed", msg.GetSource().c_str(), dfT);
      return true;
    }
    //处理上位机发送的控制指令
    if(!m_upperCommManager->processUpperMsg(key,strJson))
    {
      LOG_F(ERROR, "上位机消息处理错误: %s", key.c_str());
    }
    
#if 0 // Keep these around just for template
    string comm  = msg.GetCommunity();
    double dval  = msg.GetDouble();
    string sval  = msg.GetString(); 
    string msrc  = msg.GetSource();
    double mtime = msg.GetTime();
    bool   mdbl  = msg.IsDouble();
    bool   mstr  = msg.IsString();
#endif

     if(key == "FOO")
       LOG_F(INFO, "Received FOO message");

     else if(key != "APPCAST_REQ") // handled by AppCastingMOOSApp
       reportRunWarning("Unhandled Mail: " + key);
   }
	
   return(true);
}

//---------------------------------------------------------
// Procedure: OnConnectToServer()

bool PowerManger::OnConnectToServer()
{
   registerVariables();
   return(true);
}

//---------------------------------------------------------
// Procedure: Iterate()
//            happens AppTick times per second
//固定为4Hz
bool PowerManger::Iterate()
{
  AppCastingMOOSApp::Iterate();
  // 触发定时任务
  CycleTriggeredEvent cycleEvent;
  PowerManagerFsm::dispatch(cycleEvent);
  faultLevel = getFaultLevel();
  m_systemData->updateCommonStatus();
  // m_CcuColCmd.heartbeat
  // if (m_ticker++ % 5 == 0)
  // {
  //   m_CcuColCmd.heartbeat++;
  //   m_lowerCommManager->processPeriodicTasks();
  //   m_upperCommManager->processPeriodicTasks();
  // }
  return(true);
}

//---------------------------------------------------------
// Procedure: OnStartUp()
//            happens before connection is open

bool PowerManger::OnStartUp()
{
  AppCastingMOOSApp::OnStartUp();

  STRING_LIST sParams;
  m_MissionReader.EnableVerbatimQuoting(false);
  if(!m_MissionReader.GetConfiguration(GetAppName(), sParams))
    reportConfigWarning("No config block found for " + GetAppName());

  std::string dbPath;  // 数据库路径，不配置则默认当前目录 power_data.db
  std::string logPath; // 日志文件路径，不配置则默认当前目录 pPowerManger.log
  std::string ccuHost; // CCU 地址，不配置则沿用默认值
  long ccuPort = 0;    // CCU 端口，不配置则沿用默认值

  STRING_LIST::iterator p;
  for(p=sParams.begin(); p!=sParams.end(); p++) {
    string orig  = *p;
    string line  = *p;
    string param = stripBlankEnds(tolower(biteStringX(line, '=')));
    string value = stripBlankEnds(line);

    bool handled = false;
    if(param == "foo") {
      handled = true;
    }
    else if(param == "bar") {
      handled = true;
    }
    else if(param == "dbpath") {
      dbPath = value;
      handled = true;
    }
    else if(param == "logpath") {
      logPath = value;
      handled = true;
    }
    else if(param == "ccuhost") {
      ccuHost = value;
      handled = true;
    }
    else if(param == "ccuport") {
      ccuPort = atoi(value.c_str());
      handled = true;
    }

    if(!handled)
      reportUnhandledConfigWarning(orig);

  }

  registerVariables();	

  // 初始化日志文件：logpath 未配置则默认当前目录
  if (logPath.empty()) logPath = "pPowerManger.log";
  loguru::add_file(logPath.c_str(), loguru::Append, loguru::Verbosity_MAX);
  LOG_F(INFO, "日志文件路径: %s", logPath.c_str());

  // 初始化数据库：dbpath 未配置则默认当前目录
  if (dbPath.empty()) dbPath = "power_data.db";
  LOG_F(INFO, "数据库存储路径: %s", dbPath.c_str());
  m_db = new SQLite(dbPath);
  m_db->createTable();

  // 应用 moos 文件中的 CCU 地址配置（在监听/通信线程启动前生效）
  if (!ccuHost.empty()) m_ccuHost = ccuHost;
  if (ccuPort > 0) m_ccuPort = ccuPort;
  m_lowerCommManager->setCcuAddress(m_ccuHost, m_ccuPort);
  LOG_F(INFO, "CCU 地址: %s:%ld", m_ccuHost.c_str(), m_ccuPort);

  // 注入原始帧日志回调（RX/TX 双向落库）
  m_lowerCommManager->setCommLogger(m_db);
  // 数据库就绪后再启动下位机监听线程
  m_lowerCommManager->startListenThread();

  // 初始化状态机
  PowerManagerFsm::init(this);
  PowerManagerFsm::start();
  InitEvent initEvent;
  PowerManagerFsm::dispatch(initEvent);

  //初始化通信线程
  m_commThread.Initialise(_CommCB,this);
  m_commThread.Start();
  return(true);
}

//---------------------------------------------------------
// Procedure: registerVariables()

void PowerManger::registerVariables()
{
  AppCastingMOOSApp::RegisterVariables();
  // Register(DB_TO_PM_SUBDISSTATE, 0);

  Register(DB_TO_PM_MSCMD, 0);
  Register(DB_TO_PM_EQUIPMENTCMD, 0);
  Register(m_upperCommManager->m_secdistStateMsg.key_fb,0);
  Register(DB_TO_PM_MISSION,0);
  Register(DB_TO_PM_DEEPTH,0);
  Register(DB_TO_PM_INSDATA,0);
  Register(DB_TO_PM_SETOPCONDITION,0);
  Register(DB_TO_PM_SETOPMODE,0);
  Register(DB_TO_PM_SETPMSYS,0);
  Register(DB_TO_PM_SETDEVSWITCH,0);
  Register(DB_TO_PM_TASKSTOP,0);

  Register(uDetect_xcVoltage_cmd::key,0);
  Register(uDetect_zdpVoltage_cmd::key,0);
  Register(uDevice_thruster_st::key,0);
  // Register(uExternComm_setThrustSpeed_cmd::key,0);
}


//------------------------------------------------------------
// Procedure: buildReport()

bool PowerManger::buildReport() 
{
  m_msgs << "============================================" << endl;
  m_msgs << "File:                                       " << endl;
  m_msgs << "============================================" << endl;

  ACTable actab(4);
  actab << "Alpha | Bravo | Charlie | Delta";
  actab.addHeaderLines();
  actab << "one" << "two" << "three" << "four";
  m_msgs << actab.getFormattedString();

  return(true);
}



bool PowerManger::FsmLoop()
{
  return true;
}

int PowerManger::checkError()
{
  //TODO：更新协议
  //1.检查复合管控器状态
  // if(m_pmCurrentState.ccustate.fcuGZ_1H != 0 ||
  // m_pmCurrentState.ccustate.fcuGZ_1L != 0 ||
  // m_pmCurrentState.ccustate.fcuGZ_2H != 0 ||
  // m_pmCurrentState.ccustate.fcuGZ_2L != 0 ||
  // m_pmCurrentState.ccustate.fcuGZ_3H != 0 ||
  // m_pmCurrentState.ccustate.fcuGZ_3L != 0 ||
  // m_pmCurrentState.ccustate.insEmergencyStatusWord != 0)
  return 0;
}

void PowerManger::printState(const std::string& stateName, const std::string& desp){
  if (desp.find("故障") != string::npos) {
    LOG_F(ERROR, "STATE %s -> %s", stateName.c_str(), desp.c_str());
  } else {
    LOG_F(INFO, "STATE %s -> %s", stateName.c_str(), desp.c_str());
  }
}


void PowerManger::initControlCmd()
{
  //================初始化配电指令=============================
  m_targetDisBreakerState.bus1Breaker.fuelCellCircuitBreaker = 0;
  m_targetDisBreakerState.bus1Breaker.powerLithiumBatteryCircuitBreaker = 1; //动力锂电池默认接入
  m_targetDisBreakerState.bus1Breaker.propulsionMotorCircuitBreaker = 0; 
  m_targetDisBreakerState.bus1Breaker.lithiumBatteryGroupInstrumentCircuitBreaker = 1; //仪表锂电池默认接入
  m_targetDisBreakerState.bus1Breaker.dcDc5ModuleCircuitBreaker = 1; //DC-DC模块默认接入
  m_targetDisBreakerState.bus1Breaker.bowHighVoltageDistributionBoxCircuitBreaker = 1; //艏部高压母线断路器默认接入
  m_targetDisBreakerState.bus1Breaker.sternFTDevice45CircuitBreaker = 0; // sternFTDevice45CircuitBreaker默认接入
  m_targetDisBreakerState.bus1Breaker.sternRudderSwitch1CircuitBreaker = 0; 
  m_targetDisBreakerState.bus1Breaker.sternRudderSwitch2CircuitBreaker = 0; 
  m_targetDisBreakerState.bus1Breaker.fbReservedCircuitBreaker = 0; 
  m_targetDisBreakerState.bus1Breaker.tyzReservedCircuitBreaker = 0; 
  m_targetDisBreakerState.bus1Breaker.reservedCircuitBreaker = 0;

  m_targetDisBreakerState.bus2Breaker.bowFTDevice123CircuitBreaker = 0;
  m_targetDisBreakerState.bus2Breaker.actuatorCircuitBreaker = 0;
  m_targetDisBreakerState.bus2Breaker.mastSteeringGearControlBoxCircuitBreaker = 0;
  m_targetDisBreakerState.bus2Breaker.xczCircuitBreaker = 0;
  m_targetDisBreakerState.bus2Breaker.bowRudderControlBoxCircuitBreaker = 0;
  m_targetDisBreakerState.bus2Breaker.openWaterCoverStartCylinderCircuitBreaker = 0;
  //锂电池默认接入
  m_targetDisBreakerState.bus3Breaker.lithiumBatteryGroupInstrumentCircuitBreaker = 1;
  m_targetDisBreakerState.bus3Breaker.bowLowVoltageDistributionBoxCircuitBreaker = 1;
  m_targetDisBreakerState.bus3Breaker.unit4InstrumentDC48VCircuitBreaker = 1;
  m_targetDisBreakerState.bus3Breaker.dcC1DCDistributionPanelCircuitBreaker = 1;
  m_targetDisBreakerState.bus3Breaker.reservedCircuitBreaker1 = 0;
  m_targetDisBreakerState.bus3Breaker.reservedCircuitBreaker2 = 0;
  m_targetDisBreakerState.bus3Breaker.emergencyLithiumBatteryGroup2CircuitBreaker = 1;

  m_targetDisBreakerState.bus4Breaker.unit1CircuitBreaker = 1;
  m_targetDisBreakerState.bus4Breaker.unit2CircuitBreaker = 1;
  m_targetDisBreakerState.bus4Breaker.unit3CircuitBreaker = 1;
  m_targetDisBreakerState.bus4Breaker.unit5CircuitBreaker = 1;
  m_targetDisBreakerState.bus4Breaker.bowPZDeviceCircuitBreaker = 1;
  m_targetDisBreakerState.bus4Breaker.reservedCircuitBreaker1 = 0;
  m_targetDisBreakerState.bus4Breaker.reservedCircuitBreaker2 = 0;
  m_targetDisBreakerState.bus4Breaker.emergencyLithiumBatteryGroup1CircuitBreaker = 1; //紧急锂电池默认接入
  //================初始化复合管控=============================
  m_CcuColCmd.fcmode = 0x00; //默认无效
  m_CcuColCmd.fcuCmd = 0x00; //无效指令
  m_CcuColCmd.fcuPowerEfficiency = 0; //输出功率指令，0~250，分辨率0.1kW
  m_CcuColCmd.pitchData1 = 0; //纵倾姿态数据1（int16，分辨率0.1°；0824协议：姿态数据2已删除）
  m_CcuColCmd.rollData1 = 0; //横倾姿态数据1
  m_CcuColCmd.emergencyAllow = 0; //应急允许
  m_CcuColCmd.depth = 0; //潜深深度
  m_CcuColCmd.supplyExhaustCmd = 0x00; //补给/排放指令
  m_CcuColCmd.reservedCmd5 = 0; //预留指令5
  m_CcuColCmd.reservedCmd6 = 0; //预留指令6
  m_CcuColCmd.insBatCmd = 0x00; //无效指令
  m_CcuColCmd.powerBatCmd = 0x00; //无效指令
  m_CcuColCmd.powerBatEfficiency = 0; //动力锂电池功率配置值
  m_CcuColCmd.heartbeat = 0;
  m_CcuColCmd.masterState = 0xAA;//主站状态,运行中
  //================初始化上位机指令=========================
  m_msCmd.modCMD = 99;
  m_msCmd.powerCMD = 99;
  m_msCmd.workCMD = 99;
  //================初始化设备驱动指令========================

}

void PowerManger::addFaultCode(unsigned int code)
{
    std::lock_guard<std::mutex> lock(m_stateMutex);
    faultCode.push_back(code);
}

void PowerManger::clearFaultCodes()
{
    std::lock_guard<std::mutex> lock(m_stateMutex);
    faultCode.clear();
}

std::vector<unsigned int> PowerManger::getFaultCodes() const
{
    std::lock_guard<std::mutex> lock(m_stateMutex);
    return faultCode;
}

unsigned char PowerManger::getFaultLevel()
{
    unsigned char maxLevel = 0;
    for (auto code : getFaultCodes()) {
        // 提取0xabcd中的b位 (右移8位后取低4位)
        unsigned char level = (code >> 8) & 0xF;
        if (level > maxLevel) {
            maxLevel = level;
        }
    }
    return maxLevel;
}

bool PowerManger::commLoop()
{
    while(!m_commThread.IsQuitRequested())
    {
        {
            std::lock_guard<std::mutex> lock(m_stateMutex);
            m_CcuColCmd.heartbeat++;
        }
        m_lowerCommManager->processPeriodicTasks();
        m_upperCommManager->processPeriodicTasks();
        // 延时1秒
        usleep(1000000);
    }
    return true;
}

void PowerManger::injectFault(unsigned type,unsigned int faultCode){
  LOG_F(INFO, "注入故障: %d %d", type, faultCode);
  TestEvent testEvent("故障注入测试",type,faultCode);
  PowerManagerFsm::dispatch(testEvent);
}


void PowerManger::pushDeviceCommand(DeviceSwitchCmd cmd) {
    std::lock_guard<std::mutex> lock(m_deviceCmdMutex);
    m_deviceCmdQuenue.push(cmd);
}

bool PowerManger::popDeviceCommand(DeviceSwitchCmd &cmd) {
    std::lock_guard<std::mutex> lock(m_deviceCmdMutex);
    if(m_deviceCmdQuenue.empty()) {
        return false;
    }
    cmd = m_deviceCmdQuenue.front();
    m_deviceCmdQuenue.pop();
    return true;
}

bool PowerManger::isDeviceCmdEmpty() const {
    std::lock_guard<std::mutex> lock(m_deviceCmdMutex);
    return m_deviceCmdQuenue.empty();
}