#include "sim_io.h"

#include "sim_hal.h"

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

static uint64_t io_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 normalized(double value) {
    return isfinite(value) ? nearbyint(value * 1000000000.0) / 1000000000.0 : value;
}

static int finite_pose(const sim_posemath_pose *pose) {
    const double *values;
    uint32_t index;
    if (pose == 0) return 0;
    values = &pose->tran.x;
    for (index = 0; index < 9; ++index) if (!isfinite(values[index])) return 0;
    return 1;
}

static int valid_io(const sim_io *io) {
    return io != 0 && io->state.spindle_count > 0 &&
           io->state.spindle_count <= SIM_IO_MAX_SPINDLES;
}

static int valid_spindle(const sim_io *io, uint32_t spindle) {
    return valid_io(io) && spindle < io->state.spindle_count &&
           io->state.spindles[spindle].registered;
}

static void set_last(sim_io *io, int result) {
    if (io != 0) io->state.last_error = result;
}

static int register_pin(const char *name, uint32_t type) {
    int result = sim_hal_register_pin(name, type);
    if (result == SIM_HAL_OK) return SIM_IO_OK;
    if (result == SIM_HAL_ERR_DUPLICATE) {
        sim_hal_value existing = {0};
        if (sim_hal_get_pin(name, &existing) == SIM_HAL_OK && existing.type == type) return SIM_IO_OK;
    }
    return SIM_IO_ERR_PIN;
}

static int register_io_pins(sim_io *io) {
    static const struct { const char *name; uint32_t type; } fixed[] = {
        {"motion.probe-input", SIM_HAL_BIT},
        {"iocontrol.0.user-enable-out", SIM_HAL_BIT},
        {"iocontrol.0.emc-enable-in", SIM_HAL_BIT},
        {"iocontrol.0.user-request-enable", SIM_HAL_BIT},
        {"iocontrol.0.coolant-mist", SIM_HAL_BIT},
        {"iocontrol.0.coolant-flood", SIM_HAL_BIT},
        {"iocontrol.0.tool-prepare", SIM_HAL_BIT},
        {"iocontrol.0.tool-prep-number", SIM_HAL_S32},
        {"iocontrol.0.tool-prep-pocket", SIM_HAL_S32},
        {"iocontrol.0.tool-prep-index", SIM_HAL_S32},
        {"iocontrol.0.tool-number", SIM_HAL_S32},
        {"iocontrol.0.tool-prepared", SIM_HAL_BIT},
        {"iocontrol.0.tool-change", SIM_HAL_BIT},
        {"iocontrol.0.tool-changed", SIM_HAL_BIT},
        {"iocontrol.0.toolchanger-fault", SIM_HAL_BIT},
        {"iocontrol.0.toolchanger-fault-ack", SIM_HAL_BIT},
        {"iocontrol.0.toolchanger-reason", SIM_HAL_S32},
        {"iocontrol.0.toolchanger-faulted", SIM_HAL_BIT},
        {"iocontrol.0.toolchanger-clear-fault", SIM_HAL_BIT},
        {"iocontrol.0.emc-abort", SIM_HAL_BIT},
        {"iocontrol.0.emc-abort-ack", SIM_HAL_BIT},
        {"iocontrol.0.emc-reason", SIM_HAL_S32},
        {"iocontrol.0.start-change", SIM_HAL_BIT},
        {"iocontrol.0.start-change-ack", SIM_HAL_BIT},
        {"iocontrol.0.state", SIM_HAL_S32},
    };
    uint32_t index;
    char name[64];
    int result;
    result = sim_hal_register_thread("sim-io-servo", 1);
    if (result != SIM_HAL_OK && result != SIM_HAL_ERR_DUPLICATE) return SIM_IO_ERR_PIN;
    result = sim_hal_register_function("sim-io-servo", "sim-io");
    if (result != SIM_HAL_OK && result != SIM_HAL_ERR_DUPLICATE) return SIM_IO_ERR_PIN;
    for (index = 0; index < sizeof(fixed) / sizeof(fixed[0]); ++index) {
        result = register_pin(fixed[index].name, fixed[index].type);
        if (result != SIM_IO_OK) return result;
    }
    for (index = 0; index < io->state.spindle_count; ++index) {
        static const struct { const char *suffix; uint32_t type; } spindle_pins[] = {
            {"on", SIM_HAL_BIT}, {"forward", SIM_HAL_BIT}, {"reverse", SIM_HAL_BIT},
            {"brake", SIM_HAL_BIT}, {"speed-out", SIM_HAL_FLOAT},
            {"speed-out-abs", SIM_HAL_FLOAT}, {"speed-out-rps", SIM_HAL_FLOAT},
            {"speed-out-rps-abs", SIM_HAL_FLOAT}, {"speed-cmd-rps", SIM_HAL_FLOAT},
            {"inhibit", SIM_HAL_BIT}, {"amp-fault-in", SIM_HAL_BIT},
            {"orient-angle", SIM_HAL_FLOAT}, {"orient-mode", SIM_HAL_S32},
            {"orient", SIM_HAL_BIT}, {"locked", SIM_HAL_BIT},
            {"is-oriented", SIM_HAL_BIT}, {"orient-fault", SIM_HAL_S32},
            {"revs", SIM_HAL_FLOAT}, {"speed-in", SIM_HAL_FLOAT},
            {"at-speed", SIM_HAL_BIT}, {"index-enable", SIM_HAL_BIT},
        };
        uint32_t pin;
        for (pin = 0; pin < sizeof(spindle_pins) / sizeof(spindle_pins[0]); ++pin) {
            int written = snprintf(name, sizeof(name), "spindle.%u.%s", index, spindle_pins[pin].suffix);
            if (written < 0 || (size_t)written >= sizeof(name)) return SIM_IO_ERR_PIN;
            result = register_pin(name, spindle_pins[pin].type);
            if (result != SIM_IO_OK) return result;
        }
    }
    io->pins_registered = 1;
    return SIM_IO_OK;
}

