#include "mockvehicle.h"
#include <QSettings>
#include <QJsonDocument>
#include <QJsonObject>
#include <QJsonArray>
#include <QHostAddress>
#include <QNetworkDatagram>
#include <QtMath>

MockVehicle::MockVehicle(QObject* parent)
    : QObject(parent)
{
    QSettings s;
    m_localPort  = s.value("net/local_port", 9000).toInt();   // 发送到遥控端
    m_remotePort = s.value("net/remote_port", 9001).toInt();  // 本端监听
    m_enabled    = s.value("net/mock", true).toBool();
    m_plantSelfCheckFault = s.value("net/selfcheck_fault", false).toBool();

    if (!m_enabled)
        return;

    m_socket = new QUdpSocket(this);
    m_socket->bind(QHostAddress::AnyIPv4, m_remotePort,
                   QUdpSocket::ShareAddress | QUdpSocket::ReuseAddressHint);
    connect(m_socket, &QUdpSocket::readyRead, this, &MockVehicle::onReadyRead);

    m_sendTimer = new QTimer(this);
    connect(m_sendTimer, &QTimer::timeout, this, &MockVehicle::onSendTick);
    m_sendTimer->start(500);

    m_selfCheckTimer = new QTimer(this);
    connect(m_selfCheckTimer, &QTimer::timeout, this, &MockVehicle::onSelfCheckTick);
}

void MockVehicle::sendJson(const QJsonObject& obj)
{
    QByteArray data = QJsonDocument(obj).toJson(QJsonDocument::Compact);
    m_socket->writeDatagram(data, QHostAddress("127.0.0.1"), m_localPort);
}

void MockVehicle::onReadyRead()
{
    while (m_socket->hasPendingDatagrams()) {
        QNetworkDatagram dg = m_socket->receiveDatagram();
        QJsonParseError err;
        QJsonDocument doc = QJsonDocument::fromJson(dg.data(), &err);
        if (err.error == QJsonParseError::NoError && doc.isObject())
            handleCommand(doc.object());
    }
}

void MockVehicle::handleCommand(const QJsonObject& o)
{
    const QString t = o.value("t").toString();

    if (t == "chassis_move") {
        const QString dir = o.value("dir").toString();
        m_speed = o.value("speed").toDouble();
        if (dir == "stop") m_speed = 0;
    } else if (t == "arm_move") {
        int d = o.value("dir").toInt();
        if (d == 0) return;
        if (m_js.size() >= 6)
            m_js[1] = qBound(-90.0, m_js[1] + d * 1.0, 90.0);
    } else if (t == "arm_return") {
        m_armReturn = "returning";
        m_armHomed = false;
    } else if (t == "arm_pause") {
        m_armReturn = "idle";
    } else if (t == "gas_start") {
        m_gasRunning = true;
    } else if (t == "gas_stop") {
        m_gasRunning = false;
    } else if (t == "wash_start") {
        m_washRunning = true;
        m_washMode = (o.value("mode").toString() == "disinfect") ? "手动消毒" : "手动消除";
        m_pPlunger = true; m_pAir = true; m_pLeaf = true;
    } else if (t == "wash_stop") {
        m_washRunning = false;
        m_pPlunger = false; m_pAir = false; m_pLeaf = false;
    } else if (t == "ball_valve") {
        int v = o.value("valve").toInt();
        bool open = o.value("open").toBool();
        if (v == 1) m_valve1Open = open;
        else        m_valve2Open = open;
    } else if (t == "pump_power") {
        const QString p = o.value("pump").toString();
        bool on = o.value("on").toBool();
        if (p == "plunger") m_pPlunger = on;
        else if (p == "air") m_pAir = on;
        else if (p == "leaf") m_pLeaf = on;
    } else if (t == "pump_rpm") {
        const QString p = o.value("pump").toString();
        int rpm = o.value("rpm").toInt();
        if (p == "plunger") m_plungerRpm = rpm;
        else if (p == "air") m_airRpm = rpm;
        else if (p == "leaf") m_leafRpm = rpm;
    } else if (t == "selfcheck_start") {
        m_selfCheckActive = true;
        m_selfCheckIndex = 0;
        m_selfCheckTimer->start(450);
    } else if (t == "estop") {
        m_estop = true;
        m_washRunning = false; m_gasRunning = false;
        m_pPlunger = false; m_pAir = false; m_pLeaf = false;
        QJsonObject o2; o2["t"] = "estop"; o2["triggered"] = true;
        sendJson(o2);
    } else if (t == "estop_reset") {
        m_estop = false;
        QJsonObject o2; o2["t"] = "estop"; o2["triggered"] = false;
        sendJson(o2);
    }
    // set_param 仅用于演示，无需额外处理
}

