#include "set_firmware.h"
#include "set_can.h"
#include "set_protocol.h"
#include "set_telemetry.h"

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

static int failures;
static int checks;

static void check_impl(int condition, const char *text, int line)
{
    ++checks;
    if (!condition) {
        ++failures;
        (void)printf("FAIL line %d: %s\n", line, text);
    }
}

#define CHECK(condition) check_impl((condition) ? 1 : 0, #condition, __LINE__)

typedef struct {
    setp_frame_t frame;
    uint8_t payload[SETP_MAX_PAYLOAD];
    int count;
} capture_t;

static void capture_frame(const setp_frame_t *frame, void *user)
{
    capture_t *capture = (capture_t *)user;
    capture->frame = *frame;
    if (frame->payload_length != 0U) {
        (void)memcpy(capture->payload, frame->payload, frame->payload_length);
    }
    capture->frame.payload = capture->payload;
    ++capture->count;
}

static void test_crc_and_reference_frame(void)
{
    static const uint8_t expected[] = {
        0xA5U, 0x5AU, 0x02U, 0x08U, 0x01U, 0x00U, 0x00U, 0x00U,
        0x2AU, 0x00U, 0x34U, 0x12U, 0x00U, 0x00U, 0x33U, 0xECU,
        0x33U, 0x04U
    };
    setp_frame_t frame;
    uint8_t output[SETP_FRAME_MAX];
    size_t length;
    setp_frame_t decoded;

    CHECK(setp_crc32((const uint8_t *)"123456789", 9U) == 0xCBF43926UL);
    (void)memset(&frame, 0, sizeof(frame));
    frame.flags = SETP_FLAG_ACK_REQUIRED;
    frame.message_type = SETP_MSG_PING;
    frame.destination = 0x002AU;
    frame.sequence = 0x1234U;
    length = setp_frame_encode(&frame, output, sizeof(output));
    CHECK(length == sizeof(expected));
    CHECK(memcmp(output, expected, sizeof(expected)) == 0);
    CHECK(setp_frame_decode_datagram(output, length, &decoded));
    CHECK(decoded.message_type == SETP_MSG_PING);
    CHECK(decoded.destination == 0x002AU);
    CHECK(!setp_frame_decode_datagram(output, length - 1U, &decoded));
    CHECK(setp_frame_encode(&frame, output, 4U) == 0U);
}

static void test_parser(void)
{
    setp_frame_t frame;
    uint8_t raw[SETP_FRAME_MAX];
    uint8_t payload[] = { 0x11U, 0x22U, 0x33U };
    size_t length;
    setp_parser_t parser;
    capture_t capture;

    (void)memset(&frame, 0, sizeof(frame));
    frame.message_type = 0x3456U; /* Unknown extensions are valid frames. */
    frame.source = 7U;
    frame.destination = 9U;
    frame.sequence = 11U;
    frame.payload_length = sizeof(payload);
    frame.payload = payload;
    length = setp_frame_encode(&frame, raw, sizeof(raw));

    (void)memset(&capture, 0, sizeof(capture));
    setp_parser_init(&parser);
    CHECK(setp_parser_feed(&parser, (const uint8_t *)"noise", 5U,
                           capture_frame, &capture) == 0U);
    CHECK(setp_parser_feed(&parser, raw, 3U, capture_frame, &capture) == 0U);
    CHECK(setp_parser_feed(&parser, &raw[3], length - 3U,
                           capture_frame, &capture) == 1U);
    CHECK(capture.count == 1);
    CHECK(capture.frame.message_type == 0x3456U);
    CHECK(capture.frame.source == 7U);
    CHECK(capture.frame.destination == 9U);
    CHECK(capture.frame.sequence == 11U);
    CHECK(capture.frame.payload_length == sizeof(payload));
    CHECK(memcmp(capture.payload, payload, sizeof(payload)) == 0);
    CHECK(parser.stats.stray_bytes == 5U);

    raw[length - 1U] ^= 1U;
    CHECK(setp_parser_feed(&parser, raw, length, capture_frame, &capture) == 0U);
    CHECK(parser.stats.crc_errors == 1U);
}

