#include "unity.h"
#include "celrs/crsf.h"
#include <string.h>

void setUp(void) {}
void tearDown(void) {}

/* Helper: build a valid test frame, return total bytes */
static size_t build_frame(uint8_t* dst, uint8_t addr, uint8_t type,
                           uint8_t const* payload, uint8_t payload_len) {
    uint8_t length = 1 + payload_len + 1; /* type + payload + crc */
    dst[0] = addr;
    dst[1] = length;
    dst[2] = type;
    memcpy(dst + 3, payload, payload_len);
    uint8_t crc = cel_crsf_crc(dst + 2, 1 + payload_len);
    dst[2 + length - 1] = crc;
    return 2 + length;
}

/* Stream tests */

void test_stream_create_destroy(void) {
    cel_crsf_stream* s = cel_crsf_stream_create();
    TEST_ASSERT_NOT_NULL(s);
    cel_crsf_stream_destroy(s);
}

void test_stream_feed_empty(void) {
    cel_crsf_stream* s = cel_crsf_stream_create();
    cel_crsf_frame frames[4];
    int n = cel_crsf_stream_feed(s, NULL, 0, frames, 4);
    TEST_ASSERT_EQUAL_INT(0, n);
    cel_crsf_stream_destroy(s);
}

void test_stream_feed_single_frame(void) {
    uint8_t buf[32];
    uint8_t payload[2] = {0x80, 0x01};
    size_t len = build_frame(buf, 0xC8, CEL_CRSF_TYPE_HEARTBEAT, payload, 2);

    cel_crsf_stream* s = cel_crsf_stream_create();
    cel_crsf_frame frames[4];
    int n = cel_crsf_stream_feed(s, buf, len, frames, 4);
    TEST_ASSERT_EQUAL_INT(1, n);
    TEST_ASSERT_EQUAL_UINT8(0xC8, frames[0].addr);
    TEST_ASSERT_EQUAL_UINT8(CEL_CRSF_TYPE_HEARTBEAT, frames[0].type);
    TEST_ASSERT_EQUAL_UINT8(2, frames[0].payload_len);
    TEST_ASSERT_EQUAL_UINT8(0x80, frames[0].payload[0]);
    cel_crsf_stream_destroy(s);
}

void test_stream_feed_incremental(void) {
    uint8_t buf[32];
    uint8_t payload[2] = {0x80, 0x01};
    size_t total = build_frame(buf, 0xC8, CEL_CRSF_TYPE_HEARTBEAT, payload, 2);

    cel_crsf_stream* s = cel_crsf_stream_create();
    cel_crsf_frame frames[4];

    /* Feed first 3 bytes — not enough for a complete frame */
    int n = cel_crsf_stream_feed(s, buf, 3, frames, 4);
    TEST_ASSERT_EQUAL_INT(0, n);

    /* Feed remaining bytes — now complete */
    n = cel_crsf_stream_feed(s, buf + 3, total - 3, frames, 4);
    TEST_ASSERT_EQUAL_INT(1, n);
    TEST_ASSERT_EQUAL_UINT8(CEL_CRSF_TYPE_HEARTBEAT, frames[0].type);
    cel_crsf_stream_destroy(s);
}

void test_stream_feed_multiple_frames(void) {
    uint8_t buf[64];
    uint8_t p1[2] = {0x80, 0x01};
    uint8_t p2[3] = {0xAA, 0xBB, 0xCC};

    size_t len1 = build_frame(buf, 0xC8, CEL_CRSF_TYPE_HEARTBEAT, p1, 2);
    size_t len2 = build_frame(buf + len1, 0xEE, CEL_CRSF_TYPE_GPS, p2, 3);

    cel_crsf_stream* s = cel_crsf_stream_create();
    cel_crsf_frame frames[4];
    int n = cel_crsf_stream_feed(s, buf, len1 + len2, frames, 4);
    TEST_ASSERT_EQUAL_INT(2, n);
    TEST_ASSERT_EQUAL_UINT8(0xC8, frames[0].addr);
    TEST_ASSERT_EQUAL_UINT8(CEL_CRSF_TYPE_HEARTBEAT, frames[0].type);
    TEST_ASSERT_EQUAL_UINT8(0xEE, frames[1].addr);
    TEST_ASSERT_EQUAL_UINT8(CEL_CRSF_TYPE_GPS, frames[1].type);
    cel_crsf_stream_destroy(s);
}

