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

using namespace std;

// ==================== CrankRockingBlockMechanism_Inverse 实现 ====================

CrankRockingBlockMechanism_Inverse::CrankRockingBlockMechanism_Inverse()
    : name("曲柄摇块机构 (逆解)"),
      type(MechanismType::CrankRockingBlock)
{
    // 设置默认参数
    parameters.CrankLength2 = 1.0;         // OA - 曲柄长度
    parameters.ConnectingRodLength2 = 3.0; // AB - 连杆长度
    parameters.SliderOffset2 = 3.0;        // OC - 摇块中心距
    parameters.InputValue = 2.5;           // AC距离
    parameters.AngularVelocity = 1.0;      // 角速度
}

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

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

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

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

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

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

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

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

MechanismState CrankRockingBlockMechanism_Inverse::calculate(double acDistance)
{
    parameters.InputValue = acDistance;

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

    MechanismState state;
    state.InputValue = acDistance;

    try
    {
        // 验证AC距离范围
        auto acRange = getACRange();

        if (AC < acRange.first || AC > acRange.second)
        {
            char buffer[128];
            snprintf(buffer, sizeof(buffer),
                     "AC距离超出有效范围 [%.3f, %.3f]",
                     acRange.first, acRange.second);
            state.ErrorMessage = buffer;
            return state;
        }

        // 计算角度θ1 (∠AOC)
        double cosTheta1 = (OA * OA + OC * OC - AC * AC) / (2 * OA * OC);
        cosTheta1 = max(min(cosTheta1, 1.0), -1.0);
        double theta1 = acos(cosTheta1);

        // 计算角度φ (∠BAC)
        double cosPhi = (AC * AC + OC * OC - OA * OA) / (2 * AC * OC);
        cosPhi = max(min(cosPhi, 1.0), -1.0);
        double phi = acos(cosPhi);

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

        // B点在AC延长线上 (沿AC方向距离A点为AB长度)
        Vector2D AC_vec = C - A;
        double AC_length = AC_vec.Length();

        if (AC_length < 1e-10)
        {
            state.ErrorMessage = "A点和C点重合";
            return state;
        }

        Vector2D AC_dir = AC_vec * (1.0 / AC_length);
        Vector2D B = A + AC_dir * AB;

        // 记录轨迹（限制点数）
        trajectoryPoints.push_back(B);
        const size_t MAX_TRAJECTORY_POINTS = 100;
        if (trajectoryPoints.size() > MAX_TRAJECTORY_POINTS)
        {
            trajectoryPoints.erase(trajectoryPoints.begin());
        }

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

        state.Angles["θ1"] = theta1 * 180.0 / M_PI;
        state.Angles["φ"] = phi * 180.0 / M_PI;
        state.Angles["AC距离"] = AC;

        // 计算姿态
        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;

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

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

    // 理论最小和最大AC距离
    double minAC = max(fabs(OA - OC), 0.1);
    double maxAC = OA + OC - 0.1;

    // 考虑连杆长度约束
    minAC = max(minAC, fabs(AB - OA));
    maxAC = min(maxAC, AB + OA);

    return make_pair(minAC, maxAC);
}

pair<double, double> CrankRockingBlockMechanism_Inverse::getInputRange()
{
    return getACRange();
}

vector<Vector2D> CrankRockingBlockMechanism_Inverse::getTrajectoryPoints() const
{
    return trajectoryPoints;
}

void CrankRockingBlockMechanism_Inverse::clearTrajectory()
{
    trajectoryPoints.clear();
}

ValidationResult CrankRockingBlockMechanism_Inverse::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.Errors.push_back("曲柄+连杆长度必须大于摇块中心距");
    }

    if (OA + OC < AB)
    {
        result.Errors.push_back("曲柄+中心距必须大于连杆长度");
    }

    if (AB + OC < OA)
    {
        result.Errors.push_back("连杆+中心距必须大于曲柄长度");
    }

    // 检查运动范围
    auto range = getACRange();
    if (range.first >= range.second)
    {
        result.Errors.push_back("机构参数无效，无法形成有效运动范围");
    }

    return result;
}

string CrankRockingBlockMechanism_Inverse::getStatusText()
{
    auto range = getACRange();

    char buffer[256];
    snprintf(buffer, sizeof(buffer),
             "%s\n"
             "曲柄OA: %.3f m\n"
             "连杆AB: %.3f m\n"
             "摇块中心OC: %.3f m\n"
             "当前AC距离: %.3f m\n"
             "AC有效范围: [%.3f, %.3f] m\n"
             "角速度: %.2f rad/s\n"
             "轨迹点数: %zu",
             name.c_str(),
             parameters.CrankLength2,
             parameters.ConnectingRodLength2,
             parameters.SliderOffset2,
             parameters.InputValue,
             range.first, range.second,
             parameters.AngularVelocity,
             trajectoryPoints.size());

    return string(buffer);
}
