#include "rtcp_kinematics.h"

#include <cmath>

namespace {

constexpr double kPi = 3.141592653589793238462643383279502884;
constexpr double kRpyPitchFuzz = 1e-6;
constexpr double kGoRealEpsilon = 1e-6;
constexpr double kGoSingularEpsilon = 1.0e-15;

struct RotationMatrix {
    RtcpVector x;
    RtcpVector y;
    RtcpVector z;
};

struct HomogeneousTransform {
    RotationMatrix rot;
    RtcpVector tran;
};

struct Quaternion {
    double s;
    double x;
    double y;
    double z;
};

double radians(double degrees) {
    return degrees * kPi / 180.0;
}

double degrees(double radians_value) {
    return radians_value * 180.0 / kPi;
}

RtcpVector spherical_to_cartesian(double radius, double theta_deg, double phi_deg) {
    const double theta = radians(theta_deg);
    const double phi = radians(phi_deg);
    return {
        radius * std::sin(phi) * std::cos(theta),
        radius * std::sin(phi) * std::sin(theta),
        radius * std::cos(phi),
    };
}

RotationMatrix multiply_rotation(const RotationMatrix &lhs, const RotationMatrix &rhs) {
    RotationMatrix result{};
    result.x.x = lhs.x.x * rhs.x.x + lhs.y.x * rhs.x.y + lhs.z.x * rhs.x.z;
    result.y.x = lhs.x.x * rhs.y.x + lhs.y.x * rhs.y.y + lhs.z.x * rhs.y.z;
    result.z.x = lhs.x.x * rhs.z.x + lhs.y.x * rhs.z.y + lhs.z.x * rhs.z.z;

    result.x.y = lhs.x.y * rhs.x.x + lhs.y.y * rhs.x.y + lhs.z.y * rhs.x.z;
    result.y.y = lhs.x.y * rhs.y.x + lhs.y.y * rhs.y.y + lhs.z.y * rhs.y.z;
    result.z.y = lhs.x.y * rhs.z.x + lhs.y.y * rhs.z.y + lhs.z.y * rhs.z.z;

    result.x.z = lhs.x.z * rhs.x.x + lhs.y.z * rhs.x.y + lhs.z.z * rhs.x.z;
    result.y.z = lhs.x.z * rhs.y.x + lhs.y.z * rhs.y.y + lhs.z.z * rhs.y.z;
    result.z.z = lhs.x.z * rhs.z.x + lhs.y.z * rhs.z.y + lhs.z.z * rhs.z.z;
    return result;
}

RtcpVector rotate_vector(const RotationMatrix &matrix, const RtcpVector &vector) {
    return {
        matrix.x.x * vector.x + matrix.y.x * vector.y + matrix.z.x * vector.z,
        matrix.x.y * vector.x + matrix.y.y * vector.y + matrix.z.y * vector.z,
        matrix.x.z * vector.x + matrix.y.z * vector.y + matrix.z.z * vector.z,
    };
}

HomogeneousTransform multiply_transform(const HomogeneousTransform &lhs,
                                        const HomogeneousTransform &rhs) {
    HomogeneousTransform result{};
    result.rot = multiply_rotation(lhs.rot, rhs.rot);
    const RtcpVector rotated = rotate_vector(lhs.rot, rhs.tran);
    result.tran.x = lhs.tran.x + rotated.x;
    result.tran.y = lhs.tran.y + rotated.y;
    result.tran.z = lhs.tran.z + rotated.z;
    return result;
}

double square(double value) {
    return value * value;
}

bool go_small(double value) {
    return std::fabs(value) < kGoRealEpsilon;
}

RtcpVector add_vector(const RtcpVector &lhs, const RtcpVector &rhs) {
    return {
        lhs.x + rhs.x,
        lhs.y + rhs.y,
        lhs.z + rhs.z,
    };
}

RtcpVector cross_vector(const RtcpVector &lhs, const RtcpVector &rhs) {
    return {
        lhs.y * rhs.z - lhs.z * rhs.y,
        lhs.z * rhs.x - lhs.x * rhs.z,
        lhs.x * rhs.y - lhs.y * rhs.x,
    };
}

// Source: linuxcnc/src/libnml/posemath/_posemath.c
// Mirrors pmRpyMatConvert().
RotationMatrix posemath_rpy_to_matrix(double roll, double pitch, double yaw) {
    const double sa = std::sin(yaw);
    const double sb = std::sin(pitch);
    const double sg = std::sin(roll);
    const double ca = std::cos(yaw);
    const double cb = std::cos(pitch);
    const double cg = std::cos(roll);

    RotationMatrix matrix{};
    matrix.x.x = ca * cb;
    matrix.y.x = ca * sb * sg - sa * cg;
    matrix.z.x = ca * sb * cg + sa * sg;

    matrix.x.y = sa * cb;
    matrix.y.y = sa * sb * sg + ca * cg;
    matrix.z.y = sa * sb * cg - ca * sg;

    matrix.x.z = -sb;
    matrix.y.z = cb * sg;
    matrix.z.z = cb * cg;
    return matrix;
}

// Source: linuxcnc/src/libnml/posemath/_posemath.c
// Mirrors pmMatRpyConvert().
CncSimPose posemath_matrix_to_pose(const RtcpVector &tran, const RotationMatrix &matrix) {
    CncSimPose pose{};
    pose.x = tran.x;
    pose.y = tran.y;
    pose.z = tran.z;

    const double pitch = std::atan2(-matrix.x.z,
                                    std::sqrt(square(matrix.x.x) + square(matrix.x.y)));
    double roll = 0.0;
    double yaw = 0.0;
    if (std::fabs(pitch - kPi / 2.0) < kRpyPitchFuzz) {
        roll = std::atan2(matrix.y.x, matrix.y.y);
        pose.b = 90.0;
    } else if (std::fabs(pitch + kPi / 2.0) < kRpyPitchFuzz) {
        roll = -std::atan2(matrix.y.x, matrix.y.y);
        pose.b = -90.0;
    } else {
        roll = std::atan2(matrix.y.z, matrix.z.z);
        yaw = std::atan2(matrix.x.y, matrix.x.x);
        pose.b = degrees(pitch);
    }
    pose.a = degrees(roll);
    pose.c = degrees(yaw);
    return pose;
}

// Source: linuxcnc/src/libnml/posemath/gomath.c
// Mirrors go_mat_quat_convert() for normalized rotation matrices.
Quaternion linuxcnc_matrix_to_quaternion(const RotationMatrix &matrix) {
    Quaternion q{};
    double discr = 1.0 + matrix.x.x + matrix.y.y + matrix.z.z;
    if (discr < 0.0) {
        discr = 0.0;
    }
    q.s = 0.5 * std::sqrt(discr);
    if (go_small(q.s)) {
        q.s = 0.0;
        discr = 1.0 + matrix.x.x - matrix.y.y - matrix.z.z;
        if (discr < 0.0) {
            discr = 0.0;
        }
        q.x = std::sqrt(discr) / 2.0;
        discr = 1.0 + matrix.y.y - matrix.x.x - matrix.z.z;
        if (discr < 0.0) {
            discr = 0.0;
        }
        q.y = std::sqrt(discr) / 2.0;
        discr = 1.0 + matrix.z.z - matrix.y.y - matrix.x.x;
        if (discr < 0.0) {
            discr = 0.0;
        }
        q.z = std::sqrt(discr) / 2.0;

        if (q.x > q.y && q.x > q.z) {
            if (matrix.x.y < 0.0) {
                q.y *= -1.0;
            }
            if (matrix.x.z < 0.0) {
                q.z *= -1.0;
            }
        } else if (q.y > q.z) {
            if (matrix.x.y < 0.0) {
                q.x *= -1.0;
            }
            if (matrix.y.z < 0.0) {
                q.z *= -1.0;
            }
        } else {
            if (matrix.x.z < 0.0) {
                q.x *= -1.0;
            }
            if (matrix.y.z < 0.0) {
                q.y *= -1.0;
            }
        }
    } else {
        const double a = 4.0 * q.s;
        q.x = (matrix.y.z - matrix.z.y) / a;
        q.y = (matrix.z.x - matrix.x.z) / a;
        q.z = (matrix.x.y - matrix.y.x) / a;
    }

    const double size = std::sqrt(square(q.s) + square(q.x) + square(q.y) + square(q.z));
    if (go_small(size)) {
        return {1.0, 0.0, 0.0, 0.0};
    }
    const double inv_size = 1.0 / size;
    if (q.s >= 0.0) {
        q.s *= inv_size;
        q.x *= inv_size;
        q.y *= inv_size;
        q.z *= inv_size;
    } else {
        q.s *= -inv_size;
        q.x *= -inv_size;
        q.y *= -inv_size;
        q.z *= -inv_size;
    }
    return q;
}

// Source: linuxcnc/src/libnml/posemath/gomath.c
// Mirrors go_quat_mat_convert().
RotationMatrix linuxcnc_quaternion_to_matrix(const Quaternion &q) {
    RotationMatrix matrix{};
    matrix.x.x = 1.0 - 2.0 * (square(q.y) + square(q.z));
    matrix.y.x = 2.0 * (q.x * q.y - q.z * q.s);
    matrix.z.x = 2.0 * (q.z * q.x + q.y * q.s);

    matrix.x.y = 2.0 * (q.x * q.y + q.z * q.s);
    matrix.y.y = 1.0 - 2.0 * (square(q.z) + square(q.x));
    matrix.z.y = 2.0 * (q.y * q.z - q.x * q.s);

    matrix.x.z = 2.0 * (q.z * q.x - q.y * q.s);
    matrix.y.z = 2.0 * (q.y * q.z + q.x * q.s);
    matrix.z.z = 1.0 - 2.0 * (square(q.x) + square(q.y));
    return matrix;
}

// Source: linuxcnc/src/libnml/posemath/gomath.c
// Mirrors go_quat_inv().
Quaternion linuxcnc_quaternion_inverse(const Quaternion &q) {
    return {q.s, -q.x, -q.y, -q.z};
}

// Source: linuxcnc/src/libnml/posemath/gomath.c
// Mirrors go_quat_quat_mult().
Quaternion linuxcnc_multiply_quaternion(const Quaternion &lhs, const Quaternion &rhs) {
    Quaternion q{};
    q.s = lhs.s * rhs.s - lhs.x * rhs.x - lhs.y * rhs.y - lhs.z * rhs.z;
    if (q.s >= 0.0) {
        q.x = lhs.s * rhs.x + lhs.x * rhs.s + lhs.y * rhs.z - lhs.z * rhs.y;
        q.y = lhs.s * rhs.y - lhs.x * rhs.z + lhs.y * rhs.s + lhs.z * rhs.x;
        q.z = lhs.s * rhs.z + lhs.x * rhs.y - lhs.y * rhs.x + lhs.z * rhs.s;
    } else {
        q.s = -q.s;
        q.x = -lhs.s * rhs.x - lhs.x * rhs.s - lhs.y * rhs.z + lhs.z * rhs.y;
        q.y = -lhs.s * rhs.y + lhs.x * rhs.z - lhs.y * rhs.s - lhs.z * rhs.x;
        q.z = -lhs.s * rhs.z - lhs.x * rhs.y + lhs.y * rhs.x - lhs.z * rhs.s;
    }
    return q;
}

// Source: linuxcnc/src/libnml/posemath/gomath.c
// Mirrors go_quat_cart_mult().
RtcpVector linuxcnc_quaternion_rotate_vector(const Quaternion &q, const RtcpVector &vector) {
    const RtcpVector c{
        q.y * vector.z - q.z * vector.y,
        q.z * vector.x - q.x * vector.z,
        q.x * vector.y - q.y * vector.x,
    };
    return {
        vector.x + 2.0 * (q.s * c.x + q.y * c.z - q.z * c.y),
        vector.y + 2.0 * (q.s * c.y + q.z * c.x - q.x * c.z),
        vector.z + 2.0 * (q.s * c.z + q.x * c.y - q.y * c.x),
    };
}

// Source: linuxcnc/src/libnml/posemath/gomath.c
// Mirrors go_quat_rvec_convert().
RtcpVector linuxcnc_quaternion_to_rvec(const Quaternion &q) {
    const double sh = std::sqrt(square(q.x) + square(q.y) + square(q.z));
    if (go_small(sh)) {
        return {};
    }
    const double mag = 2.0 * std::atan2(sh, q.s) / sh;
    return {
        mag * q.x,
        mag * q.y,
        mag * q.z,
    };
}

// Source: linuxcnc/src/libnml/posemath/gomath.c
// Mirrors go_dh_pose_convert().
HomogeneousTransform linuxcnc_genser_dh_transform(double a, double alpha, double d, double theta) {
    const double sth = std::sin(theta);
    const double cth = std::cos(theta);
    const double sal = std::sin(alpha);
    const double cal = std::cos(alpha);

    HomogeneousTransform transform{};
    transform.rot.x.x = cth;
    transform.rot.y.x = -sth;
    transform.rot.z.x = 0.0;
    transform.rot.x.y = sth * cal;
    transform.rot.y.y = cth * cal;
    transform.rot.z.y = -sal;
    transform.rot.x.z = sth * sal;
    transform.rot.y.z = cth * sal;
    transform.rot.z.z = cal;
    transform.tran.x = a;
    transform.tran.y = -sal * d;
    transform.tran.z = cal * d;
    return transform;
}

HomogeneousTransform identity_transform() {
    HomogeneousTransform transform{};
    transform.rot.x.x = 1.0;
    transform.rot.y.y = 1.0;
    transform.rot.z.z = 1.0;
    return transform;
}

HomogeneousTransform linuxcnc_genser_build_pose(const double joint_radians[6],
                                                const LinuxCncGenserParameters &parameters) {
    HomogeneousTransform total = identity_transform();
    for (int i = 0; i < 6; ++i) {
        const HomogeneousTransform link =
            linuxcnc_genser_dh_transform(parameters.a[i],
                                         parameters.alpha[i],
                                         parameters.d[i],
                                         joint_radians[i]);
        total = multiply_transform(total, link);
    }
    return total;
}

// Source: linuxcnc/src/libnml/posemath/gomath.c
// Mirrors go_pose_inv().
HomogeneousTransform linuxcnc_pose_inverse(const HomogeneousTransform &pose) {
    HomogeneousTransform inverse{};
    const Quaternion q = linuxcnc_matrix_to_quaternion(pose.rot);
    const Quaternion q_inv = linuxcnc_quaternion_inverse(q);
    inverse.rot = linuxcnc_quaternion_to_matrix(q_inv);
    const RtcpVector rotated = linuxcnc_quaternion_rotate_vector(q_inv, pose.tran);
    inverse.tran.x = -rotated.x;
    inverse.tran.y = -rotated.y;
    inverse.tran.z = -rotated.z;
    return inverse;
}

bool invert_6x6(const double input[6][6], double inverse[6][6]) {
    double lu[6][6] = {};
    double scratchrow[6] = {};
    int index[6] = {};
    double d = 1.0;

    // Source: linuxcnc/src/libnml/posemath/gomath.c
    // Mirrors ludcmp().
    for (int i = 0; i < 6; ++i) {
        double big = 0.0;
        for (int j = 0; j < 6; ++j) {
            lu[i][j] = input[i][j];
            const double temp = std::fabs(lu[i][j]);
            if (temp > big) {
                big = temp;
            }
        }
        if (big < kGoSingularEpsilon) {
            return false;
        }
        scratchrow[i] = 1.0 / big;
    }

    for (int j = 0; j < 6; ++j) {
        for (int i = 0; i < j; ++i) {
            double sum = lu[i][j];
            for (int k = 0; k < i; ++k) {
                sum -= lu[i][k] * lu[k][j];
            }
            lu[i][j] = sum;
        }
        double big = 0.0;
        int imax = 0;
        for (int i = j; i < 6; ++i) {
            double sum = lu[i][j];
            for (int k = 0; k < j; ++k) {
                sum -= lu[i][k] * lu[k][j];
            }
            lu[i][j] = sum;
            const double dum = scratchrow[i] * std::fabs(sum);
            if (dum >= big) {
                big = dum;
                imax = i;
            }
        }
        if (j != imax) {
            for (int k = 0; k < 6; ++k) {
                const double dum = lu[imax][k];
                lu[imax][k] = lu[j][k];
                lu[j][k] = dum;
            }
            d = -d;
            scratchrow[imax] = scratchrow[j];
        }
        index[j] = imax;
        if (std::fabs(lu[j][j]) < kGoSingularEpsilon) {
            return false;
        }
        if (j != 5) {
            const double dum = 1.0 / lu[j][j];
            for (int i = j + 1; i < 6; ++i) {
                lu[i][j] *= dum;
            }
        }
    }
    (void)d;

    // Source: linuxcnc/src/libnml/posemath/gomath.c
    // Mirrors go_matrix_inv() and lubksb().
    for (int col = 0; col < 6; ++col) {
        double v[6] = {};
        v[col] = 1.0;
        int ii = -1;
        for (int i = 0; i < 6; ++i) {
            const int ip = index[i];
            double sum = v[ip];
            v[ip] = v[i];
            if (ii != -1) {
                for (int j = ii; j <= i - 1; ++j) {
                    sum -= lu[i][j] * v[j];
                }
            } else if (sum) {
                ii = i;
            }
            v[i] = sum;
        }
        for (int i = 5; i >= 0; --i) {
            double sum = v[i];
            for (int j = i + 1; j < 6; ++j) {
                sum -= lu[i][j] * v[j];
            }
            if (std::fabs(lu[i][i]) < kGoSingularEpsilon) {
                return false;
            }
            v[i] = sum / lu[i][i];
        }
        for (int row = 0; row < 6; ++row) {
            inverse[row][col] = v[row];
        }
    }
    return true;
}

void multiply_6x6_vector(const double matrix[6][6], const double vector[6], double output[6]) {
    // Source: linuxcnc/src/libnml/posemath/gomath.c
    // Mirrors go_matrix_vector_mult().
    for (int row = 0; row < 6; ++row) {
        output[row] = 0.0;
        for (int col = 0; col < 6; ++col) {
            output[row] += matrix[row][col] * vector[col];
        }
    }
}

bool linuxcnc_genser_compute_jfwd(const double joint_radians[6],
                                  const LinuxCncGenserParameters &parameters,
                                  double jfwd[6][6],
                                  HomogeneousTransform *t_l_0) {
    // Source: linuxcnc/src/emc/kinematics/genserfuncs.c
    // Mirrors compute_jfwd() for six DH angular joints.
    RtcpVector jv[6] = {};
    RtcpVector jw[6] = {};
    jw[0].z = 1.0;

    HomogeneousTransform pose =
        linuxcnc_genser_dh_transform(parameters.a[0],
                                     parameters.alpha[0],
                                     parameters.d[0],
                                     joint_radians[0]);
    *t_l_0 = pose;

    for (int col = 1; col < 6; ++col) {
        pose = linuxcnc_genser_dh_transform(parameters.a[col],
                                            parameters.alpha[col],
                                            parameters.d[col],
                                            joint_radians[col]);
        const Quaternion q = linuxcnc_matrix_to_quaternion(pose.rot);
        const RotationMatrix r_i_ip1 = linuxcnc_quaternion_to_matrix(linuxcnc_quaternion_inverse(q));

        RtcpVector scratch[6] = {};
        for (int i = 0; i < 6; ++i) {
            scratch[i] = add_vector(jv[i], cross_vector(jw[i], pose.tran));
            jv[i] = rotate_vector(r_i_ip1, scratch[i]);
            jw[i] = rotate_vector(r_i_ip1, jw[i]);
        }
        jv[col] = {};
        jw[col] = {0.0, 0.0, 1.0};
        *t_l_0 = multiply_transform(*t_l_0, pose);
    }

    const RotationMatrix r_inv = t_l_0->rot;
    for (int col = 0; col < 6; ++col) {
        jv[col] = rotate_vector(r_inv, jv[col]);
        jw[col] = rotate_vector(r_inv, jw[col]);
        jfwd[0][col] = jv[col].x;
        jfwd[1][col] = jv[col].y;
        jfwd[2][col] = jv[col].z;
        jfwd[3][col] = jw[col].x;
        jfwd[4][col] = jw[col].y;
        jfwd[5][col] = jw[col].z;
    }
    return true;
}

bool linuxcnc_genser_compute_jinv(const double jfwd[6][6], double jinv[6][6]) {
    // Source: linuxcnc/src/emc/kinematics/genserfuncs.c
    // compute_jinv() uses go_matrix_inv() directly for square 6x6 Jacobians.
    return invert_6x6(jfwd, jinv);
}

} // namespace

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{};
    pose.x = joints.x;
    pose.y = joints.y;
    pose.z = joints.z;
    pose.a = joints.a;
    pose.b = joints.b;
    pose.c = joints.c;
    pose.u = joints.u;
    pose.v = joints.v;
    pose.w = joints.w;
    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{};
    joints.x = pose.x;
    joints.y = pose.y;
    joints.z = pose.z;
    joints.a = pose.a;
    joints.b = pose.b;
    joints.c = pose.c;
    joints.u = pose.u;
    joints.v = pose.v;
    joints.w = pose.w;
    return joints;
}

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