static void test_status(void)
{
    uint8_t payload[4] = { 0U };
    setp_frame_t frame;
    uint16_t status;
    uint16_t body_length;
    const uint8_t *body;

    CHECK(setp_status_encode(SETP_STATUS_BUSY, payload, sizeof(payload)) == 2U);
    payload[2] = 0xAAU;
    payload[3] = 0x55U;
    (void)memset(&frame, 0, sizeof(frame));
    frame.flags = SETP_FLAG_RESPONSE | SETP_FLAG_ERROR;
    frame.payload = payload;
    frame.payload_length = sizeof(payload);
    CHECK(setp_status_decode(&frame, &status, &body, &body_length));
    CHECK(status == SETP_STATUS_BUSY);
    CHECK(body_length == 2U);
    CHECK(body[0] == 0xAAU && body[1] == 0x55U);
}

typedef struct {
    setp_can_frame_t frames[96];
    uint16_t count;
} can_capture_t;

static bool capture_can(const setp_can_frame_t *frame, void *user)
{
    can_capture_t *capture = (can_capture_t *)user;
    if (capture->count >= 96U) {
        return false;
    }
    capture->frames[capture->count++] = *frame;
    return true;
}

static void test_can_binding(void)
{
    setp_can_id_t id = { 7U, 9U, 1U, 3U };
    setp_can_id_t decoded_id;
    uint32_t raw_id = setp_can_id_pack(&id);
    setp_frame_t frame;
    uint8_t payload[40];
    uint8_t packet[SETP_FRAME_MAX];
    size_t packet_length;
    can_capture_t capture;
    setp_can_rx_t rx;
    setp_can_packet_t complete;
    setp_can_rx_result_t result = SETP_CAN_RX_NONE;
    uint16_t index;

    CHECK(setp_can_id_unpack(raw_id, &decoded_id));
    CHECK(decoded_id.source == 7U);
    CHECK(decoded_id.destination == 9U);
    CHECK(decoded_id.priority == 1U);
    CHECK(decoded_id.channel == 3U);
    for (index = 0U; index < sizeof(payload); ++index) {
        payload[index] = (uint8_t)index;
    }
    (void)memset(&frame, 0, sizeof(frame));
    frame.flags = SETP_FLAG_PRIORITY;
    frame.message_type = SETP_MSG_PUBLISH;
    frame.source = 7U;
    frame.destination = 9U;
    frame.sequence = 3U;
    frame.payload = payload;
    frame.payload_length = sizeof(payload);
    packet_length = setp_frame_encode(&frame, packet, sizeof(packet));
    (void)memset(&capture, 0, sizeof(capture));
    (void)memset(&complete, 0, sizeof(complete));
    CHECK(setp_can_segment(packet, (uint16_t)packet_length, raw_id,
                           capture_can, &capture));
    CHECK(capture.count > 1U);
    CHECK(capture.frames[0].data[0] == SETP_CAN_PCI_FIRST);
    setp_can_rx_init(&rx);
    for (index = 0U; index < capture.count; ++index) {
        result = setp_can_rx_feed(&rx, &capture.frames[index], index, &complete);
    }
    CHECK(result == SETP_CAN_RX_COMPLETE);
    CHECK(complete.length == packet_length);
    CHECK(complete.can_id == raw_id);
    CHECK(memcmp(complete.data, packet, packet_length) == 0);
}

