#include "sim_hal.h"

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

typedef struct {
    uint32_t used;
    char name[SIM_HAL_NAME_CAPACITY];
    sim_hal_value value;
} pin_entry;

typedef struct {
    uint32_t used;
    char name[SIM_HAL_NAME_CAPACITY];
    sim_hal_value value;
} param_entry;

typedef struct {
    uint32_t used;
    char name[SIM_HAL_NAME_CAPACITY];
    uint32_t period;
} thread_entry;

typedef struct {
    uint32_t used;
    char name[SIM_HAL_NAME_CAPACITY];
    uint32_t thread;
    uint32_t calls;
    uint32_t fail;
    uint32_t component_index;
    uint32_t component_kind;
} function_entry;

typedef struct {
    uint32_t used;
    char instance[SIM_HAL_NAME_CAPACITY];
    char type[SIM_HAL_NAME_CAPACITY];
    uint32_t kind;
    uint32_t function_index;
    double memory[4];
    uint32_t flags[4];
} component_entry;

typedef struct {
    uint32_t used;
    uint32_t source;
    uint32_t target;
} link_entry;

enum {
    SIM_HAL_COMPONENT_NONE = 0,
    SIM_HAL_COMPONENT_COREXY = 1,
    SIM_HAL_COMPONENT_CONSTANT,
    SIM_HAL_COMPONENT_AND2,
    SIM_HAL_COMPONENT_OR2,
    SIM_HAL_COMPONENT_XOR2,
    SIM_HAL_COMPONENT_NOT,
    SIM_HAL_COMPONENT_ABS,
    SIM_HAL_COMPONENT_ABS_S32,
    SIM_HAL_COMPONENT_ABS_S64,
    SIM_HAL_COMPONENT_LOWPASS,
    SIM_HAL_COMPONENT_DDT,
    SIM_HAL_COMPONENT_INTEG,
    SIM_HAL_COMPONENT_MUX2,
    SIM_HAL_COMPONENT_SUM2,
    SIM_HAL_COMPONENT_MULT2,
    SIM_HAL_COMPONENT_DIV2,
    SIM_HAL_COMPONENT_HYPOT,
    SIM_HAL_COMPONENT_NEAR,
    SIM_HAL_COMPONENT_COMP,
    SIM_HAL_COMPONENT_INVERT,
    SIM_HAL_COMPONENT_EDGE,
    SIM_HAL_COMPONENT_TOGGLE,
    SIM_HAL_COMPONENT_ESTOP_LATCH
};

typedef struct {
    uint32_t tick;
    pin_entry pins[SIM_HAL_PIN_CAPACITY];
    param_entry params[SIM_HAL_PARAM_CAPACITY];
    thread_entry threads[SIM_HAL_THREAD_CAPACITY];
    function_entry functions[SIM_HAL_FUNCTION_CAPACITY];
    component_entry components[SIM_HAL_COMPONENT_CAPACITY];
    link_entry links[256U];
} hal_state;

static hal_state state;

static int valid_name(const char *name) {
    return name != NULL && name[0] != '\0' && strlen(name) < SIM_HAL_NAME_CAPACITY;
}

static int find_pin(const char *name) {
    uint32_t index;
    for (index = 0; index < SIM_HAL_PIN_CAPACITY; ++index) {
        if (state.pins[index].used && strcmp(state.pins[index].name, name) == 0) return (int)index;
    }
    return -1;
}

static int find_param(const char *name) {
    uint32_t index;
    for (index = 0; index < SIM_HAL_PARAM_CAPACITY; ++index) {
        if (state.params[index].used && strcmp(state.params[index].name, name) == 0) return (int)index;
    }
    return -1;
}

static int find_thread(const char *name) {
    uint32_t index;
    for (index = 0; index < SIM_HAL_THREAD_CAPACITY; ++index) {
        if (state.threads[index].used && strcmp(state.threads[index].name, name) == 0) return (int)index;
    }
    return -1;
}

static int find_function(const char *name) {
    uint32_t index;
    for (index = 0; index < SIM_HAL_FUNCTION_CAPACITY; ++index) {
        if (state.functions[index].used && strcmp(state.functions[index].name, name) == 0) return (int)index;
    }
    return -1;
}

static int find_component(const char *instance) {
    uint32_t index;
    for (index = 0; index < SIM_HAL_COMPONENT_CAPACITY; ++index) {
        if (state.components[index].used && strcmp(state.components[index].instance, instance) == 0) return (int)index;
    }
    return -1;
}

static int finite_value(sim_hal_value value) {
    return value.type != SIM_HAL_FLOAT || isfinite((double)value.value.real);
}

static int type_from_name(const char *name, uint32_t *type) {
    if (name == NULL || type == NULL) return 0;
    if (strcmp(name, "bit") == 0) *type = SIM_HAL_BIT;
    else if (strcmp(name, "s32") == 0 || strcmp(name, "signed") == 0) *type = SIM_HAL_S32;
    else if (strcmp(name, "u32") == 0 || strcmp(name, "unsigned") == 0) *type = SIM_HAL_U32;
    else if (strcmp(name, "float") == 0) *type = SIM_HAL_FLOAT;
    else return 0;
    return 1;
}

void sim_hal_reset(void) {
    memset(&state, 0, sizeof(state));
}

int32_t sim_hal_register_pin(const char *name, uint32_t type) {
    uint32_t index;
    if (!valid_name(name) || type < SIM_HAL_BIT || type > SIM_HAL_FLOAT) return SIM_HAL_ERR_ARGUMENT;
    if (find_pin(name) >= 0) return SIM_HAL_ERR_DUPLICATE;
    for (index = 0; index < SIM_HAL_PIN_CAPACITY; ++index) {
        if (!state.pins[index].used) {
            state.pins[index].used = 1;
            strcpy(state.pins[index].name, name);
            state.pins[index].value.type = type;
            return SIM_HAL_OK;
        }
    }
    return SIM_HAL_ERR_FULL;
}

int32_t sim_hal_set_pin(const char *name, sim_hal_value value) {
    int index = find_pin(name);
    if (index < 0) return SIM_HAL_ERR_NOT_FOUND;
    if (value.type != state.pins[index].value.type) return SIM_HAL_ERR_TYPE;
    if (value.type == SIM_HAL_BIT && value.value.bit > 1U) return SIM_HAL_ERR_RANGE;
    if (!finite_value(value)) return SIM_HAL_ERR_RANGE;
    state.pins[index].value = value;
    return SIM_HAL_OK;
}

