#include "sim_kinematics.h"

#include <math.h>
#include <stddef.h>
#include <string.h>
#include "posemath.h"

#define DEG_TO_RAD 0.017453292519943295769236907684886
#define RAD_TO_DEG 57.295779513082320876798154814105

static uint64_t fnv1a64(const unsigned char *bytes, size_t length) {
    uint64_t hash = UINT64_C(1469598103934665603);
    size_t index;
    for (index = 0; index < length; ++index) { hash ^= bytes[index]; hash *= UINT64_C(1099511628211); }
    return hash;
}

static double hash_normalize(double value) {
    return isfinite(value) ? nearbyint(value * 1000000000.0) / 1000000000.0 : value;
}

/* The five-leg Newton solve can differ by a few ulps between native and
 * wasm32 math libraries.  Keep the public pose/info boundary deterministic
 * without changing the convergence test itself. */
static double pentapod_canonical(double value) {
    double rounded = nearbyint(value * 1000000.0) / 1000000.0;
    return fabs(rounded) < 0.0000005 ? 0.0 : rounded;
}

static int finite_values(const double *values, size_t count) {
    size_t index;
    if (values == 0) return 0;
    for (index = 0; index < count; ++index) if (!isfinite(values[index])) return 0;
    return 1;
}

int sim_kinematics_validate_pose(const sim_posemath_pose *pose) {
    return pose != 0 && finite_values(&pose->tran.x, 9) ? SIM_KIN_OK : SIM_KIN_ERR_NONFINITE;
}

static int axis_index(char letter) {
    switch (letter) {
        case 'X': return 0; case 'Y': return 1; case 'Z': return 2;
        case 'A': return 3; case 'B': return 4; case 'C': return 5;
        case 'U': return 6; case 'V': return 7; case 'W': return 8;
        default: return -1;
    }
}

static int has_coordinate(const char *coordinates, char wanted) {
    size_t index;
    for (index = 0; coordinates[index] != '\0'; ++index) {
        char value = coordinates[index];
        if (value >= 'a' && value <= 'z') value = (char)(value - 'a' + 'A');
        if (value == wanted) return 1;
    }
    return 0;
}

static int effective_mode(const sim_kinematics_config *config) {
    if (config->mode != SIM_KIN_MODE_DEFAULT) return config->mode;
    return config->type == SIM_KIN_TRIVKINS ? SIM_KIN_MODE_IDENTITY : SIM_KIN_MODE_BOTH;
}

static uint32_t kinematics_joint_count(const sim_kinematics_config *config) {
    return config->kinematics_joint_count == 0 ? config->joint_count : config->kinematics_joint_count;
}

static int known_type(int type) {
    switch (type) {
        case SIM_KIN_TRIVKINS:
        case SIM_KIN_XYZAC_TRT:
        case SIM_KIN_XYZBC_TRT:
        case SIM_KIN_XYZBCW:
        case SIM_KIN_COREXY:
        case SIM_KIN_ROSE:
        case SIM_KIN_MAXKINS:
        case SIM_KIN_ROTATEKINS:
        case SIM_KIN_5AXISKINS:
        case SIM_KIN_SCARA:
        case SIM_KIN_PUMA:
        case SIM_KIN_SCORBOT:
        case SIM_KIN_GENSER:
        case SIM_KIN_LINEAR_DELTA:
        case SIM_KIN_ROTARY_DELTA:
        case SIM_KIN_TRIPOD:
        case SIM_KIN_GENHEX:
        case SIM_KIN_PENTAPOD:
            return 1;
        default:
            return 0;
    }
}

static void initialize_info(sim_kinematics_result_info *info, uint64_t forward_flags, uint64_t inverse_flags) {
    if (info == 0) return;
    info->forward_flags = forward_flags;
    info->inverse_flags = inverse_flags;
    info->residual = 0.0;
    info->iterations = 0;
    info->branch_id = 0;
}

static int config_axis(const sim_kinematics *kinematics, int axis) {
    return kinematics->axis_joint[axis];
}

static int valid_config(const sim_kinematics_config *config) {
    const char *coordinates;
    size_t length;
    size_t index;
    uint32_t kinematics_count;
    if (config == 0 || config->joint_count < 1 || config->joint_count > SIM_KIN_MAX_JOINTS) return SIM_KIN_ERR_CONFIGURATION;
    kinematics_count = kinematics_joint_count(config);
    if (kinematics_count < 1 || kinematics_count > config->joint_count) return SIM_KIN_ERR_CONFIGURATION;
    if (config->extra_joint_count != 0 && config->extra_joint_count != config->joint_count - kinematics_count) return SIM_KIN_ERR_CONFIGURATION;
    if (!known_type(config->type)) return SIM_KIN_ERR_UNKNOWN_TYPE;
    if (config->mode < SIM_KIN_MODE_DEFAULT || config->mode > SIM_KIN_MODE_BOTH) return SIM_KIN_ERR_CONFIGURATION;
    if (!finite_values(&config->x_rot_point, 19) || !finite_values(&config->delta_radius, 9)) return SIM_KIN_ERR_NONFINITE;
    if (config->max_iterations > 100000u) return SIM_KIN_ERR_CONFIGURATION;
    if (config->convergence_tolerance < 0.0 || !isfinite(config->convergence_tolerance)) return SIM_KIN_ERR_CONFIGURATION;
    coordinates = config->coordinates[0] == '\0' ? "XYZABCUVW" : config->coordinates;
    length = strlen(coordinates);
    if (length == 0 || length > SIM_KIN_MAX_JOINTS || length > kinematics_count) return SIM_KIN_ERR_CONFIGURATION;
    for (index = 0; index < length; ++index) {
        int axis = axis_index((char)(coordinates[index] >= 'a' && coordinates[index] <= 'z' ? coordinates[index] - 'a' + 'A' : coordinates[index]));
        if (axis < 0) return SIM_KIN_ERR_CONFIGURATION;
        (void)axis; /* Duplicate axis letters are valid for gantry/extra-joint mappings. */
    }
    if (config->type == SIM_KIN_XYZAC_TRT && (!has_coordinate(coordinates, 'X') || !has_coordinate(coordinates, 'Y') || !has_coordinate(coordinates, 'Z') || !has_coordinate(coordinates, 'A') || !has_coordinate(coordinates, 'C'))) return SIM_KIN_ERR_CONFIGURATION;
    if (config->type == SIM_KIN_XYZBC_TRT && (!has_coordinate(coordinates, 'X') || !has_coordinate(coordinates, 'Y') || !has_coordinate(coordinates, 'Z') || !has_coordinate(coordinates, 'B') || !has_coordinate(coordinates, 'C'))) return SIM_KIN_ERR_CONFIGURATION;
    if ((config->type == SIM_KIN_XYZBCW || config->type == SIM_KIN_5AXISKINS) && (!has_coordinate(coordinates, 'X') || !has_coordinate(coordinates, 'Y') || !has_coordinate(coordinates, 'Z') || !has_coordinate(coordinates, 'B') || !has_coordinate(coordinates, 'C') || !has_coordinate(coordinates, 'W'))) return SIM_KIN_ERR_CONFIGURATION;
    if ((config->type == SIM_KIN_COREXY || config->type == SIM_KIN_ROSE ||
         config->type == SIM_KIN_MAXKINS || config->type == SIM_KIN_ROTATEKINS) &&
        (!has_coordinate(coordinates, 'X') || !has_coordinate(coordinates, 'Y') ||
         !has_coordinate(coordinates, 'Z'))) return SIM_KIN_ERR_CONFIGURATION;
    if (config->type == SIM_KIN_ROSE && kinematics_count != 3) return SIM_KIN_ERR_CONFIGURATION;
    if (config->type == SIM_KIN_SCARA && (kinematics_count != 6 || !has_coordinate(coordinates, 'C') || !has_coordinate(coordinates, 'A') || !has_coordinate(coordinates, 'B'))) return SIM_KIN_ERR_CONFIGURATION;
    if (config->type == SIM_KIN_PUMA && (kinematics_count != 6 || !has_coordinate(coordinates, 'A') || !has_coordinate(coordinates, 'B') || !has_coordinate(coordinates, 'C'))) return SIM_KIN_ERR_CONFIGURATION;
    if (config->type == SIM_KIN_SCORBOT && (kinematics_count != 5 || !has_coordinate(coordinates, 'A') || !has_coordinate(coordinates, 'B'))) return SIM_KIN_ERR_CONFIGURATION;
    if (config->type == SIM_KIN_GENSER && (kinematics_count != 6 || !has_coordinate(coordinates, 'X') || !has_coordinate(coordinates, 'Y') || !has_coordinate(coordinates, 'Z') || !has_coordinate(coordinates, 'A') || !has_coordinate(coordinates, 'B') || !has_coordinate(coordinates, 'C') || (config->dh_joint_count != 0 && config->dh_joint_count != 6))) return SIM_KIN_ERR_CONFIGURATION;
    if (config->type == SIM_KIN_GENSER && (!finite_values(config->dh_a, 6) || !finite_values(config->dh_alpha, 6) || !finite_values(config->dh_d, 6))) return SIM_KIN_ERR_NONFINITE;
    if (config->type == SIM_KIN_LINEAR_DELTA) {
        if (kinematics_count < 3 || !has_coordinate(coordinates, 'X') || !has_coordinate(coordinates, 'Y') || !has_coordinate(coordinates, 'Z')) return SIM_KIN_ERR_CONFIGURATION;
        /* A completely zero geometry means "use the LinuxCNC defaults" and
         * is filled in by init.  Partially specified or non-positive geometry
         * is always rejected instead of allowing a divide-by-zero later. */
        if (!((config->delta_radius == 0.0) && (config->delta_rod_length == 0.0)) &&
            (config->delta_radius <= 0.0 || config->delta_rod_length <= 0.0)) return SIM_KIN_ERR_CONFIGURATION;
    }
    if (config->type == SIM_KIN_ROTARY_DELTA) {
        if (kinematics_count < 3 || !has_coordinate(coordinates, 'X') || !has_coordinate(coordinates, 'Y') || !has_coordinate(coordinates, 'Z')) return SIM_KIN_ERR_CONFIGURATION;
        if (!((config->rotary_platform_radius == 0.0) && (config->rotary_thigh_length == 0.0) &&
              (config->rotary_shin_length == 0.0) && (config->rotary_foot_radius == 0.0)) &&
            (config->rotary_platform_radius <= 0.0 || config->rotary_thigh_length <= 0.0 ||
             config->rotary_shin_length <= 0.0 || config->rotary_foot_radius <= 0.0)) return SIM_KIN_ERR_CONFIGURATION;
    }
    if (config->type == SIM_KIN_TRIPOD) {
        if (kinematics_count < 3 || !has_coordinate(coordinates, 'X') || !has_coordinate(coordinates, 'Y') || !has_coordinate(coordinates, 'Z')) return SIM_KIN_ERR_CONFIGURATION;
        if (!((config->tripod_bx == 0.0) && (config->tripod_cx == 0.0) && (config->tripod_cy == 0.0)) &&
            (fabs(config->tripod_bx) < 1e-12 || fabs(config->tripod_cy) < 1e-12)) return SIM_KIN_ERR_CONFIGURATION;
    }
    if (config->type == SIM_KIN_GENHEX) {
        if (kinematics_count != SIM_KIN_GENHEX_STRUTS ||
            !has_coordinate(coordinates, 'X') || !has_coordinate(coordinates, 'Y') ||
            !has_coordinate(coordinates, 'Z') || !has_coordinate(coordinates, 'A') ||
            !has_coordinate(coordinates, 'B') || !has_coordinate(coordinates, 'C')) return SIM_KIN_ERR_CONFIGURATION;
        if (config->genhex_iteration_limit > 100000u) return SIM_KIN_ERR_CONFIGURATION;
        if (config->genhex_convergence_tolerance < 0.0 || !isfinite(config->genhex_convergence_tolerance) ||
            config->genhex_max_error < 0.0 || !isfinite(config->genhex_max_error) ||
            !finite_values(config->genhex_initial_pose, 6)) return SIM_KIN_ERR_CONFIGURATION;
        for (index = 0; index < SIM_KIN_GENHEX_STRUTS; ++index) {
            if (!finite_values(config->genhex_base[index], 3) || !finite_values(config->genhex_platform[index], 3) ||
                !finite_values(config->genhex_base_axis[index], 3) || !finite_values(config->genhex_platform_axis[index], 3)) return SIM_KIN_ERR_NONFINITE;
        }
    }
    if (config->type == SIM_KIN_PENTAPOD) {
        if (kinematics_count != SIM_KIN_PENTA_STRUTS || !has_coordinate(coordinates, 'X') || !has_coordinate(coordinates, 'Y') || !has_coordinate(coordinates, 'Z') || !has_coordinate(coordinates, 'A') || !has_coordinate(coordinates, 'B')) return SIM_KIN_ERR_CONFIGURATION;
        if (!isfinite(config->penta_tool_offset)) return SIM_KIN_ERR_NONFINITE;
        if (config->penta_iteration_limit > 100000u || config->penta_convergence_tolerance < 0.0 || !isfinite(config->penta_convergence_tolerance) || config->penta_max_error < 0.0 || !isfinite(config->penta_max_error) || !finite_values(config->penta_initial_pose, 5)) return SIM_KIN_ERR_CONFIGURATION;
        for (index = 0; index < SIM_KIN_PENTA_STRUTS; ++index) if (!finite_values(config->penta_base[index], 3) || !isfinite(config->penta_effector_radius[index]) || !isfinite(config->penta_effector_z[index]) || ((config->penta_effector_radius[index] < 0.0) || (config->penta_effector_radius[index] == 0.0 && config->penta_effector_z[index] != 0.0))) return SIM_KIN_ERR_CONFIGURATION;
    }
    for (index = 0; index < kinematics_count; ++index) {
        if (config->limit_enabled[index] && (!isfinite(config->joint_min[index]) || !isfinite(config->joint_max[index]) || config->joint_min[index] > config->joint_max[index])) return SIM_KIN_ERR_CONFIGURATION;
    }
    return SIM_KIN_OK;
}

