#ifndef LINUXCNC_WEB_WASM_SIM_POSEMATH_H
#define LINUXCNC_WEB_WASM_SIM_POSEMATH_H

#ifdef __cplusplus
extern "C" {
#endif

#include <stdint.h>

enum sim_posemath_result {
    SIM_POSEMATH_OK = 0,
    SIM_POSEMATH_ERR_ARGUMENT = -400,
    SIM_POSEMATH_ERR_NONFINITE = -401,
    SIM_POSEMATH_ERR_DEGENERATE = -402,
    SIM_POSEMATH_ERR_RANGE = -403,
    SIM_POSEMATH_ERR_MOTION_TYPE = -404,
    SIM_POSEMATH_ERR_STATE = -405,
    SIM_POSEMATH_ERR_CAPACITY = -406,
};

/* Values match LinuxCNC's tc_motion_type_t. */
enum sim_posemath_motion_type {
    SIM_POSEMATH_LINEAR = 1,
    SIM_POSEMATH_CIRCULAR = 2,
    SIM_POSEMATH_RIGID_TAP = 3,
    SIM_POSEMATH_SPHERICAL = 4,
};

typedef struct sim_posemath_cartesian {
    double x;
    double y;
    double z;
} sim_posemath_cartesian;

typedef struct sim_posemath_pose {
    sim_posemath_cartesian tran;
    double a;
    double b;
    double c;
    double u;
    double v;
    double w;
} sim_posemath_pose;

typedef struct sim_posemath_segment {
    sim_posemath_pose start;
    sim_posemath_pose end;
    sim_posemath_cartesian center;
    sim_posemath_cartesian normal;
    double velocity;
    double acceleration;
    int motion_type;
    int turn;
} sim_posemath_segment;

typedef struct sim_tp_state {
    sim_posemath_pose position;
    double velocity;
    double progress;
    uint32_t tick;
    uint32_t queue_depth;
    uint32_t active_id;
    int motion_type;
    int status;
} sim_tp_state;

#define SIM_TP_QUEUE_CAPACITY 32u

typedef struct sim_tp {
    sim_posemath_segment queue[SIM_TP_QUEUE_CAPACITY];
    uint32_t ids[SIM_TP_QUEUE_CAPACITY];
    uint32_t head;
    uint32_t count;
    uint32_t next_id;
    uint32_t active_id;
    uint32_t tick;
    double cycle_time;
    double velocity_limit;
    double acceleration_limit;
    double progress;
    double current_velocity;
    sim_posemath_pose position;
    int motion_type;
    int status;
} sim_tp;

int sim_posemath_normalize(const sim_posemath_cartesian *input, sim_posemath_cartesian *output);
int sim_posemath_dot(const sim_posemath_cartesian *left, const sim_posemath_cartesian *right, double *output);
int sim_posemath_line_point(const sim_posemath_cartesian *start, const sim_posemath_cartesian *end,
                            double fraction, sim_posemath_cartesian *output);
int sim_posemath_is_finite(const sim_posemath_cartesian *value);
int sim_posemath_pose_is_finite(const sim_posemath_pose *value);
int sim_posemath_pose_line_point(const sim_posemath_pose *start, const sim_posemath_pose *end,
                                 double fraction, sim_posemath_pose *output);
int sim_posemath_segment_validate(const sim_posemath_segment *segment);
int sim_posemath_segment_point(const sim_posemath_segment *segment, double fraction,
                               sim_posemath_pose *output);
uint64_t sim_posemath_hash_state(const sim_tp_state *state);

int sim_tp_init(sim_tp *tp, double cycle_time, double velocity_limit, double acceleration_limit);
int sim_tp_reset(sim_tp *tp);
int sim_tp_enqueue(sim_tp *tp, const sim_posemath_segment *segment, uint32_t *id_out);
int sim_tp_step(sim_tp *tp);
int sim_tp_read_state(const sim_tp *tp, sim_tp_state *state);
uint64_t sim_tp_state_hash(const sim_tp *tp);

#ifdef __cplusplus
}
#endif

#endif