#include "KinematicsWebAPI.h"
#include "FourBarMechanism/CrankSliderMechanism.h"

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

        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);

        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));

        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}};
        }

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

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

        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}};
        }
        result["points"] = points_json;

        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}};
        }
        result["poses"] = poses_json;

        json angles_json;
        for (const auto &angle_pair : state.Angles)
        {
            angles_json[angle_pair.first] = angle_pair.second;
        }
        result["angles"] = angles_json;

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

        json trajectory_json = json::array();
        for (const auto &point : mechanism->getTrajectoryPoints())
        {
            trajectory_json.push_back({
                {"x", point.X},
                {"y", point.Y}});
        }
        result["trajectory"] = trajectory_json;

        json slider_trajectory_json = json::array();
        for (const auto &point : mechanism->getSliderTrajectory())
        {
            slider_trajectory_json.push_back({
                {"x", point.X},
                {"y", point.Y}});
        }
        result["slider_trajectory"] = slider_trajectory_json;

        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())}};
    }
}