static void set_bit(const char *name, uint32_t value) {
    (void)sim_hal_set_pin(name, (sim_hal_value){SIM_HAL_BIT, {.bit = value ? 1U : 0U}});
}

static void set_s32(const char *name, int32_t value) {
    (void)sim_hal_set_pin(name, (sim_hal_value){SIM_HAL_S32, {.s32 = value}});
}

static void set_float(const char *name, double value) {
    (void)sim_hal_set_pin(name, (sim_hal_value){SIM_HAL_FLOAT, {.real = (float)value}});
}

static void mirror_pins(sim_io *io) {
    char name[64];
    uint32_t index;
    sim_io_state *state = &io->state;
    set_bit("motion.probe-input", state->probe.input);
    set_bit("iocontrol.0.user-enable-out", state->estop);
    set_bit("iocontrol.0.emc-enable-in", state->ready);
    set_bit("iocontrol.0.user-request-enable", 0);
    set_bit("iocontrol.0.coolant-mist", 0);
    set_bit("iocontrol.0.coolant-flood", 0);
    set_bit("iocontrol.0.tool-prepare", state->tool_prepare);
    set_s32("iocontrol.0.tool-prep-number", state->prepared_tool);
    set_s32("iocontrol.0.tool-prep-pocket", state->prepared_pocket);
    set_s32("iocontrol.0.tool-prep-index", state->prepared_pocket);
    set_s32("iocontrol.0.tool-number", state->current_tool);
    set_bit("iocontrol.0.tool-prepared", state->tool_prepared);
    set_bit("iocontrol.0.tool-change", state->tool_change);
    set_bit("iocontrol.0.tool-changed", state->tool_changed);
    set_bit("iocontrol.0.toolchanger-fault", state->toolchanger_fault);
    set_bit("iocontrol.0.toolchanger-fault-ack", state->toolchanger_fault ? 0 : 1);
    set_s32("iocontrol.0.toolchanger-reason", state->toolchanger_reason);
    set_bit("iocontrol.0.toolchanger-faulted", state->fault);
    set_bit("iocontrol.0.toolchanger-clear-fault", 0);
    set_bit("iocontrol.0.emc-abort", state->tool_state == SIM_IO_TOOL_ABORTED);
    set_bit("iocontrol.0.emc-abort-ack", state->tool_state != SIM_IO_TOOL_ABORTED);
    set_s32("iocontrol.0.emc-reason", state->fault_reason);
    set_bit("iocontrol.0.start-change", state->tool_change);
    set_bit("iocontrol.0.start-change-ack", state->tool_state != SIM_IO_TOOL_CHANGING);
    set_s32("iocontrol.0.state", (int32_t)state->tool_state);
    for (index = 0; index < state->spindle_count; ++index) {
        sim_io_spindle_state *spindle = &state->spindles[index];
        snprintf(name, sizeof(name), "spindle.%u.on", index); set_bit(name, spindle->enabled && spindle->speed_rpm != 0.0);
        snprintf(name, sizeof(name), "spindle.%u.forward", index); set_bit(name, spindle->forward);
        snprintf(name, sizeof(name), "spindle.%u.reverse", index); set_bit(name, spindle->reverse);
        snprintf(name, sizeof(name), "spindle.%u.brake", index); set_bit(name, spindle->brake);
        snprintf(name, sizeof(name), "spindle.%u.speed-out", index); set_float(name, spindle->speed_rpm);
        snprintf(name, sizeof(name), "spindle.%u.speed-out-abs", index); set_float(name, fabs(spindle->speed_rpm));
        snprintf(name, sizeof(name), "spindle.%u.speed-out-rps", index); set_float(name, spindle->speed_rpm / 60.0);
        snprintf(name, sizeof(name), "spindle.%u.speed-out-rps-abs", index); set_float(name, fabs(spindle->speed_rpm) / 60.0);
        snprintf(name, sizeof(name), "spindle.%u.speed-cmd-rps", index); set_float(name, spindle->command_rpm / 60.0);
        snprintf(name, sizeof(name), "spindle.%u.inhibit", index); set_bit(name, spindle->inhibit);
        snprintf(name, sizeof(name), "spindle.%u.amp-fault-in", index); set_bit(name, spindle->amp_fault);
        snprintf(name, sizeof(name), "spindle.%u.orient-angle", index); set_float(name, spindle->orient_angle);
        snprintf(name, sizeof(name), "spindle.%u.orient-mode", index); set_s32(name, spindle->orient_mode);
        snprintf(name, sizeof(name), "spindle.%u.orient", index); set_bit(name, spindle->orient);
        snprintf(name, sizeof(name), "spindle.%u.locked", index); set_bit(name, spindle->locked);
        snprintf(name, sizeof(name), "spindle.%u.is-oriented", index); set_bit(name, spindle->is_oriented);
        snprintf(name, sizeof(name), "spindle.%u.orient-fault", index); set_s32(name, spindle->orient_fault);
        snprintf(name, sizeof(name), "spindle.%u.revs", index); set_float(name, spindle->revs);
        snprintf(name, sizeof(name), "spindle.%u.speed-in", index); set_float(name, spindle->speed_rpm);
        snprintf(name, sizeof(name), "spindle.%u.at-speed", index); set_bit(name, spindle->at_speed);
        snprintf(name, sizeof(name), "spindle.%u.index-enable", index); set_bit(name, spindle->index_enable);
    }
}

