#include "rtcp_kinematics.h"

#include "linuxcnc_5axiskins_adapter.h"
#include "linuxcnc_corexykins_adapter.h"
#include "linuxcnc_genhexkins_adapter.h"
#include "linuxcnc_genserfuncs_adapter.h"
#include "linuxcnc_kins_util_adapter.h"
#include "linuxcnc_lineardeltakins_adapter.h"
#include "linuxcnc_maxkins_adapter.h"
#include "linuxcnc_pentakins_adapter.h"
#include "linuxcnc_pumakins_adapter.h"
#include "linuxcnc_rotatekins_adapter.h"
#include "linuxcnc_rosekins_adapter.h"
#include "linuxcnc_rotarydeltakins_adapter.h"
#include "linuxcnc_scarakins_adapter.h"
#include "linuxcnc_scorbot_kins_adapter.h"
#include "linuxcnc_tripodkins_adapter.h"
#include "linuxcnc_trtfuncs_adapter.h"
#include "linuxcnc_userkfuncs_adapter.h"
#include "linuxcnc_xyzab_tdr_kins_adapter.h"

LinuxCncAxisJoints linuxcnc_identity_default_joints() {
    return {};
}

// Source: linuxcnc/src/emc/kinematics/kins_util.c
// Mirrors identityKinematicsForward() with default XYZABCUVW coordinate mapping.
CncSimPose linuxcnc_identity_forward(const LinuxCncAxisJoints &joints) {
    CncSimPose pose{};
    linuxcnc_kins_util_identity_forward(joints, &pose);
    return pose;
}

// Source: linuxcnc/src/emc/kinematics/kins_util.c
// Mirrors identityKinematicsInverse() with default XYZABCUVW coordinate mapping.
LinuxCncAxisJoints linuxcnc_identity_inverse(const CncSimPose &pose) {
    LinuxCncAxisJoints joints{};
    linuxcnc_kins_util_identity_inverse(pose, &joints);
    return joints;
}

// Source: linuxcnc/src/emc/kinematics/trivkins.c
// kinematicsForward()/kinematicsInverse() delegate to identityKinematics*().

// Source: linuxcnc/src/emc/kinematics/corexykins.c
// Mirrors kinematicsForward().
CncSimPose linuxcnc_corexykins_forward(const LinuxCncAxisJoints &joints) {
    CncSimPose pose{};
    linuxcnc_corexykins_source_forward(joints, &pose);
    return pose;
}

// Source: linuxcnc/src/emc/kinematics/corexykins.c
// Mirrors kinematicsInverse().
LinuxCncAxisJoints linuxcnc_corexykins_inverse(const CncSimPose &pose) {
    LinuxCncAxisJoints joints{};
    linuxcnc_corexykins_source_inverse(pose, &joints);
    return joints;
}

// Source: linuxcnc/src/emc/kinematics/rotatekins.c
// Mirrors kinematicsForward().
CncSimPose linuxcnc_rotatekins_forward(const LinuxCncAxisJoints &joints) {
    CncSimPose pose{};
    linuxcnc_rotatekins_source_forward(joints, &pose);
    return pose;
}

// Source: linuxcnc/src/emc/kinematics/rotatekins.c
// Mirrors kinematicsInverse().
LinuxCncAxisJoints linuxcnc_rotatekins_inverse(const CncSimPose &pose) {
    LinuxCncAxisJoints joints{};
    linuxcnc_rotatekins_source_inverse(pose, &joints);
    return joints;
}

// Source: linuxcnc/src/emc/kinematics/genhexkins.c
// Mirrors genhexKinematicsInverse().
LinuxCncGenhexJoints linuxcnc_genhex_inverse(const CncSimPose &pose) {
    LinuxCncGenhexJoints joints{};
    linuxcnc_genhexkins_source_inverse(pose, &joints);
    return joints;
}

// Source: linuxcnc/src/emc/kinematics/genhexkins.c
// Mirrors genhexKinematicsForward().
CncSimPose linuxcnc_genhex_forward(const LinuxCncGenhexJoints &joints, const CncSimPose &initial_pose) {
    CncSimPose pose{};
    linuxcnc_genhexkins_source_forward(joints, initial_pose, &pose);
    return pose;
}

// Source: linuxcnc/src/emc/kinematics/pentakins.c
// Mirrors kinematicsInverse().
LinuxCncPentakinsJoints linuxcnc_pentakins_inverse(const CncSimPose &pose) {
    LinuxCncPentakinsJoints joints{};
    linuxcnc_pentakins_source_inverse(pose, &joints);
    return joints;
}