static int in_limits(const sim_kinematics *kinematics, const double *joints) {
    uint32_t index;
    for (index = 0; index < kinematics_joint_count(&kinematics->config); ++index)
        if (kinematics->config.limit_enabled[index] && (joints[index] < kinematics->config.joint_min[index] || joints[index] > kinematics->config.joint_max[index])) return SIM_KIN_ERR_RANGE;
    return SIM_KIN_OK;
}

static void map_joints(sim_kinematics *kinematics) {
    size_t index;
    for (index = 0; index < SIM_KIN_MAX_JOINTS; ++index) kinematics->axis_joint[index] = -1;
    for (index = 0; index < kinematics_joint_count(&kinematics->config); ++index) {
        int axis = axis_index((char)(kinematics->config.coordinates[index] >= 'a' && kinematics->config.coordinates[index] <= 'z' ? kinematics->config.coordinates[index] - 'a' + 'A' : kinematics->config.coordinates[index]));
        if (axis >= 0 && kinematics->axis_joint[axis] < 0) kinematics->axis_joint[axis] = (int)index;
    }
}

int sim_kinematics_init(sim_kinematics *kinematics, const sim_kinematics_config *config) {
    int result;
    int geometry_zero;
    if (kinematics == 0 || config == 0) return SIM_KIN_ERR_ARGUMENT;
    result = valid_config(config);
    if (result != SIM_KIN_OK) return result;
    memset(kinematics, 0, sizeof(*kinematics));
    kinematics->config = *config;
    if (kinematics->config.coordinates[0] == '\0') strcpy(kinematics->config.coordinates, "XYZABCUVW");
    if (kinematics->config.kinematics_joint_count == 0) kinematics->config.kinematics_joint_count = kinematics->config.joint_count;
    if (kinematics->config.extra_joint_count == 0) kinematics->config.extra_joint_count = kinematics->config.joint_count - kinematics->config.kinematics_joint_count;
    geometry_zero = kinematics->config.scara_d1 == 0.0 && kinematics->config.scara_d2 == 0.0 &&
                    kinematics->config.scara_d3 == 0.0 && kinematics->config.scara_d4 == 0.0 &&
                    kinematics->config.scara_d5 == 0.0 && kinematics->config.scara_d6 == 0.0;
    if (kinematics->config.type == SIM_KIN_SCARA && geometry_zero) {
        kinematics->config.scara_d1 = 490.0; kinematics->config.scara_d2 = 340.0;
        kinematics->config.scara_d3 = 50.0; kinematics->config.scara_d4 = 250.0;
        kinematics->config.scara_d5 = 50.0; kinematics->config.scara_d6 = 50.0;
    }
    if (kinematics->config.type == SIM_KIN_PUMA && kinematics->config.puma_a2 == 0.0 &&
        kinematics->config.puma_a3 == 0.0 && kinematics->config.puma_d3 == 0.0 &&
        kinematics->config.puma_d4 == 0.0 && kinematics->config.puma_d6 == 0.0) {
        kinematics->config.puma_a2 = 300.0; kinematics->config.puma_a3 = 50.0;
        kinematics->config.puma_d3 = 70.0; kinematics->config.puma_d4 = 400.0;
        kinematics->config.puma_d6 = 70.0;
    }
    if (kinematics->config.type == SIM_KIN_GENSER && kinematics->config.dh_joint_count == 0) kinematics->config.dh_joint_count = 6;
    if (kinematics->config.type == SIM_KIN_LINEAR_DELTA && kinematics->config.delta_radius == 0.0 && kinematics->config.delta_rod_length == 0.0) {
        kinematics->config.delta_radius = 130.25; kinematics->config.delta_rod_length = 269.0;
    }
    if (kinematics->config.type == SIM_KIN_ROTARY_DELTA && kinematics->config.rotary_platform_radius == 0.0 && kinematics->config.rotary_thigh_length == 0.0 && kinematics->config.rotary_shin_length == 0.0 && kinematics->config.rotary_foot_radius == 0.0) {
        kinematics->config.rotary_platform_radius = 10.0; kinematics->config.rotary_thigh_length = 10.0; kinematics->config.rotary_shin_length = 14.0; kinematics->config.rotary_foot_radius = 6.0;
    }
    if (kinematics->config.type == SIM_KIN_TRIPOD && kinematics->config.tripod_bx == 0.0 && kinematics->config.tripod_cx == 0.0 && kinematics->config.tripod_cy == 0.0) {
        kinematics->config.tripod_bx = 10.0; kinematics->config.tripod_cx = 5.0; kinematics->config.tripod_cy = 7.071;
    }
    if (kinematics->config.type == SIM_KIN_GENHEX) {
        size_t index;
        int all_zero = 1;
        static const double base[SIM_KIN_GENHEX_STRUTS][3] = {
            {-22.950, 13.250, 0.0}, {22.950, 13.250, 0.0}, {22.950, 13.250, 0.0},
            {0.0, -26.500, 0.0}, {0.0, -26.500, 0.0}, {-22.950, 13.250, 0.0}
        };
        static const double platform[SIM_KIN_GENHEX_STRUTS][3] = {
            {-1.000, 11.500, 0.0}, {1.000, 11.500, 0.0}, {10.459, -4.884, 0.0},
            {9.459, -6.616, 0.0}, {-9.459, -6.616, 0.0}, {-10.459, -4.884, 0.0}
        };
        static const double base_axis[SIM_KIN_GENHEX_STRUTS][3] = {
            {0.707107, 0.0, 0.707107}, {0.0, -0.707107, 0.707107}, {-0.707107, 0.0, 0.707107},
            {-0.707107, 0.0, 0.707107}, {0.0, 0.707107, 0.707107}, {0.707107, 0.0, 0.707107}
        };
        static const double platform_axis[SIM_KIN_GENHEX_STRUTS][3] = {
            {-1.0, 0.0, 0.0}, {0.866025, 0.5, 0.0}, {0.866025, 0.5, 0.0},
            {0.866025, -0.5, 0.0}, {0.866025, -0.5, 0.0}, {-1.0, 0.0, 0.0}
        };
        for (index = 0; index < SIM_KIN_GENHEX_STRUTS; ++index) {
            size_t axis;
            for (axis = 0; axis < 3; ++axis) if (kinematics->config.genhex_base[index][axis] != 0.0 || kinematics->config.genhex_platform[index][axis] != 0.0) all_zero = 0;
        }
        if (all_zero) {
            memcpy(kinematics->config.genhex_base, base, sizeof(base));
            memcpy(kinematics->config.genhex_platform, platform, sizeof(platform));
            memcpy(kinematics->config.genhex_base_axis, base_axis, sizeof(base_axis));
            memcpy(kinematics->config.genhex_platform_axis, platform_axis, sizeof(platform_axis));
        }
        if (kinematics->config.genhex_iteration_limit == 0) kinematics->config.genhex_iteration_limit = 120;
        if (kinematics->config.genhex_convergence_tolerance == 0.0) kinematics->config.genhex_convergence_tolerance = 1e-9;
        if (kinematics->config.genhex_max_error == 0.0) kinematics->config.genhex_max_error = 500.0;
        if (kinematics->config.genhex_initial_pose[2] == 0.0) kinematics->config.genhex_initial_pose[2] = 50.0;
    }
    if (kinematics->config.type == SIM_KIN_PENTAPOD) {
        static const double base[SIM_KIN_PENTA_STRUTS][3] = {{-418.0, 500.0, -140.0}, {418.0, 500.0, -140.0}, {0.0, 500.0, -310.0}, {418.0, 500.0, 510.0}, {-418.0, 500.0, 510.0}};
        static const double radius[SIM_KIN_PENTA_STRUTS] = {80.30, 80.30, 80.30, 80.30, 80.30};
        static const double effector_z[SIM_KIN_PENTA_STRUTS] = {87.0, 114.5, 140.5, 232.5, 258.5};
        int all_zero = 1;
        size_t index;
        for (index = 0; index < SIM_KIN_PENTA_STRUTS; ++index) {
            if (kinematics->config.penta_base[index][0] != 0.0 || kinematics->config.penta_base[index][1] != 0.0 || kinematics->config.penta_base[index][2] != 0.0 ||
                kinematics->config.penta_effector_radius[index] != 0.0 || kinematics->config.penta_effector_z[index] != 0.0) all_zero = 0;
        }
        if (all_zero) { memcpy(kinematics->config.penta_base, base, sizeof(base)); memcpy(kinematics->config.penta_effector_radius, radius, sizeof(radius)); memcpy(kinematics->config.penta_effector_z, effector_z, sizeof(effector_z)); }
        if (kinematics->config.penta_iteration_limit == 0) kinematics->config.penta_iteration_limit = 120;
        if (kinematics->config.penta_convergence_tolerance == 0.0) kinematics->config.penta_convergence_tolerance = 1e-9;
        if (kinematics->config.penta_max_error == 0.0) kinematics->config.penta_max_error = 1000.0;
        if (kinematics->config.penta_initial_pose[1] == 0.0) kinematics->config.penta_initial_pose[1] = 500.0;
        if (kinematics->config.penta_initial_pose[2] == 0.0) kinematics->config.penta_initial_pose[2] = 300.0;
    }
    map_joints(kinematics);
    kinematics->configured = 1;
    return SIM_KIN_OK;
}

static void zero_pose(sim_posemath_pose *pose) { memset(pose, 0, sizeof(*pose)); }

static double joint(const sim_kinematics *k, const double *joints, int axis) {
    int index = config_axis(k, axis);
    return index < 0 ? 0.0 : joints[index];
}