void test_stream_feed_skip_bad_sync(void) {
    uint8_t buf[48];
    /* Garbage bytes before valid frame */
    buf[0] = 0xFF;
    buf[1] = 0xFE;
    buf[2] = 0xFD;

    uint8_t payload[2] = {0x80, 0x01};
    size_t len = build_frame(buf + 3, 0xC8, CEL_CRSF_TYPE_HEARTBEAT, payload, 2);

    cel_crsf_stream* s = cel_crsf_stream_create();
    cel_crsf_frame frames[4];
    int n = cel_crsf_stream_feed(s, buf, 3 + len, frames, 4);
    TEST_ASSERT_EQUAL_INT(1, n);
    TEST_ASSERT_EQUAL_UINT8(0xC8, frames[0].addr);
    cel_crsf_stream_destroy(s);
}

void test_stream_feed_discard_bad_crc(void) {
    uint8_t buf[32];
    uint8_t payload[2] = {0x80, 0x01};
    size_t len = build_frame(buf, 0xC8, CEL_CRSF_TYPE_HEARTBEAT, payload, 2);
    buf[len - 1] ^= 0xFF; /* corrupt CRC */

    cel_crsf_stream* s = cel_crsf_stream_create();
    cel_crsf_frame frames[4];
    int n = cel_crsf_stream_feed(s, buf, len, frames, 4);
    TEST_ASSERT_EQUAL_INT(0, n); /* bad frame discarded */
    cel_crsf_stream_destroy(s);
}

void test_stream_feed_overflow(void) {
    uint8_t buf[32];
    uint8_t payload[2] = {0x80, 0x01};
    size_t len = build_frame(buf, 0xC8, CEL_CRSF_TYPE_HEARTBEAT, payload, 2);

    cel_crsf_stream* s = cel_crsf_stream_create();
    cel_crsf_frame frames[1];
    /* Feed two frames but only have room for one */
    size_t total = len * 2;
    memcpy(buf + len, buf, len);
    int n = cel_crsf_stream_feed(s, buf, total, frames, 1);
    TEST_ASSERT_EQUAL_INT(1, n);
    /* Second frame should still be in buffer */
    n = cel_crsf_stream_feed(s, NULL, 0, frames, 1);
    TEST_ASSERT_EQUAL_INT(1, n);
    cel_crsf_stream_destroy(s);
}

void test_stream_reset(void) {
    uint8_t buf[32];
    uint8_t payload[2] = {0x80, 0x01};
    size_t len = build_frame(buf, 0xC8, CEL_CRSF_TYPE_HEARTBEAT, payload, 2);

    cel_crsf_stream* s = cel_crsf_stream_create();
    /* Feed partial frame */
    cel_crsf_frame frames[4];
    cel_crsf_stream_feed(s, buf, 3, frames, 4);
    /* Reset should discard partial */
    cel_crsf_stream_reset(s);
    /* Feed complete frame — should parse normally */
    int n = cel_crsf_stream_feed(s, buf, len, frames, 4);
    TEST_ASSERT_EQUAL_INT(1, n);
    cel_crsf_stream_destroy(s);
}

int main(void) {
    UNITY_BEGIN();
    RUN_TEST(test_stream_create_destroy);
    RUN_TEST(test_stream_feed_empty);
    RUN_TEST(test_stream_feed_single_frame);
    RUN_TEST(test_stream_feed_incremental);
    RUN_TEST(test_stream_feed_multiple_frames);
    RUN_TEST(test_stream_feed_skip_bad_sync);
    RUN_TEST(test_stream_feed_discard_bad_crc);
    RUN_TEST(test_stream_feed_overflow);
    RUN_TEST(test_stream_reset);
    return UNITY_END();
}