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

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

/* CRC tests — CRC8/DVB-S2 (poly 0xD5) */
void test_crc_empty(void) {
    uint8_t data[1] = {0};
    TEST_ASSERT_EQUAL_UINT8(0x00, cel_crsf_crc(data, 0));
}

void test_crc_single_byte(void) {
    uint8_t data[1] = {0x01};
    uint8_t crc = cel_crsf_crc(data, 1);
    TEST_ASSERT_TRUE(crc != 0); /* non-trivial */
}

void test_crc_known_value(void) {
    uint8_t data[6] = {0x10, 0x80, 0x03, 0x02, 0x80, 0x01};
    uint8_t crc = cel_crsf_crc(data, 6);
    TEST_ASSERT_TRUE(crc != 0);
    uint8_t crc2 = cel_crsf_crc(data, 6);
    TEST_ASSERT_EQUAL_UINT8(crc, crc2);
}

/* Channel helper tests */
void test_channel_clamp_min(void) {
    TEST_ASSERT_EQUAL_INT16(CEL_CRSF_CH_MIN, cel_crsf_channel_clamp(0));
}

void test_channel_clamp_max(void) {
    TEST_ASSERT_EQUAL_INT16(CEL_CRSF_CH_MAX, cel_crsf_channel_clamp(2048));
}

void test_channel_clamp_mid(void) {
    TEST_ASSERT_EQUAL_INT16(CEL_CRSF_CH_MID, cel_crsf_channel_clamp(CEL_CRSF_CH_MID));
}

void test_channel_us_to_val_min(void) {
    TEST_ASSERT_EQUAL_INT16(CEL_CRSF_CH_MIN, cel_crsf_channel_us_to_val(988));
}

void test_channel_us_to_val_mid(void) {
    TEST_ASSERT_EQUAL_INT16(CEL_CRSF_CH_MID, cel_crsf_channel_us_to_val(1500));
}

void test_channel_us_to_val_max(void) {
    TEST_ASSERT_EQUAL_INT16(CEL_CRSF_CH_MAX, cel_crsf_channel_us_to_val(2012));
}

void test_channel_us_to_val_below_min(void) {
    TEST_ASSERT_EQUAL_INT16(CEL_CRSF_CH_MIN, cel_crsf_channel_us_to_val(0));
}

void test_channel_us_to_val_above_max(void) {
    TEST_ASSERT_EQUAL_INT16(CEL_CRSF_CH_MAX, cel_crsf_channel_us_to_val(65535));
}

void test_channel_val_to_us_min(void) {
    TEST_ASSERT_EQUAL_UINT16(988, cel_crsf_channel_val_to_us(CEL_CRSF_CH_MIN));
}

void test_channel_val_to_us_mid(void) {
    TEST_ASSERT_EQUAL_UINT16(1500, cel_crsf_channel_val_to_us(CEL_CRSF_CH_MID));
}

void test_channel_val_to_us_max(void) {
    TEST_ASSERT_EQUAL_UINT16(2012, cel_crsf_channel_val_to_us(CEL_CRSF_CH_MAX));
}

void test_channel_default_throttle_min(void) {
    int16_t ch[16];
    cel_crsf_channel_default(ch);
    TEST_ASSERT_EQUAL_INT16(CEL_CRSF_CH_MIN, ch[2]); /* throttle */
}

void test_channel_default_centered(void) {
    int16_t ch[16];
    cel_crsf_channel_default(ch);
    TEST_ASSERT_EQUAL_INT16(CEL_CRSF_CH_MID, ch[0]); /* roll */
    TEST_ASSERT_EQUAL_INT16(CEL_CRSF_CH_MID, ch[1]); /* pitch */
    TEST_ASSERT_EQUAL_INT16(CEL_CRSF_CH_MID, ch[3]); /* yaw */
}

void test_channel_default_aux_min(void) {
    int16_t ch[16];
    cel_crsf_channel_default(ch);
    for (int i = 4; i < 16; i++) {
        TEST_ASSERT_EQUAL_INT16(CEL_CRSF_CH_MIN, ch[i]);
    }
}

/* Frame parse tests — ELRS format: [addr][length][type][payload][crc] */

/* Build a valid test frame with known CRC */
static void build_test_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;
}

