#ifndef LINUXCNC_WEB_WASM_SIM_KINEMATICS_H
#define LINUXCNC_WEB_WASM_SIM_KINEMATICS_H

#include "sim_posemath.h"

#ifdef __cplusplus
extern "C" {
#endif

enum sim_kinematics_result {
    SIM_KIN_OK = 0,
    SIM_KIN_ERR_ARGUMENT = -700,
    SIM_KIN_ERR_NONFINITE = -701,
    SIM_KIN_ERR_RANGE = -702,
    SIM_KIN_ERR_CONFIGURATION = -703,
    SIM_KIN_ERR_SINGULAR = -704,
    SIM_KIN_ERR_UNKNOWN_TYPE = -705,
    SIM_KIN_ERR_UNSUPPORTED = -706,
};

enum sim_kinematics_type {
    SIM_KIN_TRIVKINS = 1,
    SIM_KIN_XYZAC_TRT = 2,
    SIM_KIN_XYZBC_TRT = 3,
    SIM_KIN_XYZBCW = 4,
    SIM_KIN_COREXY = 5,
    SIM_KIN_ROSE = 6,
    SIM_KIN_MAXKINS = 7,
    SIM_KIN_ROTATEKINS = 8,
    SIM_KIN_5AXISKINS = 9,
    SIM_KIN_SCARA = 10,
    SIM_KIN_PUMA = 11,
    SIM_KIN_SCORBOT = 12,
    SIM_KIN_GENSER = 13,
    SIM_KIN_LINEAR_DELTA = 14,
    SIM_KIN_ROTARY_DELTA = 15,
    SIM_KIN_TRIPOD = 16,
    SIM_KIN_GENHEX = 17,
    SIM_KIN_PENTAPOD = 18,
};

/* LinuxCNC kinematics mode semantics.  Zero means the module default. */
enum sim_kinematics_mode {
    SIM_KIN_MODE_DEFAULT = 0,
    SIM_KIN_MODE_IDENTITY = 1,
    SIM_KIN_MODE_FORWARD_ONLY = 2,
    SIM_KIN_MODE_INVERSE_ONLY = 3,
    SIM_KIN_MODE_BOTH = 4,
};

/* Delta mechanisms expose the two analytic roots through the flags fields.
 * The default (zero) is LinuxCNC's lower/platform root; bit zero selects the
 * alternate root for callers that need an explicit branch comparison. */
#define SIM_KIN_DELTA_ALTERNATE_ROOT UINT64_C(1)
/* Tripod forward flags select the below-base solution when set. */
#define SIM_KIN_TRIPOD_BELOW_ROOT UINT64_C(1)
/* Genhex forward flags select the alternate (below-platform) initial seed. */
#define SIM_KIN_GENHEX_ALTERNATE_SEED UINT64_C(2)

#define SIM_KIN_MAX_JOINTS 9u
#define SIM_KIN_GENHEX_STRUTS 6u
#define SIM_KIN_PENTA_STRUTS 5u

typedef struct sim_kinematics_config {
    int type;
    int mode;
    uint32_t joint_count;
    /* Total configured joints and the prefix owned by kinematics. */
    uint32_t kinematics_joint_count;
    uint32_t extra_joint_count;
    char coordinates[16];
    double x_rot_point;
    double y_rot_point;
    double z_rot_point;
    double x_offset;
    double y_offset;
    double z_offset;
    double tool_offset;
    double pivot_length;
    double scara_d1;
    double scara_d2;
    double scara_d3;
    double scara_d4;
    double scara_d5;
    double scara_d6;
    double puma_a2;
    double puma_a3;
    double puma_d3;
    double puma_d4;
    double puma_d6;
    uint32_t dh_joint_count;
    double dh_a[SIM_KIN_MAX_JOINTS];
    double dh_alpha[SIM_KIN_MAX_JOINTS];
    double dh_d[SIM_KIN_MAX_JOINTS];
    double delta_radius;
    double delta_rod_length;
    double rotary_platform_radius;
    double rotary_thigh_length;
    double rotary_shin_length;
    double rotary_foot_radius;
    double tripod_bx;
    double tripod_cx;
    double tripod_cy;
    double genhex_base[SIM_KIN_GENHEX_STRUTS][3];
    double genhex_platform[SIM_KIN_GENHEX_STRUTS][3];
    double genhex_base_axis[SIM_KIN_GENHEX_STRUTS][3];
    double genhex_platform_axis[SIM_KIN_GENHEX_STRUTS][3];
    double genhex_screw_lead;
    double genhex_tool_offset;
    double genhex_spindle_offset;
    double genhex_max_error;
    double genhex_convergence_tolerance;
    uint32_t genhex_iteration_limit;
    double genhex_initial_pose[6];
    double penta_base[SIM_KIN_PENTA_STRUTS][3];
    double penta_effector_radius[SIM_KIN_PENTA_STRUTS];
    double penta_effector_z[SIM_KIN_PENTA_STRUTS];
    double penta_tool_offset;
    double penta_max_error;
    double penta_convergence_tolerance;
    uint32_t penta_iteration_limit;
    double penta_initial_pose[5];
    uint32_t max_iterations;
    double convergence_tolerance;
    uint8_t limit_enabled[SIM_KIN_MAX_JOINTS];
    double joint_min[SIM_KIN_MAX_JOINTS];
    double joint_max[SIM_KIN_MAX_JOINTS];
} sim_kinematics_config;

typedef struct sim_kinematics {
    sim_kinematics_config config;
    int axis_joint[SIM_KIN_MAX_JOINTS];
    uint32_t configured;
    uint64_t forward_flags;
    uint64_t inverse_flags;
    sim_posemath_pose last_pose;
    double last_residual;
    uint32_t last_iterations;
    uint32_t branch_id;
    int32_t revolution_count;
    uint32_t previous_quadrant;
} sim_kinematics;

typedef struct sim_kinematics_result_info {
    uint64_t forward_flags;
    uint64_t inverse_flags;
    double residual;
    uint32_t iterations;
    uint32_t branch_id;
} sim_kinematics_result_info;

int sim_kinematics_init(sim_kinematics *kinematics, const sim_kinematics_config *config);
int sim_kinematics_forward(const sim_kinematics *kinematics, const double *joints,
                           uint32_t joint_count, sim_posemath_pose *pose);
int sim_kinematics_forward_ex(const sim_kinematics *kinematics, const double *joints,
                              uint32_t joint_count, uint64_t inverse_flags,
                              sim_posemath_pose *pose, sim_kinematics_result_info *info);
int sim_kinematics_inverse(sim_kinematics *kinematics, const sim_posemath_pose *pose,
                           double *joints, uint32_t joint_count);
int sim_kinematics_inverse_ex(sim_kinematics *kinematics, const sim_posemath_pose *pose,
                              uint32_t joint_count, uint64_t forward_flags,
                              double *joints, sim_kinematics_result_info *info);
int sim_kinematics_home(const sim_kinematics *kinematics, double *joints, uint32_t joint_count,
                        sim_posemath_pose *pose);
int sim_kinematics_validate_pose(const sim_posemath_pose *pose);
uint64_t sim_kinematics_state_hash(const sim_kinematics *kinematics,
                                   const double *joints, uint32_t joint_count,
                                   const sim_posemath_pose *pose);

#ifdef __cplusplus
}
#endif

#endif