#include "SimulationMode.h"
#include "IYModbusTCPClient.h"
#include <QDebug>
#include <cmath>

SimulationMode::SimulationMode(QObject *parent)
    : QObject(parent)
    , m_randomEngine(std::random_device{}())
    , m_positionNoise(-2.0f, 2.0f)  // ±2mm位置噪声
    , m_angleNoise(-1.0f, 1.0f)     // ±1°角度噪声
{
    m_detectionTimer = new QTimer(this);
    m_detectionTimer->setSingleShot(true);
    connect(m_detectionTimer, &QTimer::timeout, this, &SimulationMode::simulateDetectionProcess);

    initializeRegisters();
}

void SimulationMode::setEnabled(bool enabled)
{
    m_enabled = enabled;
    if (enabled) {
        emit logMessage("模拟模式已启用");
    } else {
        emit logMessage("模拟模式已禁用");
    }
}

bool SimulationMode::simulateControllerConnect()
{
    if (!m_enabled) return false;

    emit logMessage("模拟: 控制器连接成功");
    return true;
}

void SimulationMode::simulateControllerDisconnect()
{
    if (!m_enabled) return;

    emit logMessage("模拟: 控制器已断开");
}

bool SimulationMode::simulateRobotConnect()
{
    if (!m_enabled) return false;

    emit logMessage("模拟: 机械臂连接成功");
    return true;
}

void SimulationMode::simulateRobotDisconnect()
{
    if (!m_enabled) return;

    emit logMessage("模拟: 机械臂已断开");
}

bool SimulationMode::simulateReadRegister(int address, uint16_t& value)
{
    if (!m_enabled) return false;

    auto it = m_registers.find(address);
    if (it != m_registers.end()) {
        value = it->second;
        emit logMessage(QString("模拟: 读取寄存器[%1] = %2").arg(address).arg(value));
        return true;
    }

    emit logMessage(QString("模拟: 读取寄存器[%1]失败").arg(address));
    return false;
}

bool SimulationMode::simulateWriteRegister(int address, uint16_t value)
{
    if (!m_enabled) return false;

    m_registers[address] = value;
    emit logMessage(QString("模拟: 写入寄存器[%1] = %2").arg(address).arg(value));

    // 如果写入地址0且值为1，触发模拟检测
    if (address == 0 && value == 1) {
        // 清除触发信号
        m_registers[0] = 0;
        // 设置状态为工作中
        m_registers[1] = 0;
        // 启动模拟检测定时器（模拟2秒检测时间）
        m_detectionTimer->start(2000);
        emit logMessage("模拟: 触发检测，预计2秒后完成");
    }

    return true;
}

bool SimulationMode::simulateReadRegisters(int startAddress, int quantity, std::vector<uint16_t>& values)
{
    if (!m_enabled) return false;

    values.clear();
    for (int i = 0; i < quantity; ++i) {
        uint16_t value = 0;
        auto it = m_registers.find(startAddress + i);
        if (it != m_registers.end()) {
            value = it->second;
        }
        values.push_back(value);
    }

    emit logMessage(QString("模拟: 读取寄存器[%1-%2]").arg(startAddress).arg(startAddress + quantity - 1));
    return true;
}

bool SimulationMode::simulateMoveL(double x, double y, double z, double rx, double ry, double rz)
{
    if (!m_enabled) return false;

    emit logMessage(QString("模拟: MoveL到位置(%.2f, %.2f, %.2f, %.2f, %.2f, %.2f)")
                   .arg(x).arg(y).arg(z).arg(rx).arg(ry).arg(rz));

    // 模拟运动时间（根据距离计算，这里简化为固定1秒）
    QThread::msleep(1000);

    emit logMessage("模拟: MoveL完成");
    return true;
}

bool SimulationMode::simulateMoveJ(double x, double y, double z, double rx, double ry, double rz)
{
    if (!m_enabled) return false;

    emit logMessage(QString("模拟: MoveJ到位置(%.2f, %.2f, %.2f, %.2f, %.2f, %.2f)")
                   .arg(x).arg(y).arg(z).arg(rx).arg(ry).arg(rz));

    // 模拟运动时间
    QThread::msleep(1000);

    emit logMessage("模拟: MoveJ完成");
    return true;
}

void SimulationMode::setSimulatedDetectionResult(float x, float y, float z, float roll, float pitch, float yaw)
{
    m_detectionX = x;
    m_detectionY = y;
    m_detectionZ = z;
    m_detectionRoll = roll;
    m_detectionPitch = pitch;
    m_detectionYaw = yaw;

    emit logMessage(QString("模拟: 设置检测结果(%.2f, %.2f, %.2f, %.2f, %.2f, %.2f)")
                   .arg(x).arg(y).arg(z).arg(roll).arg(pitch).arg(yaw));
}

void SimulationMode::initializeRegisters()
{
    // 初始化寄存器
    m_registers[0] = 0;  // 触发信号
    m_registers[1] = 0;  // 工作状态

    // 初始化位姿数据寄存器（地址2-13）
    for (int i = 2; i <= 13; ++i) {
        m_registers[i] = 0;
    }
}

void SimulationMode::simulateDetectionProcess()
{
    emit logMessage("模拟: 检测完成");

    // 添加随机噪声
    float x = addNoise(m_detectionX, 2.0f);
    float y = addNoise(m_detectionY, 2.0f);
    float z = addNoise(m_detectionZ, 2.0f);
    float roll = addNoise(m_detectionRoll, 1.0f);
    float pitch = addNoise(m_detectionPitch, 1.0f);
    float yaw = addNoise(m_detectionYaw, 1.0f);

    // 将float转换为寄存器值
    uint16_t high, low;

    IYModbusTCPClient::FloatToRegisters(x, high, low);
    m_registers[2] = high;
    m_registers[3] = low;

    IYModbusTCPClient::FloatToRegisters(y, high, low);
    m_registers[4] = high;
    m_registers[5] = low;

    IYModbusTCPClient::FloatToRegisters(z, high, low);
    m_registers[6] = high;
    m_registers[7] = low;

    IYModbusTCPClient::FloatToRegisters(roll, high, low);
    m_registers[8] = high;
    m_registers[9] = low;

    IYModbusTCPClient::FloatToRegisters(pitch, high, low);
    m_registers[10] = high;
    m_registers[11] = low;

    IYModbusTCPClient::FloatToRegisters(yaw, high, low);
    m_registers[12] = high;
    m_registers[13] = low;

    // 设置状态为完成正常
    m_registers[1] = 1;

    emit logMessage(QString("模拟: 检测结果(%.2f, %.2f, %.2f, %.2f, %.2f, %.2f)")
                   .arg(x).arg(y).arg(z).arg(roll).arg(pitch).arg(yaw));
}

float SimulationMode::addNoise(float value, float noiseRange)
{
    std::uniform_real_distribution<float> dist(-noiseRange, noiseRange);
    return value + dist(m_randomEngine);
}