#include "KinematicsWebAPI.h"
#include "FourBarMechanism/CrankSliderMechanism.h"
#include "FourBarMechanism/CrankRockingBlockMechanism_Forward.h"
#include "FourBarMechanism/FourBarMechanism.h"
#include "FourBarMechanism/SliderCrankMechanism.h"
#include <cmath>
#include <memory>

namespace
{
// 读取兼容字段，便于新统一接口沿用旧曲柄滑块参数名。
double valueAny(const json &param, std::initializer_list<const char *> keys, double defaultValue)
{
    for (const char *key : keys)
    {
        if (param.contains(key) && param[key].is_number())
        {
            return param[key].get<double>();
        }
    }
    return defaultValue;
}

// 构造统一的业务失败返回，供四类连杆仿真复用。
json buildErrorJson(const std::string &error, const json &validationErrors = json::array(), const json &validationWarnings = json::array())
{
    json result = {
        {"success", false},
        {"error", error}};

    if (!validationErrors.empty())
    {
        result["validation_errors"] = validationErrors;
    }
    if (!validationWarnings.empty())
    {
        result["validation_warnings"] = validationWarnings;
    }

    return result;
}

// 将机构点位转换为接口统一 JSON。
json buildPointsJson(const MechanismState &state)
{
    json points_json;
    for (const auto &point_pair : state.Points)
    {
        points_json[point_pair.first] = {
            {"x", point_pair.second.X},
            {"y", point_pair.second.Y}};
    }
    return points_json;
}

// 将机构姿态转换为接口统一 JSON。
json buildPosesJson(const MechanismState &state)
{
    json poses_json;
    for (const auto &pose_pair : state.Poses)
    {
        poses_json[pose_pair.first] = {
            {"tx", pose_pair.second.tx},
            {"ty", pose_pair.second.ty},
            {"tz", pose_pair.second.tz},
            {"qx", pose_pair.second.qx},
            {"qy", pose_pair.second.qy},
            {"qz", pose_pair.second.qz},
            {"qw", pose_pair.second.qw}};
    }
    return poses_json;
}

// 将机构角度数据转换为接口统一 JSON。
json buildAnglesJson(const MechanismState &state)
{
    json angles_json;
    for (const auto &angle_pair : state.Angles)
    {
        angles_json[angle_pair.first] = angle_pair.second;
    }
    return angles_json;
}

// 将二维点列表转换为轨迹 JSON 数组。
json buildTrajectoryJson(const std::vector<Vector2D> &points)
{
    json trajectory_json = json::array();
    for (const auto &point : points)
    {
        trajectory_json.push_back({
            {"x", point.X},
            {"y", point.Y}});
    }
    return trajectory_json;
}

// 组装单帧机构状态，供实时接口和批量仿真接口复用。
json buildFrameJson(const MechanismState &state, int frame, double time, double inputValue, const std::string &inputName)
{
    json frame_json;
    frame_json["frame"] = frame;
    frame_json["time"] = time;
    frame_json[inputName] = inputValue;
    frame_json["inputName"] = inputName;
    frame_json["inputValue"] = inputValue;
    frame_json["input_value"] = state.InputValue;
    frame_json["points"] = buildPointsJson(state);
    frame_json["poses"] = buildPosesJson(state);
    frame_json["angles"] = buildAnglesJson(state);
    if (state.hasWarning())
    {
        frame_json["warning"] = state.WarningMessage;
    }
    return frame_json;
}

// 校验仿真时间参数并返回帧数。
json validateSimulationTimeline(double duration, double stepTime, int &frameCount)
{
    if (duration <= 0.0)
    {
        return buildErrorJson("Invalid simulation parameters", json::array({"duration 必须大于 0"}));
    }

    if (stepTime <= 0.0 || stepTime > duration)
    {
        return buildErrorJson("Invalid simulation parameters", json::array({"stepTime 必须大于 0 且不能大于 duration"}));
    }

    frameCount = static_cast<int>(std::ceil(duration / stepTime)) + 1;
    if (frameCount < 2 || frameCount > 1000)
    {
        return buildErrorJson("Invalid simulation parameters", json::array({"仿真帧数必须在 2 到 1000 之间"}));
    }

    return json();
}

// 将 ValidationResult 转换为统一业务错误。
json validateOrError(const ValidationResult &validation)
{
    if (validation.isValid())
    {
        return json();
    }

    return buildErrorJson("Invalid parameters", validation.Errors, validation.Warnings);
}

// 按统一格式生成逐帧仿真数据。
template <typename CalculateFunc>
json buildSimulationFrames(int frameCount, double duration, double stepTime, double startValue, double endValue, const std::string &inputName, CalculateFunc calculate)
{
    json frames_json = json::array();
    for (int frame = 0; frame < frameCount; ++frame)
    {
        const double currentTime = frame == frameCount - 1 ? duration : stepTime * frame;
        const double progress = duration <= 0.0 ? 0.0 : currentTime / duration;
        const double currentValue = startValue + (endValue - startValue) * progress;
        MechanismState state = calculate(currentValue);
        if (state.hasError())
        {
            return {
                {"success", false},
                {"error", state.ErrorMessage},
                {"failed_frame", frame},
                {"failed_time", currentTime},
                {"failed_input", currentValue},
                {"failed_input_name", inputName}};
        }

        frames_json.push_back(buildFrameJson(state, frame, currentTime, currentValue, inputName));
    }

    return frames_json;
}

// 统一四连杆仿真接口，覆盖 PDPS 的 PRRR/RPRR/RRRP/RRRR 四类结构。
json simulateUnifiedFourBar(const json &req_param)
{
    const std::string mechanismType = req_param.value("mechanismType", "RRRP");
    const double duration = req_param.value("duration", 2.0);
    const double stepTime = req_param.value("stepTime", 0.02);
    int frameCount = 0;

    json timelineError = validateSimulationTimeline(duration, stepTime, frameCount);
    if (!timelineError.is_null())
    {
        return timelineError;
    }

    if (mechanismType == "RRRP")
    {
        const double L_AB = req_param.value("L_AB", 0.5);
        const double L_BS = req_param.value("L_BS", 2.0);
        const double S_OFS = req_param.value("S_OFS", 0.0);
        const double startAngleDeg = valueAny(req_param, {"startAngleDeg", "startValue", "angleDeg"}, 0.0);
        const double endAngleDeg = valueAny(req_param, {"endAngleDeg", "endValue"}, 360.0);

        std::unique_ptr<CrankSliderMechanism> mechanism(createCrankSliderMechanism());
        mechanism->setL_AB(L_AB);
        mechanism->setL_BS(L_BS);
        mechanism->setS_OFS(S_OFS);

        json validationError = validateOrError(mechanism->validateParameters());
        if (!validationError.is_null())
        {
            return validationError;
        }

        json frames_json = buildSimulationFrames(frameCount, duration, stepTime, startAngleDeg, endAngleDeg, "angleDeg", [&](double value) {
            return mechanism->calculate(value);
        });
        if (!frames_json.is_array())
        {
            return frames_json;
        }

        return {
            {"success", true},
            {"mechanismType", mechanismType},
            {"inputName", "angleDeg"},
            {"frames", frames_json},
            {"frame_count", frameCount},
            {"duration", duration},
            {"stepTime", stepTime},
            {"startValue", startAngleDeg},
            {"endValue", endAngleDeg},
            {"startAngleDeg", startAngleDeg},
            {"endAngleDeg", endAngleDeg},
            {"trajectory", buildTrajectoryJson(mechanism->getTrajectoryPoints())},
            {"slider_trajectory", buildTrajectoryJson(mechanism->getSliderTrajectory())},
            {"parameters", {{"mechanismType", mechanismType}, {"L_AB", L_AB}, {"L_BS", L_BS}, {"S_OFS", S_OFS}, {"startAngleDeg", startAngleDeg}, {"endAngleDeg", endAngleDeg}, {"duration", duration}, {"stepTime", stepTime}}}};
    }

    if (mechanismType == "PRRR")
    {
        const double L_AB = req_param.value("L_AB", 0.5);
        const double L_BS = req_param.value("L_BS", 2.0);
        const double S_OFS = req_param.value("S_OFS", 0.0);

        std::unique_ptr<SliderCrankMechanism> mechanism(createSliderCrankMechanism());
        mechanism->setLink(L_AB, L_BS, S_OFS);

        json validationError = validateOrError(mechanism->validateParameters());
        if (!validationError.is_null())
        {
            return validationError;
        }

        const auto inputRange = mechanism->getInputRange();
        const double startSliderX = valueAny(req_param, {"startSliderX", "startValue", "sliderX"}, inputRange.first);
        const double endSliderX = valueAny(req_param, {"endSliderX", "endValue"}, inputRange.second);
        json frames_json = buildSimulationFrames(frameCount, duration, stepTime, startSliderX, endSliderX, "sliderX", [&](double value) {
            return mechanism->calculate(value);
        });
        if (!frames_json.is_array())
        {
            return frames_json;
        }

        return {
            {"success", true},
            {"mechanismType", mechanismType},
            {"inputName", "sliderX"},
            {"inputRange", {{"min", inputRange.first}, {"max", inputRange.second}}},
            {"frames", frames_json},
            {"frame_count", frameCount},
            {"duration", duration},
            {"stepTime", stepTime},
            {"startValue", startSliderX},
            {"endValue", endSliderX},
            {"startSliderX", startSliderX},
            {"endSliderX", endSliderX},
            {"trajectory", buildTrajectoryJson(mechanism->getTrajectoryPoints())},
            {"slider_trajectory", buildTrajectoryJson(mechanism->getSliderTrajectory())},
            {"alternative_trajectory", buildTrajectoryJson(mechanism->getAlternativeTrajectory())},
            {"parameters", {{"mechanismType", mechanismType}, {"L_AB", L_AB}, {"L_BS", L_BS}, {"S_OFS", S_OFS}, {"startSliderX", startSliderX}, {"endSliderX", endSliderX}, {"duration", duration}, {"stepTime", stepTime}}}};
    }

    if (mechanismType == "RRRR")
    {
        const double L1 = valueAny(req_param, {"L1", "L_AB"}, 1.0);
        const double L2 = valueAny(req_param, {"L2", "L_BS"}, 3.0);
        const double L3 = req_param.value("L3", 2.5);
        const double L4 = req_param.value("L4", 3.5);
        const double startAngleDeg = valueAny(req_param, {"startAngleDeg", "startValue", "angleDeg"}, 0.0);
        const double endAngleDeg = valueAny(req_param, {"endAngleDeg", "endValue"}, 180.0);

        std::unique_ptr<FourBarMechanism> mechanism(createFourBarMechanism());
        mechanism->setLink(L1, L2, L3, L4);

        json validationError = validateOrError(mechanism->validateParameters());
        if (!validationError.is_null())
        {
            return validationError;
        }

        json frames_json = buildSimulationFrames(frameCount, duration, stepTime, startAngleDeg, endAngleDeg, "angleDeg", [&](double value) {
            return mechanism->calculate(value);
        });
        if (!frames_json.is_array())
        {
            return frames_json;
        }

        return {
            {"success", true},
            {"mechanismType", mechanismType},
            {"inputName", "angleDeg"},
            {"frames", frames_json},
            {"frame_count", frameCount},
            {"duration", duration},
            {"stepTime", stepTime},
            {"startValue", startAngleDeg},
            {"endValue", endAngleDeg},
            {"startAngleDeg", startAngleDeg},
            {"endAngleDeg", endAngleDeg},
            {"trajectory", buildTrajectoryJson(mechanism->getTrajectoryPoints())},
            {"trajectory_c", buildTrajectoryJson(mechanism->getTrajectoryC())},
            {"crank_circle", buildTrajectoryJson(mechanism->getCrankCirclePoints())},
            {"parameters", {{"mechanismType", mechanismType}, {"L1", L1}, {"L2", L2}, {"L3", L3}, {"L4", L4}, {"startAngleDeg", startAngleDeg}, {"endAngleDeg", endAngleDeg}, {"duration", duration}, {"stepTime", stepTime}}}};
    }

    if (mechanismType == "RPRR")
    {
        const double OA = valueAny(req_param, {"OA", "L_AB"}, 1.0);
        const double AB = valueAny(req_param, {"AB", "L_BS"}, 3.0);
        const double OC = valueAny(req_param, {"OC", "L4"}, 3.0);
        const double startAngleDeg = valueAny(req_param, {"startAngleDeg", "startValue", "angleDeg"}, 0.0);
        const double endAngleDeg = valueAny(req_param, {"endAngleDeg", "endValue"}, 180.0);

        CrankRockingBlockMechanism_Forward mechanism;
        mechanism.setLink(OA, AB, OC);

        json validationError = validateOrError(mechanism.validateParameters());
        if (!validationError.is_null())
        {
            return validationError;
        }

        json frames_json = buildSimulationFrames(frameCount, duration, stepTime, startAngleDeg, endAngleDeg, "angleDeg", [&](double value) {
            return mechanism.calculate(value);
        });
        if (!frames_json.is_array())
        {
            return frames_json;
        }

        return {
            {"success", true},
            {"mechanismType", mechanismType},
            {"inputName", "angleDeg"},
            {"frames", frames_json},
            {"frame_count", frameCount},
            {"duration", duration},
            {"stepTime", stepTime},
            {"startValue", startAngleDeg},
            {"endValue", endAngleDeg},
            {"startAngleDeg", startAngleDeg},
            {"endAngleDeg", endAngleDeg},
            {"trajectory", buildTrajectoryJson(mechanism.getTrajectoryPoints())},
            {"crank_circle", buildTrajectoryJson(mechanism.getCrankCirclePoints())},
            {"rocker_trajectory", buildTrajectoryJson(mechanism.getRockerTrajectory())},
            {"parameters", {{"mechanismType", mechanismType}, {"OA", OA}, {"AB", AB}, {"OC", OC}, {"startAngleDeg", startAngleDeg}, {"endAngleDeg", endAngleDeg}, {"duration", duration}, {"stepTime", stepTime}}}};
    }

    return buildErrorJson("Unsupported mechanismType: " + mechanismType, json::array({"mechanismType 必须是 PRRR、RPRR、RRRP、RRRR 之一"}));
}
} // namespace