// Source: linuxcnc/src/emc/kinematics/pentakins.c
// Mirrors kinematicsForward().
CncSimPose linuxcnc_pentakins_forward(const LinuxCncPentakinsJoints &joints,
                                      const CncSimPose &initial_pose) {
    CncSimPose pose{};
    linuxcnc_pentakins_source_forward(joints, initial_pose, &pose);
    return pose;
}

// Source: linuxcnc/src/emc/kinematics/tripodkins.c
// Mirrors kinematicsInverse().
LinuxCncTripodJoints linuxcnc_tripodkins_inverse(const CncSimPose &pose) {
    LinuxCncTripodJoints joints{};
    linuxcnc_tripodkins_source_inverse(pose, &joints);
    return joints;
}

// Source: linuxcnc/src/emc/kinematics/tripodkins.c
// Mirrors kinematicsForward() with the default positive-Z forward flag.
CncSimPose linuxcnc_tripodkins_forward(const LinuxCncTripodJoints &joints) {
    CncSimPose pose{};
    linuxcnc_tripodkins_source_forward(joints, &pose);
    return pose;
}

// Source: linuxcnc/src/emc/kinematics/scorbot-kins.c
// Mirrors kinematicsInverse().
LinuxCncScorbotJoints linuxcnc_scorbot_kins_inverse(const CncSimPose &pose) {
    LinuxCncScorbotJoints joints{};
    linuxcnc_scorbot_kins_source_inverse(pose, &joints);
    return joints;
}

// Source: linuxcnc/src/emc/kinematics/scorbot-kins.c
// Mirrors kinematicsForward().
CncSimPose linuxcnc_scorbot_kins_forward(const LinuxCncScorbotJoints &joints) {
    CncSimPose pose{};
    linuxcnc_scorbot_kins_source_forward(joints, &pose);
    return pose;
}

// Source defaults: linuxcnc/src/emc/kinematics/lineardeltakins-common.h
LinuxCncLinearDeltaParameters linuxcnc_lineardelta_default_parameters() {
    LinuxCncLinearDeltaParameters parameters{};
    linuxcnc_lineardeltakins_source_default_parameters(&parameters);
    return parameters;
}

// Source: linuxcnc/src/emc/kinematics/lineardeltakins.c
// Mirrors kinematicsForward().
CncSimPose linuxcnc_lineardelta_forward(const LinuxCncAxisJoints &joints,
                                        const LinuxCncLinearDeltaParameters &parameters) {
    CncSimPose pose{};
    linuxcnc_lineardeltakins_source_forward(joints, parameters, &pose);
    return pose;
}

// Source: linuxcnc/src/emc/kinematics/lineardeltakins.c
// Mirrors kinematicsInverse().
LinuxCncAxisJoints linuxcnc_lineardelta_inverse(const CncSimPose &pose,
                                                const LinuxCncLinearDeltaParameters &parameters) {
    LinuxCncAxisJoints joints{};
    linuxcnc_lineardeltakins_source_inverse(pose, parameters, &joints);
    return joints;
}

// Source defaults: linuxcnc/src/emc/kinematics/rotarydeltakins-common.h
LinuxCncRotaryDeltaParameters linuxcnc_rotarydelta_default_parameters() {
    LinuxCncRotaryDeltaParameters parameters{};
    linuxcnc_rotarydeltakins_source_default_parameters(&parameters);
    return parameters;
}

// Source: linuxcnc/src/emc/kinematics/rotarydeltakins.c
// Mirrors kinematicsForward().
CncSimPose linuxcnc_rotarydelta_forward(const LinuxCncAxisJoints &joints,
                                        const LinuxCncRotaryDeltaParameters &parameters) {
    CncSimPose pose{};
    linuxcnc_rotarydeltakins_source_forward(joints, parameters, &pose);
    return pose;
}

// Source: linuxcnc/src/emc/kinematics/rotarydeltakins.c
// Mirrors kinematicsInverse().
LinuxCncAxisJoints linuxcnc_rotarydelta_inverse(const CncSimPose &pose,
                                                const LinuxCncRotaryDeltaParameters &parameters) {
    LinuxCncAxisJoints joints{};
    linuxcnc_rotarydeltakins_source_inverse(pose, parameters, &joints);
    return joints;
}

// Source defaults: linuxcnc/src/emc/kinematics/maxkins.c
LinuxCncMaxkinsParameters linuxcnc_maxkins_default_parameters() {
    LinuxCncMaxkinsParameters parameters{};
    linuxcnc_maxkins_source_default_parameters(&parameters);
    return parameters;
}

