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

// ==================== MechanismParametersExt 实现 ====================

MechanismParametersExt::MechanismParametersExt() : CrankLength(1.0),
                                                   ConnectingRodLength(3.0),
                                                   SliderOffset(0.5),
                                                   InputValue(0.0),
                                                   AngularVelocity(1.0),
                                                   SolutionMode(SolutionMode::Auto),
                                                   CrankAngleRange1(0.0, 0.0),
                                                   CrankAngleRange2(0.0, 0.0) {}

// ==================== SliderCrankMechanism 实现 ====================

SliderCrankMechanism::SliderCrankMechanism() : name("曲柄滑块机构 (逆解)")
{
    parameters.CrankLength = 1.0;
    parameters.ConnectingRodLength = 3.0;
    parameters.SliderOffset = 0.5;
    parameters.InputValue = 0.0;
    parameters.AngularVelocity = 1.0;
    parameters.SolutionMode = SolutionMode::Auto;
}

// 辅助方法
double SliderCrankMechanism::degreesToRadians(double degrees) const
{
    return degrees * M_PI / 180.0;
}

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

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

double SliderCrankMechanism::clamp(double value, double min, double max) const
{
    if (value < min)
        return min;
    if (value > max)
        return max;
    return value;
}

// 基本属性访问
std::string SliderCrankMechanism::getName() const
{
    return name;
}

MechanismType SliderCrankMechanism::getType() const
{
    return MechanismType::CrankSlider; // 使用相同的机构类型
}

MechanismParametersExt SliderCrankMechanism::getParameters() const
{
    return parameters;
}

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

void SliderCrankMechanism::setParameters(const MechanismParametersExt &params)
{
    parameters = params;
}

void SliderCrankMechanism::setLink(double crankLength, double rodLength, double sliderOffset)
{
    parameters.CrankLength = crankLength;
    parameters.ConnectingRodLength = rodLength;
    parameters.SliderOffset = sliderOffset;
}

void SliderCrankMechanism::setSolutionMode(SolutionMode mode)
{
    parameters.SolutionMode = mode;
}

// 计算滑块范围
std::pair<double, double> SliderCrankMechanism::calculateSliderRange()
{
    double L1 = parameters.CrankLength;
    double L2 = parameters.ConnectingRodLength;
    double h = parameters.SliderOffset;

    // 检查Grashof条件
    if ((L1 + L2) < std::abs(h))
        return std::make_pair(0.0, 0.0);

    double maxX = 0.0;
    if ((L1 + L2) * (L1 + L2) > h * h)
    {
        maxX = std::sqrt((L1 + L2) * (L1 + L2) - h * h);
    }

    double minX = 0.0;
    if ((L2 - L1) * (L2 - L1) > h * h)
    {
        minX = std::sqrt((L2 - L1) * (L2 - L1) - h * h);
    }
    else
    {
        minX = -std::sqrt((L1 + L2) * (L1 + L2) - h * h);
    }

    // 计算曲柄角度范围
    if ((L1 + L2) * (L1 + L2) > h * h)
    {
        double theta_max = M_PI - std::asin(h / (L1 + L2));
        double theta_min;

        if ((L2 - L1) * (L2 - L1) > h * h)
        {
            theta_min = std::asin(h / (L1 + L2));
        }
        else
        {
            theta_min = -M_PI + std::asin(h / (L1 + L2));
        }

        parameters.CrankAngleRange1 = std::make_pair(theta_min, theta_max);
        parameters.CrankAngleRange2 = std::make_pair(-theta_max, -theta_min);
    }

    return std::make_pair(minX, maxX);
}

