#include "Motor.h"
#include "MBUtils.h"
#include "../pPowerManger/logc/loguru.hpp"
#include "json/json.h"
#include <cstdio>
#include <sstream>

using namespace motor;
using namespace std;

//---------------------------------------------------------
// Constructor / Destructor

Motor::Motor() {}

Motor::~Motor() {
    if (m_web) { m_web->stop(); delete m_web; m_web = nullptr; }
}

//---------------------------------------------------------
// OnStartUp：读取配置并初始化各组件

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

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

    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 = true;
        if (param == "can_channel")   m_canChannel  = value;
        else if (param == "red_channel") m_redChannel = value;
        else if (param == "cmd_period_ms") m_cmdPeriodMs = atoi(value.c_str());
        else if (param == "reset_hold_sec") m_resetHoldSec = atof(value.c_str());
        else if (param == "web_port")  m_webPort   = atoi(value.c_str());
        else if (param == "web_enable") m_webEnable = (tolower(value) == "true" || value == "1");
        else if (param == "logpath")   m_logPath   = value;
        else handled = false;

        if (!handled)
            reportUnhandledConfigWarning(orig);
    }
    if (m_cmdPeriodMs < 50) m_cmdPeriodMs = 50;

    registerVariables();

    // 日志文件
    if (m_logPath.empty()) m_logPath = "pMotor.log";
    loguru::add_file(m_logPath.c_str(), loguru::Append, loguru::Verbosity_MAX);
    LOG_F(INFO, "pMotor log path: %s", m_logPath.c_str());

    // 网页
    if (m_webEnable) {
        m_web = new WebServer();
        m_web->setOnOpen([this]() { return buildSnapshot(); });
        m_web->setApiHandler([this](const std::string& uri, const std::string& query) {
            return handleApi(uri, query);
        });
        if (!m_web->start(m_webPort)) {
            LOG_F(ERROR, "web server start failed on port %d", m_webPort);
            delete m_web;
            m_web = nullptr;
        }
    }

    LOG_F(INFO, "pMotor started: motor CAN via pCanBridge channel=%s (red_channel=%s), "
                "cmd period=%dms, web=%s",
          m_canChannel.c_str(), m_redChannel.c_str(), m_cmdPeriodMs,
          m_web ? ("port " + to_string(m_webPort)).c_str() : "disabled");
    return true;
}

//---------------------------------------------------------
// OnConnectToServer

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

//---------------------------------------------------------
// registerVariables：订阅 pCanBridge 透传的全部 CAN 帧

void Motor::registerVariables() {
    AppCastingMOOSApp::RegisterVariables();
    // 通配订阅必须用三参数重载（变量模式 + 来源模式）
    Register("CAN_0x*", "*", 0);
}

//---------------------------------------------------------
// OnNewMail

bool Motor::OnNewMail(MOOSMSG_LIST &NewMail) {
    AppCastingMOOSApp::OnNewMail(NewMail);
    MOOSMSG_LIST::iterator p;
    for (p = NewMail.begin(); p != NewMail.end(); p++) {
        CMOOSMsg &msg = *p;
        if (msg.m_sKey.rfind("CAN_0x", 0) == 0)
            handleCanMessage(msg);
    }
    return true;
}

//---------------------------------------------------------
// handleCanMessage：解码 pCanBridge 透传的 CAN 帧
//
// 消息格式（pCanBridge 约定）：
//   m_sKey   = "CAN_0x%08X"（CAN ID，扩展帧）
//   m_sVal   = 二进制数据域（8 字节）
//   m_dfVal2 = 原始帧信息字节（bit7 FF / bit6 RTR / bit3~0 DLC）

