#include "FourBarMechanism/CrankRockingBlockMechanism_Forward.h"
#include <cmath>
#include <algorithm>
#include <stdexcept>
#include <cstdio>

using namespace std;

// ==================== CrankRockingBlockMechanism_Forward 实现 ====================

CrankRockingBlockMechanism_Forward::CrankRockingBlockMechanism_Forward()
    : name("曲柄摇块机构 (正解)"),
      type(MechanismType::CrankRockingBlock_Forward),
      frameCount(0),
      thetaADelta(0.0),
      thetaAValueFactor(1.0),
      thetaAFlip(1)
{
    // 设置默认参数
    parameters.CrankLength2 = 1.0;         // OA - 曲柄长度
    parameters.ConnectingRodLength2 = 3.0; // AB - 连杆长度
    parameters.SliderOffset2 = 3.0;        // OC - 摇块中心距
    parameters.InputValue = 0.0;           // 曲柄角度
    parameters.AngularVelocity = 2.0;      // 角速度
}

string CrankRockingBlockMechanism_Forward::getName() const
{
    return name;
}

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

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

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

void CrankRockingBlockMechanism_Forward::setLink(double OA, double AB, double OC)
{
    parameters.CrankLength2 = OA;
    parameters.ConnectingRodLength2 = AB;
    parameters.SliderOffset2 = OC;
}

void CrankRockingBlockMechanism_Forward::setOA(double length)
{
    parameters.CrankLength2 = length;
}

void CrankRockingBlockMechanism_Forward::setAB(double length)
{
    parameters.ConnectingRodLength2 = length;
}

void CrankRockingBlockMechanism_Forward::setOC(double distance)
{
    parameters.SliderOffset2 = distance;
}

void CrankRockingBlockMechanism_Forward::setThetaADelta(double delta)
{
    thetaADelta = delta;
}

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

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

