#include "sim_linuxcnc_tp.h"

#include "motion.h"
#include "tp.h"
#include "emcpose.h"
#include "motion_types.h"

#include <math.h>
#include <stddef.h>
#include <stdio.h>
#include <string.h>

extern int tcGetPosReal(TC_STRUCT const *tc, int of_point, EmcPose *pos);

struct sim_linuxcnc_tp {
    TP_STRUCT tp;
    emcmot_status_t status;
    emcmot_config_t config;
    double axis_velocity[9];
    double axis_acceleration[9];
    double axis_min[9];
    double axis_max[9];
    double velocity_scale;
    double jog_scale;
    double scale_minimum;
    uint32_t tick;
    uint32_t last_active_id;
    int just_completed;
    int initialized;
    int step_id;
};

static sim_linuxcnc_tp *active_planner;

/* Same-build checkpoint: TP/TC contain the integration and blend state which
   cannot be reconstructed by re-enqueuing the remaining geometric path.
   The queue pointer is removed on export and rebound on restore. All other
   members of these source structs are values, not process addresses. */
typedef struct {
    uint32_t magic, planner_size, tc_size, capacity;
    sim_linuxcnc_tp planner;
    TC_STRUCT queue[TP_DEFAULT_QUEUE_SIZE];
} tp_checkpoint;

uint32_t sim_linuxcnc_tp_checkpoint_size(void) { return (uint32_t)sizeof(tp_checkpoint); }

int32_t sim_linuxcnc_tp_checkpoint_export(const sim_linuxcnc_tp *planner, uint8_t *output, uint32_t capacity) {
    if (planner == NULL || !planner->initialized || output == NULL || capacity != sizeof(tp_checkpoint) || planner->tp.queue.size != TP_DEFAULT_QUEUE_SIZE) return -1228;
    tp_checkpoint *saved = (tp_checkpoint *)output;
    memset(saved, 0, sizeof(*saved));
    saved->magic = UINT32_C(0x54504331);
    saved->planner_size = sizeof(sim_linuxcnc_tp); saved->tc_size = sizeof(TC_STRUCT); saved->capacity = TP_DEFAULT_QUEUE_SIZE;
    saved->planner = *planner;
    saved->planner.tp.queue.queue = NULL;
    memcpy(saved->queue, planner->tp.queue.queue, sizeof(saved->queue));
    return 0;
}

int32_t sim_linuxcnc_tp_checkpoint_restore(sim_linuxcnc_tp *planner, const uint8_t *input, uint32_t length) {
    if (planner == NULL || !planner->initialized || input == NULL || length != sizeof(tp_checkpoint)) return -1228;
    const tp_checkpoint *saved = (const tp_checkpoint *)input;
    const TC_QUEUE_STRUCT *queue = &saved->planner.tp.queue;
    if (saved->magic != UINT32_C(0x54504331) || saved->planner_size != sizeof(sim_linuxcnc_tp) || saved->tc_size != sizeof(TC_STRUCT) || saved->capacity != TP_DEFAULT_QUEUE_SIZE || saved->planner.initialized != 1 || queue->queue != NULL || queue->size != TP_DEFAULT_QUEUE_SIZE || queue->start < 0 || queue->start >= queue->size || queue->end < 0 || queue->end >= queue->size || queue->_len < 0 || queue->_rlen < 0 || queue->_len > queue->size || queue->_rlen > queue->size - queue->_len) return -1228;
    if (!isfinite(saved->planner.tp.cycleTime) || saved->planner.tp.cycleTime != planner->tp.cycleTime || memcmp(&saved->planner.config, &planner->config, sizeof(planner->config)) != 0 || memcmp(saved->planner.axis_min, planner->axis_min, sizeof(planner->axis_min)) != 0 || memcmp(saved->planner.axis_max, planner->axis_max, sizeof(planner->axis_max)) != 0 || memcmp(saved->planner.axis_velocity, planner->axis_velocity, sizeof(planner->axis_velocity)) != 0 || memcmp(saved->planner.axis_acceleration, planner->axis_acceleration, sizeof(planner->axis_acceleration)) != 0) return -1229;
    TC_STRUCT *storage = planner->tp.queue.queue;
    *planner = saved->planner;
    planner->tp.queue.queue = storage;
    memcpy(storage, saved->queue, sizeof(saved->queue));
    active_planner = planner;
    tpMotData(&planner->status, &planner->config);
    return 0;
}