static void state_defaults(sim_io *io) {
    uint32_t index;
    sim_io_state old = io->state;
    memset(&io->state, 0, sizeof(io->state));
    io->state.spindle_count = old.spindle_count;
    io->state.tool_capacity = old.tool_capacity;
    io->state.ready = 1;
    io->state.current_tool = 0;
    io->state.prepared_tool = -1;
    io->state.prepared_pocket = -1;
    io->state.toolchanger_reason = 0;
    io->state.tool_state = SIM_IO_TOOL_IDLE;
    io->state.last_error = SIM_IO_OK;
    io->state.probe.state = SIM_IO_PROBE_IDLE;
    for (index = 0; index < io->state.spindle_count; ++index) {
        io->state.spindles[index].registered = 1;
        io->state.spindles[index].max_rpm = 100000.0;
        io->state.spindles[index].accel_rpm_s = old.spindles[index].accel_rpm_s;
        io->state.spindles[index].tolerance_rpm = old.spindles[index].tolerance_rpm;
        io->state.spindles[index].at_speed = 1;
    }
}

int sim_io_init(sim_io *io, uint32_t spindle_count, uint32_t tool_capacity,
                double cycle_time, double spindle_accel_rpm_s,
                double spindle_tolerance_rpm) {
    uint32_t index;
    int result;
    if (io == 0) return SIM_IO_ERR_ARGUMENT;
    if (spindle_count == 0 || spindle_count > SIM_IO_MAX_SPINDLES ||
        tool_capacity == 0 || tool_capacity > SIM_IO_MAX_TOOLS ||
        !isfinite(cycle_time) || cycle_time <= 0.0 || cycle_time > 60.0 ||
        !isfinite(spindle_accel_rpm_s) || spindle_accel_rpm_s <= 0.0 ||
        !isfinite(spindle_tolerance_rpm) || spindle_tolerance_rpm < 0.0)
        return SIM_IO_ERR_RANGE;
    memset(io, 0, sizeof(*io));
    io->cycle_time = cycle_time;
    io->state.spindle_count = spindle_count;
    io->state.tool_capacity = tool_capacity;
    for (index = 0; index < spindle_count; ++index) {
        io->state.spindles[index].accel_rpm_s = spindle_accel_rpm_s;
        io->state.spindles[index].tolerance_rpm = spindle_tolerance_rpm;
    }
    result = register_io_pins(io);
    if (result != SIM_IO_OK) return result;
    state_defaults(io);
    mirror_pins(io);
    return SIM_IO_OK;
}