int32_t sim_hal_set_pin_scalar(const char *name, uint32_t type, double value) {
    sim_hal_value scalar = {0};
    scalar.type = type;
    if (type == SIM_HAL_BIT) scalar.value.bit = value != 0.0;
    else if (type == SIM_HAL_S32) scalar.value.s32 = (int32_t)value;
    else if (type == SIM_HAL_U32) scalar.value.u32 = (uint32_t)value;
    else if (type == SIM_HAL_FLOAT) scalar.value.real = (float)value;
    else return SIM_HAL_ERR_TYPE;
    return sim_hal_set_pin(name, scalar);
}

int32_t sim_hal_get_pin(const char *name, sim_hal_value *out) {
    int index = find_pin(name);
    if (index < 0 || out == NULL) return index < 0 ? SIM_HAL_ERR_NOT_FOUND : SIM_HAL_ERR_ARGUMENT;
    *out = state.pins[index].value;
    return SIM_HAL_OK;
}

int32_t sim_hal_set_param(const char *name, sim_hal_value value) {
    int index = find_param(name);
    if (index < 0) return SIM_HAL_ERR_NOT_FOUND;
    if (value.type != state.params[index].value.type) return SIM_HAL_ERR_TYPE;
    if (value.type == SIM_HAL_BIT && value.value.bit > 1U) return SIM_HAL_ERR_RANGE;
    if (!finite_value(value)) return SIM_HAL_ERR_RANGE;
    state.params[index].value = value;
    return SIM_HAL_OK;
}

int32_t sim_hal_set_param_scalar(const char *name, uint32_t type, double value) {
    sim_hal_value scalar = {0};
    scalar.type = type;
    if (type == SIM_HAL_BIT) scalar.value.bit = value != 0.0;
    else if (type == SIM_HAL_S32) scalar.value.s32 = (int32_t)value;
    else if (type == SIM_HAL_U32) scalar.value.u32 = (uint32_t)value;
    else if (type == SIM_HAL_FLOAT) scalar.value.real = (float)value;
    else return SIM_HAL_ERR_TYPE;
    return sim_hal_set_param(name, scalar);
}

int32_t sim_hal_get_param(const char *name, sim_hal_value *out) {
    int index = find_param(name);
    if (index < 0 || out == NULL) return index < 0 ? SIM_HAL_ERR_NOT_FOUND : SIM_HAL_ERR_ARGUMENT;
    *out = state.params[index].value;
    return SIM_HAL_OK;
}

int32_t sim_hal_connect(const char *source, const char *target) {
    int source_index = find_pin(source);
    int target_index = find_pin(target);
    uint32_t link_index;
    if (source_index < 0 || target_index < 0) return SIM_HAL_ERR_NOT_FOUND;
    if (state.pins[source_index].value.type != state.pins[target_index].value.type) return SIM_HAL_ERR_CONNECTION;
    for (link_index = 0; link_index < 256U; ++link_index) {
        if (state.links[link_index].used && state.links[link_index].source == (uint32_t)source_index && state.links[link_index].target == (uint32_t)target_index) return SIM_HAL_ERR_DUPLICATE;
    }
    for (link_index = 0; link_index < 256U; ++link_index) {
        if (!state.links[link_index].used) {
            state.links[link_index].used = 1U;
            state.links[link_index].source = (uint32_t)source_index;
            state.links[link_index].target = (uint32_t)target_index;
            break;
        }
    }
    if (link_index == 256U) return SIM_HAL_ERR_FULL;
    state.pins[target_index].value = state.pins[source_index].value;
    return SIM_HAL_OK;
}

int32_t sim_hal_register_thread(const char *name, uint32_t period_ticks) {
    uint32_t index;
    if (!valid_name(name) || period_ticks == 0U || period_ticks > 1000000U) return SIM_HAL_ERR_RANGE;
    if (find_thread(name) >= 0) return SIM_HAL_ERR_DUPLICATE;
    for (index = 0; index < SIM_HAL_THREAD_CAPACITY; ++index) {
        if (!state.threads[index].used) {
            state.threads[index].used = 1;
            strcpy(state.threads[index].name, name);
            state.threads[index].period = period_ticks;
            return SIM_HAL_OK;
        }
    }
    return SIM_HAL_ERR_FULL;
}

int32_t sim_hal_register_function(const char *thread, const char *name) {
    uint32_t index;
    int thread_index = find_thread(thread);
    if (thread_index < 0) return SIM_HAL_ERR_NOT_FOUND;
    if (!valid_name(name)) return SIM_HAL_ERR_ARGUMENT;
    if (find_function(name) >= 0) return SIM_HAL_ERR_DUPLICATE;
    for (index = 0; index < SIM_HAL_FUNCTION_CAPACITY; ++index) {
        if (!state.functions[index].used) {
            state.functions[index].used = 1;
            strcpy(state.functions[index].name, name);
            state.functions[index].thread = (uint32_t)thread_index;
            return SIM_HAL_OK;
        }
    }
    return SIM_HAL_ERR_FULL;
}

typedef struct {
    const char *name;
    uint32_t type;
} component_pin_spec;

typedef struct {
    const char *name;
    uint32_t type;
    sim_hal_value initial;
} component_param_spec;

typedef struct {
    uint32_t kind;
    const component_pin_spec *pins;
    uint32_t pin_count;
    const component_param_spec *params;
    uint32_t param_count;
} component_spec;