static void noop_dio(int index, char value) { (void)index; (void)value; }
static void noop_aio(int index, double value) { (void)index; (void)value; }
static void noop_rotary_unlock(int index, int value) { (void)index; (void)value; }
static int noop_rotary_locked(int index) { (void)index; return 0; }
static double axis_velocity(int axis) { return active_planner && axis >= 0 && axis < 9 ? active_planner->axis_velocity[axis] : 0.0; }
static double axis_acceleration(int axis) { return active_planner && axis >= 0 && axis < 9 ? active_planner->axis_acceleration[axis] : 0.0; }

static double pose_delta(const EmcPose *start, const EmcPose *end, int axis) {
    const double values_start[9] = {start->tran.x, start->tran.y, start->tran.z, start->a, start->b, start->c, start->u, start->v, start->w};
    const double values_end[9] = {end->tran.x, end->tran.y, end->tran.z, end->a, end->b, end->c, end->u, end->v, end->w};
    return values_end[axis] - values_start[axis];
}

static void xyzac_line_limits(const sim_linuxcnc_tp *planner, const EmcPose *start, const EmcPose *end, double *velocity, double *acceleration) {
    double linear_length = 0.0, angular_length = 0.0, max_velocity_time = 0.0, max_acceleration_time = 0.0;
    int has_linear = 0, has_angular = 0;
    for (int axis = 0; axis < 9; ++axis) {
        const double delta = fabs(pose_delta(start, end, axis));
        const double velocity_limit = planner->axis_velocity[axis];
        const double acceleration_limit = planner->axis_acceleration[axis];
        if (delta <= 1e-12) continue;
        if (axis < 3 || axis >= 6) { has_linear = 1; linear_length += delta * delta; }
        else { has_angular = 1; angular_length += delta * delta; }
        if (velocity_limit > 0.0) max_velocity_time = fmax(max_velocity_time, delta / velocity_limit);
        if (acceleration_limit > 0.0) max_acceleration_time = fmax(max_acceleration_time, delta / acceleration_limit);
    }
    const double distance = has_linear ? sqrt(linear_length) : has_angular ? sqrt(angular_length) : 0.0;
    if (!(distance > 0.0)) return;
    if (max_velocity_time > 0.0) *velocity = fmin(*velocity, distance / max_velocity_time);
    if (max_acceleration_time > 0.0) *acceleration = fmin(*acceleration, distance / max_acceleration_time);
}

static void xyzac_circle_limits(const sim_linuxcnc_tp *planner, const EmcPose *start, const EmcPose *end, const PmCartesian *center, const PmCartesian *normal, int turn, double *velocity, double *initial_velocity, double *acceleration) {
    const double sx = start->tran.x - center->x, sy = start->tran.y - center->y, sz = start->tran.z - center->z;
    const double ex = end->tran.x - center->x, ey = end->tran.y - center->y, ez = end->tran.z - center->z;
    // ARC_FEED measures radius in the active plane.  For the XYZAC case the
    // normal is +Z, so the axial helix delta must not become a radial spiral
    // term (the previous 3-D radius caused a small but accumulating TP error).
    const double normal_abs_x = fabs(normal->x), normal_abs_y = fabs(normal->y), normal_abs_z = fabs(normal->z);
    const double start_radius = normal_abs_z >= fmax(normal_abs_x, normal_abs_y) ? hypot(sx, sy) : normal_abs_y >= normal_abs_x ? hypot(sx, sz) : hypot(sy, sz);
    const double end_radius = normal_abs_z >= fmax(normal_abs_x, normal_abs_y) ? hypot(ex, ey) : normal_abs_y >= normal_abs_x ? hypot(ex, ez) : hypot(ey, ez);
    const double dot = sx * ex + sy * ey + sz * ez;
    const double cross_x = sy * ez - sz * ey, cross_y = sz * ex - sx * ez, cross_z = sx * ey - sy * ex;
    double angle = fabs(atan2(normal->x * cross_x + normal->y * cross_y + normal->z * cross_z, dot));
    if (angle <= 1e-12) angle = 2.0 * M_PI;
    angle += 2.0 * M_PI * (fabs((double)turn) - 1.0);
    const double spiral = end_radius - start_radius;
    const double circular_length = fmin(start_radius, end_radius) * fabs(angle);
    const double spiral_length = hypot(circular_length, spiral);
    const double effective_radius = sqrt((spiral / fmax(fabs(angle), 1e-12)) * (spiral / fmax(fabs(angle), 1e-12)) + fmin(start_radius, end_radius) * fmin(start_radius, end_radius));
    const double planar_velocity = fmin(sqrt(planner->axis_acceleration[0] * sqrt(3.0) * 0.5 * effective_radius), fmin(planner->axis_velocity[0], fmin(planner->axis_velocity[1], planner->axis_velocity[2])));
    double total_xyz_length = hypot(spiral_length, normal->x * (end->tran.x - start->tran.x) + normal->y * (end->tran.y - start->tran.y) + normal->z * (end->tran.z - start->tran.z));
    double time_motion_velocity = 0.0, time_motion_acceleration = 0.0;
    for (int axis = 0; axis < 9; ++axis) {
        const double delta = fabs(pose_delta(start, end, axis));
        if (planner->axis_velocity[axis] > 0.0) time_motion_velocity = fmax(time_motion_velocity, delta / planner->axis_velocity[axis]);
        if (planner->axis_acceleration[axis] > 0.0) time_motion_acceleration = fmax(time_motion_acceleration, delta / planner->axis_acceleration[axis]);
    }
    const double time_spiral_velocity = planar_velocity > 0.0 ? spiral_length / planar_velocity : INFINITY;
    const double time_spiral_acceleration = planner->axis_acceleration[0] > 0.0 ? spiral_length / planner->axis_acceleration[0] : INFINITY;
    const double max_velocity = total_xyz_length / fmax(time_motion_velocity, time_spiral_velocity);
    const double max_acceleration = total_xyz_length / fmax(time_motion_acceleration, time_spiral_acceleration);
    *velocity = fmin(*velocity, max_velocity);
    *initial_velocity = max_velocity;
    *acceleration = fmin(*acceleration, max_acceleration);
}