void test_parse_valid_frame(void) {
    uint8_t buf[32];
    uint8_t payload[2] = {0x80, 0x01};
    build_test_frame(buf, 0xC8, CEL_CRSF_TYPE_HEARTBEAT, payload, 2);

    cel_crsf_frame frame;
    TEST_ASSERT_EQUAL_INT(0, cel_crsf_frame_parse(&frame, buf, sizeof(buf)));
    TEST_ASSERT_EQUAL_UINT8(0xC8, frame.addr);
    TEST_ASSERT_EQUAL_UINT8(CEL_CRSF_TYPE_HEARTBEAT, frame.type);
    TEST_ASSERT_EQUAL_UINT8(2, frame.payload_len);
    TEST_ASSERT_EQUAL_UINT8(0x80, frame.payload[0]);
    TEST_ASSERT_EQUAL_UINT8(0x01, frame.payload[1]);
}

void test_parse_null_frame(void) {
    uint8_t buf[8];
    TEST_ASSERT_EQUAL_INT(-1, cel_crsf_frame_parse(NULL, buf, 8));
}

void test_parse_null_buf(void) {
    cel_crsf_frame frame;
    TEST_ASSERT_EQUAL_INT(-1, cel_crsf_frame_parse(&frame, NULL, 8));
}

void test_parse_too_short(void) {
    cel_crsf_frame frame;
    uint8_t buf[2] = {0xC8, 0x03};
    TEST_ASSERT_EQUAL_INT(-1, cel_crsf_frame_parse(&frame, buf, 2));
}

void test_parse_bad_crc(void) {
    uint8_t buf[32];
    uint8_t payload[2] = {0x80, 0x01};
    build_test_frame(buf, 0xC8, CEL_CRSF_TYPE_HEARTBEAT, payload, 2);
    buf[5] ^= 0xFF; /* corrupt the CRC */

    cel_crsf_frame frame;
    TEST_ASSERT_EQUAL_INT(-1, cel_crsf_frame_parse(&frame, buf, sizeof(buf)));
}

void test_parse_empty_payload(void) {
    uint8_t buf[32];
    build_test_frame(buf, 0xEE, CEL_CRSF_TYPE_HEARTBEAT, NULL, 0);

    cel_crsf_frame frame;
    TEST_ASSERT_EQUAL_INT(0, cel_crsf_frame_parse(&frame, buf, sizeof(buf)));
    TEST_ASSERT_EQUAL_UINT8(0xEE, frame.addr);
    TEST_ASSERT_EQUAL_UINT8(CEL_CRSF_TYPE_HEARTBEAT, frame.type);
    TEST_ASSERT_EQUAL_UINT8(0, frame.payload_len);
}

void test_parse_module_addr(void) {
    uint8_t buf[32];
    uint8_t payload[4] = {0xAA, 0xBB, 0xCC, 0xDD};
    build_test_frame(buf, 0xEE, CEL_CRSF_TYPE_GPS, payload, 4);

    cel_crsf_frame frame;
    TEST_ASSERT_EQUAL_INT(0, cel_crsf_frame_parse(&frame, buf, sizeof(buf)));
    TEST_ASSERT_EQUAL_UINT8(0xEE, frame.addr);
    TEST_ASSERT_EQUAL_UINT8(CEL_CRSF_TYPE_GPS, frame.type);
    TEST_ASSERT_EQUAL_UINT8(4, frame.payload_len);
}

/* Frame builder tests */

void test_build_rc_frame_null_dst(void) {
    int16_t ch[16];
    TEST_ASSERT_EQUAL_UINT(0, cel_crsf_build_rc_frame(NULL, ch));
}

void test_build_rc_frame_null_channels(void) {
    uint8_t dst[32];
    size_t len = cel_crsf_build_rc_frame(dst, NULL);
    TEST_ASSERT_GREATER_THAN(0, len);
    TEST_ASSERT_EQUAL_UINT8(0xC8, dst[0]);
}

void test_build_rc_frame_roundtrip(void) {
    int16_t ch[16];
    cel_crsf_channel_default(ch);
    ch[0] = CEL_CRSF_CH_MAX;

    uint8_t dst[32];
    size_t len = cel_crsf_build_rc_frame(dst, ch);

    cel_crsf_frame frame;
    TEST_ASSERT_EQUAL_INT(0, cel_crsf_frame_parse(&frame, dst, len));
    TEST_ASSERT_EQUAL_UINT8(CEL_CRSF_TYPE_RC_CHANNELS_PACKED, frame.type);
}

