#include "sim_device_io.h"

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

static int valid_device(const sim_device_io *device) {
    return device != 0 && device->state.encoder_count > 0U && device->state.encoder_count <= SIM_DEVICE_MAX_ENCODERS && isfinite(device->cycle_time) && device->cycle_time > 0.0;
}

uint32_t sim_device_io_size(void) { return (uint32_t)sizeof(sim_device_io); }

static int finite_value(double value) { return isfinite(value); }

static void set_error(sim_device_io *device, int error) {
    if (device != 0) device->state.last_error = error;
}

static void update_safety(sim_device_io *device) {
    sim_device_safety_state *safety = &device->state.safety;
    safety->fault = (safety->door_open || !safety->input_ok) ? 1U : 0U;
    if (safety->fault || safety->estop) safety->latched_fault = 1U;
    safety->interlock_ok = (!safety->fault && !safety->estop && !safety->latched_fault) ? 1U : 0U;
}

static void update_coolant(sim_device_io *device) {
    sim_device_coolant_state *coolant = &device->state.coolant;
    const uint32_t allowed = device->state.safety.interlock_ok && !coolant->fault;
    coolant->mist = allowed ? coolant->mist_requested : 0U;
    coolant->flood = allowed ? coolant->flood_requested : 0U;
    coolant->lube = allowed ? coolant->lube_requested : 0U;
}

static void update_encoder(sim_device_io *device, uint32_t channel) {
    sim_device_encoder_state *encoder = &device->state.encoders[channel];
    const double counts_per_revolution = (double)encoder->ppr * 4.0;
    const double delta = encoder->speed_rps * counts_per_revolution * device->cycle_time;
    int64_t raw;
    uint32_t phase;
    if (!encoder->registered) return;
    device->phase_accumulator[channel] += delta;
    raw = device->phase_accumulator[channel] >= 0.0 ? (int64_t)floor(device->phase_accumulator[channel]) : (int64_t)ceil(device->phase_accumulator[channel]);
    if (raw > INT32_MAX || raw < INT32_MIN) {
        encoder->raw_count = raw > 0 ? INT32_MAX : INT32_MIN;
        encoder->count = encoder->raw_count;
        set_error(device, SIM_DEVICE_ERR_RANGE);
        return;
    }
    encoder->raw_count = (int32_t)raw;
    encoder->count = encoder->raw_count;
    encoder->position = (double)encoder->raw_count / encoder->scale;
    encoder->velocity = encoder->speed_rps * counts_per_revolution / encoder->scale;
    phase = (uint32_t)(((int64_t)encoder->raw_count % 4LL + 4LL) % 4LL);
    encoder->phase_a = phase == 0U || phase == 1U;
    encoder->phase_b = phase == 1U || phase == 2U;
    encoder->index_pulse = 0U;
    if (encoder->index_enable && encoder->ppr > 0U && encoder->raw_count % (int32_t)counts_per_revolution == 0) {
        encoder->phase_z = 1U;
        encoder->index_pulse = 1U;
        encoder->index_enable = 0U;
    } else {
        encoder->phase_z = 0U;
    }
    encoder->tick = device->state.tick;
}

static int ack_index(const sim_device_io *device, uint32_t operation_id) {
    uint32_t index;
    for (index = 0; index < SIM_DEVICE_MAX_ACKS; ++index) if (device->state.acks[index].used && device->state.acks[index].operation_id == operation_id) return (int)index;
    return -1;
}

int sim_device_init(sim_device_io *device, uint32_t encoder_count, double cycle_time) {
    uint32_t index;
    if (device == 0 || encoder_count == 0U || encoder_count > SIM_DEVICE_MAX_ENCODERS || !finite_value(cycle_time) || cycle_time <= 0.0 || cycle_time > 10.0) return SIM_DEVICE_ERR_ARGUMENT;
    memset(device, 0, sizeof(*device));
    device->cycle_time = cycle_time;
    device->state.encoder_count = encoder_count;
    device->state.last_error = SIM_DEVICE_OK;
    device->state.safety.input_ok = 1U;
    for (index = 0; index < encoder_count; ++index) {
        device->state.encoders[index].registered = 1U;
        device->state.encoders[index].ppr = 1000U;
        device->state.encoders[index].scale = 1.0;
    }
    update_safety(device);
    update_coolant(device);
    return SIM_DEVICE_OK;
}