static EmcPose to_emc_pose(const sim_posemath_pose *value) {
    EmcPose result = {0};
    if (value != NULL) {
        result.tran.x = value->tran.x; result.tran.y = value->tran.y; result.tran.z = value->tran.z;
        result.a = value->a; result.b = value->b; result.c = value->c;
        result.u = value->u; result.v = value->v; result.w = value->w;
    }
    return result;
}

static PmCartesian to_pm_cart(const sim_posemath_cartesian *value) {
    PmCartesian result = {0, 0, 0};
    if (value != NULL) { result.x = value->x; result.y = value->y; result.z = value->z; }
    return result;
}

static sim_posemath_pose from_emc_pose(const EmcPose *value) {
    sim_posemath_pose result = {{0, 0, 0}, 0, 0, 0, 0, 0, 0};
    if (value != NULL) {
        result.tran.x = value->tran.x; result.tran.y = value->tran.y; result.tran.z = value->tran.z;
        result.a = value->a; result.b = value->b; result.c = value->c;
        result.u = value->u; result.v = value->v; result.w = value->w;
    }
    return result;
}

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

uint32_t sim_linuxcnc_tp_size(void) { return (uint32_t)sizeof(sim_linuxcnc_tp); }
uint32_t sim_linuxcnc_tp_queue_capacity(const sim_linuxcnc_tp *planner) { return planner == NULL || !planner->initialized ? 0u : (uint32_t)TP_DEFAULT_QUEUE_SIZE; }