void test_build_ping_frame_null_dst(void) {
    TEST_ASSERT_EQUAL_UINT(0, cel_crsf_build_ping_frame(NULL));
}

void test_build_ping_frame_valid(void) {
    uint8_t dst[16];
    size_t len = cel_crsf_build_ping_frame(dst);
    TEST_ASSERT_GREATER_THAN(0, len);
    TEST_ASSERT_EQUAL_UINT8(0xEE, dst[0]);

    cel_crsf_frame frame;
    TEST_ASSERT_EQUAL_INT(0, cel_crsf_frame_parse(&frame, dst, len));
    TEST_ASSERT_EQUAL_UINT8(CEL_CRSF_TYPE_DEVICE_PING, frame.type);
}

void test_build_param_read_frame_null_dst(void) {
    TEST_ASSERT_EQUAL_UINT(0, cel_crsf_build_param_read_frame(NULL, 0, 0));
}

void test_build_param_read_frame_valid(void) {
    uint8_t dst[16];
    size_t len = cel_crsf_build_param_read_frame(dst, 0x42, 0);
    TEST_ASSERT_GREATER_THAN(0, len);

    cel_crsf_frame frame;
    TEST_ASSERT_EQUAL_INT(0, cel_crsf_frame_parse(&frame, dst, len));
    TEST_ASSERT_EQUAL_UINT8(CEL_CRSF_TYPE_PARAM_READ, frame.type);
}

void test_build_param_write_frame_null_dst(void) {
    TEST_ASSERT_EQUAL_UINT(0, cel_crsf_build_param_write_frame(NULL, 0, 0));
}

void test_build_param_write_frame_valid(void) {
    uint8_t dst[16];
    size_t len = cel_crsf_build_param_write_frame(dst, 0x42, 0xFF);
    TEST_ASSERT_GREATER_THAN(0, len);

    cel_crsf_frame frame;
    TEST_ASSERT_EQUAL_INT(0, cel_crsf_frame_parse(&frame, dst, len));
    TEST_ASSERT_EQUAL_UINT8(CEL_CRSF_TYPE_PARAM_WRITE, frame.type);
}

int main(void) {
    UNITY_BEGIN();
    RUN_TEST(test_crc_empty);
    RUN_TEST(test_crc_single_byte);
    RUN_TEST(test_crc_known_value);
    RUN_TEST(test_channel_clamp_min);
    RUN_TEST(test_channel_clamp_max);
    RUN_TEST(test_channel_clamp_mid);
    RUN_TEST(test_channel_us_to_val_min);
    RUN_TEST(test_channel_us_to_val_mid);
    RUN_TEST(test_channel_us_to_val_max);
    RUN_TEST(test_channel_us_to_val_below_min);
    RUN_TEST(test_channel_us_to_val_above_max);
    RUN_TEST(test_channel_val_to_us_min);
    RUN_TEST(test_channel_val_to_us_mid);
    RUN_TEST(test_channel_val_to_us_max);
    RUN_TEST(test_channel_default_throttle_min);
    RUN_TEST(test_channel_default_centered);
    RUN_TEST(test_channel_default_aux_min);
    RUN_TEST(test_parse_valid_frame);
    RUN_TEST(test_parse_null_frame);
    RUN_TEST(test_parse_null_buf);
    RUN_TEST(test_parse_too_short);
    RUN_TEST(test_parse_bad_crc);
    RUN_TEST(test_parse_empty_payload);
    RUN_TEST(test_parse_module_addr);
    RUN_TEST(test_build_rc_frame_null_dst);
    RUN_TEST(test_build_rc_frame_null_channels);
    RUN_TEST(test_build_rc_frame_roundtrip);
    RUN_TEST(test_build_ping_frame_null_dst);
    RUN_TEST(test_build_ping_frame_valid);
    RUN_TEST(test_build_param_read_frame_null_dst);
    RUN_TEST(test_build_param_read_frame_valid);
    RUN_TEST(test_build_param_write_frame_null_dst);
    RUN_TEST(test_build_param_write_frame_valid);
    return UNITY_END();
}