int sim_io_reset(sim_io *io) {
    if (!valid_io(io)) return SIM_IO_ERR_ARGUMENT;
    state_defaults(io);
    mirror_pins(io);
    return SIM_IO_OK;
}

int sim_io_register_pins(sim_io *io) {
    if (!valid_io(io)) return SIM_IO_ERR_ARGUMENT;
    return register_io_pins(io);
}

int sim_io_spindle_command(sim_io *io, uint32_t spindle, double rpm, uint32_t enabled) {
    sim_io_spindle_state *state;
    if (!valid_spindle(io, spindle) || enabled > 1U) return SIM_IO_ERR_ARGUMENT;
    if (!isfinite(rpm)) { set_last(io, SIM_IO_ERR_NONFINITE); return SIM_IO_ERR_NONFINITE; }
    if (fabs(rpm) > io->state.spindles[spindle].max_rpm) { set_last(io, SIM_IO_ERR_LIMIT); return SIM_IO_ERR_LIMIT; }
    if (io->state.estop) { set_last(io, SIM_IO_ERR_ESTOP); return SIM_IO_ERR_ESTOP; }
    if (!io->state.ready) { set_last(io, SIM_IO_ERR_NOT_READY); return SIM_IO_ERR_NOT_READY; }
    state = &io->state.spindles[spindle];
    state->command_rpm = enabled && !state->inhibit && !state->amp_fault ? rpm : 0.0;
    state->enabled = enabled ? 1U : 0U;
    set_last(io, SIM_IO_OK);
    mirror_pins(io);
    return SIM_IO_OK;
}

int sim_io_spindle_set_inhibit(sim_io *io, uint32_t spindle, uint32_t inhibit) {
    if (!valid_spindle(io, spindle) || inhibit > 1U) return SIM_IO_ERR_ARGUMENT;
    io->state.spindles[spindle].inhibit = inhibit;
    if (inhibit) io->state.spindles[spindle].command_rpm = 0.0;
    mirror_pins(io);
    return SIM_IO_OK;
}

int sim_io_spindle_set_fault(sim_io *io, uint32_t spindle, uint32_t fault) {
    if (!valid_spindle(io, spindle) || fault > 1U) return SIM_IO_ERR_ARGUMENT;
    io->state.spindles[spindle].amp_fault = fault;
    if (fault) {
        io->state.spindles[spindle].command_rpm = 0.0;
        io->state.spindles[spindle].enabled = 0;
    }
    mirror_pins(io);
    return SIM_IO_OK;
}

int sim_io_spindle_set_index_enable(sim_io *io, uint32_t spindle, uint32_t enable) {
    if (!valid_spindle(io, spindle) || enable > 1U) return SIM_IO_ERR_ARGUMENT;
    io->state.spindles[spindle].index_enable = enable;
    mirror_pins(io);
    return SIM_IO_OK;
}

int sim_io_spindle_set_feedback(sim_io *io, uint32_t spindle, double rpm) {
    if (!valid_spindle(io, spindle)) return SIM_IO_ERR_ARGUMENT;
    if (!isfinite(rpm)) return SIM_IO_ERR_NONFINITE;
    io->state.spindles[spindle].speed_rpm = rpm;
    io->state.spindles[spindle].at_speed = fabs(rpm - io->state.spindles[spindle].command_rpm) <= io->state.spindles[spindle].tolerance_rpm;
    mirror_pins(io);
    return SIM_IO_OK;
}