static const component_pin_spec pins_corexy[] = {
    {"j0-motor-pos-cmd", SIM_HAL_FLOAT}, {"j1-motor-pos-cmd", SIM_HAL_FLOAT},
    {"alpha-cmd", SIM_HAL_FLOAT}, {"beta-cmd", SIM_HAL_FLOAT},
    {"alpha-fb", SIM_HAL_FLOAT}, {"beta-fb", SIM_HAL_FLOAT},
    {"j0-motor-pos-fb", SIM_HAL_FLOAT}, {"j1-motor-pos-fb", SIM_HAL_FLOAT}
};
static const component_pin_spec pins_bit2[] = {
    {"in0", SIM_HAL_BIT}, {"in1", SIM_HAL_BIT}, {"out", SIM_HAL_BIT}
};
static const component_pin_spec pins_bit1[] = {{"in", SIM_HAL_BIT}, {"out", SIM_HAL_BIT}};
static const component_pin_spec pins_abs[] = {
    {"in", SIM_HAL_FLOAT}, {"out", SIM_HAL_FLOAT}, {"sign", SIM_HAL_BIT},
    {"is-positive", SIM_HAL_BIT}, {"is-negative", SIM_HAL_BIT}
};
static const component_pin_spec pins_abs_s32[] = {
    {"in", SIM_HAL_S32}, {"out", SIM_HAL_S32}, {"sign", SIM_HAL_BIT},
    {"is-positive", SIM_HAL_BIT}, {"is-negative", SIM_HAL_BIT}
};
static const component_pin_spec pins_float2[] = {
    {"in0", SIM_HAL_FLOAT}, {"in1", SIM_HAL_FLOAT}, {"out", SIM_HAL_FLOAT}
};
static const component_pin_spec pins_lowpass[] = {
    {"in", SIM_HAL_FLOAT}, {"out", SIM_HAL_FLOAT}, {"load", SIM_HAL_BIT}
};
static const component_pin_spec pins_ddt[] = {{"in", SIM_HAL_FLOAT}, {"out", SIM_HAL_FLOAT}};
static const component_pin_spec pins_integ[] = {
    {"in", SIM_HAL_FLOAT}, {"gain", SIM_HAL_FLOAT}, {"out", SIM_HAL_FLOAT},
    {"reset", SIM_HAL_BIT}, {"max", SIM_HAL_FLOAT}, {"min", SIM_HAL_FLOAT}
};
static const component_pin_spec pins_mux2[] = {
    {"sel", SIM_HAL_BIT}, {"out", SIM_HAL_FLOAT}, {"in1", SIM_HAL_FLOAT}, {"in0", SIM_HAL_FLOAT}
};
static const component_pin_spec pins_sum2[] = {
    {"in0", SIM_HAL_FLOAT}, {"in1", SIM_HAL_FLOAT}, {"out", SIM_HAL_FLOAT}
};
static const component_pin_spec pins_hypot[] = {
    {"in0", SIM_HAL_FLOAT}, {"in1", SIM_HAL_FLOAT}, {"in2", SIM_HAL_FLOAT}, {"out", SIM_HAL_FLOAT}
};
static const component_pin_spec pins_near[] = {
    {"in1", SIM_HAL_FLOAT}, {"in2", SIM_HAL_FLOAT}, {"out", SIM_HAL_BIT}
};
static const component_pin_spec pins_comp[] = {
    {"in0", SIM_HAL_FLOAT}, {"in1", SIM_HAL_FLOAT}, {"out", SIM_HAL_BIT}, {"equal", SIM_HAL_BIT}
};
static const component_pin_spec pins_edge[] = {
    {"in", SIM_HAL_BIT}, {"out", SIM_HAL_BIT}, {"out-invert", SIM_HAL_BIT}
};
static const component_pin_spec pins_estop[] = {
    {"ok-in", SIM_HAL_BIT}, {"fault-in", SIM_HAL_BIT}, {"reset", SIM_HAL_BIT},
    {"ok-out", SIM_HAL_BIT}, {"fault-out", SIM_HAL_BIT}, {"watchdog", SIM_HAL_BIT}
};
static const component_pin_spec pins_constant[] = {{"out", SIM_HAL_FLOAT}};
static const component_pin_spec pins_invert[] = {{"in", SIM_HAL_FLOAT}, {"out", SIM_HAL_FLOAT}};
static const component_pin_spec pins_toggle[] = {{"in", SIM_HAL_BIT}, {"out", SIM_HAL_BIT}};

static const component_param_spec param_constant[] = {
    {"value", SIM_HAL_FLOAT, {SIM_HAL_FLOAT, {.real = 0.0f}}}
};
static const component_param_spec param_lowpass[] = {
    {"gain", SIM_HAL_FLOAT, {SIM_HAL_FLOAT, {.real = 0.0f}}}
};
static const component_param_spec param_sum2[] = {
    {"gain0", SIM_HAL_FLOAT, {SIM_HAL_FLOAT, {.real = 1.0f}}},
    {"gain1", SIM_HAL_FLOAT, {SIM_HAL_FLOAT, {.real = 1.0f}}},
    {"offset", SIM_HAL_FLOAT, {SIM_HAL_FLOAT, {.real = 0.0f}}}
};
static const component_param_spec param_div2[] = {
    {"deadband", SIM_HAL_FLOAT, {SIM_HAL_FLOAT, {.real = 0.0f}}}
};
static const component_param_spec param_near[] = {
    {"scale", SIM_HAL_FLOAT, {SIM_HAL_FLOAT, {.real = 1.0f}}},
    {"difference", SIM_HAL_FLOAT, {SIM_HAL_FLOAT, {.real = 0.0f}}}
};
static const component_param_spec param_comp[] = {
    {"hyst", SIM_HAL_FLOAT, {SIM_HAL_FLOAT, {.real = 0.0f}}}
};
static const component_param_spec param_invert[] = {
    {"deadband", SIM_HAL_FLOAT, {SIM_HAL_FLOAT, {.real = 0.0f}}}
};
static const component_param_spec param_edge[] = {
    {"both", SIM_HAL_BIT, {SIM_HAL_BIT, {.bit = 0U}}},
    {"in-edge", SIM_HAL_BIT, {SIM_HAL_BIT, {.bit = 1U}}},
    {"out-width-ns", SIM_HAL_S32, {SIM_HAL_S32, {.s32 = 0}}},
    {"time-left-ns", SIM_HAL_S32, {SIM_HAL_S32, {.s32 = 0}}},
    {"last-in", SIM_HAL_BIT, {SIM_HAL_BIT, {.bit = 0U}}}
};
static const component_param_spec param_toggle[] = {
    {"debounce", SIM_HAL_U32, {SIM_HAL_U32, {.u32 = 2U}}}
};