int sim_device_reset(sim_device_io *device) {
    uint32_t count;
    double cycle;
    if (!valid_device(device)) return SIM_DEVICE_ERR_ARGUMENT;
    count = device->state.encoder_count; cycle = device->cycle_time;
    return sim_device_init(device, count, cycle);
}

int sim_device_encoder_set_speed(sim_device_io *device, uint32_t channel, double speed_rps) {
    if (!valid_device(device) || channel >= device->state.encoder_count) return SIM_DEVICE_ERR_ARGUMENT;
    if (!finite_value(speed_rps) || fabs(speed_rps) > 1000000.0) return SIM_DEVICE_ERR_NONFINITE;
    device->state.encoders[channel].speed_rps = speed_rps;
    set_error(device, SIM_DEVICE_OK);
    return SIM_DEVICE_OK;
}

int sim_device_encoder_set_config(sim_device_io *device, uint32_t channel, uint32_t ppr, double scale) {
    if (!valid_device(device) || channel >= device->state.encoder_count || ppr == 0U || ppr > 100000000U || !finite_value(scale) || scale <= 0.0) return SIM_DEVICE_ERR_RANGE;
    device->state.encoders[channel].ppr = ppr;
    device->state.encoders[channel].scale = scale;
    set_error(device, SIM_DEVICE_OK);
    return SIM_DEVICE_OK;
}

int sim_device_encoder_set_index_enable(sim_device_io *device, uint32_t channel, uint32_t enable) {
    if (!valid_device(device) || channel >= device->state.encoder_count || enable > 1U) return SIM_DEVICE_ERR_ARGUMENT;
    device->state.encoders[channel].index_enable = enable;
    set_error(device, SIM_DEVICE_OK);
    return SIM_DEVICE_OK;
}

int sim_device_coolant_command(sim_device_io *device, uint32_t mist, uint32_t flood, uint32_t lube) {
    if (!valid_device(device) || mist > 1U || flood > 1U || lube > 1U) return SIM_DEVICE_ERR_ARGUMENT;
    if ((mist || flood || lube) && !device->state.safety.interlock_ok) return SIM_DEVICE_ERR_INTERLOCK;
    device->state.coolant.mist_requested = mist;
    device->state.coolant.flood_requested = flood;
    device->state.coolant.lube_requested = lube;
    update_coolant(device);
    set_error(device, SIM_DEVICE_OK);
    return SIM_DEVICE_OK;
}

int sim_device_coolant_set_fault(sim_device_io *device, uint32_t fault) {
    if (!valid_device(device) || fault > 1U) return SIM_DEVICE_ERR_ARGUMENT;
    device->state.coolant.fault = fault;
    update_coolant(device);
    set_error(device, fault ? SIM_DEVICE_ERR_STATE : SIM_DEVICE_OK);
    return fault ? SIM_DEVICE_ERR_STATE : SIM_DEVICE_OK;
}

int sim_device_door_set(sim_device_io *device, uint32_t open) {
    if (!valid_device(device) || open > 1U) return SIM_DEVICE_ERR_ARGUMENT;
    device->state.safety.door_open = open;
    update_safety(device); update_coolant(device);
    set_error(device, open ? SIM_DEVICE_ERR_INTERLOCK : SIM_DEVICE_OK);
    return open ? SIM_DEVICE_ERR_INTERLOCK : SIM_DEVICE_OK;
}

int sim_device_interlock_set(sim_device_io *device, uint32_t input_ok) {
    if (!valid_device(device) || input_ok > 1U) return SIM_DEVICE_ERR_ARGUMENT;
    device->state.safety.input_ok = input_ok;
    update_safety(device); update_coolant(device);
    set_error(device, input_ok ? SIM_DEVICE_OK : SIM_DEVICE_ERR_INTERLOCK);
    return input_ok ? SIM_DEVICE_OK : SIM_DEVICE_ERR_INTERLOCK;
}