// Source: linuxcnc/src/emc/kinematics/rotatekins.c
// Mirrors kinematicsForward().
CncSimPose linuxcnc_rotatekins_forward(const LinuxCncAxisJoints &joints) {
    const double c_rad = -radians(joints.c);
    CncSimPose pose{};
    pose.x = joints.x * std::cos(c_rad) - joints.y * std::sin(c_rad);
    pose.y = joints.x * std::sin(c_rad) + joints.y * std::cos(c_rad);
    pose.z = joints.z;
    pose.a = joints.a;
    pose.b = joints.b;
    pose.c = joints.c;
    pose.u = joints.u;
    pose.v = joints.v;
    pose.w = joints.w;
    return pose;
}

// Source: linuxcnc/src/emc/kinematics/rotatekins.c
// Mirrors kinematicsInverse().
LinuxCncAxisJoints linuxcnc_rotatekins_inverse(const CncSimPose &pose) {
    const double c_rad = radians(pose.c);
    LinuxCncAxisJoints joints{};
    joints.x = pose.x * std::cos(c_rad) - pose.y * std::sin(c_rad);
    joints.y = pose.x * std::sin(c_rad) + pose.y * std::cos(c_rad);
    joints.z = pose.z;
    joints.a = pose.a;
    joints.b = pose.b;
    joints.c = pose.c;
    joints.u = pose.u;
    joints.v = pose.v;
    joints.w = pose.w;
    return joints;
}

