#include "sim_io.h"

#include "sim_hal.h"

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

static sim_posemath_pose pose(double x, double y, double z) {
    sim_posemath_pose value = {{x, y, z}, 0, 0, 0, 0, 0, 0};
    return value;
}

int main(void) {
    sim_io io = {0};
    sim_io_state state = {0};
    sim_hal_value pin = {0};
    char canonical[2048];
    uint64_t hash;
    uint64_t repeat_hash;
    int invalid_pin_status;
    int limit_status;
    int spindle_step_status;
    int tool_busy_status;
    int probe_status;
    int estop_command_status;
    int estop_recover_status;
    int recover_status;
    int fault_clear_status;

    sim_hal_reset();
    assert(sim_io_init(&io, 2, 32, 0.1, 600.0, 5.0) == SIM_IO_OK);
    assert(sim_hal_pin_count() >= 58 && sim_hal_function_count() == 1);
    assert(sim_hal_get_pin("motion.probe-input", &pin) == SIM_HAL_OK && pin.type == SIM_HAL_BIT);
    invalid_pin_status = sim_hal_set_pin("spindle.0.speed-out", (sim_hal_value){SIM_HAL_BIT, {.bit = 1}});
    assert(invalid_pin_status == SIM_HAL_ERR_TYPE);
    limit_status = sim_io_spindle_command(&io, 0, 100001.0, 1);
    assert(limit_status == SIM_IO_ERR_LIMIT);
    assert(sim_io_spindle_command(&io, 0, 1200.0, 1) == SIM_IO_OK);
    spindle_step_status = sim_io_step(&io, 10);
    assert(spindle_step_status == SIM_IO_OK && io.state.spindles[0].speed_rpm == 600.0);
    assert(sim_io_step(&io, 10) == SIM_IO_OK && io.state.spindles[0].at_speed == 1);

    assert(sim_io_tool_prepare(&io, 7, 3) == SIM_IO_OK);
    assert(sim_io_tool_set_prepared(&io, 1) == SIM_IO_OK);
    tool_busy_status = sim_io_tool_change(&io);
    assert(tool_busy_status == SIM_IO_ERR_BUSY);
    assert(sim_io_spindle_command(&io, 0, 0.0, 0) == SIM_IO_OK);
    assert(sim_io_step(&io, 20) == SIM_IO_OK);
    assert(sim_io_tool_change(&io) == SIM_IO_OK);
    assert(sim_io_tool_set_changed(&io, 1) == SIM_IO_OK && io.state.current_tool == 7);

    assert(sim_io_probe_arm(&io, 0) == SIM_IO_OK);
    probe_status = sim_io_probe_update(&io, &(sim_posemath_pose){{1.0, 2.0, 3.0}, 0, 0, 0, 0, 0, 0}, 1);
    assert(probe_status == SIM_IO_OK && io.state.probe.state == SIM_IO_PROBE_TRIPPED);
    assert(sim_io_probe_update(&io, &(sim_posemath_pose){{NAN, 2.0, 3.0}, 0, 0, 0, 0, 0, 0}, 0) == SIM_IO_ERR_NONFINITE);

    assert(sim_io_set_estop(&io, 1) == SIM_IO_OK);
    estop_command_status = sim_io_spindle_command(&io, 0, 100.0, 1);
    estop_recover_status = sim_io_recover(&io);
    assert(estop_command_status == SIM_IO_ERR_ESTOP && estop_recover_status == SIM_IO_ERR_ESTOP);
    assert(sim_io_set_estop(&io, 0) == SIM_IO_OK);
    recover_status = sim_io_recover(&io);
    assert(recover_status == SIM_IO_OK && io.state.ready == 1 && io.state.recovery_epoch == 1);
    assert(sim_io_set_power(&io, 0) == SIM_IO_OK && io.state.ready == 0);
    assert(sim_io_spindle_command(&io, 0, 100.0, 1) == SIM_IO_ERR_NOT_READY);
    assert(sim_io_set_power(&io, 1) == SIM_IO_OK && io.state.ready == 1 && io.state.recovery_epoch == 2);

    assert(sim_io_tool_set_fault(&io, 1, 42) == SIM_IO_OK);
    fault_clear_status = sim_io_tool_clear_fault(&io);
    assert(fault_clear_status == SIM_IO_ERR_FAULT);
    assert(sim_io_tool_set_fault(&io, 0, 0) == SIM_IO_OK);
    assert(sim_io_tool_clear_fault(&io) == SIM_IO_OK);
    hash = sim_io_state_hash(&io);
    repeat_hash = sim_io_state_hash(&io);
    assert(hash == repeat_hash);
    assert(sim_io_canonical(&io, canonical, sizeof(canonical)) > 1 && canonical[0] == '{');
    assert(sim_io_canonical(&io, 0, 0) == strlen(canonical) + 1);
    assert(sim_hal_get_pin("iocontrol.0.tool-number", &pin) == SIM_HAL_OK && pin.value.s32 == 7);
    assert(sim_hal_get_pin("spindle.0.brake", &pin) == SIM_HAL_OK && pin.value.bit == 1);
    printf("{\"schema\":\"io-samples.v1\",\"pin\":{\"invalid_type\":%d,\"count\":%u,\"functions\":%u},\"spindle\":{\"limit\":%d,\"step\":%d,\"speed\":%.3f,\"at_speed\":%u,\"tool_busy\":%d},\"tool\":{\"state\":%u,\"current\":%d,\"fault_clear\":%d},\"probe\":{\"status\":%d,\"state\":%u,\"position\":[%.3f,%.3f,%.3f]},\"estop\":{\"command\":%d,\"recover_before_clear\":%d,\"recover\":%d},\"state_hash\":%llu,\"repeat_hash\":%llu,\"canonical\":%s}\n",
           invalid_pin_status, sim_hal_pin_count(), sim_hal_function_count(), limit_status,
           spindle_step_status, io.state.spindles[0].speed_rpm, io.state.spindles[0].at_speed,
           tool_busy_status, io.state.tool_state, io.state.current_tool, fault_clear_status,
           probe_status, io.state.probe.state, io.state.probe.position.tran.x,
           io.state.probe.position.tran.y, io.state.probe.position.tran.z,
           estop_command_status, estop_recover_status, recover_status,
           (unsigned long long)hash, (unsigned long long)repeat_hash, canonical);
    (void)pose;
    return 0;
}