int sim_io_spindle_orient(sim_io *io, uint32_t spindle, double angle, int32_t mode) {
    sim_io_spindle_state *state;
    if (!valid_spindle(io, spindle)) return SIM_IO_ERR_ARGUMENT;
    if (!isfinite(angle) || mode < 0 || mode > 2) return SIM_IO_ERR_RANGE;
    if (io->state.estop) return SIM_IO_ERR_ESTOP;
    state = &io->state.spindles[spindle];
    if (fabs(state->speed_rpm) > state->tolerance_rpm || state->command_rpm != 0.0) return SIM_IO_ERR_BUSY;
    state->orient = 1; state->locked = 0; state->is_oriented = 0;
    state->orient_angle = angle; state->orient_mode = mode; state->orient_fault = 0;
    mirror_pins(io);
    return SIM_IO_OK;
}

int sim_io_spindle_set_oriented(sim_io *io, uint32_t spindle, uint32_t oriented, int32_t fault) {
    sim_io_spindle_state *state;
    if (!valid_spindle(io, spindle) || oriented > 1U) return SIM_IO_ERR_ARGUMENT;
    state = &io->state.spindles[spindle];
    if (!state->orient) return SIM_IO_ERR_STATE;
    state->is_oriented = oriented;
    state->orient_fault = fault;
    if (fault) { state->orient = 0; state->locked = 0; }
    else if (oriented) { state->orient = 0; state->locked = 1; }
    mirror_pins(io);
    return SIM_IO_OK;
}

static int spindle_stopped(const sim_io *io) {
    uint32_t index;
    for (index = 0; index < io->state.spindle_count; ++index)
        if (fabs(io->state.spindles[index].speed_rpm) > io->state.spindles[index].tolerance_rpm || io->state.spindles[index].command_rpm != 0.0) return 0;
    return 1;
}

int sim_io_tool_prepare(sim_io *io, int32_t tool, int32_t pocket) {
    if (!valid_io(io)) return SIM_IO_ERR_ARGUMENT;
    if (tool < 0 || pocket <= 0 || (uint32_t)pocket >= io->state.tool_capacity) return SIM_IO_ERR_RANGE;
    if (io->state.estop) return SIM_IO_ERR_ESTOP;
    if (!io->state.ready) return SIM_IO_ERR_NOT_READY;
    if (io->state.fault) return SIM_IO_ERR_FAULT;
    io->state.prepared_tool = tool; io->state.prepared_pocket = pocket;
    io->state.tool_prepare = 1; io->state.tool_prepared = 0; io->state.tool_changed = 0;
    io->state.tool_state = SIM_IO_TOOL_PREPARING;
    mirror_pins(io);
    return SIM_IO_OK;
}

int sim_io_tool_set_prepared(sim_io *io, uint32_t prepared) {
    if (!valid_io(io) || prepared > 1U) return SIM_IO_ERR_ARGUMENT;
    if (io->state.tool_state != SIM_IO_TOOL_PREPARING) return SIM_IO_ERR_STATE;
    if (!prepared) return SIM_IO_OK;
    io->state.tool_prepare = 0; io->state.tool_prepared = 1; io->state.tool_state = SIM_IO_TOOL_PREPARED;
    mirror_pins(io);
    return SIM_IO_OK;
}

int sim_io_tool_change(sim_io *io) {
    if (!valid_io(io)) return SIM_IO_ERR_ARGUMENT;
    if (io->state.estop) return SIM_IO_ERR_ESTOP;
    if (!io->state.ready) return SIM_IO_ERR_NOT_READY;
    if (io->state.fault) return SIM_IO_ERR_FAULT;
    if (io->state.tool_state != SIM_IO_TOOL_PREPARED || !io->state.tool_prepared) return SIM_IO_ERR_STATE;
    if (!spindle_stopped(io)) return SIM_IO_ERR_BUSY;
    io->state.tool_change = 1; io->state.tool_changed = 0; io->state.tool_state = SIM_IO_TOOL_CHANGING;
    mirror_pins(io);
    return SIM_IO_OK;
}