int sim_device_interlock_reset(sim_device_io *device) {
    if (!valid_device(device)) return SIM_DEVICE_ERR_ARGUMENT;
    update_safety(device);
    if (device->state.safety.door_open || !device->state.safety.input_ok || device->state.safety.estop) return SIM_DEVICE_ERR_INTERLOCK;
    device->state.safety.latched_fault = 0U;
    device->state.safety.recovery_epoch += 1U;
    update_safety(device); update_coolant(device);
    set_error(device, SIM_DEVICE_OK);
    return SIM_DEVICE_OK;
}

int sim_device_estop_set(sim_device_io *device, uint32_t estop) {
    if (!valid_device(device) || estop > 1U) return SIM_DEVICE_ERR_ARGUMENT;
    device->state.safety.estop = estop;
    update_safety(device); update_coolant(device);
    set_error(device, estop ? SIM_DEVICE_ERR_INTERLOCK : SIM_DEVICE_OK);
    return estop ? SIM_DEVICE_ERR_INTERLOCK : SIM_DEVICE_OK;
}

int sim_device_ack_issue(sim_device_io *device, uint32_t operation_id, uint32_t timeout_ticks) {
    uint32_t index;
    if (!valid_device(device) || operation_id == 0U || timeout_ticks == 0U || timeout_ticks > 1000000U) return SIM_DEVICE_ERR_ARGUMENT;
    if (ack_index(device, operation_id) >= 0) return SIM_DEVICE_ERR_DUPLICATE;
    for (index = 0; index < SIM_DEVICE_MAX_ACKS; ++index) if (!device->state.acks[index].used) break;
    if (index == SIM_DEVICE_MAX_ACKS) return SIM_DEVICE_ERR_CAPACITY;
    device->state.acks[index].used = 1U;
    device->state.acks[index].operation_id = operation_id;
    device->state.acks[index].issued_tick = device->state.tick;
    device->state.acks[index].deadline_tick = device->state.tick + timeout_ticks;
    device->state.acks[index].status = SIM_DEVICE_ACK_PENDING;
    set_error(device, SIM_DEVICE_OK);
    return SIM_DEVICE_OK;
}

int sim_device_ack_ack(sim_device_io *device, uint32_t operation_id) {
    int index;
    if (!valid_device(device)) return SIM_DEVICE_ERR_ARGUMENT;
    index = ack_index(device, operation_id);
    if (index < 0) return SIM_DEVICE_ERR_MISSING;
    if (device->state.acks[index].status == SIM_DEVICE_ACK_TIMED_OUT) return SIM_DEVICE_ERR_LATE_ACK;
    if (device->state.acks[index].status != SIM_DEVICE_ACK_PENDING) return SIM_DEVICE_ERR_STATE;
    device->state.acks[index].status = SIM_DEVICE_ACK_ACKED;
    set_error(device, SIM_DEVICE_OK);
    return SIM_DEVICE_OK;
}

int sim_device_ack_abort(sim_device_io *device, uint32_t operation_id) {
    int index;
    if (!valid_device(device)) return SIM_DEVICE_ERR_ARGUMENT;
    index = ack_index(device, operation_id);
    if (index < 0) return SIM_DEVICE_ERR_MISSING;
    if (device->state.acks[index].status == SIM_DEVICE_ACK_ACKED) return SIM_DEVICE_ERR_STATE;
    device->state.acks[index].status = SIM_DEVICE_ACK_ABORTED;
    set_error(device, SIM_DEVICE_OK);
    return SIM_DEVICE_OK;
}

int sim_device_ack_step(sim_device_io *device, uint32_t ticks) {
    uint32_t index, tick;
    if (!valid_device(device) || ticks > 1000000U || UINT32_MAX - device->state.tick < ticks) return SIM_DEVICE_ERR_RANGE;
    for (tick = 0; tick < ticks; ++tick) {
        device->state.tick += 1U;
        for (index = 0; index < SIM_DEVICE_MAX_ACKS; ++index) if (device->state.acks[index].used && device->state.acks[index].status == SIM_DEVICE_ACK_PENDING && device->state.tick >= device->state.acks[index].deadline_tick) device->state.acks[index].status = SIM_DEVICE_ACK_TIMED_OUT;
    }
    return SIM_DEVICE_OK;
}

