#include "sim_hal.h"

#include <assert.h>
#include <stdio.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};

    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);
    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("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}\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"));
    return 0;
}