// mechanism_simulation.cpp
#include "FourBarMechanism/CrankSliderMechanism.h"
#include <stdexcept>
#include <algorithm>
#include <cstdio>

// ==================== MechanismParameters 实现 ====================

MechanismParameters::MechanismParameters() : CrankLength2(1.0),
                                             ConnectingRodLength2(3.0),
                                             SliderOffset2(0.5),
                                             L1(1.0),
                                             L2(3.0),
                                             L3(2.5),
                                             L4(3.5),
                                             InputValue(0.0),
                                             AngularVelocity(2.0) {}

// ==================== MechanismState 实现 ====================

MechanismState::MechanismState() : InputValue(0.0) {}

bool MechanismState::hasError() const
{
    return !ErrorMessage.empty();
}

bool MechanismState::hasWarning() const
{
    return !WarningMessage.empty();
}

// ==================== ValidationResult 实现 ====================

bool ValidationResult::isValid() const
{
    return Errors.empty();
}

std::string ValidationResult::getCombinedMessage() const
{
    std::string message;

    if (!Errors.empty())
    {
        message += "错误:\n";
        for (const auto &error : Errors)
        {
            message += "  • " + error + "\n";
        }
    }

    if (!Warnings.empty())
    {
        if (!message.empty())
            message += "\n";
        message += "警告:\n";
        for (const auto &warning : Warnings)
        {
            message += "  • " + warning + "\n";
        }
    }

    return message;
}

// ==================== SimpleTimer 实现 ====================

SimpleTimer::SimpleTimer()
{
    reset();
}

void SimpleTimer::reset()
{
    start = std::chrono::steady_clock::now();
}

double SimpleTimer::elapsedSeconds() const
{
    auto now = std::chrono::steady_clock::now();
    std::chrono::duration<double> elapsed = now - start;
    return elapsed.count();
}

// ==================== CrankSliderMechanism 私有辅助方法 ====================

double CrankSliderMechanism::degreesToRadians(double degrees) const
{
    return degrees * M_PI / 180.0;
}

double CrankSliderMechanism::radiansToDegrees(double radians) const
{
    return radians * 180.0 / M_PI;
}

std::string CrankSliderMechanism::formatDouble(double value, int precision) const
{
    char buffer[32];
    snprintf(buffer, sizeof(buffer), "%.*f", precision, value);
    return std::string(buffer);
}

// ==================== CrankSliderMechanism 公有方法实现 ====================

CrankSliderMechanism::CrankSliderMechanism() : name("曲柄滑块机构 (正解)"),
                                               type(MechanismType::CrankSlider),
                                               thetaADelta(0.0),
                                               thetaAValueFactor(1.0),
                                               thetaAFlip(1),
                                               frameCount(0)
{

    parameters.CrankLength2 = 0.5;
    parameters.ConnectingRodLength2 = 2.0;
    parameters.SliderOffset2 = 0.0;
    parameters.InputValue = 0.0;
    parameters.AngularVelocity = 2.0;
}

std::string CrankSliderMechanism::getName() const
{
    return name;
}

MechanismType CrankSliderMechanism::getType() const
{
    return type;
}

MechanismParameters CrankSliderMechanism::getParameters() const
{
    return parameters;
}

void CrankSliderMechanism::setName(const std::string &newName)
{
    name = newName;
}

void CrankSliderMechanism::setParameters(const MechanismParameters &params)
{
    parameters = params;
}

void CrankSliderMechanism::setLink(double crankLength, double rodLength, double sliderOffset)
{
    parameters.CrankLength2 = crankLength;
    parameters.ConnectingRodLength2 = rodLength;
    parameters.SliderOffset2 = sliderOffset;
}

// 配置方法
void CrankSliderMechanism::setThetaADelta(double delta)
{
    thetaADelta = delta;
}

void CrankSliderMechanism::setThetaAValueFactor(double factor)
{
    thetaAValueFactor = factor;
}

void CrankSliderMechanism::setThetaAFlip(int flip)
{
    thetaAFlip = flip;
}

void CrankSliderMechanism::setL_AB(double length)
{
    parameters.CrankLength2 = length;
}

void CrankSliderMechanism::setL_BS(double length)
{
    parameters.ConnectingRodLength2 = length;
}

void CrankSliderMechanism::setS_OFS(double offset)
{
    parameters.SliderOffset2 = offset;
}

void CrankSliderMechanism::setCodeBody(const std::string &code)
{
    codeBody = code;
}