void Motor::handleCanMessage(CMOOSMsg& msg) {
    // 从变量名解析 CAN ID（跳过前缀 "CAN_0x" 共 6 字符）
    uint32_t canId = 0;
    if (std::sscanf(msg.m_sKey.c_str() + 6, "%x", &canId) != 1) return;
    if (!isCanMotorId(canId)) return;

    if (!(msg.IsBinary() && msg.GetBinaryDataSize() >= 8)) return;
    const unsigned char* d = msg.GetBinaryData();
    if (!d) return;

    // 日志值在锁内取副本，避免与 Web 线程快照读取竞争
    uint32_t rx = 0;
    int spd = 0;
    int bus = 0;
    size_t f1 = 0, f2 = 0;
    {
        std::lock_guard<std::mutex> lock(m_mutex);
        if (!decodeMotorFrame(canId, d, static_cast<int>(msg.GetBinaryDataSize()), m_state))
            return;
        m_state.rxCount++;
        double now = MOOSTime(false);
        switch (canId) {
        case MOTOR_CAN_FAULT1: case MOTOR_CAN_FAULT1_RED:
            m_state.branch1.lastRxTime = now; break;
        case MOTOR_CAN_FAULT2: case MOTOR_CAN_FAULT2_RED:
            m_state.branch2.lastRxTime = now; break;
        case MOTOR_CAN_STATE3: case MOTOR_CAN_STATE3_RED:
            m_state.state3LastRx = now; break;
        case MOTOR_CAN_STATE4: case MOTOR_CAN_STATE4_RED:
            m_state.state4LastRx = now; break;
        default: break;
        }
        rx = m_state.rxCount;
        spd = m_state.feedbackSpeed;
        bus = m_state.busVoltage;
        f1 = m_state.branch1.faults.size();
        f2 = m_state.branch2.faults.size();
    }

    // 节流日志：1s 一条
    double now = MOOSTime();
    if (now - m_lastRxLog >= 1.0) {
        m_lastRxLog = now;
        LOG_F(INFO, "[Motor/CAN] rx=%u speed=%d rpm bus=%dV faults(b1=%zu,b2=%zu)",
              rx, spd, bus, f1, f2);
    }
}

//---------------------------------------------------------
// Iterate：周期任务（AppTick 次/秒）

bool Motor::Iterate() {
    AppCastingMOOSApp::Iterate();

    double now = MOOSTime();

    // 网页指令出队并应用
    processCmdQueue();

    // 周期发送指令帧（500ms，协议要求）
    sendCmdFrame(now);

    // 网页快照 1Hz 推送
    if (m_web) {
        static double lastWebPush = 0;
        if (now - lastWebPush >= 1.0) {
            lastWebPush = now;
            m_web->broadcast(buildSnapshot());
        }
    }

    AppCastingMOOSApp::PostReport();
    return true;
}

//---------------------------------------------------------
// handleApi：/api/motor_cmd

std::string Motor::handleApi(const std::string& uri, const std::string& query) {
    if (uri == "/api/motor_cmd")
        return handleMotorCmdApi(query);
    return "";
}

//---------------------------------------------------------
// handleMotorCmdApi：电机控制指令入队（按协议指令帧字段独立下发，无联动）
//
// GET /api/motor_cmd?action=enable            使能命令=1（Byte0 bit0）
// GET /api/motor_cmd?action=disable           使能命令=0，期望转速清0（Byte0 bit0，协议"停机"示例）
// GET /api/motor_cmd?action=reset             复位命令=1（Byte0 bit1，仅故障态有效）
// GET /api/motor_cmd?action=speed&speed=1000  设定期望转速（Byte2/3，s16）
// Web 线程不可直接 Notify，指令入队后由 Iterate（MOOS 线程）应用。

std::string Motor::handleMotorCmdApi(const std::string& query) {
    // 简易 query 解析（k=v&k=v）
    auto getParam = [&query](const char* key, std::string& out) -> bool {
        std::string k = std::string(key) + "=";
        size_t p = query.find(k);
        if (p == std::string::npos) return false;
        size_t e = query.find('&', p);
        out = query.substr(p + k.size(),
                           (e == std::string::npos) ? std::string::npos
                                                    : e - p - k.size());
        return true;
    };

    std::string action, speedStr;
    getParam("action", action);
    getParam("speed", speedStr);

    MotorCmd c;
    if (action == "enable") {
        c.action = MOTOR_ACT_ENABLE;
    } else if (action == "disable") {
        c.action = MOTOR_ACT_DISABLE;
    } else if (action == "reset") {
        c.action = MOTOR_ACT_RESET;
    } else if (action == "speed") {
        c.action = MOTOR_ACT_SPEED;
        c.speed = static_cast<int16_t>(atoi(speedStr.c_str()));
    } else {
        return "{\"ok\":false,\"error\":\"bad action (enable/disable/reset/speed)\"}";
    }

    {
        std::lock_guard<std::mutex> lock(m_mutex);
        if (m_cmdQueue.size() >= 16)
            return "{\"ok\":false,\"error\":\"queue full\"}";
        m_cmdQueue.push_back(c);
        m_lastCmd = c;
        m_lastCmdValid = true;
        m_lastCmdTime = MOOSTime(false);
    }

    LOG_F(INFO, "[Motor/CAN] 指令入队: action=%s speed=%d", action.c_str(), c.speed);
    char buf[96];
    std::snprintf(buf, sizeof(buf),
                  "{\"ok\":true,\"action\":\"%s\",\"speed\":%d}", action.c_str(), c.speed);
    return buf;
}