static void set_optional_axes(const sim_kinematics *k, const double *joints, sim_posemath_pose *pose) {
    pose->a = joint(k, joints, 3); pose->b = joint(k, joints, 4); pose->c = joint(k, joints, 5);
    pose->u = joint(k, joints, 6); pose->v = joint(k, joints, 7); pose->w = joint(k, joints, 8);
}

static void set_joint(double *joints, const sim_kinematics *k, int axis, double value);

static int scara_forward(const sim_kinematics *k, const double *joints, sim_posemath_pose *pose) {
    double a0 = joint(k, joints, 0) * DEG_TO_RAD;
    double a1 = joint(k, joints, 1) * DEG_TO_RAD + a0;
    double a3 = joint(k, joints, 5) * DEG_TO_RAD + a1;
    pose->tran.x = k->config.scara_d2 * cos(a0) + k->config.scara_d4 * cos(a1) + k->config.scara_d6 * cos(a3);
    pose->tran.y = k->config.scara_d2 * sin(a0) + k->config.scara_d4 * sin(a1) + k->config.scara_d6 * sin(a3);
    pose->tran.z = k->config.scara_d1 + k->config.scara_d3 - joint(k, joints, 2) - k->config.scara_d5;
    pose->c = a3 * RAD_TO_DEG;
    pose->a = joint(k, joints, 3); pose->b = joint(k, joints, 4);
    return SIM_KIN_OK;
}

static int scara_inverse(const sim_kinematics *k, const sim_posemath_pose *pose, double *joints, uint64_t flags, uint32_t *branch) {
    double a3 = pose->c * DEG_TO_RAD;
    double xt = pose->tran.x - k->config.scara_d6 * cos(a3);
    double yt = pose->tran.y - k->config.scara_d6 * sin(a3);
    double rsq = xt * xt + yt * yt;
    double cc = (rsq - k->config.scara_d2 * k->config.scara_d2 - k->config.scara_d4 * k->config.scara_d4) /
                (2.0 * k->config.scara_d2 * k->config.scara_d4);
    double q1;
    double q0;
    if (cc < -1.0 - 1e-12 || cc > 1.0 + 1e-12) return SIM_KIN_ERR_RANGE;
    if (cc < -1.0) cc = -1.0;
    if (cc > 1.0) cc = 1.0;
    q1 = acos(cc);
    if (flags & UINT64_C(1)) q1 = -q1;
    q0 = atan2(yt, xt) - atan2(k->config.scara_d4 * sin(q1), k->config.scara_d2 + k->config.scara_d4 * cos(q1));
    set_joint(joints, k, 0, q0 * RAD_TO_DEG);
    set_joint(joints, k, 1, q1 * RAD_TO_DEG);
    set_joint(joints, k, 2, k->config.scara_d1 + k->config.scara_d3 - k->config.scara_d5 - pose->tran.z);
    set_joint(joints, k, 5, pose->c - (q0 + q1) * RAD_TO_DEG);
    set_joint(joints, k, 3, pose->a); set_joint(joints, k, 4, pose->b);
    if (branch != 0) *branch = (q1 < 0.0) ? 1U : 0U;
    return SIM_KIN_OK;
}

static int scorbot_forward(const sim_kinematics *k, const double *joints, sim_posemath_pose *pose) {
    double j0 = joint(k, joints, 0) * DEG_TO_RAD;
    double j1 = joint(k, joints, 1) * DEG_TO_RAD;
    double j2 = joint(k, joints, 2) * DEG_TO_RAD;
    double radius = 16.0 + 221.0 * cos(j1) + 221.0 * cos(j2);
    pose->tran.x = radius * cos(j0);
    pose->tran.y = radius * sin(j0);
    pose->tran.z = 140.0 + 221.0 * sin(j1) + 221.0 * sin(j2);
    pose->a = joint(k, joints, 3); pose->b = joint(k, joints, 4);
    return SIM_KIN_OK;
}

static int scorbot_inverse(const sim_kinematics *k, const sim_posemath_pose *pose, double *joints) {
    double r = hypot(pose->tran.x, pose->tran.y) - 16.0;
    double z = pose->tran.z - 140.0;
    double distance = hypot(r, z);
    double angle;
    double j1;
    if (distance < 1e-12 || distance > 442.0 + 1e-9) return SIM_KIN_ERR_RANGE;
    angle = acos(r / distance) * (z < 0.0 ? -1.0 : 1.0);
    j1 = angle + acos(distance / 442.0);
    set_joint(joints, k, 0, atan2(pose->tran.y, pose->tran.x) * RAD_TO_DEG);
    set_joint(joints, k, 1, j1 * RAD_TO_DEG);
    set_joint(joints, k, 2, -asin((221.0 * sin(j1) - z) / 221.0) * RAD_TO_DEG);
    set_joint(joints, k, 3, pose->a); set_joint(joints, k, 4, pose->b);
    return SIM_KIN_OK;
}

static int puma_forward(const sim_kinematics *k, const double *joint_values, sim_posemath_pose *pose, uint32_t *branch) {
    double q0 = joint(k, joint_values, 0) * DEG_TO_RAD;
    double q1 = joint(k, joint_values, 1) * DEG_TO_RAD;
    double q2 = joint(k, joint_values, 2) * DEG_TO_RAD;
    double q3 = joint(k, joint_values, 3) * DEG_TO_RAD;
    double q4 = joint(k, joint_values, 4) * DEG_TO_RAD;
    double q5 = joint(k, joint_values, 5) * DEG_TO_RAD;
    double s1 = sin(q0), s2 = sin(q1), s3 = sin(q2), s4 = sin(q3), s5 = sin(q4), s6 = sin(q5);
    double c1 = cos(q0), c2 = cos(q1), c3 = cos(q2), c4 = cos(q3), c5 = cos(q4), c6 = cos(q5);
    double s23 = c2 * s3 + s2 * c3;
    double c23 = c2 * c3 - s2 * s3;
    double t1, t2, t3, t4, t5;
    PmHomogeneous hom = {0};
    PmRpy rpy = {0};
    t1 = c4 * c5 * c6 - s4 * s6; t2 = s23 * s5 * c6; t3 = s4 * c5 * c6 + c4 * s6; t4 = c23 * t1 - t2; t5 = c23 * s5 * c6;
    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 = k->config.puma_a2 * c2 + k->config.puma_a3 * c23 - k->config.puma_d4 * s23;
    hom.tran.x = c1 * t1 - k->config.puma_d3 * s1;
    hom.tran.y = s1 * t1 + k->config.puma_d3 * c1;
    hom.tran.z = -k->config.puma_a3 * s23 - k->config.puma_a2 * s2 - k->config.puma_d4 * c23;
    hom.tran.x += hom.rot.z.x * k->config.puma_d6;
    hom.tran.y += hom.rot.z.y * k->config.puma_d6;
    hom.tran.z += hom.rot.z.z * k->config.puma_d6;
    if (pmMatRpyConvert(&hom.rot, &rpy) != 0) return SIM_KIN_ERR_SINGULAR;
    pose->tran.x = hom.tran.x; pose->tran.y = hom.tran.y; pose->tran.z = hom.tran.z;
    pose->a = rpy.r * RAD_TO_DEG; pose->b = rpy.p * RAD_TO_DEG; pose->c = rpy.y * RAD_TO_DEG;
    if (branch != 0) *branch = (fabs(s5) < 1e-9) ? 8U : 0U;
    return SIM_KIN_OK;
}

static int puma_inverse(const sim_kinematics *k, const sim_posemath_pose *pose, double *joints, uint64_t flags, uint32_t *branch) {
    PmRotationMatrix rot = {0};
    PmRpy rpy = {pose->a * DEG_TO_RAD, pose->b * DEG_TO_RAD, pose->c * DEG_TO_RAD};
    double px, py, pz, sum_sq, kterm, th1, th2, th3, th23, th4, th5, th6;
    double s1, c1, s3, c3, s23, c23, s4, c4, s5, c5, s6, c6, t1, t2, t3;
    if (pmRpyMatConvert(&rpy, &rot) != 0) return SIM_KIN_ERR_SINGULAR;
    px = pose->tran.x - k->config.puma_d6 * rot.z.x;
    py = pose->tran.y - k->config.puma_d6 * rot.z.y;
    pz = pose->tran.z - k->config.puma_d6 * rot.z.z;
    sum_sq = px * px + py * py - k->config.puma_d3 * k->config.puma_d3;
    if (sum_sq < -1e-9) return SIM_KIN_ERR_RANGE;
    if (sum_sq < 0.0) sum_sq = 0.0;
    th1 = atan2(py, px) - atan2(k->config.puma_d3, (flags & 1U) ? -sqrt(sum_sq) : sqrt(sum_sq));
    s1 = sin(th1); c1 = cos(th1);
    kterm = (sum_sq + pz * pz - k->config.puma_a2 * k->config.puma_a2 - k->config.puma_a3 * k->config.puma_a3 - k->config.puma_d4 * k->config.puma_d4) / (2.0 * k->config.puma_a2);
    if (kterm * kterm > k->config.puma_a3 * k->config.puma_a3 + k->config.puma_d4 * k->config.puma_d4 + 1e-9) return SIM_KIN_ERR_RANGE;
    if (kterm * kterm > k->config.puma_a3 * k->config.puma_a3 + k->config.puma_d4 * k->config.puma_d4) kterm = copysign(sqrt(k->config.puma_a3 * k->config.puma_a3 + k->config.puma_d4 * k->config.puma_d4), kterm);
    th3 = atan2(k->config.puma_a3, k->config.puma_d4) - atan2(kterm, (flags & 2U) ? -sqrt(fmax(0.0, k->config.puma_a3 * k->config.puma_a3 + k->config.puma_d4 * k->config.puma_d4 - kterm * kterm)) : sqrt(fmax(0.0, k->config.puma_a3 * k->config.puma_a3 + k->config.puma_d4 * k->config.puma_d4 - kterm * kterm)));
    s3 = sin(th3); c3 = cos(th3);
    t1 = (-k->config.puma_a3 - k->config.puma_a2 * c3) * pz + (c1 * px + s1 * py) * (k->config.puma_a2 * s3 - k->config.puma_d4);
    t2 = (k->config.puma_a2 * s3 - k->config.puma_d4) * pz + (k->config.puma_a3 + k->config.puma_a2 * c3) * (c1 * px + s1 * py);
    t3 = pz * pz + (c1 * px + s1 * py) * (c1 * px + s1 * py);
    if (t3 < 1e-18) return SIM_KIN_ERR_SINGULAR;
    th23 = atan2(t1, t2); th2 = th23 - th3; s23 = t1 / t3; c23 = t2 / t3;
    t1 = -rot.z.x * s1 + rot.z.y * c1;
    t2 = -rot.z.x * c1 * c23 - rot.z.y * s1 * c23 + rot.z.z * s23;
    if (fabs(t1) < 1e-9 && fabs(t2) < 1e-9) return SIM_KIN_ERR_SINGULAR;
    th4 = atan2(t1, t2); s4 = sin(th4); c4 = cos(th4);
    s5 = rot.z.z * (s23 * c4) - rot.z.x * (c1 * c23 * c4 + s1 * s4) - rot.z.y * (s1 * c23 * c4 - c1 * s4);
    c5 = -rot.z.x * (c1 * s23) - rot.z.y * (s1 * s23) - rot.z.z * c23; th5 = atan2(s5, c5);
    s6 = rot.x.z * (s23 * s4) - rot.x.x * (c1 * c23 * s4 - s1 * c4) - rot.x.y * (s1 * c23 * s4 + c1 * c4);
    c6 = rot.x.x * ((c1 * c23 * c4 + s1 * s4) * c5 - c1 * s23 * s5) + rot.x.y * ((s1 * c23 * c4 - c1 * s4) * c5 - s1 * s23 * s5) - rot.x.z * (s23 * c4 * c5 + c23 * s5);
    th6 = atan2(s6, c6);
    if (flags & 4U) { th4 += PM_PI; th5 = -th5; th6 += PM_PI; }
    set_joint(joints, k, 0, th1 * RAD_TO_DEG); set_joint(joints, k, 1, th2 * RAD_TO_DEG); set_joint(joints, k, 2, th3 * RAD_TO_DEG);
    set_joint(joints, k, 3, th4 * RAD_TO_DEG); set_joint(joints, k, 4, th5 * RAD_TO_DEG); set_joint(joints, k, 5, th6 * RAD_TO_DEG);
    if (branch != 0) *branch = (uint32_t)(flags & 7U);
    return SIM_KIN_OK;
}