static int component_spec_for(const char *type, component_spec *out) {
    if (type == NULL || out == NULL) return 0;
    memset(out, 0, sizeof(*out));
#define SPEC(kind_value, pin_array) do { out->kind = (kind_value); out->pins = (pin_array); out->pin_count = (uint32_t)(sizeof(pin_array) / sizeof((pin_array)[0])); return 1; } while (0)
    if (strcmp(type, "corexy_by_hal") == 0) SPEC(SIM_HAL_COMPONENT_COREXY, pins_corexy);
    if (strcmp(type, "constant") == 0) { out->kind = SIM_HAL_COMPONENT_CONSTANT; out->pins = pins_constant; out->pin_count = 1U; out->params = param_constant; out->param_count = 1U; return 1; }
    if (strcmp(type, "and2") == 0) SPEC(SIM_HAL_COMPONENT_AND2, pins_bit2);
    if (strcmp(type, "or2") == 0) SPEC(SIM_HAL_COMPONENT_OR2, pins_bit2);
    if (strcmp(type, "xor2") == 0) SPEC(SIM_HAL_COMPONENT_XOR2, pins_bit2);
    if (strcmp(type, "not") == 0 || strcmp(type, "invert_bit") == 0) SPEC(SIM_HAL_COMPONENT_NOT, pins_bit1);
    if (strcmp(type, "abs") == 0) SPEC(SIM_HAL_COMPONENT_ABS, pins_abs);
    if (strcmp(type, "abs_s32") == 0) SPEC(SIM_HAL_COMPONENT_ABS_S32, pins_abs_s32);
    if (strcmp(type, "lowpass") == 0) { out->kind = SIM_HAL_COMPONENT_LOWPASS; out->pins = pins_lowpass; out->pin_count = 3U; out->params = param_lowpass; out->param_count = 1U; return 1; }
    if (strcmp(type, "ddt") == 0) SPEC(SIM_HAL_COMPONENT_DDT, pins_ddt);
    if (strcmp(type, "integ") == 0) SPEC(SIM_HAL_COMPONENT_INTEG, pins_integ);
    if (strcmp(type, "mux2") == 0) SPEC(SIM_HAL_COMPONENT_MUX2, pins_mux2);
    if (strcmp(type, "sum2") == 0) { out->kind = SIM_HAL_COMPONENT_SUM2; out->pins = pins_sum2; out->pin_count = 3U; out->params = param_sum2; out->param_count = 3U; return 1; }
    if (strcmp(type, "mult2") == 0) SPEC(SIM_HAL_COMPONENT_MULT2, pins_float2);
    if (strcmp(type, "div2") == 0) { out->kind = SIM_HAL_COMPONENT_DIV2; out->pins = pins_float2; out->pin_count = 3U; out->params = param_div2; out->param_count = 1U; return 1; }
    if (strcmp(type, "hypot") == 0) SPEC(SIM_HAL_COMPONENT_HYPOT, pins_hypot);
    if (strcmp(type, "near") == 0) { out->kind = SIM_HAL_COMPONENT_NEAR; out->pins = pins_near; out->pin_count = 3U; out->params = param_near; out->param_count = 2U; return 1; }
    if (strcmp(type, "comp") == 0) { out->kind = SIM_HAL_COMPONENT_COMP; out->pins = pins_comp; out->pin_count = 4U; out->params = param_comp; out->param_count = 1U; return 1; }
    if (strcmp(type, "invert") == 0) { out->kind = SIM_HAL_COMPONENT_INVERT; out->pins = pins_invert; out->pin_count = 2U; out->params = param_invert; out->param_count = 1U; return 1; }
    if (strcmp(type, "edge") == 0) { out->kind = SIM_HAL_COMPONENT_EDGE; out->pins = pins_edge; out->pin_count = 3U; out->params = param_edge; out->param_count = 5U; return 1; }
    if (strcmp(type, "toggle") == 0) { out->kind = SIM_HAL_COMPONENT_TOGGLE; out->pins = pins_toggle; out->pin_count = 2U; out->params = param_toggle; out->param_count = 1U; return 1; }
    if (strcmp(type, "estop_latch") == 0) SPEC(SIM_HAL_COMPONENT_ESTOP_LATCH, pins_estop);
#undef SPEC
    return 0;
}

static int32_t qualified_name(const char *instance, const char *suffix, char *out) {
    size_t left;
    size_t right;
    if (!valid_name(instance) || suffix == NULL || out == NULL) return SIM_HAL_ERR_ARGUMENT;
    left = strlen(instance); right = strlen(suffix);
    if (left + 1U + right >= SIM_HAL_NAME_CAPACITY) return SIM_HAL_ERR_RANGE;
    memcpy(out, instance, left); out[left] = '.'; memcpy(out + left + 1U, suffix, right + 1U);
    return SIM_HAL_OK;
}

static int32_t register_param_internal(const char *name, sim_hal_value value) {
    uint32_t index;
    if (!valid_name(name) || !finite_value(value)) return SIM_HAL_ERR_ARGUMENT;
    if (find_param(name) >= 0) return SIM_HAL_ERR_DUPLICATE;
    for (index = 0; index < SIM_HAL_PARAM_CAPACITY; ++index) {
        if (!state.params[index].used) {
            state.params[index].used = 1U;
            strcpy(state.params[index].name, name);
            state.params[index].value = value;
            return SIM_HAL_OK;
        }
    }
    return SIM_HAL_ERR_FULL;
}

static void component_initialize_defaults(const component_entry *component);

