#ifndef LINUXCNC_WEB_WASM_SIM_KINEMATICS_MOTION_H
#define LINUXCNC_WEB_WASM_SIM_KINEMATICS_MOTION_H

#include "sim_kinematics.h"
#include "sim_posemath.h"

#ifdef __cplusplus
extern "C" {
#endif

enum sim_kinematics_motion_result {
    SIM_KIN_MOTION_OK = 0,
    SIM_KIN_MOTION_ERR_ARGUMENT = -800,
    SIM_KIN_MOTION_ERR_STATE = -801,
    SIM_KIN_MOTION_ERR_CAPACITY = -802,
    SIM_KIN_MOTION_ERR_PLANNER = -803,
    SIM_KIN_MOTION_ERR_INVERSE = -804,
    SIM_KIN_MOTION_ERR_NONFINITE = -805,
    SIM_KIN_MOTION_ERR_CONFIGURATION = -806,
};

#define SIM_KIN_MOTION_QUEUE_CAPACITY 64u

typedef struct sim_kinematics_motion_sample {
    sim_posemath_pose pose;
    double joints[SIM_KIN_MAX_JOINTS];
    uint32_t joint_count;
    uint32_t tick;
    uint32_t planner_id;
} sim_kinematics_motion_sample;

typedef struct sim_kinematics_motion {
    sim_tp planner;
    sim_kinematics *kinematics;
    sim_kinematics_motion_sample samples[SIM_KIN_MOTION_QUEUE_CAPACITY];
    uint32_t head;
    uint32_t count;
    uint32_t tick;
    uint32_t joint_count;
    uint32_t paused;
    int last_error;
    int planner_error;
    int kinematics_error;
    sim_posemath_pose last_pose;
    double last_joints[SIM_KIN_MAX_JOINTS];
} sim_kinematics_motion;

int sim_kinematics_motion_init(sim_kinematics_motion *adapter,
                               sim_kinematics *kinematics, double cycle_time,
                               double velocity_limit, double acceleration_limit);
int sim_kinematics_motion_reset(sim_kinematics_motion *adapter);
int sim_kinematics_motion_enqueue(sim_kinematics_motion *adapter,
                                  const sim_posemath_segment *segment,
                                  uint32_t *id_out);
int sim_kinematics_motion_pause(sim_kinematics_motion *adapter, uint32_t paused);
int sim_kinematics_motion_step(sim_kinematics_motion *adapter);
int sim_kinematics_motion_peek(const sim_kinematics_motion *adapter,
                               sim_kinematics_motion_sample *sample);
int sim_kinematics_motion_pop(sim_kinematics_motion *adapter,
                              sim_kinematics_motion_sample *sample);
uint32_t sim_kinematics_motion_depth(const sim_kinematics_motion *adapter);
uint64_t sim_kinematics_motion_state_hash(const sim_kinematics_motion *adapter);

#ifdef __cplusplus
}
#endif

#endif