static void mat4_identity(double m[4][4]) {
    int row, col;
    for (row = 0; row < 4; ++row) for (col = 0; col < 4; ++col) m[row][col] = row == col ? 1.0 : 0.0;
}

static void mat4_multiply(const double left[4][4], const double right[4][4], double out[4][4]) {
    double value[4][4];
    int row, col, inner;
    for (row = 0; row < 4; ++row) for (col = 0; col < 4; ++col) {
        value[row][col] = 0.0;
        for (inner = 0; inner < 4; ++inner) value[row][col] += left[row][inner] * right[inner][col];
    }
    memcpy(out, value, sizeof(value));
}

static int genser_forward(const sim_kinematics *k, const double *joints, sim_posemath_pose *pose) {
    double total[4][4];
    uint32_t index;
    PmRotationMatrix rotation;
    PmRpy rpy;
    mat4_identity(total);
    for (index = 0; index < 6; ++index) {
        double theta = joints[index] * DEG_TO_RAD;
        double alpha = k->config.dh_alpha[index];
        double ct = cos(theta), st = sin(theta), ca = cos(alpha), sa = sin(alpha);
        double link[4][4] = {
            {ct, -st, 0.0, k->config.dh_a[index]},
            {st * ca, ct * ca, -sa, -sa * k->config.dh_d[index]},
            {st * sa, ct * sa, ca, ca * k->config.dh_d[index]},
            {0.0, 0.0, 0.0, 1.0},
        };
        mat4_multiply(total, link, total);
    }
    rotation.x.x = total[0][0]; rotation.y.x = total[0][1]; rotation.z.x = total[0][2];
    rotation.x.y = total[1][0]; rotation.y.y = total[1][1]; rotation.z.y = total[1][2];
    rotation.x.z = total[2][0]; rotation.y.z = total[2][1]; rotation.z.z = total[2][2];
    if (pmMatRpyConvert(&rotation, &rpy) != 0) return SIM_KIN_ERR_SINGULAR;
    pose->tran.x = total[0][3]; pose->tran.y = total[1][3]; pose->tran.z = total[2][3];
    pose->a = rpy.r * RAD_TO_DEG; pose->b = rpy.p * RAD_TO_DEG; pose->c = rpy.y * RAD_TO_DEG;
    return SIM_KIN_OK;
}

static double wrap_degrees(double value) {
    value = fmod(value + 180.0, 360.0);
    if (value < 0.0) value += 360.0;
    return value - 180.0;
}

static int solve_linear6(double matrix[6][6], double rhs[6], double result[6]) {
    int pivot, row, col;
    for (pivot = 0; pivot < 6; ++pivot) {
        int best = pivot;
        double best_abs = fabs(matrix[pivot][pivot]);
        for (row = pivot + 1; row < 6; ++row) if (fabs(matrix[row][pivot]) > best_abs) { best = row; best_abs = fabs(matrix[row][pivot]); }
        if (best_abs < 1e-14) return SIM_KIN_ERR_SINGULAR;
        if (best != pivot) {
            for (col = pivot; col < 6; ++col) { double tmp = matrix[pivot][col]; matrix[pivot][col] = matrix[best][col]; matrix[best][col] = tmp; }
            { double tmp = rhs[pivot]; rhs[pivot] = rhs[best]; rhs[best] = tmp; }
        }
        for (row = pivot + 1; row < 6; ++row) {
            double factor = matrix[row][pivot] / matrix[pivot][pivot];
            for (col = pivot; col < 6; ++col) matrix[row][col] -= factor * matrix[pivot][col];
            rhs[row] -= factor * rhs[pivot];
        }
    }
    for (row = 5; row >= 0; --row) {
        double value = rhs[row];
        for (col = row + 1; col < 6; ++col) value -= matrix[row][col] * result[col];
        result[row] = value / matrix[row][row];
    }
    return SIM_KIN_OK;
}

static int genser_inverse(const sim_kinematics *k, const sim_posemath_pose *target, double *joints, uint32_t max_iterations, double tolerance, sim_kinematics_result_info *info) {
    uint32_t iteration;
    double current[SIM_KIN_MAX_JOINTS] = {0};
    double residual = 0.0;
    if (max_iterations == 0) max_iterations = 100;
    if (tolerance == 0.0) tolerance = 1e-9;
    for (iteration = 0; iteration < max_iterations; ++iteration) {
        sim_posemath_pose estimate;
        double error[6];
        double jacobian[6][6];
        double normal[6][6] = {{0}};
        double rhs[6] = {0};
        double delta[6] = {0};
        int column, row, inner;
        if (genser_forward(k, current, &estimate) != SIM_KIN_OK) return SIM_KIN_ERR_SINGULAR;
        error[0] = target->tran.x - estimate.tran.x; error[1] = target->tran.y - estimate.tran.y; error[2] = target->tran.z - estimate.tran.z;
        error[3] = wrap_degrees(target->a - estimate.a); error[4] = wrap_degrees(target->b - estimate.b); error[5] = wrap_degrees(target->c - estimate.c);
        residual = 0.0; for (row = 0; row < 6; ++row) residual += error[row] * error[row]; residual = sqrt(residual);
        if (residual <= tolerance) {
            memcpy(joints, current, sizeof(double) * 6);
            if (info != 0) { info->iterations = iteration; info->residual = residual; info->branch_id = 0; }
            return SIM_KIN_OK;
        }
        for (column = 0; column < 6; ++column) {
            double perturbed[SIM_KIN_MAX_JOINTS];
            sim_posemath_pose next;
            memcpy(perturbed, current, sizeof(perturbed)); perturbed[column] += 1e-4;
            if (genser_forward(k, perturbed, &next) != SIM_KIN_OK) return SIM_KIN_ERR_SINGULAR;
            jacobian[0][column] = (next.tran.x - estimate.tran.x) / 1e-4; jacobian[1][column] = (next.tran.y - estimate.tran.y) / 1e-4; jacobian[2][column] = (next.tran.z - estimate.tran.z) / 1e-4;
            jacobian[3][column] = wrap_degrees(next.a - estimate.a) / 1e-4; jacobian[4][column] = wrap_degrees(next.b - estimate.b) / 1e-4; jacobian[5][column] = wrap_degrees(next.c - estimate.c) / 1e-4;
        }
        for (row = 0; row < 6; ++row) for (column = 0; column < 6; ++column) {
            for (inner = 0; inner < 6; ++inner) normal[row][column] += jacobian[inner][row] * jacobian[inner][column];
            normal[row][column] += row == column ? 1e-6 : 0.0;
            for (inner = 0; inner < 6; ++inner) if (column == row) rhs[row] += jacobian[inner][row] * error[inner];
        }
        if (solve_linear6(normal, rhs, delta) != SIM_KIN_OK) return SIM_KIN_ERR_SINGULAR;
        for (column = 0; column < 6; ++column) current[column] += delta[column];
    }
    if (info != 0) { info->iterations = max_iterations; info->residual = residual; info->branch_id = 0; }
    return SIM_KIN_ERR_SINGULAR;
}

static int linear_delta_forward(const sim_kinematics *k, const double *joints, uint64_t flags,
                                sim_posemath_pose *pose, uint32_t *branch) {
    double R = k->config.delta_radius, L = k->config.delta_rod_length;
    double Ay = R, Bx = -sqrt(3.0) * 0.5 * R, By = -0.5 * R, Cx = -Bx, Cy = By;
    double q1 = joint(k, joints, 0), q2 = joint(k, joints, 1), q3 = joint(k, joints, 2);
    double den = (By - Ay) * Cx - (Cy - Ay) * Bx;
    double w1 = Ay * Ay + q1 * q1, w2 = Bx * Bx + By * By + q2 * q2, w3 = Cx * Cx + Cy * Cy + q3 * q3;
    double a1 = (q2 - q1) * (Cy - Ay) - (q3 - q1) * (By - Ay);
    double b1 = -((w2 - w1) * (Cy - Ay) - (w3 - w1) * (By - Ay)) / 2.0;
    double a2 = -(q2 - q1) * Cx + (q3 - q1) * Bx;
    double b2 = ((w2 - w1) * Cx - (w3 - w1) * Bx) / 2.0;
    double aa = a1 * a1 + a2 * a2 + den * den;
    double bb = 2.0 * (a1 * b1 + a2 * (b2 - Ay * den) - q1 * den * den);
    double cc = (b2 - Ay * den) * (b2 - Ay * den) + b1 * b1 + den * den * (q1 * q1 - L * L);
    double discr = bb * bb - 4.0 * aa * cc;
    double z;
    if (fabs(den) < 1e-18 || fabs(aa) < 1e-18) return SIM_KIN_ERR_SINGULAR;
    if (discr < -1e-12) return SIM_KIN_ERR_RANGE;
    if (discr < 0.0) discr = 0.0;
    z = (-bb + ((flags & SIM_KIN_DELTA_ALTERNATE_ROOT) ? sqrt(discr) : -sqrt(discr))) / (2.0 * aa);
    pose->tran.z = z; pose->tran.x = (a1 * z + b1) / den; pose->tran.y = (a2 * z + b2) / den;
    set_optional_axes(k, joints, pose);
    if (branch != 0) *branch = (flags & SIM_KIN_DELTA_ALTERNATE_ROOT) ? 1U : 0U;
    return SIM_KIN_OK;
}

static int linear_delta_inverse(const sim_kinematics *k, const sim_posemath_pose *pose, uint64_t flags,
                                double *joints, uint32_t *branch) {
    double R = k->config.delta_radius, L = k->config.delta_rod_length;
    double Ax = 0.0, Ay = R, Bx = -sqrt(3.0) * 0.5 * R, By = -0.5 * R, Cx = -Bx, Cy = By;
    double x = pose->tran.x, y = pose->tran.y, z = pose->tran.z;
    double rad[3] = {L * L - (Ax - x) * (Ax - x) - (Ay - y) * (Ay - y), L * L - (Bx - x) * (Bx - x) - (By - y) * (By - y), L * L - (Cx - x) * (Cx - x) - (Cy - y) * (Cy - y)};
    int index;
    for (index = 0; index < 3; ++index) if (rad[index] < -1e-12) return SIM_KIN_ERR_RANGE;
    for (index = 0; index < 3; ++index) {
        double root = sqrt(fmax(0.0, rad[index]));
        set_joint(joints, k, index, z + ((flags & SIM_KIN_DELTA_ALTERNATE_ROOT) ? -root : root));
    }
    set_joint(joints, k, 3, pose->a); set_joint(joints, k, 4, pose->b); set_joint(joints, k, 5, pose->c); set_joint(joints, k, 6, pose->u); set_joint(joints, k, 7, pose->v); set_joint(joints, k, 8, pose->w);
    if (branch != 0) *branch = (flags & SIM_KIN_DELTA_ALTERNATE_ROOT) ? 1U : 0U;
    return SIM_KIN_OK;
}