int32_t sim_hal_register_component(const char *instance, const char *type, const char *thread) {
    component_spec spec;
    hal_state before;
    char qualified[SIM_HAL_NAME_CAPACITY];
    uint32_t index;
    int component_index;
    int function_index;
    int32_t result;
    if (!valid_name(instance) || !valid_name(type) || !valid_name(thread)) return SIM_HAL_ERR_ARGUMENT;
    if (!component_spec_for(type, &spec)) return SIM_HAL_ERR_REAL_EXCLUDED;
    if (find_component(instance) >= 0 || find_function(instance) >= 0) return SIM_HAL_ERR_DUPLICATE;
    if (find_thread(thread) < 0) return SIM_HAL_ERR_NOT_FOUND;
    memcpy(&before, &state, sizeof(state));
    component_index = -1;
    for (index = 0; index < SIM_HAL_COMPONENT_CAPACITY; ++index) {
        if (!state.components[index].used) { component_index = (int)index; break; }
    }
    if (component_index < 0) return SIM_HAL_ERR_FULL;
    state.components[component_index].used = 1U;
    strcpy(state.components[component_index].instance, instance);
    strcpy(state.components[component_index].type, type);
    state.components[component_index].kind = spec.kind;
    state.components[component_index].function_index = 0U;
    for (index = 0; index < spec.pin_count; ++index) {
        result = qualified_name(instance, spec.pins[index].name, qualified);
        if (result != SIM_HAL_OK || (result = sim_hal_register_pin(qualified, spec.pins[index].type)) != SIM_HAL_OK) { memcpy(&state, &before, sizeof(state)); return result; }
    }
    for (index = 0; index < spec.param_count; ++index) {
        result = qualified_name(instance, spec.params[index].name, qualified);
        if (result != SIM_HAL_OK || (result = register_param_internal(qualified, spec.params[index].initial)) != SIM_HAL_OK) { memcpy(&state, &before, sizeof(state)); return result; }
    }
    result = sim_hal_register_function(thread, instance);
    if (result != SIM_HAL_OK) { memcpy(&state, &before, sizeof(state)); return result; }
    function_index = find_function(instance);
    state.components[component_index].function_index = (uint32_t)function_index;
    state.functions[function_index].component_index = (uint32_t)component_index;
    state.functions[function_index].component_kind = spec.kind;
    component_initialize_defaults(&state.components[component_index]);
    return SIM_HAL_OK;
}

static int32_t corexy_pin_names(const char *component, char names[8][SIM_HAL_NAME_CAPACITY]) {
    static const char *suffixes[8] = {
        ".j0-motor-pos-cmd", ".j1-motor-pos-cmd", ".alpha-cmd", ".beta-cmd",
        ".alpha-fb", ".beta-fb", ".j0-motor-pos-fb", ".j1-motor-pos-fb",
    };
    uint32_t index;
    if (!valid_name(component)) return SIM_HAL_ERR_ARGUMENT;
    for (index = 0; index < 8; ++index) {
        size_t prefix = strlen(component);
        size_t suffix = strlen(suffixes[index]);
        if (prefix + suffix >= SIM_HAL_NAME_CAPACITY) return SIM_HAL_ERR_RANGE;
        memcpy(names[index], component, prefix);
        memcpy(names[index] + prefix, suffixes[index], suffix + 1);
    }
    return SIM_HAL_OK;
}

int32_t sim_hal_register_corexy_by_hal(const char *component, const char *thread) {
    char names[8][SIM_HAL_NAME_CAPACITY];
    uint32_t index;
    int32_t result;
    if (component == NULL || thread == NULL) return SIM_HAL_ERR_ARGUMENT;
    result = corexy_pin_names(component, names);
    if (result != SIM_HAL_OK) return result;
    for (index = 0; index < 8; ++index) {
        if (find_pin(names[index]) >= 0) return SIM_HAL_ERR_DUPLICATE;
    }
    if (find_function(component) >= 0) return SIM_HAL_ERR_DUPLICATE;
    for (index = 0; index < 8; ++index) {
        result = sim_hal_register_pin(names[index], SIM_HAL_FLOAT);
        if (result != SIM_HAL_OK) return result;
    }
    result = sim_hal_register_function(thread, component);
    if (result != SIM_HAL_OK) return result;
    state.functions[find_function(component)].component_index = UINT32_MAX;
    state.functions[find_function(component)].component_kind = SIM_HAL_COMPONENT_COREXY;
    return SIM_HAL_OK;
}

static int32_t float_pin(const char *name, float *value) {
    sim_hal_value pin = {0};
    int32_t result = sim_hal_get_pin(name, &pin);
    if (result != SIM_HAL_OK) return result;
    if (pin.type != SIM_HAL_FLOAT) return SIM_HAL_ERR_TYPE;
    *value = pin.value.real;
    return SIM_HAL_OK;
}

static int32_t set_float_pin(const char *name, float value) {
    sim_hal_value pin = {SIM_HAL_FLOAT, {.real = value}};
    return sim_hal_set_pin(name, pin);
}

int32_t sim_hal_corexy_transform(const char *component) {
    char names[8][SIM_HAL_NAME_CAPACITY];
    float j0_cmd, j1_cmd, alpha_fb, beta_fb;
    int32_t result = corexy_pin_names(component, names);
    if (result != SIM_HAL_OK) return result;
    result = float_pin(names[0], &j0_cmd); if (result != SIM_HAL_OK) return result;
    result = float_pin(names[1], &j1_cmd); if (result != SIM_HAL_OK) return result;
    result = float_pin(names[4], &alpha_fb); if (result != SIM_HAL_OK) return result;
    result = float_pin(names[5], &beta_fb); if (result != SIM_HAL_OK) return result;
    result = set_float_pin(names[2], 0.5f * (j0_cmd + j1_cmd)); if (result != SIM_HAL_OK) return result;
    result = set_float_pin(names[3], 0.5f * (j0_cmd - j1_cmd)); if (result != SIM_HAL_OK) return result;
    result = set_float_pin(names[6], alpha_fb + beta_fb); if (result != SIM_HAL_OK) return result;
    return set_float_pin(names[7], alpha_fb - beta_fb);
}

static int32_t component_pin_name(const component_entry *component, const char *suffix, char out[SIM_HAL_NAME_CAPACITY]) {
    return qualified_name(component->instance, suffix, out);
}

static int32_t component_float_pin(const component_entry *component, const char *suffix, float *value) {
    char name[SIM_HAL_NAME_CAPACITY];
    sim_hal_value pin = {0};
    int32_t result = component_pin_name(component, suffix, name);
    if (result != SIM_HAL_OK) return result;
    result = sim_hal_get_pin(name, &pin);
    if (result != SIM_HAL_OK) return result;
    if (pin.type != SIM_HAL_FLOAT || !finite_value(pin)) return SIM_HAL_ERR_TYPE;
    *value = pin.value.real;
    return SIM_HAL_OK;
}

static int32_t component_bit_pin(const component_entry *component, const char *suffix, uint32_t *value) {
    char name[SIM_HAL_NAME_CAPACITY];
    sim_hal_value pin = {0};
    int32_t result = component_pin_name(component, suffix, name);
    if (result != SIM_HAL_OK) return result;
    result = sim_hal_get_pin(name, &pin);
    if (result != SIM_HAL_OK) return result;
    if (pin.type != SIM_HAL_BIT) return SIM_HAL_ERR_TYPE;
    *value = pin.value.bit;
    return SIM_HAL_OK;
}

