#include <stdint.h>
#include <stdio.h>

#include "trikke_protocol.h"

static int fail(int code, const char *message)
{
    fprintf(stderr, "protocol fixture failure %d: %s\n", code, message);
    return code;
}

int main(void)
{
    uint8_t packet[TRIKKE_WIRE_MAX_PACKET_SIZE] = {0};
    const trikke_wire_metadata_t metadata = {
        .sample_rate_hz = 100,
        .accel_range_g = 8,
        .gyro_range_dps = 500,
        .flags = TRIKKE_METADATA_FLAG_ACCEL_Y_NEGX_Z |
                 TRIKKE_METADATA_FLAG_GYRO_IDENTITY,
        .accel_offset_counts = {-1.5f, -4.5f, 12.0f},
        .accel_counts_per_g = {259.0f, 260.0f, 246.0f},
        .gyro_bias_counts = {9.0f, -154.0f, -7.0f},
        .gyro_mdps_per_lsb = 17.5f,
    };
    size_t size = trikke_encode_metadata_packet(
        packet, sizeof(packet), 41, 1234567, 2, 3, &metadata);
    if (size == 0 || fwrite(packet, 1, size, stdout) != size) {
        return fail(1, "metadata encoding or output");
    }

    const trikke_wire_sample_t samples[] = {
        {
            .sequence = 1000,
            .timestamp_us = 2000000,
            .accel_x = 1,
            .accel_y = -2,
            .accel_z = 258,
            .gyro_x = -10,
            .gyro_y = 20,
            .gyro_z = -30,
            .accel_status = 0x82,
            .gyro_status = 0x0F,
        },
        {
            .sequence = 1001,
            .timestamp_us = 2010000,
            .accel_x = 4,
            .accel_y = -5,
            .accel_z = 257,
            .gyro_x = 40,
            .gyro_y = -50,
            .gyro_z = 60,
            .accel_status = 0x02,
            .gyro_status = 0xFF,
        },
    };
    size = trikke_encode_sample_packet(packet, sizeof(packet), 42, 2, 3,
                                       samples, 2);
    if (size == 0 || fwrite(packet, 1, size, stdout) != size) {
        return fail(2, "basic sample encoding or output");
    }

    trikke_wire_sample_t full_packet[TRIKKE_WIRE_MAX_RECORDS] = {0};
    for (size_t i = 0; i < TRIKKE_WIRE_MAX_RECORDS; ++i) {
        full_packet[i].sequence = 2000 + (uint32_t)i;
        full_packet[i].timestamp_us = 3000000 + (int64_t)i * 10000;
        full_packet[i].accel_x = (int16_t)i;
        full_packet[i].gyro_z = -(int16_t)i;
        full_packet[i].accel_status = 0x80;
        full_packet[i].gyro_status = 0x08;
    }
    size = trikke_encode_sample_packet(
        packet, sizeof(packet), 43, 2, 3, full_packet,
        TRIKKE_WIRE_MAX_RECORDS);
    if (size != TRIKKE_WIRE_MAX_PACKET_SIZE ||
        fwrite(packet, 1, size, stdout) != size) {
        return fail(3, "maximum-size sample encoding or output");
    }

    const trikke_wire_sample_t saturated[] = {
        {.sequence = 3000, .timestamp_us = 4000000},
        {.sequence = 3001, .timestamp_us = 4700000},
    };
    size = trikke_encode_sample_packet(packet, sizeof(packet), 44, 2, 3,
                                       saturated, 2);
    if (size == 0 || fwrite(packet, 1, size, stdout) != size) {
        return fail(4, "saturated timestamp encoding or output");
    }

    const trikke_wire_status_t status = {
        .sensor_read_failure_count = 5,
        .queue_overflow_count = 6,
        .transport_begin_retry_count = 7,
        .transport_disconnect_count = 8,
        .transport_send_failure_count = 9,
        .transport_replay_count = 10,
        .transport_invalid_ack_count = 11,
    };
    size = trikke_encode_status_packet(packet, sizeof(packet), 45, 5000000,
                                       11, 12, &status);
    if (size != TRIKKE_WIRE_HEADER_SIZE + TRIKKE_WIRE_STATUS_SIZE ||
        fwrite(packet, 1, size, stdout) != size) {
        return fail(5, "status encoding or output");
    }

    if (!trikke_wire_timestamp_delta_fits(0, 655350) ||
        trikke_wire_timestamp_delta_fits(0, 655351) ||
        trikke_wire_timestamp_delta_fits(1, 0)) {
        return fail(6, "timestamp-delta boundary contract");
    }
    if (trikke_encode_metadata_packet(
            packet, TRIKKE_WIRE_HEADER_SIZE + TRIKKE_WIRE_METADATA_SIZE - 1,
            0, 0, 0, 0, &metadata) != 0 ||
        trikke_encode_sample_packet(packet, sizeof(packet), 0, 0, 0,
                                    samples, 0) != 0 ||
        trikke_encode_sample_packet(packet, sizeof(packet), 0, 0, 0,
                                    full_packet,
                                    TRIKKE_WIRE_MAX_RECORDS + 1) != 0 ||
        trikke_encode_sample_packet(packet, TRIKKE_WIRE_MAX_PACKET_SIZE - 1,
                                    0, 0, 0, full_packet,
                                    TRIKKE_WIRE_MAX_RECORDS) != 0 ||
        trikke_encode_status_packet(
            packet, TRIKKE_WIRE_HEADER_SIZE + TRIKKE_WIRE_STATUS_SIZE - 1,
            0, 0, 0, 0, &status) != 0) {
        return fail(7, "invalid argument rejection contract");
    }
    return 0;
}