static int rotary_delta_forward(const sim_kinematics *k, const double *joints, uint64_t flags,
                                sim_posemath_pose *pose, uint32_t *branch) {
    double pfr = k->config.rotary_platform_radius, tl = k->config.rotary_thigh_length, sl = k->config.rotary_shin_length, fr = k->config.rotary_foot_radius;
    double j0 = joint(k, joints, 0) * DEG_TO_RAD, j1 = joint(k, joints, 1) * DEG_TO_RAD, j2 = joint(k, joints, 2) * DEG_TO_RAD;
    double y1 = -(pfr - fr + tl * cos(j0)), z1 = -tl * sin(j0);
    double y2 = (pfr - fr + tl * cos(j1)) * 0.5, x2 = y2 * sqrt(3.0), z2 = -tl * sin(j1);
    double y3 = (pfr - fr + tl * cos(j2)) * 0.5, x3 = -y3 * sqrt(3.0), z3 = -tl * sin(j2);
    double den = x3 * (y2 - y1) - x2 * (y3 - y1);
    double w1 = y1 * y1 + z1 * z1, w2 = x2 * x2 + y2 * y2 + z2 * z2, w3 = x3 * x3 + y3 * y3 + z3 * z3;
    double a1 = (z2 - z1) * (y3 - y1) - (z3 - z1) * (y2 - y1), b1 = -((w2 - w1) * (y3 - y1) - (w3 - w1) * (y2 - y1)) / 2.0;
    double a2 = -(z2 - z1) * x3 + (z3 - z1) * x2, b2 = ((w2 - w1) * x3 - (w3 - w1) * x2) / 2.0;
    double aa = a1 * a1 + a2 * a2 + den * den, bb = 2.0 * (a1 * b1 + a2 * (b2 - y1 * den) - z1 * den * den), cc = (b2 - y1 * den) * (b2 - y1 * den) + b1 * b1 + den * den * (z1 * z1 - sl * sl);
    double discr = bb * bb - 4.0 * aa * cc;
    if (fabs(den) < 1e-18 || fabs(aa) < 1e-18) return SIM_KIN_ERR_SINGULAR;
    if (discr < -1e-12) return SIM_KIN_ERR_RANGE;
    if (discr < 0.0) discr = 0.0;
    pose->tran.z = (-bb + ((flags & SIM_KIN_DELTA_ALTERNATE_ROOT) ? sqrt(discr) : -sqrt(discr))) / (2.0 * aa);
    pose->tran.x = (a1 * pose->tran.z + b1) / den; pose->tran.y = (a2 * pose->tran.z + b2) / den;
    set_optional_axes(k, joints, pose);
    if (branch != 0) *branch = (flags & SIM_KIN_DELTA_ALTERNATE_ROOT) ? 1U : 0U;
    return SIM_KIN_OK;
}

static int rotary_delta_inverse(const sim_kinematics *k, const sim_posemath_pose *pose, uint64_t flags,
                                double *joints, uint32_t *branch) {
    double pfr = k->config.rotary_platform_radius, tl = k->config.rotary_thigh_length, sl = k->config.rotary_shin_length, fr = k->config.rotary_foot_radius;
    double x = pose->tran.x, y = pose->tran.y, z = pose->tran.z;
    double a, b, d, knee_y, knee_z, xr, yr;
    if (fabs(z) < 1e-12) return SIM_KIN_ERR_SINGULAR;
    a = 0.5 * (x * x + (y - fr) * (y - fr) + z * z + tl * tl - sl * sl - pfr * pfr) / z;
    b = (fr - pfr - y) / z;
    d = tl * tl * (b * b + 1.0) - (a - b * pfr) * (a - b * pfr);
    if (d < 0.0) return SIM_KIN_ERR_RANGE;
    knee_y = (pfr + a * b + ((flags & SIM_KIN_DELTA_ALTERNATE_ROOT) ? -sqrt(d) : sqrt(d))) / (b * b + 1.0); knee_z = b * knee_y - a;
    set_joint(joints, k, 0, atan2(knee_z, knee_y - pfr) * RAD_TO_DEG);
    xr = x; yr = y; { double c = cos(-2.0 * 3.14159265358979323846 / 3.0), s = sin(-2.0 * 3.14159265358979323846 / 3.0); double tx = xr * c - yr * s; yr = xr * s + yr * c; xr = tx; }
    a = 0.5 * (xr * xr + (yr - fr) * (yr - fr) + z * z + tl * tl - sl * sl - pfr * pfr) / z; b = (fr - pfr - yr) / z; d = tl * tl * (b * b + 1.0) - (a - b * pfr) * (a - b * pfr); if (d < 0.0) return SIM_KIN_ERR_RANGE; knee_y = (pfr + a * b + ((flags & SIM_KIN_DELTA_ALTERNATE_ROOT) ? -sqrt(d) : sqrt(d))) / (b * b + 1.0); knee_z = b * knee_y - a; set_joint(joints, k, 1, atan2(knee_z, knee_y - pfr) * RAD_TO_DEG);
    xr = x; yr = y; { double c = cos(2.0 * 3.14159265358979323846 / 3.0), s = sin(2.0 * 3.14159265358979323846 / 3.0); double tx = xr * c - yr * s; yr = xr * s + yr * c; xr = tx; }
    a = 0.5 * (xr * xr + (yr - fr) * (yr - fr) + z * z + tl * tl - sl * sl - pfr * pfr) / z; b = (fr - pfr - yr) / z; d = tl * tl * (b * b + 1.0) - (a - b * pfr) * (a - b * pfr); if (d < 0.0) return SIM_KIN_ERR_RANGE; knee_y = (pfr + a * b + ((flags & SIM_KIN_DELTA_ALTERNATE_ROOT) ? -sqrt(d) : sqrt(d))) / (b * b + 1.0); knee_z = b * knee_y - a; set_joint(joints, k, 2, atan2(knee_z, knee_y - pfr) * RAD_TO_DEG);
    set_joint(joints, k, 3, pose->a); set_joint(joints, k, 4, pose->b); set_joint(joints, k, 5, pose->c); set_joint(joints, k, 6, pose->u); set_joint(joints, k, 7, pose->v); set_joint(joints, k, 8, pose->w);
    if (flags & SIM_KIN_DELTA_ALTERNATE_ROOT) {
        /* The alternate knee branch is the reflected root of each leg. */
        set_joint(joints, k, 0, atan2(knee_z, knee_y - pfr) * RAD_TO_DEG);
        /* Recompute the rotated legs through the common helper below; the
         * default branch remains byte-for-byte compatible with LinuxCNC. */
        {
            double xr2 = x, yr2 = y;
            double c2 = cos(-2.0 * 3.14159265358979323846 / 3.0), s2 = sin(-2.0 * 3.14159265358979323846 / 3.0);
            double tx2 = xr2 * c2 - yr2 * s2; yr2 = xr2 * s2 + yr2 * c2; xr2 = tx2;
            a = 0.5 * (xr2 * xr2 + (yr2 - fr) * (yr2 - fr) + z * z + tl * tl - sl * sl - pfr * pfr) / z;
            b = (fr - pfr - yr2) / z; d = tl * tl * (b * b + 1.0) - (a - b * pfr) * (a - b * pfr);
            if (d < 0.0) return SIM_KIN_ERR_RANGE;
            knee_y = (pfr + a * b - sqrt(d)) / (b * b + 1.0); knee_z = b * knee_y - a;
            set_joint(joints, k, 1, atan2(knee_z, knee_y - pfr) * RAD_TO_DEG);
            xr2 = x; yr2 = y; c2 = cos(2.0 * 3.14159265358979323846 / 3.0); s2 = sin(2.0 * 3.14159265358979323846 / 3.0);
            tx2 = xr2 * c2 - yr2 * s2; yr2 = xr2 * s2 + yr2 * c2; xr2 = tx2;
            a = 0.5 * (xr2 * xr2 + (yr2 - fr) * (yr2 - fr) + z * z + tl * tl - sl * sl - pfr * pfr) / z;
            b = (fr - pfr - yr2) / z; d = tl * tl * (b * b + 1.0) - (a - b * pfr) * (a - b * pfr);
            if (d < 0.0) return SIM_KIN_ERR_RANGE;
            knee_y = (pfr + a * b - sqrt(d)) / (b * b + 1.0); knee_z = b * knee_y - a;
            set_joint(joints, k, 2, atan2(knee_z, knee_y - pfr) * RAD_TO_DEG);
        }
    }
    if (branch != 0) *branch = (flags & SIM_KIN_DELTA_ALTERNATE_ROOT) ? 1U : 0U;
    return SIM_KIN_OK;
}

/* Point-vertex tripod kinematics from LinuxCNC's tripodkins.c.  The three
 * strut lengths determine X/Y uniquely; the quadratic Z root is selected by
 * the forward flag (above-base by default, below-base when bit zero is set). */
static int tripod_forward(const sim_kinematics *k, const double *joints, uint64_t flags,
                          sim_posemath_pose *pose, uint32_t *branch) {
    double bx = k->config.tripod_bx, cx = k->config.tripod_cx, cy = k->config.tripod_cy;
    double ad = joint(k, joints, 0), bd = joint(k, joints, 1), cd = joint(k, joints, 2);
    double p = ad * ad;
    double q = bd * bd - bx * bx;
    double r = cd * cd - cx * cx - cy * cy;
    double s = -2.0 * bx, t = -2.0 * cx, u = -2.0 * cy;
    double x, y, z2;
    if (fabs(s) < 1e-12 || fabs(u) < 1e-12) return SIM_KIN_ERR_CONFIGURATION;
    x = (q - p) / s;
    y = (r - q - (t - s) * x) / u;
    z2 = p - x * x - y * y;
    if (z2 < -1e-12) return SIM_KIN_ERR_RANGE;
    if (z2 <= 1e-12) return SIM_KIN_ERR_SINGULAR;
    pose->tran.x = x;
    pose->tran.y = y;
    pose->tran.z = (flags & SIM_KIN_TRIPOD_BELOW_ROOT) ? -sqrt(z2) : sqrt(z2);
    pose->a = 0.0; pose->b = 0.0; pose->c = 0.0;
    set_optional_axes(k, joints, pose);
    if (branch != 0) *branch = (flags & SIM_KIN_TRIPOD_BELOW_ROOT) ? 1U : 0U;
    return SIM_KIN_OK;
}

static int tripod_inverse(const sim_kinematics *k, const sim_posemath_pose *pose,
                          double *joints, uint32_t *branch) {
    double bx = k->config.tripod_bx, cx = k->config.tripod_cx, cy = k->config.tripod_cy;
    double x = pose->tran.x, y = pose->tran.y, z = pose->tran.z;
    set_joint(joints, k, 0, sqrt(x * x + y * y + z * z));
    set_joint(joints, k, 1, sqrt((x - bx) * (x - bx) + y * y + z * z));
    set_joint(joints, k, 2, sqrt((x - cx) * (x - cx) + (y - cy) * (y - cy) + z * z));
    set_joint(joints, k, 3, pose->a); set_joint(joints, k, 4, pose->b); set_joint(joints, k, 5, pose->c);
    set_joint(joints, k, 6, pose->u); set_joint(joints, k, 7, pose->v); set_joint(joints, k, 8, pose->w);
    if (branch != 0) *branch = z < 0.0 ? 1U : 0U;
    return SIM_KIN_OK;
}

/* Generalized six-strut Stewart platform.  Inverse kinematics is the
 * closed-form leg-length calculation; forward kinematics follows LinuxCNC's
 * Newton solve from a deterministic pose seed.  The rotational Jacobian uses
 * the platform attachment vector crossed with the unit strut vector, which
 * is the small-angle derivative used by genhexkins.c. */
