#include "sim_hal.h"

#include <assert.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

static float get_float(const char *name) {
    sim_hal_value value = {0};
    assert(sim_hal_get_pin(name, &value) == SIM_HAL_OK);
    assert(value.type == SIM_HAL_FLOAT);
    return value.value.real;
}

int sim_hal_test_run(void) {
    sim_hal_value bit = {SIM_HAL_BIT, {.bit = 1}};
    sim_hal_value wrong = {SIM_HAL_S32, {.s32 = 1}};
    sim_hal_value out = {0};
    uint32_t component_gate = 0;
    float component_constant = 0.0f;
    uint32_t component_tick_after_failure = 0;

    sim_hal_reset();
    assert(sim_hal_register_pin("source", SIM_HAL_BIT) == SIM_HAL_OK);
    assert(sim_hal_register_pin("target", SIM_HAL_BIT) == SIM_HAL_OK);
    assert(sim_hal_register_pin("source", SIM_HAL_BIT) == SIM_HAL_ERR_DUPLICATE);
    assert(sim_hal_set_pin("source", bit) == SIM_HAL_OK);
    assert(sim_hal_set_pin("source", wrong) == SIM_HAL_ERR_TYPE);
    assert(sim_hal_connect("source", "target") == SIM_HAL_OK);
    assert(sim_hal_get_pin("target", &out) == SIM_HAL_OK && out.value.bit == 1);
    assert(sim_hal_register_thread("servo", 2) == SIM_HAL_OK);
    assert(sim_hal_register_thread("servo", 2) == SIM_HAL_ERR_DUPLICATE);
    assert(sim_hal_register_function("servo", "update") == SIM_HAL_OK);
    assert(sim_hal_register_function("missing", "bad") == SIM_HAL_ERR_NOT_FOUND);
    assert(sim_hal_step(5) == SIM_HAL_OK);
    assert(sim_hal_tick() == 5 && sim_hal_function_calls("update") == 2);
    const uint32_t saved_size = sim_hal_checkpoint_size();
    uint8_t *saved = malloc(saved_size), *restored = malloc(saved_size);
    assert(saved && restored);
    assert(sim_hal_checkpoint_export(saved, saved_size) == SIM_HAL_OK);
    assert(sim_hal_step(3) == SIM_HAL_OK);
    assert(sim_hal_tick() == 8 && sim_hal_function_calls("update") == 4);
    assert(sim_hal_checkpoint_restore(saved, saved_size - 1) == SIM_HAL_ERR_ARGUMENT);
    assert(sim_hal_tick() == 8);
    assert(sim_hal_checkpoint_restore(saved, saved_size) == SIM_HAL_OK);
    assert(sim_hal_tick() == 5 && sim_hal_function_calls("update") == 2);
    assert(sim_hal_checkpoint_export(restored, saved_size) == SIM_HAL_OK);
    assert(memcmp(saved, restored, saved_size) == 0);
    free(saved); free(restored);
    assert(sim_hal_set_function_failure("update", 1) == SIM_HAL_OK);
    assert(sim_hal_step(1) == SIM_HAL_ERR_FUNCTION);
    assert(sim_hal_tick() == 5);
    assert(sim_hal_register_thread("invalid", 0) == SIM_HAL_ERR_RANGE);

    sim_hal_reset();
    assert(sim_hal_register_thread("component-thread", 1) == SIM_HAL_OK);
    assert(sim_hal_register_component("gate", "and2", "component-thread") == SIM_HAL_OK);
    assert(sim_hal_register_component("value", "constant", "component-thread") == SIM_HAL_OK);
    assert(sim_hal_register_component("unsupported", "not-a-static-component", "component-thread") == SIM_HAL_ERR_REAL_EXCLUDED);
    assert(sim_hal_set_pin("gate.in0", bit) == SIM_HAL_OK);
    assert(sim_hal_set_pin("gate.in1", bit) == SIM_HAL_OK);
    assert(sim_hal_set_param("value.value", (sim_hal_value){SIM_HAL_FLOAT, {.real = 3.5f}}) == SIM_HAL_OK);
    assert(sim_hal_step(1) == SIM_HAL_OK);
    assert(sim_hal_get_pin("gate.out", &out) == SIM_HAL_OK && out.value.bit == 1U);
    assert(sim_hal_get_pin("value.out", &out) == SIM_HAL_OK && out.value.real == 3.5f);
    component_gate = out.value.real == 3.5f ? 1U : 0U;
    assert(sim_hal_set_function_failure("gate", 1) == SIM_HAL_OK);
    assert(sim_hal_step(1) == SIM_HAL_ERR_FUNCTION);
    component_tick_after_failure = sim_hal_tick();
    assert(component_tick_after_failure == 1U);
    component_constant = out.value.real;

    sim_hal_reset();
    assert(sim_hal_register_thread("servo-thread", 1) == SIM_HAL_OK);
    assert(sim_hal_register_corexy_by_hal("corexy_by_hal", "servo-thread") == SIM_HAL_OK);
    assert(sim_hal_set_pin("corexy_by_hal.j0-motor-pos-cmd", (sim_hal_value){SIM_HAL_FLOAT, {.real = 6.0f}}) == SIM_HAL_OK);
    assert(sim_hal_set_pin("corexy_by_hal.j1-motor-pos-cmd", (sim_hal_value){SIM_HAL_FLOAT, {.real = 2.0f}}) == SIM_HAL_OK);
    assert(sim_hal_set_pin("corexy_by_hal.alpha-fb", (sim_hal_value){SIM_HAL_FLOAT, {.real = 4.0f}}) == SIM_HAL_OK);
    assert(sim_hal_set_pin("corexy_by_hal.beta-fb", (sim_hal_value){SIM_HAL_FLOAT, {.real = 1.0f}}) == SIM_HAL_OK);
    assert(sim_hal_step(1) == SIM_HAL_OK);
    assert(get_float("corexy_by_hal.alpha-cmd") == 4.0f);
    assert(get_float("corexy_by_hal.beta-cmd") == 2.0f);
    assert(get_float("corexy_by_hal.j0-motor-pos-fb") == 5.0f);
    assert(get_float("corexy_by_hal.j1-motor-pos-fb") == 3.0f);
    assert(sim_hal_function_calls("corexy_by_hal") == 1);
    assert(sim_hal_corexy_transform("missing") == SIM_HAL_ERR_NOT_FOUND);
    assert(sim_hal_register_corexy_by_hal("corexy_by_hal", "servo-thread") == SIM_HAL_ERR_DUPLICATE);
    printf("{\"schema\":\"hal-corexy-sample.v1\",\"command\":{\"j0\":%.3f,\"j1\":%.3f,\"alpha\":%.3f,\"beta\":%.3f},\"feedback\":{\"alpha\":%.3f,\"beta\":%.3f,\"j0\":%.3f,\"j1\":%.3f},\"tick\":%u,\"function_calls\":%u,\"component_batch\":{\"gate\":%u,\"constant\":%.3f,\"rollback_tick\":%u}}\n",
           (double)get_float("corexy_by_hal.j0-motor-pos-cmd"), (double)get_float("corexy_by_hal.j1-motor-pos-cmd"),
           (double)get_float("corexy_by_hal.alpha-cmd"), (double)get_float("corexy_by_hal.beta-cmd"),
           (double)get_float("corexy_by_hal.alpha-fb"), (double)get_float("corexy_by_hal.beta-fb"),
           (double)get_float("corexy_by_hal.j0-motor-pos-fb"), (double)get_float("corexy_by_hal.j1-motor-pos-fb"),
           sim_hal_tick(), sim_hal_function_calls("corexy_by_hal"), component_gate, (double)component_constant, component_tick_after_failure);
    return 0;
}