static void test_firmware(void)
{
    setp_fw_begin_t begin;
    setp_fw_begin_t decoded_begin;
    setp_fw_data_t decoded_data;
    uint8_t signature[64];
    uint8_t image[32];
    uint8_t manifest[SETP_FW_MANIFEST_SIZE];
    uint8_t payload[SETP_MAX_PAYLOAD];
    size_t length;
    uint16_t index;

    (void)memset(&begin, 0, sizeof(begin));
    (void)memset(signature, 0xA5, sizeof(signature));
    for (index = 0U; index < sizeof(image); ++index) {
        image[index] = (uint8_t)index;
    }
    begin.image_size = 4096U;
    begin.image_crc32 = 0x12345678UL;
    begin.image_version = 0x00010203UL;
    begin.base_address = 0x08010000UL;
    begin.slot = 1U;
    begin.flags = SETP_FW_FLAG_RESUME | SETP_FW_FLAG_SIGNED;
    begin.block_size = 480U;
    (void)memset(begin.sha256, 0x5A, sizeof(begin.sha256));
    begin.signing_key_id = 3U;
    begin.signature_length = sizeof(signature);
    begin.signature = signature;
    length = setp_fw_begin_encode(&begin, payload, sizeof(payload));
    CHECK(length == SETP_FW_BEGIN_FIXED_SIZE + sizeof(signature));
    CHECK(setp_fw_begin_decode(payload, (uint16_t)length, &decoded_begin));
    CHECK(decoded_begin.image_size == begin.image_size);
    CHECK(decoded_begin.block_size == 480U);
    CHECK(decoded_begin.signing_key_id == 3U);
    CHECK(decoded_begin.signature_length == sizeof(signature));
    CHECK(memcmp(decoded_begin.signature, signature, sizeof(signature)) == 0);
    CHECK(setp_fw_manifest_encode(&decoded_begin, manifest, sizeof(manifest))
          == SETP_FW_MANIFEST_SIZE);
    CHECK(memcmp(manifest, "SETPFW2", 7U) == 0);
    CHECK(setp_get_u32(&manifest[8]) == begin.image_size);

    length = setp_fw_data_encode(1024U, 0U, image, sizeof(image),
                                 payload, sizeof(payload));
    CHECK(length == SETP_FW_DATA_HEADER_SIZE + sizeof(image));
    CHECK(setp_fw_data_decode(payload, (uint16_t)length, &decoded_data));
    CHECK(decoded_data.offset == 1024U);
    CHECK(decoded_data.data_length == sizeof(image));
    CHECK(memcmp(decoded_data.data, image, sizeof(image)) == 0);
    payload[length - 1U] ^= 1U;
    CHECK(!setp_fw_data_decode(payload, (uint16_t)length, &decoded_data));
}

static void test_telemetry(void)
{
    uint32_t addresses[] = { 0x10U, 0x12345678UL };
    uint8_t payload[SETP_MAX_PAYLOAD];
    uint16_t subscription_id;
    uint16_t count;
    uint32_t period;
    uint32_t address;
    size_t length;
    setp_publish_builder_t builder;
    setp_publish_view_t view;
    setp_publish_item_t item;
    uint8_t value1[] = { 0x34U, 0x12U };
    uint8_t value2[] = { 1U, 2U, 3U, 4U };

    length = setp_subscribe_encode(7U, 50U, addresses, 2U, payload, sizeof(payload));
    CHECK(length == 16U);
    CHECK(setp_subscribe_decode_header(payload, (uint16_t)length,
                                       &subscription_id, &period, &count));
    CHECK(subscription_id == 7U);
    CHECK(period == 50U);
    CHECK(count == 2U);
    CHECK(setp_subscribe_address(payload, (uint16_t)length, 1U, &address));
    CHECK(address == 0x12345678UL);

    CHECK(setp_publish_begin(&builder, 7U, 9U, 123456U,
                             payload, sizeof(payload)));
    CHECK(setp_publish_add(&builder, 0x10U, SETP_VALUE_U16, 1U,
                           value1, sizeof(value1)));
    CHECK(setp_publish_add(&builder, 0x20U, SETP_VALUE_BYTES, 4U,
                           value2, sizeof(value2)));
    length = setp_publish_finish(&builder);
    CHECK(length == SETP_PUBLISH_HEADER_SIZE
          + (2U * SETP_PUBLISH_ITEM_HEADER_SIZE) + sizeof(value1) + sizeof(value2));
    CHECK(setp_publish_view_init(&view, payload, (uint16_t)length));
    CHECK(view.subscription_id == 7U);
    CHECK(view.sample_sequence == 9U);
    CHECK(view.timestamp_ms == 123456U);
    CHECK(setp_publish_next(&view, &item));
    CHECK(item.address == 0x10U);
    CHECK(item.encoding == SETP_VALUE_U16);
    CHECK(item.data_length == sizeof(value1));
    CHECK(setp_publish_next(&view, &item));
    CHECK(item.address == 0x20U);
    CHECK(item.element_count == 4U);
    CHECK(view.remaining == 0U);
    CHECK(!setp_publish_next(&view, &item));
}

int main(void)
{
    test_crc_and_reference_frame();
    test_parser();
    test_status();
    test_can_binding();
    test_firmware();
    test_telemetry();
    (void)printf("checks: %d, failures: %d\n", checks, failures);
    return failures == 0 ? 0 : 1;
}