static int32_t component_s32_pin(const component_entry *component, const char *suffix, int32_t *value) {
    char name[SIM_HAL_NAME_CAPACITY];
    sim_hal_value pin = {0};
    int32_t result = component_pin_name(component, suffix, name);
    if (result != SIM_HAL_OK) return result;
    result = sim_hal_get_pin(name, &pin);
    if (result != SIM_HAL_OK) return result;
    if (pin.type != SIM_HAL_S32) return SIM_HAL_ERR_TYPE;
    *value = pin.value.s32;
    return SIM_HAL_OK;
}

static int32_t component_write_float(const component_entry *component, const char *suffix, float value) {
    char name[SIM_HAL_NAME_CAPACITY];
    int32_t result = component_pin_name(component, suffix, name);
    if (result != SIM_HAL_OK) return result;
    return sim_hal_set_pin(name, (sim_hal_value){SIM_HAL_FLOAT, {.real = value}});
}

static int32_t component_write_bit(const component_entry *component, const char *suffix, uint32_t value) {
    char name[SIM_HAL_NAME_CAPACITY];
    int32_t result = component_pin_name(component, suffix, name);
    if (result != SIM_HAL_OK) return result;
    return sim_hal_set_pin(name, (sim_hal_value){SIM_HAL_BIT, {.bit = value ? 1U : 0U}});
}

static int32_t component_write_s32(const component_entry *component, const char *suffix, int32_t value) {
    char name[SIM_HAL_NAME_CAPACITY];
    int32_t result = component_pin_name(component, suffix, name);
    if (result != SIM_HAL_OK) return result;
    return sim_hal_set_pin(name, (sim_hal_value){SIM_HAL_S32, {.s32 = value}});
}

static int32_t component_float_param(const component_entry *component, const char *suffix, float *value) {
    char name[SIM_HAL_NAME_CAPACITY];
    sim_hal_value param = {0};
    int32_t result = component_pin_name(component, suffix, name);
    if (result != SIM_HAL_OK) return result;
    result = sim_hal_get_param(name, &param);
    if (result != SIM_HAL_OK) return result;
    if (param.type != SIM_HAL_FLOAT || !finite_value(param)) return SIM_HAL_ERR_TYPE;
    *value = param.value.real;
    return SIM_HAL_OK;
}

static int32_t component_bit_param(const component_entry *component, const char *suffix, uint32_t *value) {
    char name[SIM_HAL_NAME_CAPACITY];
    sim_hal_value param = {0};
    int32_t result = component_pin_name(component, suffix, name);
    if (result != SIM_HAL_OK) return result;
    result = sim_hal_get_param(name, &param);
    if (result != SIM_HAL_OK) return result;
    if (param.type != SIM_HAL_BIT) return SIM_HAL_ERR_TYPE;
    *value = param.value.bit;
    return SIM_HAL_OK;
}

static int32_t component_s32_param(const component_entry *component, const char *suffix, int32_t *value) {
    char name[SIM_HAL_NAME_CAPACITY];
    sim_hal_value param = {0};
    int32_t result = component_pin_name(component, suffix, name);
    if (result != SIM_HAL_OK) return result;
    result = sim_hal_get_param(name, &param);
    if (result != SIM_HAL_OK) return result;
    if (param.type != SIM_HAL_S32) return SIM_HAL_ERR_TYPE;
    *value = param.value.s32;
    return SIM_HAL_OK;
}

static int32_t component_write_bit_param(const component_entry *component, const char *suffix, uint32_t value) {
    char name[SIM_HAL_NAME_CAPACITY];
    int32_t result = component_pin_name(component, suffix, name);
    if (result != SIM_HAL_OK) return result;
    return sim_hal_set_param(name, (sim_hal_value){SIM_HAL_BIT, {.bit = value ? 1U : 0U}});
}

static void component_initialize_defaults(const component_entry *component) {
    if (component->kind == SIM_HAL_COMPONENT_INTEG) {
        (void)component_write_float(component, "gain", 1.0f);
        (void)component_write_float(component, "max", 1.0e20f);
        (void)component_write_float(component, "min", -1.0e20f);
    } else if (component->kind == SIM_HAL_COMPONENT_ESTOP_LATCH) {
        (void)component_write_bit(component, "ok-in", 1U);
        (void)component_write_bit(component, "fault-in", 0U);
        (void)component_write_bit(component, "ok-out", 0U);
        (void)component_write_bit(component, "fault-out", 1U);
    }
}