int sim_io_tool_set_changed(sim_io *io, uint32_t changed) {
    if (!valid_io(io) || changed > 1U) return SIM_IO_ERR_ARGUMENT;
    if (io->state.tool_state != SIM_IO_TOOL_CHANGING) return SIM_IO_ERR_STATE;
    if (!changed) return SIM_IO_OK;
    io->state.tool_change = 0; io->state.tool_changed = 1; io->state.tool_prepared = 0;
    io->state.current_tool = io->state.prepared_tool; io->state.tool_state = SIM_IO_TOOL_READY;
    mirror_pins(io);
    return SIM_IO_OK;
}

int sim_io_tool_abort(sim_io *io, int32_t reason) {
    if (!valid_io(io)) return SIM_IO_ERR_ARGUMENT;
    io->state.tool_prepare = 0; io->state.tool_change = 0; io->state.tool_prepared = 0;
    io->state.tool_changed = 0; io->state.toolchanger_reason = reason;
    io->state.tool_state = SIM_IO_TOOL_ABORTED;
    io->state.last_error = SIM_IO_ERR_RECOVERY;
    mirror_pins(io);
    return SIM_IO_OK;
}

int sim_io_tool_set_fault(sim_io *io, uint32_t fault, int32_t reason) {
    if (!valid_io(io) || fault > 1U) return SIM_IO_ERR_ARGUMENT;
    io->state.toolchanger_fault = fault; io->state.toolchanger_reason = reason;
    io->state.fault = fault; io->state.fault_reason = reason;
    if (fault) { io->state.tool_state = SIM_IO_TOOL_FAULT; io->state.tool_prepare = 0; io->state.tool_change = 0; }
    mirror_pins(io);
    return SIM_IO_OK;
}

int sim_io_tool_clear_fault(sim_io *io) {
    if (!valid_io(io)) return SIM_IO_ERR_ARGUMENT;
    if (io->state.toolchanger_fault) return SIM_IO_ERR_FAULT;
    io->state.fault = 0; io->state.fault_reason = 0;
    if (io->state.tool_state == SIM_IO_TOOL_FAULT || io->state.tool_state == SIM_IO_TOOL_ABORTED) io->state.tool_state = SIM_IO_TOOL_IDLE;
    mirror_pins(io);
    return SIM_IO_OK;
}

int sim_io_probe_arm(sim_io *io, uint32_t probe_type) {
    if (!valid_io(io)) return SIM_IO_ERR_ARGUMENT;
    if (io->state.estop) return SIM_IO_ERR_ESTOP;
    if (!io->state.ready) return SIM_IO_ERR_NOT_READY;
    io->state.probe.probe_type = probe_type; io->state.probe.state = SIM_IO_PROBE_ARMED;
    io->state.probe.tripped = 0; io->state.probe.tick = io->state.tick;
    mirror_pins(io);
    return SIM_IO_OK;
}

int sim_io_probe_update(sim_io *io, const sim_posemath_pose *pose, uint32_t input) {
    uint32_t when_clears;
    if (!valid_io(io) || pose == 0) return SIM_IO_ERR_ARGUMENT;
    if (!finite_pose(pose)) return SIM_IO_ERR_NONFINITE;
    if (input > 1U) return SIM_IO_ERR_ARGUMENT;
    io->state.probe.input = input;
    when_clears = (io->state.probe.probe_type & 2U) != 0;
    if (io->state.probe.state == SIM_IO_PROBE_ARMED && ((input != 0U) ^ when_clears)) {
        io->state.probe.position = *pose; io->state.probe.tripped = 1;
        io->state.probe.state = SIM_IO_PROBE_TRIPPED; io->state.probe.tick = io->state.tick;
    }
    mirror_pins(io);
    return SIM_IO_OK;
}

int sim_io_probe_disarm(sim_io *io) {
    if (!valid_io(io)) return SIM_IO_ERR_ARGUMENT;
    if (io->state.probe.state == SIM_IO_PROBE_ARMED) io->state.probe.state = SIM_IO_PROBE_IDLE;
    io->state.probe.probe_type = 0; io->state.probe.tripped = 0;
    mirror_pins(io);
    return SIM_IO_OK;
}