int32_t sim_linuxcnc_tp_init(sim_linuxcnc_tp *planner, double cycle_time, double velocity_limit, double acceleration_limit) {
    if (planner == NULL || !isfinite(cycle_time) || cycle_time <= 0.0 || cycle_time > 60.0 || !isfinite(velocity_limit) || velocity_limit <= 0.0 || !isfinite(acceleration_limit) || acceleration_limit <= 0.0) return -1200;
    memset(planner, 0, sizeof(*planner));
    planner->config.numJoints = 5; planner->config.numExtraJoints = 0; planner->config.numSpindles = 1;
    planner->config.numDIO = 0; planner->config.numAIO = 0; planner->config.numMiscError = 0;
    planner->config.trajCycleTime = cycle_time; planner->config.servoCycleTime = cycle_time;
    planner->config.limitVel = velocity_limit; planner->config.maxFeedScale = 2.0;
    planner->config.arcBlendEnable = 1; planner->config.arcBlendFallbackEnable = 0;
    planner->config.arcBlendOptDepth = 50; planner->config.arcBlendGapCycles = 4;
    planner->config.arcBlendRampFreq = 100.0; planner->config.arcBlendTangentKinkRatio = 0.1;
    // The caller configures each axis from INI before motion is queued.
    // Standalone planners use their supplied trajectory limits.
    for (int axis = 0; axis < 9; ++axis) {
        planner->axis_velocity[axis] = velocity_limit;
        planner->axis_acceleration[axis] = acceleration_limit;
        planner->axis_min[axis] = -INFINITY;
        planner->axis_max[axis] = INFINITY;
    }
    planner->velocity_scale = 1.0;
    planner->jog_scale = 1.0;
    planner->scale_minimum = 0.0;
    planner->status.feed_scale = 1.0; planner->status.rapid_scale = 1.0; planner->status.net_feed_scale = 1.0;
    planner->status.enables_new = FS_ENABLED | FH_ENABLED; planner->status.enables_queued = planner->status.enables_new;
    planner->status.motionFlag = EMCMOT_MOTION_ENABLE_BIT | EMCMOT_MOTION_COORD_BIT | EMCMOT_MOTION_INPOS_BIT;
    active_planner = planner;
    tpMotData(&planner->status, &planner->config);
    tpMotFunctions(noop_dio, noop_aio, noop_rotary_unlock, noop_rotary_locked, axis_velocity, axis_acceleration);
    if (tpCreate(&planner->tp, TP_DEFAULT_QUEUE_SIZE, 0) != TP_ERR_OK) return -1201;
    if (tpSetCycleTime(&planner->tp, cycle_time) != TP_ERR_OK || tpSetVlimit(&planner->tp, velocity_limit) != TP_ERR_OK || tpSetAmax(&planner->tp, acceleration_limit) != TP_ERR_OK) return -1202;
    planner->initialized = 1;
    return 0;
}

int32_t sim_linuxcnc_tp_reset(sim_linuxcnc_tp *planner) {
    if (planner == NULL || !planner->initialized) return -1203;
    active_planner = planner;
    planner->last_active_id = 0;
    planner->just_completed = 0;
    return tpClear(&planner->tp) == TP_ERR_OK ? 0 : -1204;
}

int32_t sim_linuxcnc_tp_set_profile(sim_linuxcnc_tp *planner, int profile, double tolerance) {
    if (planner == NULL || !planner->initialized || profile < 0 || profile > 2 || !isfinite(tolerance) || tolerance < 0.0) return -1205;
    active_planner = planner;
    return tpSetTermCond(&planner->tp, profile, tolerance) == TP_ERR_OK ? 0 : -1206;
}

int32_t sim_linuxcnc_tp_set_feed_override(sim_linuxcnc_tp *planner, double scale) {
    if (planner == NULL || !planner->initialized || !isfinite(scale) || scale < 0.0 || scale > planner->config.maxFeedScale) return -1207;
    planner->status.net_feed_scale = scale; planner->status.feed_scale = scale; return 0;
}

int32_t sim_linuxcnc_tp_set_velocity_scale(sim_linuxcnc_tp *planner, double scale) {
    if (planner == NULL || !planner->initialized || !isfinite(scale) || scale <= 0.0 || scale < planner->scale_minimum || scale > 1.0) return -1211;
    active_planner = planner;
    if (tpSetVlimit(&planner->tp, planner->config.limitVel * scale) != TP_ERR_OK) return -1212;
    planner->velocity_scale = scale;
    return 0;
}

int32_t sim_linuxcnc_tp_set_jog_scale(sim_linuxcnc_tp *planner, double scale) {
    if (planner == NULL || !planner->initialized || !isfinite(scale) || scale <= 0.0 || scale < planner->scale_minimum || scale > 1.0) return -1213;
    planner->jog_scale = scale;
    return 0;
}

int32_t sim_linuxcnc_tp_set_axis_limit(sim_linuxcnc_tp *planner, int axis, double minimum, double maximum) {
    if (planner == NULL || !planner->initialized || axis < 0 || axis >= 9 || !isfinite(minimum) || !isfinite(maximum) || minimum >= maximum) return -1214;
    planner->axis_min[axis] = minimum;
    planner->axis_max[axis] = maximum;
    return 0;
}