static int32_t component_step(component_entry *component) {
    float a, b, c, value;
    uint32_t bit, other;
    int32_t integer;
    int32_t result;
    if (component->kind == SIM_HAL_COMPONENT_COREXY) return sim_hal_corexy_transform(component->instance);
    switch (component->kind) {
    case SIM_HAL_COMPONENT_CONSTANT:
        result = component_float_param(component, "value", &value); if (result != SIM_HAL_OK) return result;
        return component_write_float(component, "out", value);
    case SIM_HAL_COMPONENT_AND2: case SIM_HAL_COMPONENT_OR2: case SIM_HAL_COMPONENT_XOR2:
        result = component_bit_pin(component, "in0", &bit); if (result != SIM_HAL_OK) return result;
        result = component_bit_pin(component, "in1", &other); if (result != SIM_HAL_OK) return result;
        value = (float)(component->kind == SIM_HAL_COMPONENT_AND2 ? (bit && other) : component->kind == SIM_HAL_COMPONENT_OR2 ? (bit || other) : (bit != other));
        return component_write_bit(component, "out", value != 0.0f);
    case SIM_HAL_COMPONENT_NOT:
        result = component_bit_pin(component, "in", &bit); if (result != SIM_HAL_OK) return result;
        return component_write_bit(component, "out", !bit);
    case SIM_HAL_COMPONENT_ABS:
        result = component_float_pin(component, "in", &a); if (result != SIM_HAL_OK) return result;
        result = component_write_float(component, "out", fabsf(a)); if (result != SIM_HAL_OK) return result;
        result = component_write_bit(component, "sign", a < 0.0f); if (result != SIM_HAL_OK) return result;
        result = component_write_bit(component, "is-positive", a > 0.000001f); if (result != SIM_HAL_OK) return result;
        return component_write_bit(component, "is-negative", a < -0.000001f);
    case SIM_HAL_COMPONENT_ABS_S32: case SIM_HAL_COMPONENT_ABS_S64:
        result = component_s32_pin(component, "in", &integer); if (result != SIM_HAL_OK) return result;
        if (integer == INT32_MIN) return SIM_HAL_ERR_RANGE;
        result = component_write_s32(component, "out", integer < 0 ? -integer : integer); if (result != SIM_HAL_OK) return result;
        result = component_write_bit(component, "sign", integer < 0); if (result != SIM_HAL_OK) return result;
        result = component_write_bit(component, "is-positive", integer > 0); if (result != SIM_HAL_OK) return result;
        return component_write_bit(component, "is-negative", integer < 0);
    case SIM_HAL_COMPONENT_LOWPASS:
        result = component_float_pin(component, "in", &a); if (result != SIM_HAL_OK) return result;
        result = component_float_pin(component, "out", &b); if (result != SIM_HAL_OK) return result;
        result = component_bit_pin(component, "load", &bit); if (result != SIM_HAL_OK) return result;
        result = component_float_param(component, "gain", &c); if (result != SIM_HAL_OK) return result;
        return component_write_float(component, "out", bit ? a : b + (a - b) * c);
    case SIM_HAL_COMPONENT_DDT:
        result = component_float_pin(component, "in", &a); if (result != SIM_HAL_OK) return result;
        result = component_write_float(component, "out", (float)(a - component->memory[0]));
        component->memory[0] = a; return result;
    case SIM_HAL_COMPONENT_INTEG:
        result = component_float_pin(component, "in", &a); if (result != SIM_HAL_OK) return result;
        result = component_float_pin(component, "gain", &b); if (result != SIM_HAL_OK) return result;
        result = component_bit_pin(component, "reset", &bit); if (result != SIM_HAL_OK) return result;
        result = component_float_pin(component, "max", &c); if (result != SIM_HAL_OK) return result;
        result = component_float_pin(component, "min", &value); if (result != SIM_HAL_OK) return result;
        if (bit) component->memory[0] = 0.0; else component->memory[0] += (double)a * (double)b;
        if (component->memory[0] > c) component->memory[0] = c;
        if (component->memory[0] < value) component->memory[0] = value;
        return component_write_float(component, "out", (float)component->memory[0]);
    case SIM_HAL_COMPONENT_MUX2:
        result = component_bit_pin(component, "sel", &bit); if (result != SIM_HAL_OK) return result;
        result = component_float_pin(component, bit ? "in1" : "in0", &a); if (result != SIM_HAL_OK) return result;
        return component_write_float(component, "out", a);
    case SIM_HAL_COMPONENT_SUM2:
        result = component_float_pin(component, "in0", &a); if (result != SIM_HAL_OK) return result;
        result = component_float_pin(component, "in1", &b); if (result != SIM_HAL_OK) return result;
        result = component_float_param(component, "gain0", &c); if (result != SIM_HAL_OK) return result;
        result = component_float_param(component, "gain1", &value); if (result != SIM_HAL_OK) return result;
        { float offset; result = component_float_param(component, "offset", &offset); if (result != SIM_HAL_OK) return result; return component_write_float(component, "out", a * c + b * value + offset); }
    case SIM_HAL_COMPONENT_MULT2:
        result = component_float_pin(component, "in0", &a); if (result != SIM_HAL_OK) return result;
        result = component_float_pin(component, "in1", &b); if (result != SIM_HAL_OK) return result;
        return component_write_float(component, "out", a * b);
    case SIM_HAL_COMPONENT_DIV2:
        result = component_float_pin(component, "in0", &a); if (result != SIM_HAL_OK) return result;
        result = component_float_pin(component, "in1", &b); if (result != SIM_HAL_OK) return result;
        result = component_float_param(component, "deadband", &c); if (result != SIM_HAL_OK) return result;
        if (c < 1e-12f) c = 1e-12f; if (fabsf(b) < c) b = b < 0.0f ? -c : c;
        return component_write_float(component, "out", a / b);
    case SIM_HAL_COMPONENT_HYPOT:
        result = component_float_pin(component, "in0", &a); if (result != SIM_HAL_OK) return result;
        result = component_float_pin(component, "in1", &b); if (result != SIM_HAL_OK) return result;
        result = component_float_pin(component, "in2", &c); if (result != SIM_HAL_OK) return result;
        return component_write_float(component, "out", sqrtf(a * a + b * b + c * c));
    case SIM_HAL_COMPONENT_NEAR:
        result = component_float_pin(component, "in1", &a); if (result != SIM_HAL_OK) return result;
        result = component_float_pin(component, "in2", &b); if (result != SIM_HAL_OK) return result;
        result = component_float_param(component, "scale", &c); if (result != SIM_HAL_OK) return result;
        result = component_float_param(component, "difference", &value); if (result != SIM_HAL_OK) return result;
        if (a < 0.0f) { a = -a; b = -b; }
        return component_write_bit(component, "out", ((c > 1.0f && a / c <= b && b <= a * c) || fabsf(a - b) <= value));
    case SIM_HAL_COMPONENT_COMP:
        result = component_float_pin(component, "in0", &a); if (result != SIM_HAL_OK) return result;
        result = component_float_pin(component, "in1", &b); if (result != SIM_HAL_OK) return result;
        result = component_float_param(component, "hyst", &c); if (result != SIM_HAL_OK) return result;
        if (b - a < -0.5f * c) { result = component_write_bit(component, "out", 0U); if (result != SIM_HAL_OK) return result; return component_write_bit(component, "equal", 0U); }
        if (b - a > 0.5f * c) { result = component_write_bit(component, "out", 1U); if (result != SIM_HAL_OK) return result; return component_write_bit(component, "equal", 0U); }
        return component_write_bit(component, "equal", 1U);
    case SIM_HAL_COMPONENT_INVERT:
        result = component_float_pin(component, "in", &a); if (result != SIM_HAL_OK) return result;
        result = component_float_param(component, "deadband", &c); if (result != SIM_HAL_OK) return result;
        if (c < 1e-12f) c = 1e-12f; if (fabsf(a) < c) a = a < 0.0f ? -c : c;
        return component_write_float(component, "out", 1.0f / a);
    case SIM_HAL_COMPONENT_EDGE:
        result = component_bit_pin(component, "in", &bit); if (result != SIM_HAL_OK) return result;
        result = component_bit_param(component, "last-in", &other); if (result != SIM_HAL_OK) return result;
        if (component->flags[0] == 0U) { component->flags[0] = 1U; component->memory[0] = bit; return component_write_bit(component, "out-invert", 1U); }
        { uint32_t both, in_edge; result = component_bit_param(component, "both", &both); if (result != SIM_HAL_OK) return result; result = component_bit_param(component, "in-edge", &in_edge); if (result != SIM_HAL_OK) return result; uint32_t rise = bit && !other, fall = !bit && other; uint32_t hit = both ? (rise || fall) : (in_edge ? fall : rise); result = component_write_bit(component, "out", hit); if (result != SIM_HAL_OK) return result; result = component_write_bit(component, "out-invert", !hit); if (result != SIM_HAL_OK) return result; result = component_write_bit_param(component, "last-in", bit); component->memory[0] = bit; return result; }
    case SIM_HAL_COMPONENT_TOGGLE:
        result = component_bit_pin(component, "in", &bit); if (result != SIM_HAL_OK) return result;
        result = component_bit_pin(component, "out", &other); if (result != SIM_HAL_OK) return result;
        { sim_hal_value param = {0}; char name[SIM_HAL_NAME_CAPACITY]; component_pin_name(component, "debounce", name); result = sim_hal_get_param(name, &param); if (result != SIM_HAL_OK) return result; uint32_t debounce = param.value.u32 < 1U || param.value.u32 > 10000U ? 2U : param.value.u32; if (bit) { if (component->flags[0] < debounce) component->flags[0] += 1U; if (component->flags[0] >= debounce && component->flags[1] == 0U) { other = !other; component->flags[1] = 1U; } } else { component->flags[0] = 0U; component->flags[1] = 0U; } return component_write_bit(component, "out", other); }
    case SIM_HAL_COMPONENT_ESTOP_LATCH:
        result = component_bit_pin(component, "ok-in", &bit); if (result != SIM_HAL_OK) return result;
        result = component_bit_pin(component, "fault-in", &other); if (result != SIM_HAL_OK) return result;
        { uint32_t reset; result = component_bit_pin(component, "reset", &reset); if (result != SIM_HAL_OK) return result; if (!bit || other) { component->flags[0] = 0U; } else if (reset && component->flags[1] == 0U) component->flags[0] = 1U; component->flags[1] = reset; result = component_write_bit(component, "ok-out", component->flags[0]); if (result != SIM_HAL_OK) return result; result = component_write_bit(component, "fault-out", !component->flags[0]); if (result != SIM_HAL_OK) return result; if (component->flags[0]) component->flags[2] ^= 1U; return component_write_bit(component, "watchdog", component->flags[2]); }
    default: return SIM_HAL_ERR_REAL_EXCLUDED;
    }
}