int sim_device_step(sim_device_io *device, uint32_t ticks) {
    sim_device_io before;
    uint32_t tick, index;
    if (!valid_device(device) || ticks > 1000000U || UINT32_MAX - device->state.tick < ticks) return SIM_DEVICE_ERR_RANGE;
    memcpy(&before, device, sizeof(before));
    for (tick = 0; tick < ticks; ++tick) {
        device->state.tick += 1U;
        update_safety(device); update_coolant(device);
        for (index = 0; index < device->state.encoder_count; ++index) update_encoder(device, index);
        for (index = 0; index < SIM_DEVICE_MAX_ACKS; ++index) if (device->state.acks[index].used && device->state.acks[index].status == SIM_DEVICE_ACK_PENDING && device->state.tick >= device->state.acks[index].deadline_tick) device->state.acks[index].status = SIM_DEVICE_ACK_TIMED_OUT;
    }
    if (device->state.last_error == SIM_DEVICE_ERR_RANGE) { memcpy(device, &before, sizeof(before)); return SIM_DEVICE_ERR_RANGE; }
    set_error(device, SIM_DEVICE_OK);
    return SIM_DEVICE_OK;
}

int sim_device_read_state(const sim_device_io *device, sim_device_io_state *state) {
    if (!valid_device(device) || state == 0) return SIM_DEVICE_ERR_ARGUMENT;
    *state = device->state;
    return SIM_DEVICE_OK;
}

static uint64_t fnv1a(const unsigned char *bytes, size_t length) {
    uint64_t hash = UINT64_C(1469598103934665603);
    size_t index;
    for (index = 0; index < length; ++index) { hash ^= bytes[index]; hash *= UINT64_C(1099511628211); }
    return hash;
}

uint64_t sim_device_state_hash(const sim_device_io *device) {
    unsigned char bytes[sizeof(sim_device_io_state) + sizeof(double) * SIM_DEVICE_MAX_ENCODERS];
    if (!valid_device(device)) return 0;
    memcpy(bytes, &device->state, sizeof(device->state));
    memcpy(bytes + sizeof(device->state), device->phase_accumulator, sizeof(device->phase_accumulator));
    return fnv1a(bytes, sizeof(bytes));
}

uint32_t sim_device_canonical(const sim_device_io *device, char *output, uint32_t capacity) {
    char discard[1];
    char *destination = output == 0 ? discard : output;
    uint32_t destination_capacity = output == 0 ? 1U : capacity;
    int written;
    uint32_t index;
    if (!valid_device(device)) return 0;
    written = snprintf(destination, destination_capacity, "{\"coolant\":{\"flood\":%u,\"lube\":%u,\"mist\":%u},\"encoders\":[", device->state.coolant.flood, device->state.coolant.lube, device->state.coolant.mist);
    if (written < 0) return 0;
    for (index = 0; index < device->state.encoder_count; ++index) {
        const sim_device_encoder_state *encoder = &device->state.encoders[index];
        int part = snprintf(destination + (written < (int)destination_capacity ? written : 0), written < (int)destination_capacity ? destination_capacity - (uint32_t)written : 0U, "%s{\"count\":%d,\"index_pulse\":%u,\"position\":%.9g,\"velocity\":%.9g}", index ? "," : "", encoder->count, encoder->index_pulse, encoder->position, encoder->velocity);
        if (part < 0) return 0; written += part;
    }
    { int part = snprintf(destination + (written < (int)destination_capacity ? written : 0), written < (int)destination_capacity ? destination_capacity - (uint32_t)written : 0U, "],\"safety\":{\"door_open\":%u,\"estop\":%u,\"interlock_ok\":%u,\"latched_fault\":%u},\"tick\":%u}", device->state.safety.door_open, device->state.safety.estop, device->state.safety.interlock_ok, device->state.safety.latched_fault, device->state.tick); if (part < 0) return 0; written += part; }
    return (uint32_t)written + 1U;
}