int32_t sim_linuxcnc_tp_set_axis_dynamics(sim_linuxcnc_tp *planner, int axis, double velocity, double acceleration) {
    if (planner == NULL || !planner->initialized || axis < 0 || axis >= 9 || !isfinite(velocity) || velocity <= 0.0 || !isfinite(acceleration) || acceleration <= 0.0) return -1214;
    planner->axis_velocity[axis] = velocity;
    planner->axis_acceleration[axis] = acceleration;
    return 0;
}

int32_t sim_linuxcnc_tp_set_feed_limit(sim_linuxcnc_tp *planner, double maximum) {
    if (planner == NULL || !planner->initialized || !isfinite(maximum) || maximum < 1.0) return -1207;
    planner->config.maxFeedScale = maximum;
    return 0;
}

int32_t sim_linuxcnc_tp_set_scale_minimum(sim_linuxcnc_tp *planner, double minimum) {
    if (planner == NULL || !planner->initialized || !isfinite(minimum) || minimum <= 0.0 || minimum > 1.0) return -1211;
    planner->scale_minimum = minimum;
    return 0;
}

int32_t sim_linuxcnc_tp_set_blend(sim_linuxcnc_tp *planner, int enable, int fallback, int depth, int gap, double ramp, double kink) {
    if (planner == NULL || !planner->initialized || (enable != 0 && enable != 1) || (fallback != 0 && fallback != 1) || depth < 1 || gap < 0 || !isfinite(ramp) || ramp <= 0.0 || !isfinite(kink) || kink <= 0.0) return -1205;
    planner->config.arcBlendEnable = enable;
    planner->config.arcBlendFallbackEnable = fallback;
    planner->config.arcBlendOptDepth = depth;
    planner->config.arcBlendGapCycles = gap;
    planner->config.arcBlendRampFreq = ramp;
    planner->config.arcBlendTangentKinkRatio = kink;
    return 0;
}

int32_t sim_linuxcnc_tp_jog(sim_linuxcnc_tp *planner, int axis, int direction, double increment, uint32_t tag) {
    EmcPose end;
    double *value;
    double velocity;
    struct state_tag_t state_tag = {0};
    if (planner == NULL || !planner->initialized || axis < 0 || axis >= 9 || (direction != -1 && direction != 1) || !isfinite(increment) || increment < 0.0) return -1215;
    active_planner = planner;
    end = planner->tp.goalPos;
    value = axis == 0 ? &end.tran.x : axis == 1 ? &end.tran.y : axis == 2 ? &end.tran.z : axis == 3 ? &end.a : axis == 4 ? &end.b : axis == 5 ? &end.c : axis == 6 ? &end.u : axis == 7 ? &end.v : &end.w;
    // command.c::EMCMOT_JOG_CONT targets the directional jog limit until abort.
    // Zero is the continuous-mode encoding; positive increments retain their ABI.
    if (increment == 0.0) {
        double target = direction > 0 ? planner->axis_max[axis] : planner->axis_min[axis];
        if ((target - *value) * direction <= 0.0) return -1217;
        *value = target;
    } else {
        *value += (double)direction * increment;
    }
    if (!isfinite(*value) || *value < planner->axis_min[axis] || *value > planner->axis_max[axis]) return -1217;
    velocity = planner->axis_velocity[axis] * planner->jog_scale;
    state_tag.fields_float[GM_FIELD_FLOAT_LINE_NUMBER] = (float)tag;
    if (tpSetId(&planner->tp, (int)tag) != TP_ERR_OK) return -1218;
    return tpAddLine(&planner->tp, end, TC_LINEAR, velocity, velocity, planner->axis_acceleration[axis], planner->status.enables_new, 0, -1, state_tag) == TP_ERR_OK ? 0 : -1219;
}

double sim_linuxcnc_tp_velocity_scale(const sim_linuxcnc_tp *planner) {
    return planner == NULL || !planner->initialized ? 0.0 : planner->velocity_scale;
}

double sim_linuxcnc_tp_velocity_limit(const sim_linuxcnc_tp *planner) {
    return planner == NULL || !planner->initialized ? 0.0 : planner->tp.vLimit;
}

double sim_linuxcnc_tp_velocity_mm_min(const sim_linuxcnc_tp *planner) {
    return planner == NULL || !planner->initialized ? 0.0 : planner->status.current_vel * 60.0;
}

double sim_linuxcnc_tp_jog_scale(const sim_linuxcnc_tp *planner) {
    return planner == NULL || !planner->initialized ? 0.0 : planner->jog_scale;
}