static int genhex_lengths(const sim_kinematics *k, const double q[6], double lengths[SIM_KIN_GENHEX_STRUTS], double jacobian[SIM_KIN_GENHEX_STRUTS][6]) {
    PmRpy rpy;
    PmRotationMatrix rotation;
    size_t index;
    rpy.r = q[3]; rpy.p = q[4]; rpy.y = q[5];
    if (pmRpyMatConvert(&rpy, &rotation) != 0) return SIM_KIN_ERR_SINGULAR;
    for (index = 0; index < SIM_KIN_GENHEX_STRUTS; ++index) {
        PmCartesian platform = {k->config.genhex_platform[index][0], k->config.genhex_platform[index][1], k->config.genhex_platform[index][2]};
        PmCartesian base = {k->config.genhex_base[index][0], k->config.genhex_base[index][1], k->config.genhex_base[index][2]};
        PmCartesian rotated;
        PmCartesian world;
        PmCartesian vector;
        PmCartesian unit;
        PmCartesian cross;
        double length;
        pmMatCartMult(&rotation, &platform, &rotated);
        world.x = q[0] + rotated.x; world.y = q[1] + rotated.y; world.z = q[2] + rotated.z;
        vector.x = world.x - base.x; vector.y = world.y - base.y; vector.z = world.z - base.z;
        if (pmCartUnit(&vector, &unit) != 0 || pmCartMag(&vector, &length) != 0 || !isfinite(length)) return SIM_KIN_ERR_SINGULAR;
        lengths[index] = length;
        if (jacobian != 0) {
            pmCartCartCross(&rotated, &unit, &cross);
            jacobian[index][0] = unit.x; jacobian[index][1] = unit.y; jacobian[index][2] = unit.z;
            jacobian[index][3] = cross.x; jacobian[index][4] = cross.y; jacobian[index][5] = cross.z;
        }
    }
    return SIM_KIN_OK;
}

static int genhex_inverse(const sim_kinematics *k, const sim_posemath_pose *pose, double *joints) {
    double q[6] = {pose->tran.x, pose->tran.y, pose->tran.z, pose->a * DEG_TO_RAD, pose->b * DEG_TO_RAD, pose->c * DEG_TO_RAD};
    double lengths[SIM_KIN_GENHEX_STRUTS];
    size_t index;
    int result = genhex_lengths(k, q, lengths, 0);
    if (result != SIM_KIN_OK) return result;
    for (index = 0; index < SIM_KIN_GENHEX_STRUTS; ++index) joints[index] = lengths[index];
    return SIM_KIN_OK;
}

static int genhex_forward(const sim_kinematics *k, const double *joints, uint64_t flags,
                          sim_posemath_pose *pose, sim_kinematics_result_info *info) {
    double q[6];
    double lengths[SIM_KIN_GENHEX_STRUTS];
    double jacobian[SIM_KIN_GENHEX_STRUTS][6];
    double residual = 0.0;
    uint32_t iteration;
    int result;
    for (size_t index = 0; index < 6; ++index) q[index] = k->config.genhex_initial_pose[index];
    if (flags & SIM_KIN_GENHEX_ALTERNATE_SEED) q[2] = -q[2];
    for (iteration = 1; iteration <= k->config.genhex_iteration_limit; ++iteration) {
        double matrix[6][6];
        double rhs[6];
        double delta[6] = {0};
        int row;
        result = genhex_lengths(k, q, lengths, jacobian);
        if (result != SIM_KIN_OK) return result;
        residual = 0.0;
        for (row = 0; row < (int)SIM_KIN_GENHEX_STRUTS; ++row) {
            double difference = lengths[row] - joints[row];
            rhs[row] = difference;
            residual += fabs(difference);
            for (int column = 0; column < 6; ++column) matrix[row][column] = jacobian[row][column];
        }
        if (residual <= k->config.genhex_convergence_tolerance) {
            pose->tran.x = q[0]; pose->tran.y = q[1]; pose->tran.z = q[2];
            pose->a = q[3] * RAD_TO_DEG; pose->b = q[4] * RAD_TO_DEG; pose->c = q[5] * RAD_TO_DEG;
            pose->u = 0.0; pose->v = 0.0; pose->w = 0.0;
            if (info != 0) { info->iterations = iteration; info->residual = residual; info->branch_id = (flags & SIM_KIN_GENHEX_ALTERNATE_SEED) ? 1U : 0U; }
            return SIM_KIN_OK;
        }
        if (residual > k->config.genhex_max_error) {
            if (info != 0) { info->iterations = iteration; info->residual = residual; info->branch_id = (flags & SIM_KIN_GENHEX_ALTERNATE_SEED) ? 1U : 0U; }
            return SIM_KIN_ERR_RANGE;
        }
        result = solve_linear6(matrix, rhs, delta);
        if (result != SIM_KIN_OK) {
            if (info != 0) { info->iterations = iteration; info->residual = residual; info->branch_id = (flags & SIM_KIN_GENHEX_ALTERNATE_SEED) ? 1U : 0U; }
            return result;
        }
        for (int column = 0; column < 6; ++column) q[column] -= delta[column];
    }
    if (info != 0) { info->iterations = k->config.genhex_iteration_limit; info->residual = residual; info->branch_id = (flags & SIM_KIN_GENHEX_ALTERNATE_SEED) ? 1U : 0U; }
    return SIM_KIN_ERR_RANGE;
}

static int solve_linear5(double matrix[5][5], double rhs[5], double result[5]) {
    for (int pivot = 0; pivot < 5; ++pivot) {
        int best = pivot;
        double magnitude = fabs(matrix[pivot][pivot]);
        for (int row = pivot + 1; row < 5; ++row) if (fabs(matrix[row][pivot]) > magnitude) { best = row; magnitude = fabs(matrix[row][pivot]); }
        if (magnitude < 1e-14) return SIM_KIN_ERR_SINGULAR;
        if (best != pivot) { for (int col = pivot; col < 5; ++col) { double tmp = matrix[pivot][col]; matrix[pivot][col] = matrix[best][col]; matrix[best][col] = tmp; } { double tmp = rhs[pivot]; rhs[pivot] = rhs[best]; rhs[best] = tmp; } }
        for (int row = pivot + 1; row < 5; ++row) { double factor = matrix[row][pivot] / matrix[pivot][pivot]; for (int col = pivot; col < 5; ++col) matrix[row][col] -= factor * matrix[pivot][col]; rhs[row] -= factor * rhs[pivot]; }
    }
    for (int row = 4; row >= 0; --row) { double value = rhs[row]; for (int col = row + 1; col < 5; ++col) value -= matrix[row][col] * result[col]; result[row] = value / matrix[row][row]; }
    return SIM_KIN_OK;
}

static int pentapod_lengths(const sim_kinematics *k, const double q[5], double lengths[SIM_KIN_PENTA_STRUTS]) {
    PmRpy rpy = {q[3], q[4], 0.0};
    PmRotationMatrix rotation, inverse;
    if (pmRpyMatConvert(&rpy, &rotation) != 0 || pmMatInv(&rotation, &inverse) != 0) return SIM_KIN_ERR_SINGULAR;
    for (size_t index = 0; index < SIM_KIN_PENTA_STRUTS; ++index) {
        /* pentakins_read_hal_pins applies tool-offset to both ends of each
         * strut; preserve that world-frame translation before rotating into
         * the effector frame. */
        PmCartesian base = {k->config.penta_base[index][0] - q[0], k->config.penta_base[index][1] - q[1], k->config.penta_base[index][2] + k->config.penta_tool_offset - q[2]};
        PmCartesian local;
        double radial;
        pmMatCartMult(&inverse, &base, &local);
        radial = hypot(local.x, local.y);
        lengths[index] = hypot(radial - k->config.penta_effector_radius[index], local.z - (k->config.penta_effector_z[index] + k->config.penta_tool_offset));
        if (!isfinite(lengths[index])) return SIM_KIN_ERR_NONFINITE;
    }
    return SIM_KIN_OK;
}

static int pentapod_inverse(const sim_kinematics *k, const sim_posemath_pose *pose, double *joints) {
    double q[5] = {pose->tran.x, pose->tran.y, pose->tran.z, pose->a * DEG_TO_RAD, pose->b * DEG_TO_RAD};
    double lengths[SIM_KIN_PENTA_STRUTS];
    int result = pentapod_lengths(k, q, lengths);
    if (result != SIM_KIN_OK) return result;
    for (size_t index = 0; index < SIM_KIN_PENTA_STRUTS; ++index) joints[index] = lengths[index];
    return SIM_KIN_OK;
}

static int pentapod_forward(const sim_kinematics *k, const double *joints, uint64_t flags, sim_posemath_pose *pose, sim_kinematics_result_info *info) {
    double q[5];
    double lengths[SIM_KIN_PENTA_STRUTS];
    double residual = 0.0;
    uint32_t iteration;
    for (size_t index = 0; index < SIM_KIN_PENTA_STRUTS; ++index) {
        /* LinuxCNC's pentakins rejects non-positive strut lengths before
         * entering Newton iteration. */
        if (joints[index] <= 0.0) {
            if (info != 0) { info->iterations = 0; info->residual = 0.0; info->branch_id = (flags & SIM_KIN_GENHEX_ALTERNATE_SEED) ? 1U : 0U; }
            return SIM_KIN_ERR_RANGE;
        }
    }
    for (size_t index = 0; index < 5; ++index) q[index] = k->config.penta_initial_pose[index];
    if (flags & SIM_KIN_GENHEX_ALTERNATE_SEED) q[2] = -q[2];
    for (iteration = 1; iteration <= k->config.penta_iteration_limit; ++iteration) {
        double jacobian[5][5];
        double rhs[5];
        double delta[5] = {0};
        int result = pentapod_lengths(k, q, lengths);
        if (result != SIM_KIN_OK) return result;
        residual = 0.0;
        for (int row = 0; row < 5; ++row) {
            double perturbed[5];
            rhs[row] = lengths[row] - joints[row]; residual += fabs(rhs[row]);
            for (int column = 0; column < 5; ++column) {
                double next[SIM_KIN_PENTA_STRUTS];
                memcpy(perturbed, q, sizeof(perturbed)); perturbed[column] += 1e-4;
                if (pentapod_lengths(k, perturbed, next) != SIM_KIN_OK) return SIM_KIN_ERR_SINGULAR;
                jacobian[row][column] = (next[row] - lengths[row]) * 1e4;
            }
        }
        if (residual <= k->config.penta_convergence_tolerance) {
            pose->tran.x = pentapod_canonical(q[0]); pose->tran.y = pentapod_canonical(q[1]); pose->tran.z = pentapod_canonical(q[2]); pose->a = pentapod_canonical(q[3] * RAD_TO_DEG); pose->b = pentapod_canonical(q[4] * RAD_TO_DEG); pose->c = 0.0; pose->u = pose->v = pose->w = 0.0;
            if (info != 0) { info->iterations = iteration; info->residual = nearbyint(residual * 1000000000.0) / 1000000000.0; info->branch_id = (flags & SIM_KIN_GENHEX_ALTERNATE_SEED) ? 1U : 0U; }
            return SIM_KIN_OK;
        }
        if (residual > k->config.penta_max_error) {
            if (info != 0) { info->iterations = iteration; info->residual = residual; info->branch_id = (flags & SIM_KIN_GENHEX_ALTERNATE_SEED) ? 1U : 0U; }
            return SIM_KIN_ERR_RANGE;
        }
        if (solve_linear5(jacobian, rhs, delta) != SIM_KIN_OK) return SIM_KIN_ERR_SINGULAR;
        for (int column = 0; column < 5; ++column) q[column] -= delta[column];
    }
    if (info != 0) { info->iterations = k->config.penta_iteration_limit; info->residual = residual; info->branch_id = (flags & SIM_KIN_GENHEX_ALTERNATE_SEED) ? 1U : 0U; }
    return SIM_KIN_ERR_RANGE;
}