void CrankSliderMechanism::setL1ABModelName(const std::string &modelName)
{
    l1ABModelName = modelName;
}

void CrankSliderMechanism::setL2BSModelName(const std::string &modelName)
{
    l2BSModelName = modelName;
}

void CrankSliderMechanism::setL3SModelName(const std::string &modelName)
{
    l3SModelName = modelName;
}

MechanismState CrankSliderMechanism::calculate(double _angleDeg)
{
    double angleDeg = thetaADelta + _angleDeg * thetaAValueFactor * thetaAFlip;
    parameters.InputValue = angleDeg;

    double angle = degreesToRadians(angleDeg);
    double crankLength = parameters.CrankLength2;
    double rodLength = parameters.ConnectingRodLength2;
    double sliderOffset = parameters.SliderOffset2;

    Vector2D A(0.0, 0.0);
    MechanismState state;
    state.InputValue = angleDeg;

    try
    {
        // 验证Grashof条件
        bool grashofCondition = crankLength + rodLength > std::abs(sliderOffset);
        if (!grashofCondition)
        {
            state.ErrorMessage = "错误：不满足Grashof条件\n(曲柄+连杆长度必须大于滑块偏移量的绝对值)";
            return state;
        }

        // 检查滑块偏移量
        double maxOffset = rodLength - crankLength;
        if (std::abs(sliderOffset) > maxOffset)
        {
            state.ErrorMessage = "错误：滑块偏移量过大\n最大允许值: ±" + formatDouble(maxOffset) + "m";
            return state;
        }
        else if (std::abs(sliderOffset) > 0.8 * maxOffset)
        {
            state.WarningMessage = "警告：滑块偏移量接近极限值\n建议值: ±" + formatDouble(0.8 * maxOffset) + "m以内";
        }

        // 计算B点位置
        Vector2D B = A + Vector2D(
                             crankLength * std::cos(angle),
                             crankLength * std::sin(angle));

        // 计算滑块位置（S点）
        double Bx = B.X;
        double By = B.Y;
        double y_s = sliderOffset;
        double L = rodLength;

        // 解二次方程：x_s^2 - 2*Bx*x_s + Bx^2 + (y_s - By)^2 - L^2 = 0
        double a = 1.0;
        double b = -2.0 * Bx;
        double c = Bx * Bx + (y_s - By) * (y_s - By) - L * L;

        double discriminant = b * b - 4.0 * a * c;

        if (discriminant < 0.0)
        {
            state.ErrorMessage = "错误：无法找到滑块位置，机构可能无法装配";
            return state;
        }

        double sqrtDiscriminant = std::sqrt(discriminant);
        double x1 = (-b + sqrtDiscriminant) / (2.0 * a);
        double x2 = (-b - sqrtDiscriminant) / (2.0 * a);

        // 选择正确的解（基于连续性）
        // 修正：改进解的选择逻辑
        double x_s;
        if (!sliderTrajectory.empty())
        {
            // 使用连续性原则：选择与上一个位置最接近的解
            double lastX = sliderTrajectory.back().X;
            x_s = (std::abs(x1 - lastX) < std::abs(x2 - lastX)) ? x1 : x2;
        }
        else
        {
            x_s = (x1 > x2) ? x1 : x2; // 选择较大的解（通常对应正向运动）
        }

        Vector2D S(x_s, y_s);

        // 记录轨迹点
        // 记录轨迹点
        trajectoryB.push_back(B);
        sliderTrajectory.push_back(S);
        crankCirclePoints.push_back(B);

        // 限制轨迹点数量
        const size_t MAX_TRAJECTORY_POINTS = 1000;
        if (trajectoryB.size() > MAX_TRAJECTORY_POINTS)
        {
            trajectoryB.erase(trajectoryB.begin());
            sliderTrajectory.erase(sliderTrajectory.begin());
            crankCirclePoints.erase(crankCirclePoints.begin());
        }

        // 更新帧计数
        frameCount++;

        // 设置状态
        state.Points["A"] = A;
        state.Points["B"] = B;
        state.Points["S"] = S;

        state.Poses["AB"] = PoseCalculator::CalculatePoseAndQuaternion(A, B);
        state.Poses["BS"] = PoseCalculator::CalculatePoseAndQuaternion(B, S);

        // TCP姿态
        Pose7 tcpPose;
        tcpPose.tx = S.X;
        tcpPose.ty = S.Y;
        tcpPose.tz = 0.0;
        tcpPose.qw = 1.0;
        state.Poses["TCP"] = tcpPose;

        // 角度信息
        state.Angles["曲柄角度"] = std::fmod(angleDeg, 360.0);
        state.Angles["滑块位置X"] = x_s;

        return state;
    }
    catch (const std::exception &ex)
    {
        state.ErrorMessage = std::string("计算失败: ") + ex.what();
        return state;
    }
}