double sim_linuxcnc_tp_jog_velocity(const sim_linuxcnc_tp *planner, int axis) {
    return planner == NULL || !planner->initialized || axis < 0 || axis >= 9 ? 0.0 : planner->axis_velocity[axis] * planner->jog_scale;
}

double sim_linuxcnc_tp_feed_override(const sim_linuxcnc_tp *planner) {
    return planner == NULL || !planner->initialized ? 0.0 : planner->status.net_feed_scale;
}

int32_t sim_linuxcnc_tp_set_dynamics(sim_linuxcnc_tp *planner, double acceleration_limit) {
    if (planner == NULL || !planner->initialized || !isfinite(acceleration_limit) || acceleration_limit <= 0.0) return -1217;
    active_planner = planner;
    for (int axis = 0; axis < 9; ++axis) planner->axis_acceleration[axis] = acceleration_limit;
    return tpSetAmax(&planner->tp, acceleration_limit) == TP_ERR_OK ? 0 : -1218;
}

int32_t sim_linuxcnc_tp_set_paused(sim_linuxcnc_tp *planner, int paused) {
    if (planner == NULL || !planner->initialized || (paused != 0 && paused != 1)) return -1219;
    active_planner = planner;
    planner->status.stepping = 0;
    if (paused) return tpPause(&planner->tp) == TP_ERR_OK ? 0 : -1220;
    return tpResume(&planner->tp) == TP_ERR_OK ? 0 : -1221;
}

int32_t sim_linuxcnc_tp_step_line(sim_linuxcnc_tp *planner) {
    if (planner == NULL || !planner->initialized) return -1219;
    TC_STRUCT *active = tcqItem(&planner->tp.queue, 0);
    if (active == NULL) return -1219;
    active_planner = planner;
    planner->step_id = active->id;
    planner->status.stepping = 1;
    return tpResume(&planner->tp) == TP_ERR_OK ? 0 : -1221;
}

int32_t sim_linuxcnc_tp_is_stepping(const sim_linuxcnc_tp *planner) {
    return planner != NULL && planner->initialized && planner->status.stepping;
}

double sim_linuxcnc_tp_axis_limit(const sim_linuxcnc_tp *planner, int axis, int maximum) {
    if (planner == NULL || !planner->initialized || axis < 0 || axis > 8) return NAN;
    return maximum ? planner->axis_max[axis] : planner->axis_min[axis];
}

int32_t sim_linuxcnc_tp_abort(sim_linuxcnc_tp *planner) {
    if (planner == NULL || !planner->initialized) return -1222;
    active_planner = planner;
    planner->last_active_id = 0;
    planner->just_completed = 0;
    planner->status.stepping = 0;
    if (tpAbort(&planner->tp) != TP_ERR_OK) return -1223;
    // A browser PROGRAM_ABORT has no background emcmot servo thread after
    // the command response. Complete the source TP abort transition here so
    // the authoritative snapshot cannot retain an active queue indefinitely.
    return tpClear(&planner->tp) == TP_ERR_OK ? 0 : -1223;
}

int32_t sim_linuxcnc_tp_set_position(sim_linuxcnc_tp *planner, const sim_posemath_pose *position) {
    if (planner == NULL || !planner->initialized || position == NULL) return -1208;
    active_planner = planner;
    EmcPose pose = to_emc_pose(position);
    return tpSetPos(&planner->tp, &pose) == TP_ERR_OK ? 0 : -1209;
}

int32_t sim_linuxcnc_tp_set_goal_position(sim_linuxcnc_tp *planner, const sim_posemath_pose *position) {
    if (planner == NULL || !planner->initialized || position == NULL) return -1228;
    planner->tp.goalPos = to_emc_pose(position);
    return 0;
}

int32_t sim_linuxcnc_tp_add_line(sim_linuxcnc_tp *planner, const sim_posemath_pose *end, double velocity, double acceleration, uint32_t tag, int canon_motion_type) {
    if (planner == NULL || !planner->initialized || end == NULL || !isfinite(velocity) || velocity < 0.0 || !isfinite(acceleration) || acceleration < 0.0) return -1210;
    active_planner = planner;
    struct state_tag_t state_tag = {0}; state_tag.fields_float[GM_FIELD_FLOAT_LINE_NUMBER] = (float)tag;
    EmcPose pose = to_emc_pose(end), start = planner->tp.goalPos;
    xyzac_line_limits(planner, &start, &pose, &velocity, &acceleration);
    if (tpSetId(&planner->tp, (int)tag) != TP_ERR_OK) return -1211;
    return tpAddLine(&planner->tp, pose, canon_motion_type, velocity, velocity, acceleration, planner->status.enables_new, 0, -1, state_tag) == TP_ERR_OK ? 0 : -1211;
}