//---------------------------------------------------------
// processCmdQueue：出队网页指令并更新指令状态

void Motor::processCmdQueue() {
    MotorCmd c;
    double now = MOOSTime();
    {
        std::lock_guard<std::mutex> lock(m_mutex);
        while (!m_cmdQueue.empty()) {
            c = m_cmdQueue.front();
            m_cmdQueue.erase(m_cmdQueue.begin());

            switch (c.action) {
            case MOTOR_ACT_ENABLE:
                m_enable = true;
                break;
            case MOTOR_ACT_DISABLE:
                m_enable = false;
                m_speed = 0;
                m_resetUntil = 0;
                break;
            case MOTOR_ACT_SPEED:
                m_speed = c.speed;
                break;
            case MOTOR_ACT_RESET:
                m_resetUntil = now + m_resetHoldSec;
                break;
            default:
                break;
            }
            LOG_F(INFO, "[Motor/CAN] 指令应用: action=%d speed=%d enable=%d",
                  c.action, m_speed, m_enable ? 1 : 0);
        }
    }
}

//---------------------------------------------------------
// sendCmdFrame：按周期编码并发送指令帧
//   主通道 0x18EF2010；配置 red_channel 时同帧发 0x18EF2011 到冗余通道

void Motor::sendCmdFrame(double now) {
    double lastTx = 0;
    {
        std::lock_guard<std::mutex> lock(m_mutex);
        if (now - m_lastTx < m_cmdPeriodMs / 1000.0)
            return;
        lastTx = m_lastTx;
    }
    if (now - lastTx < m_cmdPeriodMs / 1000.0)
        return;

    uint8_t data[8];
    {
        std::lock_guard<std::mutex> lock(m_mutex);
        bool reset = (now < m_resetUntil);
        buildMotorCmdData(m_enable, reset, m_enable ? m_speed : 0, data);
        m_lastTx = now;
    }

    bool ok = postCanFrame(MOTOR_CAN_CMD, data, m_canChannel);

    int okCount = ok ? 1 : 0, errCount = ok ? 0 : 1;
    if (!m_redChannel.empty()) {
        bool okRed = postCanFrame(MOTOR_CAN_CMD_RED, data, m_redChannel);
        okCount += okRed ? 1 : 0;
        errCount += okRed ? 0 : 1;
    }
    {
        std::lock_guard<std::mutex> lock(m_mutex);
        m_txCount += static_cast<unsigned long>(okCount);
        m_txErr   += static_cast<unsigned long>(errCount);
    }
}

//---------------------------------------------------------
// postCanFrame：MOOS CAN_TX_0x%08X（二进制数据域，m_sSrcAux=目标通道）
// 链路：MOOS -> pCanBridge 订阅 -> 按通道路由 CanEndpoint(TCP)
//       -> CANET -> CAN 总线

bool Motor::postCanFrame(uint32_t canId, const uint8_t data[8], const std::string& channel) {
    char key[32];
    std::snprintf(key, sizeof(key), "CAN_TX_0x%08X", canId);
    CMOOSMsg msg(MOOS_NOTIFY, key, 8u, data);
    msg.SetSourceAux(channel);
    bool ok = m_Comms.Post(msg);
    if (!ok)
        LOG_F(ERROR, "[Motor/CAN] 指令帧 0x%08X 投递 MOOS 失败 (通道=%s)", canId, channel.c_str());
    return ok;
}

//---------------------------------------------------------
// buildSnapshot：网页 JSON 快照（jsoncpp）

namespace {

void putI(Json::Value& j, const char* k, int v)      { j[k] = Json::Value(v); }
void putU(Json::Value& j, const char* k, unsigned long v) { j[k] = Json::Value(static_cast<Json::UInt64>(v)); }
void putD(Json::Value& j, const char* k, double v)    { j[k] = Json::Value(v); }

// age：距最后收到该帧的秒数；valid=false 时前端显示"从未收到"
void putAge(Json::Value& j, double lastRx, double now, bool valid) {
    j["valid"] = valid ? 1 : 0;
    j["age"]   = valid ? (now - lastRx) : -1;
}

Json::Value branchToJson(const MotorBranchState& br, double now) {
    Json::Value j(Json::objectValue);
    putAge(j, br.lastRxTime, now, br.valid);
    Json::Value words(Json::arrayValue);
    for (int i = 0; i < 4; ++i) words.append(static_cast<int>(br.word[i]));
    j["words"] = words;
    Json::Value faults(Json::arrayValue);
    for (size_t i = 0; i < br.faults.size(); ++i) faults.append(br.faults[i]);
    j["faults"] = faults;
    Json::Value alarms(Json::arrayValue);
    for (size_t i = 0; i < br.alarms.size(); ++i) alarms.append(br.alarms[i]);
    j["alarms"] = alarms;
    return j;
}

} // namespace