MechanismState CrankRockingBlockMechanism_Forward::calculate(double crankAngleDeg)
{
    // 应用配置参数
    double angleDeg = thetaADelta + crankAngleDeg * thetaAValueFactor * thetaAFlip;
    parameters.InputValue = angleDeg;

    double OA = parameters.CrankLength2;         // 曲柄长度
    double AB = parameters.ConnectingRodLength2; // 连杆长度
    double OC = parameters.SliderOffset2;        // 摇块中心距

    double crankAngle = degreesToRadians(angleDeg);

    MechanismState state;
    state.InputValue = angleDeg;

    try
    {
        // 验证机构参数
        auto validation = validateParameters();
        if (!validation.isValid())
        {
            state.ErrorMessage = validation.getCombinedMessage();
            return state;
        }

        // 计算各点坐标
        Vector2D O(0.0, 0.0); // 固定点
        Vector2D C(OC, 0.0);  // 摇块中心
        Vector2D A(           // 曲柄端点
            OA * cos(crankAngle),
            OA * sin(crankAngle));

        // 计算滑块B的位置 (B点在直线AC上)
        Vector2D B = calculatePointB(A, C, AB);

        if (B.X == 0.0 && B.Y == 0.0 && AB > 1e-10)
        {
            state.ErrorMessage = "无法计算滑块位置，机构可能无法装配";
            return state;
        }

        // 记录轨迹
        trajectoryB.push_back(B);
        crankCirclePoints.push_back(A);
        rockerTrajectory.push_back(B);

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

        // 更新帧计数
        frameCount++;

        // 计算角度信息
        double rockerAngle = atan2(B.Y - C.Y, B.X - C.X);
        double acDistance = Vector2D::Distance(A, C);

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

        state.Angles["曲柄角度"] = normalizeAngle(angleDeg);
        state.Angles["摇块角度"] = normalizeAngle(radiansToDegrees(rockerAngle));
        state.Angles["AC距离"] = acDistance;

        // 计算姿态
        state.Poses["OA"] = PoseCalculator::CalculatePoseAndQuaternion(O, A);
        state.Poses["AB"] = PoseCalculator::CalculatePoseAndQuaternion(A, B);
        state.Poses["BC"] = PoseCalculator::CalculatePoseAndQuaternion(B, C);

        // TCP姿态（滑块B点）
        Pose7 tcpPose;
        tcpPose.tx = B.X;
        tcpPose.ty = B.Y;
        tcpPose.tz = 0.0;
        tcpPose.qw = 1.0;
        state.Poses["TCP"] = tcpPose;

        // 检查极限位置
        checkLimitPositions(state, A, B, C, OA, AB, OC);

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

Vector2D CrankRockingBlockMechanism_Forward::calculatePointB(
    const Vector2D &A, const Vector2D &C, double AB)
{
    // 直线方程: B在直线AC上，且|AB| = 给定长度
    Vector2D AC = C - A;
    double AC_length = AC.Length();

    if (AC_length < 1e-10)
        return Vector2D::Zero();

    // 单位向量
    Vector2D AC_unit = AC * (1.0 / AC_length);

    // 有两个可能的解
    Vector2D B1 = A + AC_unit * AB; // 沿AC方向
    Vector2D B2 = A - AC_unit * AB; // 反向

    // 选择合理的解（基于连续性）
    if (trajectoryB.empty())
    {
        // 初始状态选择B1（通常滑块在C点附近）
        return B1;
    }
    else
    {
        // 选择与上一帧更接近的解
        Vector2D lastB = trajectoryB.back();
        double dist1 = Vector2D::Distance(B1, lastB);
        double dist2 = Vector2D::Distance(B2, lastB);
        return (dist1 < dist2) ? B1 : B2;
    }
}

void CrankRockingBlockMechanism_Forward::checkLimitPositions(
    MechanismState &state, const Vector2D &A, const Vector2D &B, const Vector2D &C,
    double OA, double AB, double OC)
{
    // 检查死点位置（OA与AC共线）
    Vector2D OA_vec = A;
    Vector2D AC_vec = C - A;

    // 计算夹角
    double dotProduct = OA_vec.Dot(AC_vec);
    double productLength = OA_vec.Length() * AC_vec.Length();

    if (productLength > 1e-10)
    {
        double cosAngle = dotProduct / productLength;
        double angle = acos(fabs(cosAngle)) * 180.0 / M_PI;

        if (angle < 5.0 || angle > 175.0)
        {
            if (!state.WarningMessage.empty())
                state.WarningMessage += "\n";
            state.WarningMessage += "警告：接近死点位置";
        }
    }

    // 检查机构装配条件
    double acDistance = Vector2D::Distance(A, C);
    if (fabs(acDistance - AB) < 0.1)
    {
        if (!state.WarningMessage.empty())
            state.WarningMessage += "\n";
        state.WarningMessage += "警告：接近极限位置 (AC ≈ AB)";
    }

    // 检查曲柄角度范围
    auto range = getValidCrankRange();
    if (range.first > 0 || range.second < 360)
    {
        double currentAngle = normalizeAngle(parameters.InputValue);
        if (currentAngle < range.first + 10 || currentAngle > range.second - 10)
        {
            char buffer[128];
            snprintf(buffer, sizeof(buffer),
                     "警告：接近曲柄角度极限 [%.1f°, %.1f°]",
                     range.first, range.second);

            if (!state.WarningMessage.empty())
                state.WarningMessage += "\n";
            state.WarningMessage += buffer;
        }
    }
}

double CrankRockingBlockMechanism_Forward::normalizeAngle(double angle)
{
    angle = fmod(angle, 360.0);
    if (angle < 0)
        angle += 360.0;
    return angle;
}

pair<double, double> CrankRockingBlockMechanism_Forward::getValidCrankRange() const
{
    double OA = parameters.CrankLength2;
    double AB = parameters.ConnectingRodLength2;
    double OC = parameters.SliderOffset2;

    // 检查机构能否整周旋转
    bool canFullRotation = checkFullRotationCondition(OA, AB, OC);

    if (canFullRotation)
    {
        return make_pair(0.0, 360.0);
    }
    else
    {
        // 计算曲柄的摆动范围
        double cosTheta = (OA * OA + OC * OC - AB * AB) / (2 * OA * OC);
        cosTheta = max(min(cosTheta, 1.0), -1.0);
        double theta = acos(cosTheta) * 180.0 / M_PI;

        double minAngle = 180.0 - theta;
        double maxAngle = 180.0 + theta;

        // 确保角度在0-360范围内
        minAngle = fmod(minAngle + 360.0, 360.0);
        maxAngle = fmod(maxAngle + 360.0, 360.0);

        return make_pair(minAngle, maxAngle);
    }
}

bool CrankRockingBlockMechanism_Forward::checkFullRotationCondition(
    double OA, double AB, double OC) const
{
    // 曲柄摇块机构整周旋转条件：
    // 1. 曲柄是最短杆
    // 2. 满足三角形不等式

    double minLength = min(min(OA, AB), OC);

    // 曲柄不是最短杆
    if (fabs(minLength - OA) > 1e-10)
        return false;

    // 检查几何约束
    if (OA + AB < OC || fabs(OA - AB) > OC)
        return false;

    return true;
}

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

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

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

vector<Vector2D> CrankRockingBlockMechanism_Forward::getRockerTrajectory() const
{
    return rockerTrajectory;
}

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

ValidationResult CrankRockingBlockMechanism_Forward::validateParameters()
{
    ValidationResult result;

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

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

    if (parameters.SliderOffset2 <= 0.0)
        result.Errors.push_back("摇块中心距OC必须大于0");

    double OA = parameters.CrankLength2;
    double AB = parameters.ConnectingRodLength2;
    double OC = parameters.SliderOffset2;

    // 检查机构装配条件
    if (OA + AB < OC)
    {
        result.Warnings.push_back("机构可能无法装配或不满足整周旋转条件");
    }

    // 检查曲柄是否为最短杆
    double minLength = min(min(OA, AB), OC);
    if (fabs(minLength - OA) > 1e-10)
    {
        result.Warnings.push_back("曲柄不是最短杆，可能无法整周旋转");
    }

    return result;
}

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

    auto range = getValidCrankRange();
    string rotationInfo;

    if (range.first == 0.0 && range.second == 360.0)
    {
        rotationInfo = "曲柄可整周旋转";
    }
    else
    {
        char buffer[64];
        snprintf(buffer, sizeof(buffer),
                 "曲柄摆动范围: [%.1f°, %.1f°]",
                 range.first, range.second);
        rotationInfo = buffer;
    }

    char statusBuffer[512];
    snprintf(statusBuffer, sizeof(statusBuffer),
             "%s\n"
             "曲柄OA: %.3f m\n"
             "连杆AB: %.3f m\n"
             "摇块中心OC: %.3f m\n"
             "曲柄角度: %.1f°\n"
             "%s\n"
             "角速度: %.2f rad/s\n"
             "运行时间: %.1f s\n"
             "帧率: %.1f FPS\n"
             "轨迹点数: %zu/500",
             name.c_str(),
             parameters.CrankLength2,
             parameters.ConnectingRodLength2,
             parameters.SliderOffset2,
             parameters.InputValue,
             rotationInfo.c_str(),
             parameters.AngularVelocity,
             runTime,
             fps,
             trajectoryB.size());

    return string(statusBuffer);
}

double CrankRockingBlockMechanism_Forward::getThetaADelta() const
{
    return thetaADelta;
}

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

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

double CrankRockingBlockMechanism_Forward::getOA() const
{
    return parameters.CrankLength2;
}

double CrankRockingBlockMechanism_Forward::getAB() const
{
    return parameters.ConnectingRodLength2;
}

double CrankRockingBlockMechanism_Forward::getOC() const
{
    return parameters.SliderOffset2;
}

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

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

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