int32_t sim_hal_set_function_failure(const char *name, uint32_t fail) {
    int index = find_function(name);
    if (index < 0) return SIM_HAL_ERR_NOT_FOUND;
    state.functions[index].fail = fail ? 1U : 0U;
    return SIM_HAL_OK;
}

int32_t sim_hal_step(uint32_t ticks) {
    hal_state before;
    uint64_t target;
    uint64_t tick;
    uint32_t function_index;
    if ((uint64_t)state.tick + (uint64_t)ticks > UINT32_MAX) return SIM_HAL_ERR_RANGE;
    memcpy(&before, &state, sizeof(state));
    target = (uint64_t)state.tick + (uint64_t)ticks;
    for (tick = (uint64_t)state.tick + 1U; tick <= target; ++tick) {
        for (function_index = 0; function_index < 256U; ++function_index) {
            if (state.links[function_index].used) {
                uint32_t source = state.links[function_index].source;
                uint32_t target_pin = state.links[function_index].target;
                state.pins[target_pin].value = state.pins[source].value;
            }
        }
        for (function_index = 0; function_index < SIM_HAL_FUNCTION_CAPACITY; ++function_index) {
            function_entry *function = &state.functions[function_index];
            if (!function->used || tick % state.threads[function->thread].period != 0U) continue;
            if (function->fail) { memcpy(&state, &before, sizeof(state)); return SIM_HAL_ERR_FUNCTION; }
            if (function->component_kind != SIM_HAL_COMPONENT_NONE) {
                int32_t result;
                if (function->component_kind == SIM_HAL_COMPONENT_COREXY && function->component_index == UINT32_MAX) result = sim_hal_corexy_transform(function->name);
                else if (function->component_index >= SIM_HAL_COMPONENT_CAPACITY || !state.components[function->component_index].used) result = SIM_HAL_ERR_NOT_FOUND;
                else result = component_step(&state.components[function->component_index]);
                if (result != SIM_HAL_OK) { memcpy(&state, &before, sizeof(state)); return SIM_HAL_ERR_FUNCTION; }
            }
            function->calls += 1U;
        }
    }
    state.tick = (uint32_t)target;
    return SIM_HAL_OK;
}

uint32_t sim_hal_tick(void) { return state.tick; }
uint32_t sim_hal_function_calls(const char *name) { int index = find_function(name); return index < 0 ? 0U : state.functions[index].calls; }
uint32_t sim_hal_pin_count(void) { uint32_t i, count = 0; for (i = 0; i < SIM_HAL_PIN_CAPACITY; ++i) count += state.pins[i].used; return count; }
uint32_t sim_hal_thread_count(void) { uint32_t i, count = 0; for (i = 0; i < SIM_HAL_THREAD_CAPACITY; ++i) count += state.threads[i].used; return count; }
uint32_t sim_hal_function_count(void) { uint32_t i, count = 0; for (i = 0; i < SIM_HAL_FUNCTION_CAPACITY; ++i) count += state.functions[i].used; return count; }
uint32_t sim_hal_component_count(void) { uint32_t i, count = 0; for (i = 0; i < SIM_HAL_COMPONENT_CAPACITY; ++i) count += state.components[i].used; return count; }
uint32_t sim_hal_param_count(void) { uint32_t i, count = 0; for (i = 0; i < SIM_HAL_PARAM_CAPACITY; ++i) count += state.params[i].used; return count; }