// Source: linuxcnc/src/emc/kinematics/maxkins.c
// Mirrors kinematicsForward().
CncSimPose linuxcnc_maxkins_forward(const LinuxCncAxisJoints &joints,
                                    const LinuxCncMaxkinsParameters &parameters) {
    CncSimPose pose{};
    linuxcnc_maxkins_source_forward(joints, parameters, &pose);
    return pose;
}

// Source: linuxcnc/src/emc/kinematics/maxkins.c
// Mirrors kinematicsInverse().
LinuxCncAxisJoints linuxcnc_maxkins_inverse(const CncSimPose &pose,
                                            const LinuxCncMaxkinsParameters &parameters) {
    LinuxCncAxisJoints joints{};
    linuxcnc_maxkins_source_inverse(pose, parameters, &joints);
    return joints;
}

// Source: linuxcnc/src/emc/kinematics/rosekins.c
// Mirrors kinematicsForward().
CncSimPose linuxcnc_rosekins_forward(const LinuxCncAxisJoints &joints) {
    CncSimPose pose{};
    linuxcnc_rosekins_source_forward(joints, &pose);
    return pose;
}

// Source: linuxcnc/src/emc/kinematics/rosekins.c
// Mirrors kinematicsInverse().
LinuxCncAxisJoints linuxcnc_rosekins_inverse(const CncSimPose &pose) {
    LinuxCncAxisJoints joints{};
    linuxcnc_rosekins_source_inverse(pose, &joints);
    return joints;
}

// Source: linuxcnc/src/emc/kinematics/5axiskins.c
// Mirrors s2r(), fiveaxis_KinematicsForward().
CncSimPose linuxcnc_5axis_forward(const LinuxCncFiveAxisJoints &joints, double pivot_length) {
    CncSimPose pose{};
    linuxcnc_5axiskins_forward(joints, pivot_length, &pose);
    return pose;
}

// Source: linuxcnc/src/emc/kinematics/5axiskins.c
// Mirrors s2r(), fiveaxis_KinematicsInverse().
LinuxCncFiveAxisJoints linuxcnc_5axis_inverse(const CncSimPose &pose, double pivot_length) {
    LinuxCncFiveAxisJoints joints{};
    linuxcnc_5axiskins_inverse(pose, pivot_length, &joints);
    return joints;
}

// Source defaults: linuxcnc/configs/sim/axis/vismach/5axis/table-rotary-tilting/xyzbc-trt.ini
LinuxCncXyzbcTrtParameters linuxcnc_xyzbc_trt_default_parameters(double tool_offset) {
    LinuxCncXyzbcTrtParameters parameters{};
    parameters.x_offset = -20.0;
    parameters.y_offset = 0.0;
    parameters.z_offset = -15.0;
    parameters.tool_offset = tool_offset;
    parameters.conventional_directions = false;
    return parameters;
}

// Source: linuxcnc/src/emc/kinematics/trtfuncs.c
// Mirrors xyzbcKinematicsForward().
CncSimPose linuxcnc_xyzbc_trt_forward(const LinuxCncFiveAxisJoints &joints,
                                      const LinuxCncXyzbcTrtParameters &parameters) {
    CncSimPose pose{};
    linuxcnc_trtfuncs_xyzbc_forward(joints, parameters, &pose);
    return pose;
}

// Source: linuxcnc/src/emc/kinematics/trtfuncs.c
// Mirrors xyzbcKinematicsInverse().
LinuxCncFiveAxisJoints linuxcnc_xyzbc_trt_inverse(const CncSimPose &pose,
                                                  const LinuxCncXyzbcTrtParameters &parameters) {
    LinuxCncFiveAxisJoints joints{};
    linuxcnc_trtfuncs_xyzbc_inverse(pose, parameters, &joints);
    return joints;
}

// Source: linuxcnc/src/emc/kinematics/trtfuncs.c
// Mirrors xyzacKinematicsForward().
CncSimPose linuxcnc_xyzac_trt_forward(const LinuxCncFiveAxisJoints &joints,
                                      const LinuxCncXyzbcTrtParameters &parameters) {
    CncSimPose pose{};
    linuxcnc_trtfuncs_xyzac_forward(joints, parameters, &pose);
    return pose;
}

// Source: linuxcnc/src/emc/kinematics/trtfuncs.c
// Mirrors xyzacKinematicsInverse().
LinuxCncFiveAxisJoints linuxcnc_xyzac_trt_inverse(const CncSimPose &pose,
                                                  const LinuxCncXyzbcTrtParameters &parameters) {
    LinuxCncFiveAxisJoints joints{};
    linuxcnc_trtfuncs_xyzac_inverse(pose, parameters, &joints);
    return joints;
}