ValidationResult CrankSliderMechanism::validateParameters()
{
    ValidationResult result;

    if (parameters.CrankLength2 <= 0.0)
    {
        result.Errors.push_back("曲柄长度必须大于0");
    }

    if (parameters.ConnectingRodLength2 <= 0.0)
    {
        result.Errors.push_back("连杆长度必须大于0");
    }

    if (parameters.CrankLength2 + parameters.ConnectingRodLength2 <= std::abs(parameters.SliderOffset2))
    {
        result.Errors.push_back("不满足Grashof条件: 曲柄+连杆长度必须大于滑块偏移量的绝对值");
    }

    double maxOffset = parameters.ConnectingRodLength2 - parameters.CrankLength2;
    if (std::abs(parameters.SliderOffset2) > maxOffset)
    {
        result.Errors.push_back("滑块偏移量过大，最大允许值: ±" + formatDouble(maxOffset) + "m");
    }
    else if (std::abs(parameters.SliderOffset2) > 0.8 * maxOffset)
    {
        result.Warnings.push_back("滑块偏移量接近极限值，建议值: ±" + formatDouble(0.8 * maxOffset) + "m以内");
    }

    return result;
}

std::pair<double, double> CrankSliderMechanism::getInputRange()
{
    return std::make_pair(0.0, 360.0);
}

std::vector<Vector2D> CrankSliderMechanism::getTrajectoryPoints()
{
    return trajectoryB;
}

std::vector<Vector2D> CrankSliderMechanism::getSliderTrajectory() const
{
    return sliderTrajectory;
}

std::vector<Vector2D> CrankSliderMechanism::getCrankCirclePoints() const
{
    return crankCirclePoints;
}

void CrankSliderMechanism::clearTrajectory()
{
    trajectoryB.clear();
    sliderTrajectory.clear();
    crankCirclePoints.clear();
    frameCount = 0;
    timer.reset();
}

std::string CrankSliderMechanism::getStatusText()
{
    double runTime = timer.elapsedSeconds();
    double fps = (runTime > 0.0) ? frameCount / runTime : 0.0;

    return name + "\n" +
           "曲柄长度: " + formatDouble(parameters.CrankLength2) + "m\n" +
           "连杆长度: " + formatDouble(parameters.ConnectingRodLength2) + "m\n" +
           "滑块偏移: " + formatDouble(parameters.SliderOffset2) + "m\n" +
           "角速度: " + formatDouble(parameters.AngularVelocity, 1) + "rad/s\n" +
           "运行时间: " + formatDouble(runTime, 1) + "s\n" +
           "帧率: " + formatDouble(fps, 1) + " FPS\n" +
           "轨迹点数: " + std::to_string(trajectoryB.size()) + "/1000";
}

// 获取配置参数
double CrankSliderMechanism::getThetaADelta() const
{
    return thetaADelta;
}

double CrankSliderMechanism::getThetaAValueFactor() const
{
    return thetaAValueFactor;
}

int CrankSliderMechanism::getThetaAFlip() const
{
    return thetaAFlip;
}

double CrankSliderMechanism::getL_AB() const
{
    return parameters.CrankLength2;
}

double CrankSliderMechanism::getL_BS() const
{
    return parameters.ConnectingRodLength2;
}

double CrankSliderMechanism::getS_OFS() const
{
    return parameters.SliderOffset2;
}

std::string CrankSliderMechanism::getCodeBody() const
{
    return codeBody;
}

std::string CrankSliderMechanism::getL1ABModelName() const
{
    return l1ABModelName;
}

std::string CrankSliderMechanism::getL2BSModelName() const
{
    return l2BSModelName;
}

std::string CrankSliderMechanism::getL3SModelName() const
{
    return l3SModelName;
}

// ==================== 工厂函数实现 ====================

CrankSliderMechanism *createCrankSliderMechanism()
{
    return new CrankSliderMechanism();
}

void deleteCrankSliderMechanism(CrankSliderMechanism *mechanism)
{
    delete mechanism;
}

CrankSliderMechanismPtr createCrankSliderMechanismSmart()
{
    return std::make_shared<CrankSliderMechanism>();
}