// Source: linuxcnc/src/emc/kinematics/5axiskins.c
// Mirrors s2r(), fiveaxis_KinematicsForward().
CncSimPose linuxcnc_5axis_forward(const LinuxCncFiveAxisJoints &joints, double pivot_length) {
    const RtcpVector radius = spherical_to_cartesian(pivot_length + joints.w,
                                                    joints.c,
                                                    180.0 - joints.b);
    CncSimPose pose{};
    pose.x = joints.x + radius.x;
    pose.y = joints.y + radius.y;
    pose.z = joints.z + pivot_length + radius.z;
    pose.b = joints.b;
    pose.c = joints.c;
    pose.w = joints.w;
    return pose;
}

// Source: linuxcnc/src/emc/kinematics/5axiskins.c
// Mirrors s2r(), fiveaxis_KinematicsInverse().
LinuxCncFiveAxisJoints linuxcnc_5axis_inverse(const CncSimPose &pose, double pivot_length) {
    const RtcpVector radius = spherical_to_cartesian(pivot_length + pose.w,
                                                    pose.c,
                                                    180.0 - pose.b);
    LinuxCncFiveAxisJoints joints{};
    joints.x = pose.x - radius.x;
    joints.y = pose.y - radius.y;
    joints.z = pose.z - pivot_length - radius.z;
    joints.b = pose.b;
    joints.c = pose.c;
    joints.w = pose.w;
    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) {
    const double x_rot_point = parameters.x_rot_point;
    const double y_rot_point = parameters.y_rot_point;
    const double z_rot_point = parameters.z_rot_point;
    const double dx = parameters.x_offset;
    const double dz = parameters.z_offset + parameters.tool_offset;
    const double b_rad = radians(joints.b);
    const double c_rad = radians(joints.c);
    const double con = parameters.conventional_directions ? 1.0 : -1.0;

    CncSimPose pose{};
    pose.x = +std::cos(c_rad) * std::cos(b_rad) * (joints.x - dx - x_rot_point)
             - con * std::sin(c_rad) * (joints.y - y_rot_point)
             + con * std::cos(c_rad) * std::sin(b_rad) * (joints.z - dz - z_rot_point)
             + std::cos(c_rad) * dx
             + x_rot_point;

    pose.y = +con * std::sin(c_rad) * std::cos(b_rad) * (joints.x - dx - x_rot_point)
             + std::cos(c_rad) * (joints.y - y_rot_point)
             + std::sin(c_rad) * std::sin(b_rad) * (joints.z - dz - z_rot_point)
             + con * std::sin(c_rad) * dx
             + y_rot_point;

    pose.z = -con * std::sin(b_rad) * (joints.x - dx - x_rot_point)
             + std::cos(b_rad) * (joints.z - dz - z_rot_point)
             + dz
             + z_rot_point;

    pose.b = joints.b;
    pose.c = joints.c;
    return pose;
}