// Source: linuxcnc/src/emc/kinematics/userkfuncs.c
// userkKinematicsForward() delegates to identityKinematicsForward(),
// implemented in linuxcnc/src/emc/kinematics/kins_util.c.
CncSimPose linuxcnc_xyzbc_userk_forward(const LinuxCncFiveAxisJoints &joints) {
    CncSimPose pose{};
    linuxcnc_userkfuncs_forward(joints, &pose);
    return pose;
}

// Source: linuxcnc/src/emc/kinematics/userkfuncs.c
// userkKinematicsInverse() delegates to identityKinematicsInverse(),
// implemented in linuxcnc/src/emc/kinematics/kins_util.c.
LinuxCncFiveAxisJoints linuxcnc_xyzbc_userk_inverse(const CncSimPose &pose) {
    LinuxCncFiveAxisJoints joints{};
    linuxcnc_userkfuncs_inverse(pose, &joints);
    return joints;
}

// Source defaults: linuxcnc/src/emc/kinematics/scarakins.c
LinuxCncScaraParameters linuxcnc_scara_default_parameters() {
    LinuxCncScaraParameters parameters{};
    parameters.d1 = 490.0;
    parameters.d2 = 340.0;
    parameters.d3 = 50.0;
    parameters.d4 = 250.0;
    parameters.d5 = 50.0;
    parameters.d6 = 50.0;
    return parameters;
}

// Source: linuxcnc/src/emc/kinematics/scarakins.c
CncSimPose linuxcnc_scara_forward(const LinuxCncScaraJoints &joints,
                                  const LinuxCncScaraParameters &parameters,
                                  int *iflags) {
    CncSimPose pose{};
    linuxcnc_scarakins_forward(joints, parameters, &pose, iflags);
    return pose;
}

// Source: linuxcnc/src/emc/kinematics/scarakins.c
LinuxCncScaraJoints linuxcnc_scara_inverse(const CncSimPose &pose,
                                           const LinuxCncScaraParameters &parameters,
                                           int iflags) {
    LinuxCncScaraJoints joints{};
    linuxcnc_scarakins_inverse(pose, parameters, iflags, &joints);
    return joints;
}

LinuxCncXyzabTdrParameters linuxcnc_xyzab_tdr_default_parameters(double tool_offset_z) {
    LinuxCncXyzabTdrParameters parameters{};
    parameters.tool_offset_z = tool_offset_z;
    return parameters;
}

// Source: linuxcnc/src/hal/components/xyzab_tdr_kins.comp
// Mirrors kinematicsForward() switchkins type 0.
CncSimPose linuxcnc_xyzab_tdr_identity_forward(const LinuxCncAxisJoints &joints) {
    CncSimPose pose{};
    LinuxCncXyzabTdrParameters parameters{};
    linuxcnc_xyzab_tdr_kins_forward(joints, parameters, 0, &pose);
    return pose;
}

// Source: linuxcnc/src/hal/components/xyzab_tdr_kins.comp
// Mirrors kinematicsInverse() switchkins type 0.
LinuxCncAxisJoints linuxcnc_xyzab_tdr_identity_inverse(const CncSimPose &pose) {
    LinuxCncAxisJoints joints{};
    LinuxCncXyzabTdrParameters parameters{};
    linuxcnc_xyzab_tdr_kins_inverse(pose, parameters, 0, &joints);
    return joints;
}

// Source: linuxcnc/src/hal/components/xyzab_tdr_kins.comp
// Mirrors kinematicsForward() switchkins type 1.
CncSimPose linuxcnc_xyzab_tdr_tcp_forward(const LinuxCncAxisJoints &joints,
                                          const LinuxCncXyzabTdrParameters &parameters) {
    CncSimPose pose{};
    linuxcnc_xyzab_tdr_kins_forward(joints, parameters, 1, &pose);
    return pose;
}

// Source: linuxcnc/src/hal/components/xyzab_tdr_kins.comp
// Mirrors kinematicsInverse() switchkins type 1.
LinuxCncAxisJoints linuxcnc_xyzab_tdr_tcp_inverse(const CncSimPose &pose,
                                                  const LinuxCncXyzabTdrParameters &parameters) {
    LinuxCncAxisJoints joints{};
    linuxcnc_xyzab_tdr_kins_inverse(pose, parameters, 1, &joints);
    return joints;
}