json KinematicsWebAPI::handleFourBarCommand(const std::string &req_cmd, const json &req_param)
{
    try
    {
        log("Handling " + req_cmd + " command");

        if (req_cmd == "Cmd_FourBar_Simulate")
        {
            return simulateUnifiedFourBar(req_param);
        }

        double L_AB = req_param.value("L_AB", 0.5);
        double L_BS = req_param.value("L_BS", 2.0);
        double S_OFS = req_param.value("S_OFS", 0.0);
        double angleDeg = req_param.value("angleDeg", 0.0);
        double startAngleDeg = req_param.value("startAngleDeg", 0.0);
        double endAngleDeg = req_param.value("endAngleDeg", 360.0);
        double duration = req_param.value("duration", 2.0);
        double stepTime = req_param.value("stepTime", 0.02);

        log("Parameters: L_AB=" + std::to_string(L_AB) +
            ", L_BS=" + std::to_string(L_BS) +
            ", S_OFS=" + std::to_string(S_OFS) +
            ", angleDeg=" + std::to_string(angleDeg) +
            ", startAngleDeg=" + std::to_string(startAngleDeg) +
            ", endAngleDeg=" + std::to_string(endAngleDeg) +
            ", duration=" + std::to_string(duration) +
            ", stepTime=" + std::to_string(stepTime));

        std::unique_ptr<CrankSliderMechanism> mechanism(createCrankSliderMechanism());
        mechanism->setL_AB(L_AB);
        mechanism->setL_BS(L_BS);
        mechanism->setS_OFS(S_OFS);

        ValidationResult validation = mechanism->validateParameters();
        if (!validation.isValid())
        {
            return {
                    {"success", false},
                    {"error", "Invalid parameters"},
                    {"validation_errors", validation.Errors},
                    {"validation_warnings", validation.Warnings}};
        }

        if (req_cmd == "Cmd_FourBar_CrankSlider_Simulate")
        {
            if (duration <= 0.0)
            {
                return {
                    {"success", false},
                    {"error", "Invalid simulation parameters"},
                    {"validation_errors", json::array({"duration 必须大于 0"})},
                    {"validation_warnings", json::array()}};
            }

            if (stepTime <= 0.0 || stepTime > duration)
            {
                return {
                    {"success", false},
                    {"error", "Invalid simulation parameters"},
                    {"validation_errors", json::array({"stepTime 必须大于 0 且不能大于 duration"})},
                    {"validation_warnings", json::array()}};
            }

            const int frameCount = static_cast<int>(std::ceil(duration / stepTime)) + 1;
            if (frameCount < 2 || frameCount > 1000)
            {
                return {
                    {"success", false},
                    {"error", "Invalid simulation parameters"},
                    {"validation_errors", json::array({"仿真帧数必须在 2 到 1000 之间"})},
                    {"validation_warnings", json::array()}};
            }

            json frames_json = json::array();
            for (int frame = 0; frame < frameCount; ++frame)
            {
                const double currentTime = frame == frameCount - 1 ? duration : stepTime * frame;
                const double progress = duration <= 0.0 ? 0.0 : currentTime / duration;
                const double currentAngleDeg = startAngleDeg + (endAngleDeg - startAngleDeg) * progress;
                MechanismState state = mechanism->calculate(currentAngleDeg);
                if (state.hasError())
                {
                    return {
                        {"success", false},
                        {"error", state.ErrorMessage},
                        {"failed_frame", frame},
                        {"failed_time", currentTime},
                        {"failed_angleDeg", currentAngleDeg}};
                }

                frames_json.push_back(buildFrameJson(state, frame, currentTime, currentAngleDeg, "angleDeg"));
            }

            json result;
            result["success"] = true;
            result["frames"] = frames_json;
            result["frame_count"] = frameCount;
            result["duration"] = duration;
            result["stepTime"] = stepTime;
            result["startAngleDeg"] = startAngleDeg;
            result["endAngleDeg"] = endAngleDeg;
            result["trajectory"] = buildTrajectoryJson(mechanism->getTrajectoryPoints());
            result["slider_trajectory"] = buildTrajectoryJson(mechanism->getSliderTrajectory());
            result["parameters"] = {
                {"L_AB", L_AB},
                {"L_BS", L_BS},
                {"S_OFS", S_OFS},
                {"startAngleDeg", startAngleDeg},
                {"endAngleDeg", endAngleDeg},
                {"duration", duration},
                {"stepTime", stepTime}};

            return result;
        }

        MechanismState state = mechanism->calculate(angleDeg);
        if (state.hasError())
        {
            return {
                {"success", false},
                {"error", state.ErrorMessage}};
        }

        json result;
        result["success"] = true;

        result["points"] = buildPointsJson(state);
        result["poses"] = buildPosesJson(state);
        result["angles"] = buildAnglesJson(state);

        result["input_value"] = state.InputValue;
        if (state.hasWarning())
        {
            result["warning"] = state.WarningMessage;
        }

        result["trajectory"] = buildTrajectoryJson(mechanism->getTrajectoryPoints());
        result["slider_trajectory"] = buildTrajectoryJson(mechanism->getSliderTrajectory());

        result["parameters"] = {
            {"L_AB", L_AB},
            {"L_BS", L_BS},
            {"S_OFS", S_OFS},
            {"angleDeg", angleDeg}};

        return result;
    }
    catch (const std::exception &e)
    {
        return {
            {"success", false},
            {"error", "Failed to Cmd_FourBar_CrankSlider: " + std::string(e.what())}};
    }
}
