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

// ==================== FourBarMechanism 实现 ====================

FourBarMechanism::FourBarMechanism() : name("四杆机构 (正解)"),
                                       frameCount(0),
                                       startTime(std::chrono::steady_clock::now())
{
    parameters.L1 = 1.0;
    parameters.L2 = 3.0;
    parameters.L3 = 2.5;
    parameters.L4 = 3.5;
    parameters.InputValue = 0.0;
    parameters.AngularVelocity = 2.0;
}

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

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

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

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

MechanismType FourBarMechanism::getType() const
{
    return MechanismType::FourBar;
}

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

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

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

void FourBarMechanism::setLink(double l1, double l2, double l3, double l4)
{
    parameters.L1 = l1;
    parameters.L2 = l2;
    parameters.L3 = l3;
    parameters.L4 = l4;
}

bool FourBarMechanism::calculatePointC(const Vector2D &A, const Vector2D &D,
                                       const Vector2D &B, double l2, double l3,
                                       Vector2D &C) const
{
    C = Vector2D::Zero();

    Vector2D BD = D - B;
    double d = BD.Length();

    // 检查三角形不等式
    if (d > l2 + l3 || d < std::abs(l2 - l3))
        return false;

    // 计算C点位置（几何法）
    double a = (l2 * l2 - l3 * l3 + d * d) / (2.0 * d);
    double h_sq = l2 * l2 - a * a;

    if (h_sq < 0)
        return false;

    double h = std::sqrt(h_sq);

    // 单位向量
    Vector2D u = BD * (1.0 / d); // u = BD / d
    Vector2D v(-u.Y, u.X);       // 垂直向量

    // 两个可能的C点
    Vector2D C1 = B + u * a + v * h;
    Vector2D C2 = B + u * a - v * h;

    // 选择正确的解（基于连续性）
    if (trajectoryC.empty())
    {
        // 首次计算，根据输入角度选择
        C = (parameters.InputValue <= 180.0) ? C1 : C2;
    }
    else
    {
        // 选择与上一个位置最接近的解
        Vector2D lastC = trajectoryC.back();
        double dist1 = Vector2D::Distance(C1, lastC);
        double dist2 = Vector2D::Distance(C2, lastC);
        C = (dist1 < dist2) ? C1 : C2;
    }

    return true;
}

bool FourBarMechanism::checkGrashofCondition() const
{
    double lengths[4] = {parameters.L1, parameters.L2, parameters.L3, parameters.L4};
    std::sort(lengths, lengths + 4);

    double s = lengths[0];
    double l = lengths[3];
    double p = lengths[1];
    double q = lengths[2];

    // Grashof条件：最短杆+最长杆 ≤ 其他两杆之和
    return (s + l) <= (p + q);
}

bool FourBarMechanism::isShortestLinkCrank() const
{
    double lengths[4] = {parameters.L1, parameters.L2, parameters.L3, parameters.L4};
    double minLength = *std::min_element(lengths, lengths + 4);

    // 判断最短杆是否为L1（曲柄）
    return std::abs(minLength - parameters.L1) < 1e-10;
}