LinuxCncPumaParameters linuxcnc_puma_default_parameters() {
    LinuxCncPumaParameters parameters{};
    parameters.a2 = 300.0;
    parameters.a3 = 50.0;
    parameters.d3 = 70.0;
    parameters.d4 = 400.0;
    parameters.d6 = 70.0;
    return parameters;
}

// Source: linuxcnc/src/emc/kinematics/pumakins.c
CncSimPose linuxcnc_puma_forward(const LinuxCncAxisJoints &joints,
                                 const LinuxCncPumaParameters &parameters,
                                 int *iflags) {
    CncSimPose pose{};
    linuxcnc_pumakins_forward(joints, parameters, &pose, iflags);
    return pose;
}

// Source: linuxcnc/src/emc/kinematics/pumakins.c
LinuxCncAxisJoints linuxcnc_puma_inverse(const CncSimPose &pose,
                                         const LinuxCncAxisJoints &current_joints,
                                         const LinuxCncPumaParameters &parameters,
                                         int iflags,
                                         int *fflags) {
    LinuxCncAxisJoints joints{};
    linuxcnc_pumakins_inverse(pose, current_joints, parameters, iflags, &joints, fflags);
    return joints;
}

LinuxCncGenserParameters linuxcnc_genser_puma560_parameters() {
    LinuxCncGenserParameters parameters{};
    // Source: linuxcnc/configs/sim/axis/vismach/puma/puma560_dh.hal.
    parameters.a[0] = 0.0;
    parameters.a[1] = 0.0;
    parameters.a[2] = 17.0;
    parameters.a[3] = 0.0;
    parameters.a[4] = 0.0;
    parameters.a[5] = 0.0;

    parameters.alpha[0] = 0.0;
    parameters.alpha[1] = 1.570796326;
    parameters.alpha[2] = 0.0;
    parameters.alpha[3] = 1.570796326;
    parameters.alpha[4] = -1.570796326;
    parameters.alpha[5] = 1.570796326;

    parameters.d[0] = 26.45;
    parameters.d[1] = -5.5;
    parameters.d[2] = 0.0;
    parameters.d[3] = 17.05;
    parameters.d[4] = 0.0;
    parameters.d[5] = 2.2;
    return parameters;
}

LinuxCncGenserParameters linuxcnc_genser_default_parameters(int *max_iterations) {
    LinuxCncGenserParameters parameters{};
    linuxcnc_genserfuncs_source_default_parameters(&parameters, max_iterations);
    return parameters;
}

// Source: linuxcnc/src/emc/kinematics/genserfuncs.c
// Mirrors genserKinematicsForward() -> genser_kin_fwd() -> go_link_pose_build()
// for the six angular DH joints used by the LinuxCNC puma560 M428 configuration.
CncSimPose linuxcnc_genser_forward(const LinuxCncAxisJoints &joints,
                                   const LinuxCncGenserParameters &parameters) {
    CncSimPose pose{};
    linuxcnc_genserfuncs_forward(joints, parameters, &pose);
    return pose;
}

// Source: linuxcnc/src/emc/kinematics/genserfuncs.c
// Mirrors genserKinematicsInverse() for the six angular DH joints used by
// the LinuxCNC puma560 M428 configuration.
LinuxCncAxisJoints linuxcnc_genser_inverse(const CncSimPose &pose,
                                           const LinuxCncAxisJoints &joint_estimate,
                                           const LinuxCncGenserParameters &parameters,
                                           int *iterations,
                                           int max_iterations) {
    if (iterations) {
        *iterations = -1;
    }
    LinuxCncAxisJoints joints = joint_estimate;
    linuxcnc_genserfuncs_inverse(pose, joint_estimate, parameters, max_iterations, &joints, iterations);
    return joints;
}

// Source: linuxcnc/src/emc/kinematics/genserfuncs.c
// Exposes genserKinematicsInverse() success/failure without changing the
// LinuxCNC-computed joint buffer.
bool linuxcnc_genser_inverse_checked(const CncSimPose &pose,
                                     const LinuxCncAxisJoints &joint_estimate,
                                     const LinuxCncGenserParameters &parameters,
                                     LinuxCncAxisJoints *joints,
                                     int *iterations,
                                     int max_iterations) {
    if (iterations) {
        *iterations = -1;
    }
    LinuxCncAxisJoints out = joint_estimate;
    const bool ok = linuxcnc_genserfuncs_inverse(pose,
                                                 joint_estimate,
                                                 parameters,
                                                 max_iterations,
                                                 &out,
                                                 iterations);
    if (joints) {
        *joints = out;
    }
    return ok;
}