int sim_io_set_estop(sim_io *io, uint32_t estop) {
    uint32_t index;
    if (!valid_io(io) || estop > 1U) return SIM_IO_ERR_ARGUMENT;
    io->state.estop = estop;
    if (estop) {
        io->state.ready = 0; io->state.tool_prepare = 0; io->state.tool_change = 0;
        io->state.tool_prepared = 0; io->state.tool_changed = 0;
        io->state.tool_state = SIM_IO_TOOL_ABORTED;
        for (index = 0; index < io->state.spindle_count; ++index) {
            io->state.spindles[index].command_rpm = 0.0; io->state.spindles[index].enabled = 0;
            io->state.spindles[index].brake = 1;
        }
        io->state.last_error = SIM_IO_ERR_ESTOP;
    }
    mirror_pins(io);
    return SIM_IO_OK;
}

int sim_io_recover(sim_io *io) {
    if (!valid_io(io)) return SIM_IO_ERR_ARGUMENT;
    if (io->state.estop) return SIM_IO_ERR_ESTOP;
    if (io->state.fault || io->state.toolchanger_fault) return SIM_IO_ERR_FAULT;
    if (!spindle_stopped(io)) return SIM_IO_ERR_BUSY;
    io->state.ready = 1; io->state.recovery_epoch += 1; io->state.tool_state = SIM_IO_TOOL_IDLE;
    io->state.last_error = SIM_IO_OK;
    mirror_pins(io);
    return SIM_IO_OK;
}

int sim_io_step(sim_io *io, uint32_t ticks) {
    uint32_t tick, index;
    if (!valid_io(io)) return SIM_IO_ERR_ARGUMENT;
    if (ticks == 0) return SIM_IO_OK;
    if (ticks > 1000000U || UINT32_MAX - io->state.tick < ticks) return SIM_IO_ERR_RANGE;
    if (sim_hal_step(ticks) != SIM_HAL_OK) return SIM_IO_ERR_PIN;
    for (tick = 0; tick < ticks; ++tick) {
        io->state.tick += 1;
        for (index = 0; index < io->state.spindle_count; ++index) {
            sim_io_spindle_state *spindle = &io->state.spindles[index];
            double target = spindle->command_rpm;
            double delta = spindle->accel_rpm_s * io->cycle_time;
            if (spindle->amp_fault || spindle->inhibit || !io->state.ready || io->state.estop) target = 0.0;
            if (spindle->speed_rpm < target) spindle->speed_rpm = fmin(target, spindle->speed_rpm + delta);
            else if (spindle->speed_rpm > target) spindle->speed_rpm = fmax(target, spindle->speed_rpm - delta);
            spindle->forward = spindle->speed_rpm > spindle->tolerance_rpm;
            spindle->reverse = spindle->speed_rpm < -spindle->tolerance_rpm;
            spindle->brake = fabs(target - spindle->speed_rpm) > spindle->tolerance_rpm && target == 0.0;
            spindle->at_speed = fabs(spindle->speed_rpm - target) <= spindle->tolerance_rpm;
            spindle->revs += spindle->speed_rpm * io->cycle_time / 60.0;
            spindle->tick = io->state.tick;
            if (spindle->index_enable && floor(spindle->revs) != floor(spindle->revs - spindle->speed_rpm * io->cycle_time / 60.0)) {
                spindle->index_enable = 0; spindle->revs = spindle->revs - floor(spindle->revs);
            }
            if (spindle->orient && spindle->at_speed && spindle->speed_rpm == 0.0) {
                spindle->orient = 0; spindle->locked = 1; spindle->is_oriented = 1;
            }
        }
    }
    mirror_pins(io);
    set_last(io, SIM_IO_OK);
    return SIM_IO_OK;
}

int sim_io_read_state(const sim_io *io, sim_io_state *state) {
    if (!valid_io(io) || state == 0) return SIM_IO_ERR_ARGUMENT;
    *state = io->state;
    return SIM_IO_OK;
}