void MockVehicle::onSendTick()
{
    sendHeartbeat();
    sendTelemetry();
}

void MockVehicle::sendHeartbeat()
{
    QJsonObject o; o["t"] = "heartbeat"; o["ok"] = true;
    sendJson(o);
}

void MockVehicle::sendTelemetry()
{
    // 底盘
    {
        QJsonObject o;
        QJsonArray faults;
        if (m_estop) faults << QStringLiteral("急停触发");
        o["t"] = "chassis";
        o["soc"] = 85; o["volt"] = 360.0; o["speed"] = m_speed;
        o["handbrake"] = true; o["faults"] = faults;
        sendJson(o);
    }
    // 机械臂
    {
        if (m_armReturn == "returning") {
            bool allZero = true;
            for (double& a : m_js) { if (qAbs(a) > 1.0) a *= 0.7; else a = 0; if (qAbs(a) > 0.1) allZero = false; }
            if (allZero) { m_armReturn = "done"; m_armHomed = true; }
        }
        QJsonObject o;
        QJsonArray js;
        for (double a : m_js) js << a;
        o["t"] = "arm";
        o["js"] = js;
        o["pos"] = QString("X:%1 Y:%2 Z:%3").arg(1200).arg(300).arg(800);
        o["pose"] = "RX:0 RY:0 RZ:0";
        o["vel"] = (m_armReturn == "returning") ? 0.5 : 0.0;
        o["return_state"] = m_armReturn;
        o["homed"] = m_armHomed;
        sendJson(o);
    }
    // 洗消
    {
        QJsonObject o;
        o["t"] = "wash";
        o["mode"] = m_washRunning ? m_washMode : "--";
        o["flowA"] = m_washRunning ? 20.0 : 0.0;
        o["flowB"] = m_washRunning ? 0.5 : 0.0;
        o["gasFlow"] = m_washRunning ? 100.0 : 0.0;
        o["press"] = m_washRunning ? 0.3 : 0.0;
        o["airPress"] = m_washRunning ? 0.35 : 0.0;
        o["foam"] = (m_washMode == "手动消毒") ? 6.0 : 8.0;
        o["faults"] = QJsonArray();
        sendJson(o);
    }
    // 液位
    {
        QJsonObject o; o["t"] = "tank"; o["a"] = 100.0; o["foam"] = 30.0;
        sendJson(o);
    }
    // 气体检测仪
    {
        static int step = 0; step++;
        double conc = m_gasRunning ? 250.0 + 50.0 * qSin(step * 0.4) : 0.0;
        QJsonObject o;
        o["t"] = "gas";
        o["host"] = m_gasRunning ? "detecting" : "standby";
        o["type"] = m_gasRunning ? "S类" : "--";
        o["level"] = m_gasRunning ? "2级" : "--";
        o["conc"] = qMax(0.0, conc);
        sendJson(o);
    }
    // 球阀
    {
        QJsonObject o;
        o["t"] = "valve";
        o["d1open"] = m_valve1Open; o["d1close"] = !m_valve1Open;
        o["d2open"] = m_valve2Open; o["d2close"] = !m_valve2Open;
        o["fault"] = "";
        sendJson(o);
    }
    // 泵
    {
        QJsonObject o;
        o["t"] = "pump";
        o["plunger"] = m_pPlunger; o["air"] = m_pAir; o["leaf"] = m_pLeaf;
        o["faults"] = QJsonArray();
        sendJson(o);
    }
}

void MockVehicle::onSelfCheckTick()
{
    // 自检 10 项：通信系统/底盘通信/机械臂通信/洗消通信/传感器信号/清水液位/球阀D1开/球阀D1关/球阀D2开/球阀D2关
    bool ok = true;
    if (m_plantSelfCheckFault && m_selfCheckIndex == 6) ok = false; // 球阀D1关闭故障
    QJsonObject o; o["t"] = "selfcheck"; o["item"] = m_selfCheckIndex; o["ok"] = ok;
    sendJson(o);
    m_selfCheckIndex++;
    if (m_selfCheckIndex >= 10) {
        m_selfCheckTimer->stop();
        m_selfCheckActive = false;
        QJsonObject o2; o2["t"] = "selfcheck_done"; o2["all_ok"] = !m_plantSelfCheckFault;
        sendJson(o2);
    }
}