std::string Motor::buildSnapshot() {
    Json::Value root(Json::objectValue);
    double now = MOOSTime(false);

    {
        std::lock_guard<std::mutex> lock(m_mutex);

        // 通信链路
        Json::Value link(Json::objectValue);
        link["channel"]    = m_canChannel;
        link["redChannel"] = m_redChannel;
        link["periodMs"]   = m_cmdPeriodMs;
        putU(link, "txCount", m_txCount);
        putU(link, "txErr", m_txErr);
        putU(link, "rxCount", m_state.rxCount);
        link["lastTxValid"] = (m_lastTx > 0) ? 1 : 0;
        putD(link, "lastTxAge", m_lastTx > 0 ? (now - m_lastTx) : -1);
        root["link"] = link;

        // 指令状态
        Json::Value cmd(Json::objectValue);
        putI(cmd, "enable", m_enable ? 1 : 0);
        putI(cmd, "speed", m_speed);
        putI(cmd, "resetActive", (now < m_resetUntil) ? 1 : 0);
        cmd["lastCmdValid"]  = m_lastCmdValid ? 1 : 0;
        if (m_lastCmdValid) {
            static const char* kActionName[] = {"", "enable", "disable", "speed", "reset"};
            int idx = (m_lastCmd.action >= 1 && m_lastCmd.action <= 4) ? m_lastCmd.action : 0;
            cmd["lastCmdAction"] = kActionName[idx];
            putI(cmd, "lastCmdSpeed", m_lastCmd.speed);
            putD(cmd, "lastCmdAge", m_lastCmdTime > 0 ? (now - m_lastCmdTime) : -1);
        }
        root["cmd"] = cmd;

        // 状态帧3：转速 / 母线电压
        Json::Value s3(Json::objectValue);
        putAge(s3, m_state.state3LastRx, now, m_state.state3Valid);
        putI(s3, "speed", m_state.feedbackSpeed);
        putI(s3, "voltage", m_state.busVoltage);
        root["state3"] = s3;

        // 状态帧4：温度
        Json::Value s4(Json::objectValue);
        putAge(s4, m_state.state4LastRx, now, m_state.state4Valid);
        putI(s4, "tA", m_state.tempA);
        putI(s4, "tB", m_state.tempB);
        putI(s4, "tC", m_state.tempC);
        putI(s4, "tInv", m_state.tempInv);
        putI(s4, "tFilter", m_state.tempFilter);
        putI(s4, "tAir", m_state.tempAir);
        root["state4"] = s4;

        // 故障/报警
        root["fault1"] = branchToJson(m_state.branch1, now);
        root["fault2"] = branchToJson(m_state.branch2, now);
    }

    Json::FastWriter writer;
    return writer.write(root);
}

//---------------------------------------------------------
// buildReport

bool Motor::buildReport() {
    m_msgs << "============================================" << "\n";
    m_msgs << "pMotor 推进电机操作程序" << "\n";
    m_msgs << "============================================" << "\n";
    m_msgs << "CAN 通道: " << m_canChannel
           << (m_redChannel.empty() ? "" : (" 冗余: " + m_redChannel)).c_str()
           << "  指令周期: " << m_cmdPeriodMs << "ms" << "\n";
    m_msgs << "指令帧下发: " << m_txCount << " 成功 / " << m_txErr << " 失败" << "\n";
    {
        std::lock_guard<std::mutex> lock(m_mutex);
        m_msgs << "电机报文接收: " << m_state.rxCount << "\n";
        m_msgs << "指令状态: enable=" << (m_enable ? 1 : 0)
               << " speed=" << m_speed
               << " reset=" << (MOOSTime(false) < m_resetUntil ? 1 : 0) << "\n";
        m_msgs << "反馈转速: " << m_state.feedbackSpeed
               << " rpm  母线电压: " << m_state.busVoltage << "\n";
        m_msgs << "支路一故障: " << m_state.branch1.faults.size()
               << " 条, 支路二故障: " << m_state.branch2.faults.size() << " 条" << "\n";
    }
    return true;
}