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

#include "trikke_protocol.h"

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 1;
    }

    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 1;
    }
    return 0;
}