#include "buf.h"
#include <assert.h>
#include <math.h>
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>

/*
 * Inspired by Vedders bldc code.
 *
 * I stumbled upon this beautiful solution when reading the motor control firmware from the vesc project.
 * It effectively packs/serializes data into a buffer that could easily be sent over, say, uart. The original solution
 * also does fp magic in a seemingly portable way.
 *
 * For the original GPLv3 licensed source, see:
 *      https://github.com/vedderb/bldc/blob/master/util/buffer.c
 *
 * For a more permissive MIT licensed source, see:
 *      https://github.com/waas-rent/vesc_can_sdk/blob/main/vesc_buffer.c
 */

/* This buffer can live anywhere, make sure its long enough to hold your data! */
uint8_t buf[100];

/* Float comparison with error tolerance */
static inline bool float_eq(float a, float b, float epsilon) {
    return fabsf(a - b) < epsilon;
}

int tests(void);

int main(void) {
    (void)tests();

    /* This index acts as a read/write pointer */
    size_t index = 0;

    /* Declare and define data to be serialized */
    uint8_t uint8bit = 8;
    int16_t int16bit = 16;
    int32_t int32bit = 32;

    /* When appending the buffer, the index is passed along and incremented
     * accordingly */
    buf_append_u8(buf, uint8bit, &index);
    assert(index == 1);
    buf_append_i16(buf, int16bit, &index);
    assert(index == 3);
    buf_append_i32(buf, int32bit, &index);
    assert(index == 7);

    /*
     * Note that after we're done appending, the index value will hold the
     * length of the 'message', so, if you're piping this through uart or some
     * other medium, you dont have to send the entire buffer length, just send
     * up to the index.
     *
     * For bonus points for doing it vesc-style, prepend the message with the
     * message type (a byte-length enum shared by both sides, for example), as
     * well as a message length. You could also use the last two bytes of every
     * message for a crc16 checksum for error resilience.
     */

    /* Zero the original data */
    uint8bit = 0;
    int16bit = 0;
    int32bit = 0;

    /* Read from beginning */
    index = 0;

    /* Re-populate our original data */
    uint8bit = buf_read_u8(buf, &index);
    assert(index == 1);
    int16bit = buf_read_i16(buf, &index);
    assert(index == 3);
    int32bit = buf_read_i32(buf, &index);
    assert(index == 7);

    /* Assert that it all worked out */
    assert(uint8bit == 8);
    assert(int16bit == 16);
    assert(int32bit == 32);

    printf("All asserts passed!\n");

    return 0;
}

void test_i8_u8() {
    uint8_t buf[10];
    size_t  index = 0;

    int8_t  i8_val = -123;
    uint8_t u8_val = 234;

    buf_append_i8(buf, i8_val, &index);
    buf_append_u8(buf, u8_val, &index);

    assert(index == 2);

    size_t  read_index = 0;
    int8_t  i8_read = buf_read_i8(buf, &read_index);
    uint8_t u8_read = buf_read_u8(buf, &read_index);

    assert(i8_read == i8_val);
    assert(u8_read == u8_val);
    assert(read_index == 2);
}

void test_i16_u16() {
    uint8_t buf[10];
    size_t  index = 0;

    int16_t  i16_val = -32000;
    uint16_t u16_val = 65000;

    buf_append_i16(buf, i16_val, &index);
    buf_append_u16(buf, u16_val, &index);

    assert(index == 4);

    size_t   read_index = 0;
    int16_t  i16_read = buf_read_i16(buf, &read_index);
    uint16_t u16_read = buf_read_u16(buf, &read_index);

    assert(i16_read == i16_val);
    assert(u16_read == u16_val);
    assert(read_index == 4);
}

void test_i32_u32() {
    uint8_t buf[20];
    size_t  index = 0;

    int32_t  i32_val = -2000000000;
    uint32_t u32_val = 4000000000U;

    buf_append_i32(buf, i32_val, &index);
    buf_append_u32(buf, u32_val, &index);

    assert(index == 8);

    size_t   read_index = 0;
    int32_t  i32_read = buf_read_i32(buf, &read_index);
    uint32_t u32_read = buf_read_u32(buf, &read_index);

    assert(i32_read == i32_val);
    assert(u32_read == u32_val);
    assert(read_index == 8);
}

void test_i64_u64() {
    uint8_t buf[20];
    size_t  index = 0;

    int64_t  i64_val = -9000000000000000000LL;
    uint64_t u64_val = 18000000000000000000ULL;

    buf_append_i64(buf, i64_val, &index);
    buf_append_u64(buf, u64_val, &index);

    assert(index == 16);

    size_t   read_index = 0;
    int64_t  i64_read = buf_read_i64(buf, &read_index);
    uint64_t u64_read = buf_read_u64(buf, &read_index);

    assert(i64_read == i64_val);
    assert(u64_read == u64_val);
    assert(read_index == 16);
}

int tests() {
    test_i8_u8();
    test_i16_u16();
    test_i32_u32();
    test_i64_u64();

    printf("All tests passed.\n");
    return 0;
}