// Source: linuxcnc/src/emc/kinematics/trtfuncs.c
// Mirrors xyzbcKinematicsInverse().
LinuxCncFiveAxisJoints linuxcnc_xyzbc_trt_inverse(const CncSimPose &pose,
                                                  const LinuxCncXyzbcTrtParameters &parameters) {
    const double x_rot_point = parameters.x_rot_point;
    const double y_rot_point = parameters.y_rot_point;
    const double z_rot_point = parameters.z_rot_point;
    const double dx = parameters.x_offset;
    const double dz = parameters.z_offset + parameters.tool_offset;
    const double b_rad = radians(pose.b);
    const double c_rad = radians(pose.c);
    const double dpx = -std::cos(b_rad) * dx + std::sin(b_rad) * dz + dx;
    const double dpz = -std::sin(b_rad) * dx - std::cos(b_rad) * dz + dz;
    const double con = parameters.conventional_directions ? 1.0 : -1.0;

    LinuxCncFiveAxisJoints joints{};
    joints.x = +std::cos(c_rad) * std::cos(b_rad) * (pose.x - x_rot_point)
               + con * std::sin(c_rad) * std::cos(b_rad) * (pose.y - y_rot_point)
               - con * std::sin(b_rad) * (pose.z - z_rot_point)
               + dpx
               + x_rot_point;

    joints.y = -con * std::sin(c_rad) * (pose.x - x_rot_point)
               + std::cos(c_rad) * (pose.y - y_rot_point)
               + y_rot_point;

    joints.z = +con * std::cos(c_rad) * std::sin(b_rad) * (pose.x - x_rot_point)
               + std::sin(c_rad) * std::sin(b_rad) * (pose.y - y_rot_point)
               + std::cos(b_rad) * (pose.z - z_rot_point)
               + dpz
               + z_rot_point;

    joints.b = pose.b;
    joints.c = pose.c;
    return joints;
}