uint64_t sim_io_state_hash(const sim_io *io) {
    unsigned char bytes[8192];
    size_t offset = 0;
    uint32_t index;
    if (!valid_io(io)) return 0;
#define APPEND(value) do { memcpy(bytes + offset, &(value), sizeof(value)); offset += sizeof(value); } while (0)
    APPEND(io->state.ready); APPEND(io->state.estop); APPEND(io->state.fault); APPEND(io->state.fault_reason);
    APPEND(io->state.tick); APPEND(io->state.recovery_epoch); APPEND(io->state.spindle_count); APPEND(io->state.tool_capacity);
    for (index = 0; index < io->state.spindle_count; ++index) {
        sim_io_spindle_state spindle = io->state.spindles[index];
        spindle.command_rpm = normalized(spindle.command_rpm); spindle.speed_rpm = normalized(spindle.speed_rpm);
        spindle.max_rpm = normalized(spindle.max_rpm); spindle.accel_rpm_s = normalized(spindle.accel_rpm_s);
        spindle.tolerance_rpm = normalized(spindle.tolerance_rpm); spindle.orient_angle = normalized(spindle.orient_angle);
        spindle.revs = normalized(spindle.revs); APPEND(spindle);
    }
    APPEND(io->state.tool_state); APPEND(io->state.current_tool); APPEND(io->state.prepared_tool); APPEND(io->state.prepared_pocket);
    APPEND(io->state.tool_prepare); APPEND(io->state.tool_prepared); APPEND(io->state.tool_change); APPEND(io->state.tool_changed);
    APPEND(io->state.toolchanger_fault); APPEND(io->state.toolchanger_reason); APPEND(io->state.probe.input);
    APPEND(io->state.probe.probe_type); APPEND(io->state.probe.state); APPEND(io->state.probe.tripped); APPEND(io->state.probe.tick);
    { sim_posemath_pose pose = io->state.probe.position; double *values = &pose.tran.x; for (index = 0; index < 9; ++index) values[index] = normalized(values[index]); APPEND(pose); }
    APPEND(io->state.last_error);
#undef APPEND
    return io_fnv1a64(bytes, offset);
}

uint32_t sim_io_canonical(const sim_io *io, char *output, uint32_t capacity) {
    char discard[1];
    char *destination = output == 0 ? discard : output;
    uint32_t destination_capacity = output == 0 ? 1 : capacity;
    int written;
    uint32_t index;
    if (!valid_io(io)) return 0;
    written = snprintf(destination, destination_capacity,
        "{\"estop\":%u,\"fault\":%u,\"fault_reason\":%d,\"last_error\":%d,\"probe\":{\"input\":%u,\"position\":[%.9g,%.9g,%.9g],\"state\":%u,\"tick\":%u,\"tripped\":%u},\"ready\":%u,\"recovery_epoch\":%u,\"spindles\":[",
        io->state.estop, io->state.fault, io->state.fault_reason, io->state.last_error,
        io->state.probe.input, io->state.probe.position.tran.x, io->state.probe.position.tran.y,
        io->state.probe.position.tran.z, io->state.probe.state, io->state.probe.tick,
        io->state.probe.tripped, io->state.ready, io->state.recovery_epoch);
    if (written < 0) return 0;
    for (index = 0; index < io->state.spindle_count; ++index) {
        const sim_io_spindle_state *spindle = &io->state.spindles[index];
        int part = snprintf(destination + (output && (uint32_t)written < capacity ? written : 0),
            output && (uint32_t)written < capacity ? capacity - (uint32_t)written : 0,
            "%s{\"at_speed\":%u,\"brake\":%u,\"command_rpm\":%.9g,\"forward\":%u,\"reverse\":%u,\"speed_rpm\":%.9g,\"tick\":%u}",
            index ? "," : "", spindle->at_speed, spindle->brake, spindle->command_rpm,
            spindle->forward, spindle->reverse, spindle->speed_rpm, spindle->tick);
        if (part < 0) return 0; written += part;
    }
    { int part = snprintf(destination + (output && (uint32_t)written < capacity ? written : 0),
        output && (uint32_t)written < capacity ? capacity - (uint32_t)written : 0,
        "],\"tick\":%u,\"tool\":{\"changed\":%u,\"current\":%d,\"prepared\":%u,\"prepared_pocket\":%d,\"prepared_tool\":%d,\"prepare\":%u,\"state\":%u}}",
        io->state.tick, io->state.tool_changed, io->state.current_tool, io->state.tool_prepared,
        io->state.prepared_pocket, io->state.prepared_tool, io->state.tool_prepare, io->state.tool_state);
        if (part < 0) return 0; written += part; }
    return (uint32_t)written + 1;
}