#include "sim_device_io.h"

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

int main(void) {
    sim_device_io device = {0};
    sim_device_io_state state = {0};
    char canonical[4096];
    uint64_t hash;

    assert(sim_device_init(&device, 2, 0.1) == SIM_DEVICE_OK);
    assert(sim_device_encoder_set_config(&device, 0, 10, 4.0) == SIM_DEVICE_OK);
    assert(sim_device_encoder_set_speed(&device, 0, 1.0) == SIM_DEVICE_OK);
    assert(sim_device_encoder_set_index_enable(&device, 0, 1) == SIM_DEVICE_OK);
    assert(sim_device_step(&device, 1) == SIM_DEVICE_OK);
    assert(device.state.encoders[0].count == 4);
    assert(device.state.encoders[0].position == 1.0);
    assert(device.state.encoders[0].index_pulse == 0);
    assert(sim_device_step(&device, 9) == SIM_DEVICE_OK);
    assert(device.state.encoders[0].count == 40);
    assert(device.state.encoders[0].index_pulse == 1 && device.state.encoders[0].index_enable == 0);
    assert(sim_device_encoder_set_speed(&device, 0, NAN) == SIM_DEVICE_ERR_NONFINITE);

    assert(sim_device_coolant_command(&device, 1, 0, 1) == SIM_DEVICE_OK);
    assert(device.state.coolant.mist == 1 && device.state.coolant.lube == 1);
    assert(sim_device_door_set(&device, 1) == SIM_DEVICE_ERR_INTERLOCK);
    assert(device.state.safety.interlock_ok == 0 && device.state.coolant.mist == 0);
    assert(sim_device_coolant_command(&device, 1, 0, 0) == SIM_DEVICE_ERR_INTERLOCK);
    assert(sim_device_door_set(&device, 0) == SIM_DEVICE_OK);
    assert(sim_device_interlock_reset(&device) == SIM_DEVICE_OK);
    assert(device.state.safety.interlock_ok == 1 && device.state.safety.recovery_epoch == 1);
    assert(sim_device_coolant_command(&device, 1, 0, 0) == SIM_DEVICE_OK && device.state.coolant.mist == 1);
    assert(sim_device_estop_set(&device, 1) == SIM_DEVICE_ERR_INTERLOCK);
    assert(sim_device_estop_set(&device, 0) == SIM_DEVICE_OK);
    assert(sim_device_interlock_reset(&device) == SIM_DEVICE_OK);

    assert(sim_device_ack_issue(&device, 11, 2) == SIM_DEVICE_OK);
    assert(sim_device_ack_issue(&device, 11, 2) == SIM_DEVICE_ERR_DUPLICATE);
    assert(sim_device_ack_step(&device, 1) == SIM_DEVICE_OK);
    assert(sim_device_ack_ack(&device, 11) == SIM_DEVICE_OK);
    assert(sim_device_ack_issue(&device, 12, 1) == SIM_DEVICE_OK);
    assert(sim_device_ack_step(&device, 1) == SIM_DEVICE_OK);
    assert(sim_device_ack_ack(&device, 12) == SIM_DEVICE_ERR_LATE_ACK);

    assert(sim_device_read_state(&device, &state) == SIM_DEVICE_OK);
    hash = sim_device_state_hash(&device);
    assert(hash == sim_device_state_hash(&device));
    assert(sim_device_canonical(&device, canonical, sizeof(canonical)) > 1 && canonical[0] == '{');
    printf("{\"schema\":\"device-io-s2-sample.v1\",\"tick\":%u,\"encoder\":{\"count\":%d,\"position\":%.3f,\"velocity\":%.3f,\"index_pulse\":%u},\"coolant\":{\"mist\":%u,\"flood\":%u,\"lube\":%u},\"safety\":{\"interlock_ok\":%u,\"recovery_epoch\":%u},\"ack\":{\"acked\":%u,\"timed_out\":%u},\"state_hash\":%llu,\"canonical\":%s}\n",
           device.state.tick, state.encoders[0].count, state.encoders[0].position, state.encoders[0].velocity, state.encoders[0].index_pulse,
           state.coolant.mist, state.coolant.flood, state.coolant.lube, state.safety.interlock_ok, state.safety.recovery_epoch,
           state.acks[0].status, state.acks[1].status, (unsigned long long)hash, canonical);
    return 0;
}