int sim_kinematics_forward_ex(const sim_kinematics *k, const double *joints, uint32_t joint_count,
                              uint64_t inverse_flags, sim_posemath_pose *pose,
                              sim_kinematics_result_info *info) {
    double a, b, c, x, y, z;
    initialize_info(info, 0, inverse_flags);
    if (k == 0 || joints == 0 || pose == 0) return SIM_KIN_ERR_ARGUMENT;
    if (!k->configured || joint_count != kinematics_joint_count(&k->config)) return SIM_KIN_ERR_CONFIGURATION;
    if (effective_mode(&k->config) == SIM_KIN_MODE_INVERSE_ONLY) return SIM_KIN_ERR_UNSUPPORTED;
    if (!finite_values(joints, joint_count)) return SIM_KIN_ERR_NONFINITE;
    if (in_limits(k, joints) != SIM_KIN_OK) return SIM_KIN_ERR_RANGE;
    zero_pose(pose);
    x = joint(k, joints, 0); y = joint(k, joints, 1); z = joint(k, joints, 2);
    if (k->config.type == SIM_KIN_TRIVKINS) { pose->tran.x = x; pose->tran.y = y; pose->tran.z = z; set_optional_axes(k, joints, pose); return SIM_KIN_OK; }
    if (k->config.type == SIM_KIN_SCARA) return scara_forward(k, joints, pose);
    if (k->config.type == SIM_KIN_SCORBOT) return scorbot_forward(k, joints, pose);
    if (k->config.type == SIM_KIN_PUMA) {
        uint32_t branch = 0;
        int result = puma_forward(k, joints, pose, &branch);
        if (info != 0) info->branch_id = branch;
        return result;
    }
    if (k->config.type == SIM_KIN_GENSER) return genser_forward(k, joints, pose);
    if (k->config.type == SIM_KIN_LINEAR_DELTA) {
        uint32_t branch = 0;
        int result = linear_delta_forward(k, joints, inverse_flags, pose, &branch);
        if (info != 0) { info->branch_id = branch; info->iterations = 0; info->residual = 0.0; }
        return result;
    }
    if (k->config.type == SIM_KIN_ROTARY_DELTA) {
        uint32_t branch = 0;
        int result = rotary_delta_forward(k, joints, inverse_flags, pose, &branch);
        if (info != 0) { info->branch_id = branch; info->iterations = 0; info->residual = 0.0; }
        return result;
    }
    if (k->config.type == SIM_KIN_TRIPOD) {
        uint32_t branch = 0;
        int result = tripod_forward(k, joints, inverse_flags, pose, &branch);
        if (info != 0) { info->branch_id = branch; info->iterations = 0; info->residual = 0.0; }
        return result;
    }
    if (k->config.type == SIM_KIN_GENHEX) {
        return genhex_forward(k, joints, inverse_flags, pose, info);
    }
    if (k->config.type == SIM_KIN_PENTAPOD) {
        return pentapod_forward(k, joints, inverse_flags, pose, info);
    }
    if (k->config.type == SIM_KIN_COREXY) {
        pose->tran.x = 0.5 * (x + y);
        pose->tran.y = 0.5 * (x - y);
        pose->tran.z = z;
        set_optional_axes(k, joints, pose);
        return SIM_KIN_OK;
    }
    if (k->config.type == SIM_KIN_ROSE) {
        double theta = z * DEG_TO_RAD;
        pose->tran.x = x * cos(theta);
        pose->tran.y = x * sin(theta);
        pose->tran.z = y;
        return sim_kinematics_validate_pose(pose);
    }
    c = joint(k, joints, 5) * DEG_TO_RAD;
    if (k->config.type == SIM_KIN_XYZAC_TRT) {
        double dy = k->config.y_offset;
        double dz = k->config.z_offset + k->config.tool_offset;
        a = joint(k, joints, 3) * DEG_TO_RAD;
        pose->tran.x = cos(c) * (x - k->config.x_rot_point) + sin(c) * cos(a) * (y - dy - k->config.y_rot_point) + sin(c) * sin(a) * (z - dz - k->config.z_rot_point) + sin(c) * dy + k->config.x_rot_point;
        pose->tran.y = -sin(c) * (x - k->config.x_rot_point) + cos(c) * cos(a) * (y - dy - k->config.y_rot_point) + cos(c) * sin(a) * (z - dz - k->config.z_rot_point) + cos(c) * dy + k->config.y_rot_point;
        pose->tran.z = -sin(a) * (y - dy - k->config.y_rot_point) + cos(a) * (z - dz - k->config.z_rot_point) + dz + k->config.z_rot_point;
    } else if (k->config.type == SIM_KIN_XYZBC_TRT) {
        double dx = k->config.x_offset;
        double dz = k->config.z_offset + k->config.tool_offset;
        b = joint(k, joints, 4) * DEG_TO_RAD;
        pose->tran.x = cos(c) * cos(b) * (x - dx - k->config.x_rot_point) + sin(c) * (y - k->config.y_rot_point) - cos(c) * sin(b) * (z - dz - k->config.z_rot_point) + cos(c) * dx + k->config.x_rot_point;
        pose->tran.y = -sin(c) * cos(b) * (x - dx - k->config.x_rot_point) + cos(c) * (y - k->config.y_rot_point) + sin(c) * sin(b) * (z - dz - k->config.z_rot_point) - sin(c) * dx + k->config.y_rot_point;
        pose->tran.z = sin(b) * (x - dx - k->config.x_rot_point) + cos(b) * (z - dz - k->config.z_rot_point) + dz + k->config.z_rot_point;
    } else if (k->config.type == SIM_KIN_XYZBCW || k->config.type == SIM_KIN_5AXISKINS) {
        double radius = k->config.pivot_length + joint(k, joints, 8);
        double theta = c;
        double phi = (180.0 - joint(k, joints, 4)) * DEG_TO_RAD;
        double rx = radius * sin(phi) * cos(theta);
        double ry = radius * sin(phi) * sin(theta);
        double rz = radius * cos(phi);
        pose->tran.x = x + rx; pose->tran.y = y + ry; pose->tran.z = z + k->config.pivot_length + rz;
    } else if (k->config.type == SIM_KIN_MAXKINS) {
        double pivot_radius = k->config.pivot_length + joint(k, joints, 8);
        double zb;
        double xb;
        double xyr;
        double xytheta;
        double zv;
        double xv;
        b = joint(k, joints, 4) * DEG_TO_RAD;
        zb = pivot_radius * cos(b);
        xb = pivot_radius * sin(b);
        xyr = hypot(x, y);
        xytheta = atan2(y, x) + c;
        zv = joint(k, joints, 6) * sin(b);
        xv = joint(k, joints, 6) * cos(b);
        pose->tran.x = xyr * cos(xytheta) + xb - xv;
        pose->tran.y = xyr * sin(xytheta) - joint(k, joints, 7);
        pose->tran.z = z - zb + zv + k->config.pivot_length;
    } else if (k->config.type == SIM_KIN_ROTATEKINS) {
        pose->tran.x = x * cos(-c) - y * sin(-c);
        pose->tran.y = x * sin(-c) + y * cos(-c);
        pose->tran.z = z;
    } else return SIM_KIN_ERR_UNKNOWN_TYPE;
    set_optional_axes(k, joints, pose);
    return sim_kinematics_validate_pose(pose);
}

int sim_kinematics_forward(const sim_kinematics *k, const double *joints, uint32_t joint_count, sim_posemath_pose *pose) {
    return sim_kinematics_forward_ex(k, joints, joint_count, 0, pose, 0);
}

static void set_joint(double *joints, const sim_kinematics *k, int axis, double value) {
    int index = config_axis(k, axis);
    if (index >= 0) joints[index] = value;
}

static void fill_duplicate_joints(const sim_kinematics *k, double *joints) {
    uint32_t index;
    for (index = 0; index < kinematics_joint_count(&k->config); ++index) {
        int axis = axis_index((char)(k->config.coordinates[index] >= 'a' && k->config.coordinates[index] <= 'z' ? k->config.coordinates[index] - 'a' + 'A' : k->config.coordinates[index]));
        if (axis >= 0) joints[index] = joints[config_axis(k, axis)];
    }
}