MechanismState FourBarMechanism::calculate(double angleDeg)
{
    parameters.InputValue = angleDeg;
    double angle = degreesToRadians(angleDeg);

    double l1 = parameters.L1;
    double l2 = parameters.L2;
    double l3 = parameters.L3;
    double l4 = parameters.L4;

    Vector2D A(0.0, 0.0);
    Vector2D D(l4, 0.0);

    MechanismState state;
    state.InputValue = angleDeg;

    try
    {
        // 验证Grashof条件
        if (!checkGrashofCondition())
        {
            state.ErrorMessage = "不满足四杆机构装配条件: s + l > p + q";
            return state;
        }

        // 计算B点位置（曲柄末端）
        Vector2D B = A + Vector2D(
                             l1 * std::cos(angle),
                             l1 * std::sin(angle));

        // 计算C点位置（连杆末端）
        Vector2D C;
        if (!calculatePointC(A, D, B, l2, l3, C))
        {
            state.ErrorMessage = "无法找到连杆位置，机构可能无法装配";
            return state;
        }

        // 记录轨迹点
        trajectoryB.push_back(B);
        trajectoryC.push_back(C);
        crankCirclePoints.push_back(B);

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

        // 更新帧计数
        frameCount++;

        // 计算摇杆角度
        Vector2D CD = D - C;
        double rockerAngle = std::atan2(CD.Y, CD.X);

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

        // 计算各杆的姿态
        state.Poses["AB"] = PoseCalculator::CalculatePoseAndQuaternion(A, B);
        state.Poses["BC"] = PoseCalculator::CalculatePoseAndQuaternion(B, C);
        state.Poses["CD"] = PoseCalculator::CalculatePoseAndQuaternion(C, D);

        // 角度信息
        state.Angles["曲柄角度"] = std::fmod(angleDeg, 360.0);
        state.Angles["摇杆角度"] = radiansToDegrees(rockerAngle);

        // 检查最短杆是否为曲柄
        if (!isShortestLinkCrank())
        {
            state.WarningMessage = "警告: 最短杆不是曲柄，可能无法做整周旋转";
        }

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

ValidationResult FourBarMechanism::validateParameters()
{
    ValidationResult result;

    // 检查杆件长度
    if (parameters.L1 <= 0.0)
        result.Errors.push_back("L1（曲柄）长度必须大于0");
    if (parameters.L2 <= 0.0)
        result.Errors.push_back("L2（连杆）长度必须大于0");
    if (parameters.L3 <= 0.0)
        result.Errors.push_back("L3（摇杆）长度必须大于0");
    if (parameters.L4 <= 0.0)
        result.Errors.push_back("L4（机架）长度必须大于0");

    // 检查Grashof条件
    if (!checkGrashofCondition())
    {
        result.Errors.push_back("不满足四杆机构装配条件: s + l > p + q");
    }

    // 检查最短杆是否为曲柄
    if (!isShortestLinkCrank())
    {
        result.Warnings.push_back("警告: 最短杆不是曲柄，可能无法做整周旋转");
    }

    return result;
}

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

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

std::vector<Vector2D> FourBarMechanism::getTrajectoryC() const
{
    return trajectoryC;
}

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

void FourBarMechanism::clearTrajectory()
{
    trajectoryB.clear();
    trajectoryC.clear();
    crankCirclePoints.clear();
    frameCount = 0;
    startTime = std::chrono::steady_clock::now();
}

std::string FourBarMechanism::getStatusText()
{
    auto now = std::chrono::steady_clock::now();
    std::chrono::duration<double> elapsed = now - startTime;
    double runTime = elapsed.count();
    double fps = (runTime > 0.0) ? frameCount / runTime : 0.0;

    std::ostringstream oss;
    oss << name << "\n"
        << "L1=" << formatDouble(parameters.L1) << "m, L2=" << formatDouble(parameters.L2) << "m\n"
        << "L3=" << formatDouble(parameters.L3) << "m, L4=" << formatDouble(parameters.L4) << "m\n"
        << "角速度: " << formatDouble(parameters.AngularVelocity, 1) << "rad/s\n"
        << "运行时间: " << formatDouble(runTime, 1) << "s\n"
        << "帧率: " << formatDouble(fps, 1) << " FPS\n"
        << "轨迹点数: " << trajectoryB.size() << "/500";

    return oss.str();
}

double FourBarMechanism::getL1() const { return parameters.L1; }
double FourBarMechanism::getL2() const { return parameters.L2; }
double FourBarMechanism::getL3() const { return parameters.L3; }
double FourBarMechanism::getL4() const { return parameters.L4; }

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

FourBarMechanism *createFourBarMechanism()
{
    return new FourBarMechanism();
}

void deleteFourBarMechanism(FourBarMechanism *mechanism)
{
    delete mechanism;
}

FourBarMechanismPtr createFourBarMechanismSmart()
{
    return std::make_shared<FourBarMechanism>();
}