int32_t sim_linuxcnc_tp_add_circle(sim_linuxcnc_tp *planner, const sim_posemath_pose *end, const sim_posemath_cartesian *center, const sim_posemath_cartesian *normal, int turn, double velocity, double acceleration, uint32_t tag, int canon_motion_type) {
    if (planner == NULL || !planner->initialized || end == NULL || center == NULL || normal == NULL || !isfinite(velocity) || velocity < 0.0 || !isfinite(acceleration) || acceleration < 0.0) return -1212;
    active_planner = planner;
    struct state_tag_t state_tag = {0}; state_tag.fields_float[GM_FIELD_FLOAT_LINE_NUMBER] = (float)tag;
    EmcPose pose = to_emc_pose(end), start = planner->tp.goalPos; PmCartesian pm_center = to_pm_cart(center); PmCartesian pm_normal = to_pm_cart(normal);
    double initial_velocity = velocity;
    xyzac_circle_limits(planner, &start, &pose, &pm_center, &pm_normal, turn, &velocity, &initial_velocity, &acceleration);
    if (tpSetId(&planner->tp, (int)tag) != TP_ERR_OK) return -1212;
    // emccanon computes ini_maxvel independently of the programmed feed for
    // ARC_FEED.  For this XYZAC case the planar XYZ limit (20 mm/s) is the
    // governing bound; passing the feed here loses the native corner ramp.
    return tpAddCircle(&planner->tp, pose, pm_center, pm_normal, turn, canon_motion_type, velocity, initial_velocity, acceleration, planner->status.enables_new, 0, state_tag) == TP_ERR_OK ? 0 : -1213;
}

int32_t sim_linuxcnc_tp_step(sim_linuxcnc_tp *planner) {
    if (planner == NULL || !planner->initialized) return -1214;
    active_planner = planner;
    planner->just_completed = 0;
    const int prior_id = tpGetExecId(&planner->tp);
    const int prior_depth = tcqLen(&planner->tp.queue);
    const int was_stepping = planner->status.stepping;
    const int result = tpRunCycle(&planner->tp, (long)llround(planner->tp.cycleTime * 1e9));
    // motion/control.c stops a step when the execution ID changes. Compare
    // the next queued ID too, before the following TC can start moving.
    TC_STRUCT *next = tcqItem(&planner->tp.queue, 0);
    if (planner->status.stepping && (next == NULL || next->id != planner->step_id)) {
        tpPause(&planner->tp);
        planner->status.stepping = 0;
    }
    planner->tick += 1u;
    if (!was_stepping && prior_id > 0 && prior_depth > 0 && tcqLen(&planner->tp.queue) == 0) {
        planner->last_active_id = (uint32_t)prior_id;
        planner->just_completed = 1;
    }
    return result >= TP_ERR_OK && result <= TP_ERR_WAITING ? 0 : result;
}

int32_t sim_linuxcnc_tp_idle(sim_linuxcnc_tp *planner) {
    if (planner == NULL || !planner->initialized) return -1227;
    planner->just_completed = 0;
    planner->tick += 1u;
    return 0;
}

int32_t sim_linuxcnc_tp_read_state(const sim_linuxcnc_tp *planner, sim_tp_state *state) {
    if (planner == NULL || !planner->initialized || state == NULL) return -1215;
    EmcPose position = {0};
    if (tpGetPos(&planner->tp, &position) != TP_ERR_OK) return -1216;
    memset(state, 0, sizeof(*state)); state->position = from_emc_pose(&position); state->velocity = planner->status.current_vel;
    TC_STRUCT *active = tcqItem(&planner->tp.queue, 0);
    if (active != NULL && active->target > 0.0) state->progress = active->progress / active->target;
    // tpQueueDepth() exposes TP's legacy cached depth, which is only updated
    // when appending a TC.  Native emcmot reports the live tcq length after a
    // segment pop, so use the upstream queue directly for oracle parity.
    state->tick = planner->tick;
    const uint32_t live_depth = (uint32_t)tcqLen(&planner->tp.queue);
    state->queue_depth = live_depth > 0u ? live_depth : planner->just_completed ? 1u : 0u;
    {
        const int exec_id = tpGetExecId((TP_STRUCT *)&planner->tp);
        state->active_id = state->queue_depth > 0 && exec_id > 0 ? (uint32_t)exec_id : planner->just_completed ? planner->last_active_id : 0u;
    }
    state->motion_type = tpGetMotionType((TP_STRUCT *)&planner->tp);
    state->status = tpIsDone((TP_STRUCT *)&planner->tp) ? 0 : 1; return 0;
}