// 主要计算函数（逆解）
MechanismState SliderCrankMechanism::calculate(double sliderX)
{
    parameters.InputValue = sliderX;

    double crankLength = parameters.CrankLength;
    double rodLength = parameters.ConnectingRodLength;
    double sliderOffset = parameters.SliderOffset;
    auto solutionMode = parameters.SolutionMode;

    Vector2D A(0.0, 0.0);
    Vector2D S(sliderX, sliderOffset);

    MechanismState state;
    state.InputValue = sliderX;

    try
    {
        // 计算A到S的向量和距离
        Vector2D AS_vec = S - A;
        double AS_distance = AS_vec.length();

        // 检查机构是否可以装配
        if (AS_distance > crankLength + rodLength ||
            AS_distance < std::abs(crankLength - rodLength))
        {
            state.ErrorMessage = "滑块位置 " + formatDouble(sliderX) + " 导致机构无法装配";
            return state;
        }

        // 使用余弦定理计算角度
        double cosTheta = (crankLength * crankLength + AS_distance * AS_distance -
                           rodLength * rodLength) /
                          (2.0 * crankLength * AS_distance);
        cosTheta = clamp(cosTheta, -1.0, 1.0);
        double theta = std::acos(cosTheta);

        // 计算基准角度
        double alpha = std::atan2(AS_vec.Y, AS_vec.X);

        // 计算两个可能的解
        double angle1 = alpha + theta;
        double angle2 = alpha - theta;

        Vector2D B1 = A + Vector2D(
                              crankLength * std::cos(angle1),
                              crankLength * std::sin(angle1));

        Vector2D B2 = A + Vector2D(
                              crankLength * std::cos(angle2),
                              crankLength * std::sin(angle2));

        Vector2D B, B_alt;

        // 默认选择解1
        B = B1;
        B_alt = B2;

        // 根据解模式选择
        if (solutionMode == SolutionMode::Solution2)
        {
            B = B2;
            B_alt = B1;
        }
        else if (solutionMode == SolutionMode::Auto)
        {
            if (!trajectoryB.empty())
            {
                Vector2D lastB = trajectoryB.back();
                double dist1 = Vector2D::Distance(B1, lastB);
                double dist2 = Vector2D::Distance(B2, lastB);
                if (dist2 < dist1)
                {
                    B = B2;
                    B_alt = B1;
                }
            }
        }

        // 记录轨迹
        trajectoryB.push_back(B);
        trajectoryBAlt.push_back(B_alt);
        sliderTrajectory.push_back(S);

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

        // 计算角度
        double crankAngle = std::atan2(B.Y, B.X) * 180.0 / M_PI;
        double crankAngleAlt = std::atan2(B_alt.Y, B_alt.X) * 180.0 / M_PI;

        // 检查机构是否可以装配
        auto range = calculateSliderRange();
        if (std::abs(range.first - range.second) < 1e-10)
        {
            state.WarningMessage = "警告：机构无法装配！(曲柄+连杆长度小于滑块偏移量)";
        }

        // 设置状态
        state.Points["A"] = A;
        state.Points["B"] = B;
        state.Points["B_alt"] = B_alt;
        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["当前解角度"] = crankAngle;
        state.Angles["备选解角度"] = crankAngleAlt;
        state.Angles["滑块位置"] = sliderX;

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

// 参数验证
ValidationResult SliderCrankMechanism::validateParameters()
{
    ValidationResult result;

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

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

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

    return result;
}

// 获取输入范围
std::pair<double, double> SliderCrankMechanism::getInputRange()
{
    return calculateSliderRange();
}

// 获取轨迹点
std::vector<Vector2D> SliderCrankMechanism::getTrajectoryPoints()
{
    return trajectoryB;
}

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

std::vector<Vector2D> SliderCrankMechanism::getAlternativeTrajectory() const
{
    return trajectoryBAlt;
}

// 清除轨迹
void SliderCrankMechanism::clearTrajectory()
{
    trajectoryB.clear();
    trajectoryBAlt.clear();
    sliderTrajectory.clear();
}

// 获取状态文本
std::string SliderCrankMechanism::getStatusText()
{
    auto range = calculateSliderRange();
    std::string status = name + "\n";

    if (std::abs(range.first - range.second) < 1e-10)
    {
        status += "⚠️ 警告：机构无法装配！(曲柄+连杆长度小于滑块偏移量)\n";
    }
    else
    {
        status += "曲柄=" + formatDouble(parameters.CrankLength) + "m, ";
        status += "连杆=" + formatDouble(parameters.ConnectingRodLength) + "m\n";
        status += "偏移=" + formatDouble(parameters.SliderOffset) + "m\n";
        status += "滑块范围: [" + formatDouble(range.first) + ", " + formatDouble(range.second) + "]m\n";
        status += "行程: " + formatDouble(range.second - range.first) + "m\n";

        status += "解1角度范围: [" + formatDouble(parameters.CrankAngleRange1.first * 180.0 / M_PI, 1) + "°, ";
        status += formatDouble(parameters.CrankAngleRange1.second * 180.0 / M_PI, 1) + "°]\n";
        status += "解2角度范围: [" + formatDouble(parameters.CrankAngleRange2.first * 180.0 / M_PI, 1) + "°, ";
        status += formatDouble(parameters.CrankAngleRange2.second * 180.0 / M_PI, 1) + "°]";
    }

    return status;
}

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

SliderCrankMechanism *createSliderCrankMechanism()
{
    return new SliderCrankMechanism();
}

void deleteSliderCrankMechanism(SliderCrankMechanism *mechanism)
{
    delete mechanism;
}

SliderCrankMechanismPtr createSliderCrankMechanismSmart()
{
    return std::make_shared<SliderCrankMechanism>();
}