// Source: linuxcnc/src/emc/kinematics/trtfuncs.c
// Mirrors xyzacKinematicsForward().
CncSimPose linuxcnc_xyzac_trt_forward(const LinuxCncFiveAxisJoints &joints,
                                      const LinuxCncXyzbcTrtParameters &parameters) {
    const double x_rot_point = parameters.x_rot_point;
    const double y_rot_point = parameters.y_rot_point;
    const double z_rot_point = parameters.z_rot_point;
    const double dy = parameters.y_offset;
    const double dz = parameters.z_offset + parameters.tool_offset;
    const double a_rad = radians(joints.a);
    const double c_rad = radians(joints.c);
    const double con = parameters.conventional_directions ? 1.0 : -1.0;

    CncSimPose pose{};
    pose.x = +std::cos(c_rad) * (joints.x - x_rot_point)
             - con * std::sin(c_rad) * std::cos(a_rad) * (joints.y - dy - y_rot_point)
             + std::sin(c_rad) * std::sin(a_rad) * (joints.z - dz - z_rot_point)
             - con * std::sin(c_rad) * dy
             + x_rot_point;

    pose.y = +con * std::sin(c_rad) * (joints.x - x_rot_point)
             + std::cos(c_rad) * std::cos(a_rad) * (joints.y - dy - y_rot_point)
             - con * std::cos(c_rad) * std::sin(a_rad) * (joints.z - dz - z_rot_point)
             + std::cos(c_rad) * dy
             + y_rot_point;

    pose.z = +con * std::sin(a_rad) * (joints.y - dy - y_rot_point)
             + std::cos(a_rad) * (joints.z - dz - z_rot_point)
             + dz
             + z_rot_point;

    pose.a = joints.a;
    pose.c = joints.c;
    return pose;
}