uint32_t sim_linuxcnc_tp_queue_depth(const sim_linuxcnc_tp *planner) {
    return planner == NULL || !planner->initialized ? 0u : (uint32_t)tcqLen(&planner->tp.queue);
}

int32_t sim_linuxcnc_tp_queue_segment(const sim_linuxcnc_tp *planner, uint32_t index, sim_linuxcnc_tp_segment *segment) {
    TC_STRUCT *tc;
    EmcPose start = {0}, end = {0};
    if (planner == NULL || !planner->initialized || segment == NULL) return -1224;
    tc = tcqItem(&planner->tp.queue, (int)index);
    if (tc == NULL || tcGetPosReal(tc, TC_GET_STARTPOINT, &start) != TP_ERR_OK || tcGetPosReal(tc, TC_GET_ENDPOINT, &end) != TP_ERR_OK) return -1225;
    memset(segment, 0, sizeof(*segment));
    segment->segment.start = from_emc_pose(&start);
    segment->segment.end = from_emc_pose(&end);
    segment->segment.velocity = tc->reqvel;
    segment->segment.acceleration = tc->maxaccel;
    segment->segment.motion_type = tc->motion_type;
    if (tc->motion_type == TC_CIRCULAR) {
        const double turns = fabs(tc->coords.circle.xyz.angle) / (2.0 * M_PI);
        segment->segment.turn = (tc->coords.circle.xyz.angle < 0.0 ? -1 : 1) * (turns >= 1.0 ? (int)llround(turns) : 1);
    }
    if (tc->motion_type == TC_CIRCULAR) {
        segment->segment.center.x = tc->coords.circle.xyz.center.x;
        segment->segment.center.y = tc->coords.circle.xyz.center.y;
        segment->segment.center.z = tc->coords.circle.xyz.center.z;
        segment->segment.normal.x = tc->coords.circle.xyz.normal.x;
        segment->segment.normal.y = tc->coords.circle.xyz.normal.y;
        segment->segment.normal.z = tc->coords.circle.xyz.normal.z;
    } else {
        segment->segment.normal.z = 1.0;
    }
    segment->id = tc->id;
    segment->term_cond = tc->term_cond;
    segment->progress = tc->progress;
    segment->target = tc->target;
    return 0;
}

int32_t sim_linuxcnc_tp_restore_progress(sim_linuxcnc_tp *planner, const sim_posemath_pose *position, double velocity, uint32_t tick) {
    if (planner == NULL || !planner->initialized || position == NULL || !isfinite(velocity) || velocity < 0.0) return -1226;
    active_planner = planner;
    planner->tp.currentPos = to_emc_pose(position);
    planner->status.current_vel = velocity;
    planner->tick = tick;
    return 0;
}

uint64_t sim_linuxcnc_tp_state_hash(const sim_linuxcnc_tp *planner) {
    sim_tp_state state; if (sim_linuxcnc_tp_read_state(planner, &state) != 0) return 0; return hash_bytes((const unsigned char *)&state, sizeof(state));
}

uint32_t sim_linuxcnc_tp_canonical(const sim_linuxcnc_tp *planner, char *output, uint32_t capacity) {
    sim_tp_state state; int written;
    if (sim_linuxcnc_tp_read_state(planner, &state) != 0) return 0;
    written = snprintf(output, capacity, "{\"active_id\":%u,\"motion_type\":%d,\"position\":{\"a\":%.17g,\"c\":%.17g,\"x\":%.17g,\"y\":%.17g,\"z\":%.17g},\"queue_depth\":%u,\"schema\":\"linuxcnc-source-tp.v1\",\"tick\":%u,\"velocity\":%.17g}", state.active_id, state.motion_type, state.position.a, state.position.c, state.position.tran.x, state.position.tran.y, state.position.tran.z, state.queue_depth, state.tick, state.velocity);
    return written < 0 ? 0 : (uint32_t)written + 1u;
}