int sim_kinematics_inverse_ex(sim_kinematics *k, const sim_posemath_pose *pose, uint32_t joint_count,
                              uint64_t forward_flags, double *joints,
                              sim_kinematics_result_info *info) {
    double a, b, c;
    initialize_info(info, forward_flags, 0);
    if (k == 0 || pose == 0 || joints == 0) return SIM_KIN_ERR_ARGUMENT;
    if (!k->configured || joint_count != kinematics_joint_count(&k->config)) return SIM_KIN_ERR_CONFIGURATION;
    if (effective_mode(&k->config) == SIM_KIN_MODE_FORWARD_ONLY) return SIM_KIN_ERR_UNSUPPORTED;
    if (sim_kinematics_validate_pose(pose) != SIM_KIN_OK) return SIM_KIN_ERR_NONFINITE;
    memset(joints, 0, sizeof(double) * joint_count);
    if (k->config.type == SIM_KIN_TRIVKINS) {
        set_joint(joints, k, 0, pose->tran.x); set_joint(joints, k, 1, pose->tran.y); set_joint(joints, k, 2, pose->tran.z);
        set_joint(joints, k, 3, pose->a); set_joint(joints, k, 4, pose->b); set_joint(joints, k, 5, pose->c); set_joint(joints, k, 6, pose->u); set_joint(joints, k, 7, pose->v); set_joint(joints, k, 8, pose->w);
    } else if (k->config.type == SIM_KIN_SCARA) {
        uint32_t branch = 0;
        int result = scara_inverse(k, pose, joints, forward_flags, &branch);
        if (info != 0) info->branch_id = branch;
        return result == SIM_KIN_OK ? in_limits(k, joints) : result;
    } else if (k->config.type == SIM_KIN_SCORBOT) {
        int result = scorbot_inverse(k, pose, joints);
        return result == SIM_KIN_OK ? in_limits(k, joints) : result;
    } else if (k->config.type == SIM_KIN_PUMA) {
        uint32_t branch = 0;
        int result = puma_inverse(k, pose, joints, forward_flags, &branch);
        if (info != 0) info->branch_id = branch;
        return result == SIM_KIN_OK ? in_limits(k, joints) : result;
    } else if (k->config.type == SIM_KIN_GENSER) {
        return genser_inverse(k, pose, joints, k->config.max_iterations, k->config.convergence_tolerance, info);
    } else if (k->config.type == SIM_KIN_LINEAR_DELTA) {
        uint32_t branch = 0;
        int result = linear_delta_inverse(k, pose, forward_flags, joints, &branch);
        if (info != 0) { info->branch_id = branch; info->iterations = 0; info->residual = 0.0; }
        if (result == SIM_KIN_OK) k->branch_id = branch;
        return result == SIM_KIN_OK ? in_limits(k, joints) : result;
    } else if (k->config.type == SIM_KIN_ROTARY_DELTA) {
        uint32_t branch = 0;
        int result = rotary_delta_inverse(k, pose, forward_flags, joints, &branch);
        if (info != 0) { info->branch_id = branch; info->iterations = 0; info->residual = 0.0; }
        if (result == SIM_KIN_OK) k->branch_id = branch;
        return result == SIM_KIN_OK ? in_limits(k, joints) : result;
    } else if (k->config.type == SIM_KIN_TRIPOD) {
        uint32_t branch = 0;
        int result = tripod_inverse(k, pose, joints, &branch);
        if (info != 0) { info->forward_flags = branch; info->branch_id = branch; info->iterations = 0; info->residual = 0.0; }
        if (result == SIM_KIN_OK) k->branch_id = branch;
        return result == SIM_KIN_OK ? in_limits(k, joints) : result;
    } else if (k->config.type == SIM_KIN_GENHEX) {
        int result = genhex_inverse(k, pose, joints);
        if (info != 0) { info->forward_flags = pose->tran.z < 0.0 ? SIM_KIN_GENHEX_ALTERNATE_SEED : 0; info->branch_id = pose->tran.z < 0.0 ? 1U : 0U; info->iterations = 0; info->residual = 0.0; }
        if (result == SIM_KIN_OK) k->branch_id = pose->tran.z < 0.0 ? 1U : 0U;
        return result == SIM_KIN_OK ? in_limits(k, joints) : result;
    } else if (k->config.type == SIM_KIN_PENTAPOD) {
        int result = pentapod_inverse(k, pose, joints);
        if (info != 0) { info->forward_flags = pose->tran.z < 0.0 ? SIM_KIN_GENHEX_ALTERNATE_SEED : 0; info->branch_id = pose->tran.z < 0.0 ? 1U : 0U; info->iterations = 0; info->residual = 0.0; }
        if (result == SIM_KIN_OK) k->branch_id = pose->tran.z < 0.0 ? 1U : 0U;
        return result == SIM_KIN_OK ? in_limits(k, joints) : result;
    } else if (k->config.type == SIM_KIN_COREXY) {
        set_joint(joints, k, 0, pose->tran.x + pose->tran.y);
        set_joint(joints, k, 1, pose->tran.x - pose->tran.y);
        set_joint(joints, k, 2, pose->tran.z);
        set_joint(joints, k, 3, pose->a); set_joint(joints, k, 4, pose->b);
        set_joint(joints, k, 5, pose->c); set_joint(joints, k, 6, pose->u);
        set_joint(joints, k, 7, pose->v); set_joint(joints, k, 8, pose->w);
    } else if (k->config.type == SIM_KIN_ROSE) {
        uint32_t quadrant;
        double theta;
        double bigtheta;
        if (pose->tran.x >= 0.0 && pose->tran.y >= 0.0) quadrant = 1;
        else if (pose->tran.x < 0.0 && pose->tran.y >= 0.0) quadrant = 2;
        else if (pose->tran.x < 0.0 && pose->tran.y < 0.0) quadrant = 3;
        else quadrant = 4;
        if (k->previous_quadrant == 2 && quadrant == 3) k->revolution_count += 1;
        if (k->previous_quadrant == 3 && quadrant == 2) k->revolution_count -= 1;
        theta = atan2(pose->tran.y, pose->tran.x);
        bigtheta = theta + 2.0 * 3.14159265358979323846264338327950288 * k->revolution_count;
        set_joint(joints, k, 0, hypot(pose->tran.x, pose->tran.y));
        set_joint(joints, k, 1, pose->tran.z);
        set_joint(joints, k, 2, bigtheta * RAD_TO_DEG);
        k->previous_quadrant = quadrant;
        k->branch_id = (uint32_t)k->revolution_count;
        if (info != 0) info->branch_id = k->branch_id;
    } else if (k->config.type == SIM_KIN_XYZAC_TRT) {
        double dy = k->config.y_offset;
        double dz = k->config.z_offset + k->config.tool_offset;
        a = pose->a * DEG_TO_RAD; c = pose->c * DEG_TO_RAD;
        set_joint(joints, k, 3, pose->a); set_joint(joints, k, 5, pose->c);
        set_joint(joints, k, 0, cos(c) * (pose->tran.x - k->config.x_rot_point) - sin(c) * (pose->tran.y - k->config.y_rot_point) + k->config.x_rot_point);
        set_joint(joints, k, 1, sin(c) * cos(a) * (pose->tran.x - k->config.x_rot_point) + cos(c) * cos(a) * (pose->tran.y - k->config.y_rot_point) - sin(a) * (pose->tran.z - k->config.z_rot_point) - cos(a) * dy + sin(a) * dz + dy + k->config.y_rot_point);
        set_joint(joints, k, 2, sin(c) * sin(a) * (pose->tran.x - k->config.x_rot_point) + cos(c) * sin(a) * (pose->tran.y - k->config.y_rot_point) + cos(a) * (pose->tran.z - k->config.z_rot_point) - sin(a) * dy - cos(a) * dz + dz + k->config.z_rot_point);
    } else if (k->config.type == SIM_KIN_XYZBC_TRT) {
        double dx = k->config.x_offset;
        double dz = k->config.z_offset + k->config.tool_offset;
        b = pose->b * DEG_TO_RAD; c = pose->c * DEG_TO_RAD;
        if (fabs(cos(b)) < 1e-12) return SIM_KIN_ERR_SINGULAR;
        set_joint(joints, k, 4, pose->b); set_joint(joints, k, 5, pose->c);
        set_joint(joints, k, 0, cos(c) * cos(b) * (pose->tran.x - k->config.x_rot_point) - sin(c) * cos(b) * (pose->tran.y - k->config.y_rot_point) + sin(b) * (pose->tran.z - k->config.z_rot_point) - cos(b) * dx - sin(b) * dz + dx + k->config.x_rot_point);
        set_joint(joints, k, 1, sin(c) * (pose->tran.x - k->config.x_rot_point) + cos(c) * (pose->tran.y - k->config.y_rot_point) + k->config.y_rot_point);
        set_joint(joints, k, 2, -cos(c) * sin(b) * (pose->tran.x - k->config.x_rot_point) + sin(c) * sin(b) * (pose->tran.y - k->config.y_rot_point) + cos(b) * (pose->tran.z - k->config.z_rot_point) + sin(b) * dx - cos(b) * dz + dz + k->config.z_rot_point);
    } else if (k->config.type == SIM_KIN_XYZBCW || k->config.type == SIM_KIN_5AXISKINS) {
        double radius = k->config.pivot_length + pose->w;
        double phi = (180.0 - pose->b) * DEG_TO_RAD;
        double theta = pose->c * DEG_TO_RAD;
        set_joint(joints, k, 4, pose->b); set_joint(joints, k, 5, pose->c); set_joint(joints, k, 8, pose->w);
        set_joint(joints, k, 0, pose->tran.x - radius * sin(phi) * cos(theta));
        set_joint(joints, k, 1, pose->tran.y - radius * sin(phi) * sin(theta));
        set_joint(joints, k, 2, pose->tran.z - k->config.pivot_length - radius * cos(phi));
    } else if (k->config.type == SIM_KIN_MAXKINS) {
        double pivot_radius = k->config.pivot_length + pose->w;
        double zb;
        double xb;
        double xyr;
        double xytheta;
        double zv;
        double xv;
        b = pose->b * DEG_TO_RAD;
        c = pose->c * DEG_TO_RAD;
        zb = pivot_radius * cos(b);
        xb = pivot_radius * sin(b);
        xyr = hypot(pose->tran.x, pose->tran.y);
        xytheta = atan2(pose->tran.y, pose->tran.x) - c;
        zv = pose->u * sin(b);
        xv = pose->u * cos(b);
        set_joint(joints, k, 0, xyr * cos(xytheta) - xb + xv);
        set_joint(joints, k, 1, xyr * sin(xytheta) + pose->v);
        set_joint(joints, k, 2, pose->tran.z + zb + zv - k->config.pivot_length);
        set_joint(joints, k, 4, pose->b); set_joint(joints, k, 5, pose->c);
        set_joint(joints, k, 8, pose->w);
    } else if (k->config.type == SIM_KIN_ROTATEKINS) {
        c = pose->c * DEG_TO_RAD;
        set_joint(joints, k, 0, pose->tran.x * cos(c) - pose->tran.y * sin(c));
        set_joint(joints, k, 1, pose->tran.x * sin(c) + pose->tran.y * cos(c));
        set_joint(joints, k, 2, pose->tran.z);
        set_joint(joints, k, 4, pose->b); set_joint(joints, k, 5, pose->c);
        set_joint(joints, k, 8, pose->w);
    } else return SIM_KIN_ERR_UNKNOWN_TYPE;
    set_joint(joints, k, 3, pose->a); set_joint(joints, k, 6, pose->u); set_joint(joints, k, 7, pose->v);
    fill_duplicate_joints(k, joints);
    return in_limits(k, joints);
}

int sim_kinematics_inverse(sim_kinematics *k, const sim_posemath_pose *pose, double *joints, uint32_t joint_count) {
    return sim_kinematics_inverse_ex(k, pose, joint_count, 0, joints, 0);
}

int sim_kinematics_home(const sim_kinematics *k, double *joints, uint32_t joint_count, sim_posemath_pose *pose) {
    if (k == 0 || joints == 0 || pose == 0) return SIM_KIN_ERR_ARGUMENT;
    if (!k->configured || joint_count != kinematics_joint_count(&k->config)) return SIM_KIN_ERR_CONFIGURATION;
    memset(joints, 0, sizeof(double) * joint_count);
    if (k->config.type == SIM_KIN_XYZBCW || k->config.type == SIM_KIN_5AXISKINS) joints[config_axis(k, 8)] = 0.0;
    if (k->config.type == SIM_KIN_TRIPOD) {
        double z = 1.0;
        joints[config_axis(k, 0)] = z;
        joints[config_axis(k, 1)] = hypot(k->config.tripod_bx, z);
        joints[config_axis(k, 2)] = hypot(hypot(k->config.tripod_cx, k->config.tripod_cy), z);
    }
    if (k->config.type == SIM_KIN_GENHEX) {
        sim_posemath_pose seed = {0};
        seed.tran.z = k->config.genhex_initial_pose[2];
        if (sim_kinematics_inverse((sim_kinematics *)k, &seed, joints, joint_count) != SIM_KIN_OK) return SIM_KIN_ERR_RANGE;
    }
    if (k->config.type == SIM_KIN_PENTAPOD) {
        sim_posemath_pose seed = {0};
        seed.tran.x = k->config.penta_initial_pose[0]; seed.tran.y = k->config.penta_initial_pose[1]; seed.tran.z = k->config.penta_initial_pose[2]; seed.a = k->config.penta_initial_pose[3] * RAD_TO_DEG; seed.b = k->config.penta_initial_pose[4] * RAD_TO_DEG;
        if (sim_kinematics_inverse((sim_kinematics *)k, &seed, joints, joint_count) != SIM_KIN_OK) return SIM_KIN_ERR_RANGE;
    }
    return sim_kinematics_forward(k, joints, joint_count, pose);
}

uint64_t sim_kinematics_state_hash(const sim_kinematics *k, const double *joints, uint32_t joint_count, const sim_posemath_pose *pose) {
    unsigned char bytes[sizeof(sim_kinematics_config) + sizeof(double) * SIM_KIN_MAX_JOINTS + sizeof(sim_posemath_pose) + sizeof(int32_t) + sizeof(uint32_t) * 2];
    double normalized_joints[SIM_KIN_MAX_JOINTS] = {0};
    sim_posemath_pose normalized_pose;
    size_t offset = 0;
    if (k == 0 || joints == 0 || pose == 0 || joint_count > SIM_KIN_MAX_JOINTS) return 0;
    for (size_t index = 0; index < joint_count; ++index) normalized_joints[index] = hash_normalize(joints[index]);
    normalized_pose = *pose;
    normalized_pose.tran.x = hash_normalize(normalized_pose.tran.x); normalized_pose.tran.y = hash_normalize(normalized_pose.tran.y); normalized_pose.tran.z = hash_normalize(normalized_pose.tran.z);
    normalized_pose.a = hash_normalize(normalized_pose.a); normalized_pose.b = hash_normalize(normalized_pose.b); normalized_pose.c = hash_normalize(normalized_pose.c);
    normalized_pose.u = hash_normalize(normalized_pose.u); normalized_pose.v = hash_normalize(normalized_pose.v); normalized_pose.w = hash_normalize(normalized_pose.w);
    memcpy(bytes + offset, &k->config, sizeof(k->config)); offset += sizeof(k->config);
    memcpy(bytes + offset, normalized_joints, sizeof(double) * joint_count); offset += sizeof(double) * joint_count;
    memcpy(bytes + offset, &normalized_pose, sizeof(normalized_pose)); offset += sizeof(normalized_pose);
    memcpy(bytes + offset, &k->revolution_count, sizeof(k->revolution_count)); offset += sizeof(k->revolution_count);
    memcpy(bytes + offset, &k->previous_quadrant, sizeof(k->previous_quadrant)); offset += sizeof(k->previous_quadrant);
    memcpy(bytes + offset, &k->branch_id, sizeof(k->branch_id)); offset += sizeof(k->branch_id);
    return fnv1a64(bytes, offset);
}