// Source: linuxcnc/src/emc/kinematics/trtfuncs.c
// Mirrors xyzacKinematicsInverse().
LinuxCncFiveAxisJoints linuxcnc_xyzac_trt_inverse(const CncSimPose &pose,
                                                  const LinuxCncXyzbcTrtParameters &parameters) {
    const double x_rot_point = parameters.x_rot_point;
    const double y_rot_point = parameters.y_rot_point;
    const double z_rot_point = parameters.z_rot_point;
    const double dy = parameters.y_offset;
    const double dz = parameters.z_offset + parameters.tool_offset;
    const double a_rad = radians(pose.a);
    const double c_rad = radians(pose.c);
    const double con = parameters.conventional_directions ? 1.0 : -1.0;

    LinuxCncFiveAxisJoints joints{};
    joints.x = +std::cos(c_rad) * (pose.x - x_rot_point)
               + con * std::sin(c_rad) * (pose.y - y_rot_point)
               + x_rot_point;

    joints.y = -con * std::sin(c_rad) * std::cos(a_rad) * (pose.x - x_rot_point)
               + std::cos(c_rad) * std::cos(a_rad) * (pose.y - y_rot_point)
               + con * std::sin(a_rad) * (pose.z - z_rot_point)
               - std::cos(a_rad) * dy
               - con * std::sin(a_rad) * dz
               + dy
               + y_rot_point;

    joints.z = +std::sin(c_rad) * std::sin(a_rad) * (pose.x - x_rot_point)
               - con * std::cos(c_rad) * std::sin(a_rad) * (pose.y - y_rot_point)
               + std::cos(a_rad) * (pose.z - z_rot_point)
               + con * std::sin(a_rad) * dy
               - std::cos(a_rad) * dz
               + dz
               + z_rot_point;

    joints.a = pose.a;
    joints.c = pose.c;
    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{};
    pose.x = joints.x;
    pose.y = joints.y;
    pose.z = joints.z;
    pose.b = joints.b;
    pose.c = joints.c;
    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{};
    joints.x = pose.x;
    joints.y = pose.y;
    joints.z = pose.z;
    joints.b = pose.b;
    joints.c = pose.c;
    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
// Mirrors scaraKinematicsForward().
CncSimPose linuxcnc_scara_forward(const LinuxCncScaraJoints &joints,
                                  const LinuxCncScaraParameters &parameters,
                                  int *iflags) {
    double a0 = radians(joints.j0);
    double a1 = radians(joints.j1);
    double a3 = radians(joints.j3);

    a1 = a1 + a0;
    a3 = a3 + a1;

    CncSimPose pose{};
    pose.x = parameters.d2 * std::cos(a0) +
             parameters.d4 * std::cos(a1) +
             parameters.d6 * std::cos(a3);
    pose.y = parameters.d2 * std::sin(a0) +
             parameters.d4 * std::sin(a1) +
             parameters.d6 * std::sin(a3);
    pose.z = parameters.d1 + parameters.d3 - joints.j2 - parameters.d5;
    pose.c = degrees(a3);
    pose.a = joints.j4;
    pose.b = joints.j5;

    if (iflags) {
        *iflags = joints.j1 < 90.0 ? 1 : 0;
    }
    return pose;
}

// Source: linuxcnc/src/emc/kinematics/scarakins.c
// Mirrors scaraKinematicsInverse().
LinuxCncScaraJoints linuxcnc_scara_inverse(const CncSimPose &pose,
                                           const LinuxCncScaraParameters &parameters,
                                           int iflags) {
    const double a3 = radians(pose.c);
    const double xt = pose.x - parameters.d6 * std::cos(a3);
    const double yt = pose.y - parameters.d6 * std::sin(a3);
    const double rsq = xt * xt + yt * yt;
    double cc = (rsq - parameters.d2 * parameters.d2 - parameters.d4 * parameters.d4) /
                (2.0 * parameters.d2 * parameters.d4);
    if (cc < -1.0) {
        cc = -1.0;
    }
    if (cc > 1.0) {
        cc = 1.0;
    }

    double q1 = std::acos(cc);
    if (iflags) {
        q1 = -q1;
    }

    double q0 = std::atan2(yt, xt);
    const double local_xt = parameters.d2 + parameters.d4 * std::cos(q1);
    const double local_yt = parameters.d4 * std::sin(q1);
    q0 = q0 - std::atan2(local_yt, local_xt);

    LinuxCncScaraJoints joints{};
    joints.j0 = degrees(q0);
    joints.j1 = degrees(q1);
    joints.j2 = parameters.d1 + parameters.d3 - parameters.d5 - pose.z;
    joints.j3 = pose.c - (joints.j0 + joints.j1);
    joints.j4 = pose.a;
    joints.j5 = pose.b;
    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{};
    pose.x = joints.x;
    pose.y = joints.y;
    pose.z = joints.z;
    pose.a = joints.a;
    pose.b = joints.b;
    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{};
    joints.x = pose.x;
    joints.y = pose.y;
    joints.z = pose.z;
    joints.a = pose.a;
    joints.b = pose.b;
    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) {
    const double x_rot_point = parameters.x_rot_point;
    const double y_rot_point = parameters.y_rot_point;
    const double z_rot_point = parameters.z_rot_point;
    const double dz = parameters.z_offset;
    const double dt = parameters.tool_offset_z;
    const double sa = std::sin(radians(joints.a));
    const double ca = std::cos(radians(joints.a));
    const double sb = std::sin(radians(joints.b));
    const double cb = std::cos(radians(joints.b));
    const double px = joints.x - x_rot_point;
    const double py = joints.y - y_rot_point;
    const double pz = joints.z - z_rot_point - dt;

    CncSimPose pose{};
    pose.x = cb * px + sb * pz + x_rot_point;
    pose.y = sa * sb * px + ca * py - cb * sa * pz + sa * dz + y_rot_point;
    pose.z = -ca * sb * px + sa * py + ca * cb * pz - ca * dz + z_rot_point + dz + dt;
    pose.a = joints.a;
    pose.b = joints.b;
    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) {
    const double x_rot_point = parameters.x_rot_point;
    const double y_rot_point = parameters.y_rot_point;
    const double z_rot_point = parameters.z_rot_point;
    const double dx = parameters.x_offset;
    const double dz = parameters.z_offset;
    const double dt = parameters.tool_offset_z;
    const double sa = std::sin(radians(pose.a));
    const double ca = std::cos(radians(pose.a));
    const double sb = std::sin(radians(pose.b));
    const double cb = std::cos(radians(pose.b));
    const double qx = pose.x - x_rot_point - dx;
    const double qy = pose.y - y_rot_point;
    const double qz = pose.z - z_rot_point - dz - dt;

    LinuxCncAxisJoints joints{};
    joints.x = cb * qx + sa * sb * qy - ca * sb * qz + cb * dx - sb * dz + x_rot_point;
    joints.y = ca * qy + sa * qz + y_rot_point;
    joints.z = sb * qx - sa * cb * qy + ca * cb * qz + sb * dx + cb * dz + z_rot_point + dt;
    joints.a = pose.a;
    joints.b = pose.b;
    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.h
constexpr int kPumaShoulderRight = 0x01;
constexpr int kPumaElbowDown = 0x02;
constexpr int kPumaWristFlip = 0x04;
constexpr int kPumaSingular = 0x08;
constexpr int kPumaReach = 0x01;
constexpr double kPumaSingularFuzz = 0.000001;
constexpr double kPumaFlagFuzz = 0.000001;

// Source: linuxcnc/src/emc/kinematics/pumakins.c
// Mirrors pumaKinematicsForward(), with posemath RPY conversion from _posemath.c.
CncSimPose linuxcnc_puma_forward(const LinuxCncAxisJoints &joints,
                                 const LinuxCncPumaParameters &parameters,
                                 int *iflags) {
    const double s1 = std::sin(radians(joints.x));
    const double s2 = std::sin(radians(joints.y));
    const double s3 = std::sin(radians(joints.z));
    const double s4 = std::sin(radians(joints.a));
    const double s5 = std::sin(radians(joints.b));
    const double s6 = std::sin(radians(joints.c));
    const double c1 = std::cos(radians(joints.x));
    const double c2 = std::cos(radians(joints.y));
    const double c3 = std::cos(radians(joints.z));
    const double c4 = std::cos(radians(joints.a));
    const double c5 = std::cos(radians(joints.b));
    const double c6 = std::cos(radians(joints.c));

    const double s23 = c2 * s3 + s2 * c3;
    const double c23 = c2 * c3 - s2 * s3;

    double t1 = c4 * c5 * c6 - s4 * s6;
    double t2 = s23 * s5 * c6;
    double t3 = s4 * c5 * c6 + c4 * s6;
    double t4 = c23 * t1 - t2;
    double t5 = c23 * s5 * c6;

    RotationMatrix hom_rot{};
    hom_rot.x.x = c1 * t4 + s1 * t3;
    hom_rot.x.y = s1 * t4 - c1 * t3;
    hom_rot.x.z = -s23 * t1 - t5;

    t1 = -c4 * c5 * s6 - s4 * c6;
    t2 = s23 * s5 * s6;
    t3 = c4 * c6 - s4 * c5 * s6;
    t4 = c23 * t1 + t2;
    t5 = c23 * s5 * s6;

    hom_rot.y.x = c1 * t4 + s1 * t3;
    hom_rot.y.y = s1 * t4 - c1 * t3;
    hom_rot.y.z = -s23 * t1 + t5;

    t1 = c23 * c4 * s5 + s23 * c5;
    hom_rot.z.x = -c1 * t1 - s1 * s4 * s5;
    hom_rot.z.y = -s1 * t1 + c1 * s4 * s5;
    hom_rot.z.z = s23 * c4 * s5 - c23 * c5;

    t1 = parameters.a2 * c2 + parameters.a3 * c23 - parameters.d4 * s23;
    RtcpVector tran{};
    tran.x = c1 * t1 - parameters.d3 * s1;
    tran.y = s1 * t1 + parameters.d3 * c1;
    tran.z = -parameters.a3 * s23 - parameters.a2 * s2 - parameters.d4 * c23;

    int flags = 0;
    const double sum_sq = tran.x * tran.x + tran.y * tran.y - parameters.d3 * parameters.d3;
    const double k = (sum_sq + tran.z * tran.z - parameters.a2 * parameters.a2 -
                      parameters.a3 * parameters.a3 - parameters.d4 * parameters.d4) /
                     (2.0 * parameters.a2);
    if (std::fabs(radians(joints.x) - std::atan2(tran.y, tran.x) +
                  std::atan2(parameters.d3, -std::sqrt(sum_sq))) < kPumaFlagFuzz) {
        flags |= kPumaShoulderRight;
    }
    if (std::fabs(radians(joints.z) - std::atan2(parameters.a3, parameters.d4) +
                  std::atan2(k, -std::sqrt(parameters.a3 * parameters.a3 +
                                           parameters.d4 * parameters.d4 - k * k))) < kPumaFlagFuzz) {
        flags |= kPumaElbowDown;
    }
    t1 = -hom_rot.z.x * s1 + hom_rot.z.y * c1;
    t2 = -hom_rot.z.x * c1 * c23 - hom_rot.z.y * s1 * c23 + hom_rot.z.z * s23;
    if (std::fabs(t1) < kPumaSingularFuzz && std::fabs(t2) < kPumaSingularFuzz) {
        flags |= kPumaSingular;
    } else if (!(std::fabs(radians(joints.a) - std::atan2(t1, t2)) < kPumaFlagFuzz)) {
        flags |= kPumaWristFlip;
    }
    if (iflags) {
        *iflags = flags;
    }

    tran.x = tran.x + hom_rot.z.x * parameters.d6;
    tran.y = tran.y + hom_rot.z.y * parameters.d6;
    tran.z = tran.z + hom_rot.z.z * parameters.d6;
    return posemath_matrix_to_pose(tran, hom_rot);
}

// Source: linuxcnc/src/emc/kinematics/pumakins.c
// Mirrors pumaKinematicsInverse(), with posemath RPY conversion from _posemath.c.
LinuxCncAxisJoints linuxcnc_puma_inverse(const CncSimPose &pose,
                                         const LinuxCncAxisJoints &current_joints,
                                         const LinuxCncPumaParameters &parameters,
                                         int iflags,
                                         int *fflags) {
    if (fflags) {
        *fflags = 0;
    }

    RotationMatrix hom_rot = posemath_rpy_to_matrix(radians(pose.a),
                                                    radians(pose.b),
                                                    radians(pose.c));
    const double px = pose.x - parameters.d6 * hom_rot.z.x;
    const double py = pose.y - parameters.d6 * hom_rot.z.y;
    const double pz = pose.z - parameters.d6 * hom_rot.z.z;

    const double sum_sq = px * px + py * py - parameters.d3 * parameters.d3;
    double th1 = 0.0;
    if (iflags & kPumaShoulderRight) {
        th1 = std::atan2(py, px) - std::atan2(parameters.d3, -std::sqrt(sum_sq));
    } else {
        th1 = std::atan2(py, px) - std::atan2(parameters.d3, std::sqrt(sum_sq));
    }
    const double s1 = std::sin(th1);
    const double c1 = std::cos(th1);

    const double k = (sum_sq + pz * pz - parameters.a2 * parameters.a2 -
                      parameters.a3 * parameters.a3 - parameters.d4 * parameters.d4) /
                     (2.0 * parameters.a2);
    double th3 = 0.0;
    if (iflags & kPumaElbowDown) {
        th3 = std::atan2(parameters.a3, parameters.d4) -
              std::atan2(k, -std::sqrt(parameters.a3 * parameters.a3 +
                                       parameters.d4 * parameters.d4 - k * k));
    } else {
        th3 = std::atan2(parameters.a3, parameters.d4) -
              std::atan2(k, std::sqrt(parameters.a3 * parameters.a3 +
                                      parameters.d4 * parameters.d4 - k * k));
    }

    const double s3 = std::sin(th3);
    const double c3 = std::cos(th3);
    double t1 = (-parameters.a3 - parameters.a2 * c3) * pz +
                (c1 * px + s1 * py) * (parameters.a2 * s3 - parameters.d4);
    double t2 = (parameters.a2 * s3 - parameters.d4) * pz +
                (parameters.a3 + parameters.a2 * c3) * (c1 * px + s1 * py);
    const double t3 = pz * pz + (c1 * px + s1 * py) * (c1 * px + s1 * py);
    const double th23 = std::atan2(t1, t2);
    const double th2 = th23 - th3;
    const double s23 = t1 / t3;
    const double c23 = t2 / t3;

    t1 = -hom_rot.z.x * s1 + hom_rot.z.y * c1;
    t2 = -hom_rot.z.x * c1 * c23 - hom_rot.z.y * s1 * c23 + hom_rot.z.z * s23;
    double th4 = 0.0;
    if (std::fabs(t1) < kPumaSingularFuzz && std::fabs(t2) < kPumaSingularFuzz) {
        if (fflags) {
            *fflags |= kPumaReach;
        }
        th4 = radians(current_joints.a);
    } else {
        th4 = std::atan2(t1, t2);
    }
    const double s4 = std::sin(th4);
    const double c4 = std::cos(th4);

    const double s5 = hom_rot.z.z * (s23 * c4) -
                      hom_rot.z.x * (c1 * c23 * c4 + s1 * s4) -
                      hom_rot.z.y * (s1 * c23 * c4 - c1 * s4);
    const double c5 = -hom_rot.z.x * (c1 * s23) -
                      hom_rot.z.y * (s1 * s23) -
                      hom_rot.z.z * c23;
    double th5 = std::atan2(s5, c5);

    const double s6 = hom_rot.x.z * (s23 * s4) -
                      hom_rot.x.x * (c1 * c23 * s4 - s1 * c4) -
                      hom_rot.x.y * (s1 * c23 * s4 + c1 * c4);
    const double c6 = hom_rot.x.x * ((c1 * c23 * c4 + s1 * s4) * c5 - c1 * s23 * s5) +
                      hom_rot.x.y * ((s1 * c23 * c4 - c1 * s4) * c5 - s1 * s23 * s5) -
                      hom_rot.x.z * (s23 * c4 * c5 + c23 * s5);
    double th6 = std::atan2(s6, c6);

    if (iflags & kPumaWristFlip) {
        th4 = th4 + kPi;
        th5 = -th5;
        th6 = th6 + kPi;
    }

    LinuxCncAxisJoints joints{};
    joints.x = degrees(th1);
    joints.y = degrees(th2);
    joints.z = degrees(th3);
    joints.a = degrees(th4);
    joints.b = degrees(th5);
    joints.c = degrees(th6);
    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;
}

// 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) {
    const double raw_joints[6] = {
        joints.x,
        joints.y,
        joints.z,
        joints.a,
        joints.b,
        joints.c,
    };
    double joint_radians[6] = {};
    for (int i = 0; i < 6; ++i) {
        joint_radians[i] = radians(raw_joints[i]);
        if (i != 0 && parameters.unrotate[i]) {
            joint_radians[i] -= parameters.unrotate[i] * joint_radians[i - 1];
        }
    }

    const HomogeneousTransform total = linuxcnc_genser_build_pose(joint_radians, parameters);
    return posemath_matrix_to_pose(total.tran, total.rot);
}

// 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) {
    const double estimate_degrees[6] = {
        joint_estimate.x,
        joint_estimate.y,
        joint_estimate.z,
        joint_estimate.a,
        joint_estimate.b,
        joint_estimate.c,
    };
    double jest[6] = {};
    for (int link = 0; link < 6; ++link) {
        jest[link] = radians(estimate_degrees[link]);
    }

    HomogeneousTransform target{};
    target.tran = {pose.x, pose.y, pose.z};
    target.rot = posemath_rpy_to_matrix(radians(pose.a), radians(pose.b), radians(pose.c));

    int completed_iterations = 0;
    for (; completed_iterations < max_iterations; ++completed_iterations) {
        double jfwd[6][6] = {};
        double jinv[6][6] = {};
        HomogeneousTransform t_l_0{};
        if (!linuxcnc_genser_compute_jfwd(jest, parameters, jfwd, &t_l_0) ||
            !linuxcnc_genser_compute_jinv(jfwd, jinv)) {
            break;
        }

        const HomogeneousTransform pest = linuxcnc_genser_build_pose(jest, parameters);
        const HomogeneousTransform pestinv = linuxcnc_pose_inverse(pest);
        const HomogeneousTransform tdelta = multiply_transform(pestinv, target);

        const Quaternion pest_quat = linuxcnc_matrix_to_quaternion(pest.rot);
        RtcpVector cart = linuxcnc_quaternion_rotate_vector(pest_quat, tdelta.tran);
        double dvw[6] = {};
        dvw[0] = cart.x;
        dvw[1] = cart.y;
        dvw[2] = cart.z;

        const Quaternion tdelta_quat = linuxcnc_matrix_to_quaternion(tdelta.rot);
        const RtcpVector rvec = linuxcnc_quaternion_to_rvec(tdelta_quat);
        cart = linuxcnc_quaternion_rotate_vector(pest_quat, rvec);
        dvw[3] = cart.x;
        dvw[4] = cart.y;
        dvw[5] = cart.z;

        double dj[6] = {};
        multiply_6x6_vector(jinv, dvw, dj);

        int smalls = 0;
        for (int link = 0; link < 6; ++link) {
            if (go_small(dj[link])) {
                ++smalls;
            }
        }
        if (smalls == 6) {
            if (iterations) {
                *iterations = completed_iterations;
            }
            LinuxCncAxisJoints joints{};
            double out_degrees[6] = {};
            for (int link = 0; link < 6; ++link) {
                out_degrees[link] = degrees(jest[link]);
                if (link != 0 && parameters.unrotate[link]) {
                    out_degrees[link] += parameters.unrotate[link] * out_degrees[link - 1];
                }
            }
            joints.x = out_degrees[0];
            joints.y = out_degrees[1];
            joints.z = out_degrees[2];
            joints.a = out_degrees[3];
            joints.b = out_degrees[4];
            joints.c = out_degrees[5];
            joints.u = pose.u;
            joints.v = pose.v;
            joints.w = pose.w;
            return joints;
        }
        for (int link = 0; link < 6; ++link) {
            jest[link] += dj[link];
        }
    }

    if (iterations) {
        *iterations = completed_iterations;
    }
    LinuxCncAxisJoints joints{};
    joints.x = degrees(jest[0]);
    joints.y = degrees(jest[1]);
    joints.z = degrees(jest[2]);
    joints.a = degrees(jest[3]);
    joints.b = degrees(jest[4]);
    joints.c = degrees(jest[5]);
    joints.u = pose.u;
    joints.v = pose.v;
    joints.w = pose.w;
    return joints;
}