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

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

/* Helper: build a valid test frame with known CRC */
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;
    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;
}

/* Telemetry parse tests */

void test_parse_link_stats(void) {
    uint8_t buf[32];
    uint8_t payload[10] = {
        80, 70, 95, 0x7F, /* rssi1, rssi2, uplink_qual, snr(-127..127) */
        0, 5, 20, 60, 90, 0x60 /* antenna, rf_mode, power, down_rssi, down_qual, snr */
    };
    build_frame(buf, 0x08, CEL_CRSF_TYPE_LINK_STATS, payload, 10);

    cel_crsf_frame frame;
    TEST_ASSERT_EQUAL_INT(0, cel_crsf_frame_parse(&frame, buf, sizeof(buf)));

    cel_telemetry telem;
    TEST_ASSERT_EQUAL_INT(0, cel_crsf_telemetry_parse(&frame, &telem));
    TEST_ASSERT_EQUAL_UINT(CEL_TELEM_LINK, telem.type);
    TEST_ASSERT_EQUAL_UINT8(80, telem.data.link.uplink_rssi1);
    TEST_ASSERT_EQUAL_UINT8(70, telem.data.link.uplink_rssi2);
    TEST_ASSERT_EQUAL_UINT8(95, telem.data.link.uplink_quality);
    TEST_ASSERT_EQUAL_INT8(0x7F, telem.data.link.uplink_snr);
    TEST_ASSERT_EQUAL_UINT8(0, telem.data.link.active_antenna);
    TEST_ASSERT_EQUAL_UINT8(5, telem.data.link.rf_mode);
    TEST_ASSERT_EQUAL_UINT8(20, telem.data.link.uplink_power);
    TEST_ASSERT_EQUAL_UINT8(60, telem.data.link.downlink_rssi);
    TEST_ASSERT_EQUAL_UINT8(90, telem.data.link.downlink_qual);
    TEST_ASSERT_EQUAL_INT8(0x60, telem.data.link.downlink_snr);
}

void test_parse_link_stats_short(void) {
    uint8_t buf[32];
    uint8_t payload[5] = {80, 70, 95, 0x7F, 0};
    build_frame(buf, 0x08, CEL_CRSF_TYPE_LINK_STATS, payload, 5);

    cel_crsf_frame frame;
    TEST_ASSERT_EQUAL_INT(0, cel_crsf_frame_parse(&frame, buf, sizeof(buf)));

    cel_telemetry telem;
    TEST_ASSERT_EQUAL_INT(-1, cel_crsf_telemetry_parse(&frame, &telem));
}

void test_parse_battery(void) {
    uint8_t buf[32];
    /* CRSF battery: voltage(u16 BE 0.1V), current(u16 BE 0.1A),
       capacity(u24 BE mAh), remaining(u8 %) = 8 bytes */
    uint8_t payload[8] = {
        0x03, 0xE8, /* voltage: 0x03E8 = 1000 -> 100.0V (10S LiPo) */
        0x00, 0x64, /* current: 0x0064 = 100 -> 10.0A */
        0x00, 0x03, 0xE8, /* capacity: 0x0003E8 = 1000mAh */
        0x64         /* remaining: 100% */
    };
    build_frame(buf, 0x08, CEL_CRSF_TYPE_BATTERY, payload, 8);

    cel_crsf_frame frame;
    TEST_ASSERT_EQUAL_INT(0, cel_crsf_frame_parse(&frame, buf, sizeof(buf)));

    cel_telemetry telem;
    TEST_ASSERT_EQUAL_INT(0, cel_crsf_telemetry_parse(&frame, &telem));
    TEST_ASSERT_EQUAL_UINT(CEL_TELEM_BATTERY, telem.type);
    TEST_ASSERT_EQUAL_UINT16(0x03E8, telem.data.battery.voltage_x10);
    TEST_ASSERT_EQUAL_UINT16(0x0064, telem.data.battery.current_x10);
    TEST_ASSERT_EQUAL_UINT32(0x0003E8, telem.data.battery.capacity_mah);
    TEST_ASSERT_EQUAL_UINT8(0x64, telem.data.battery.remaining_pct);
}

void test_parse_heartbeat(void) {
    uint8_t buf[32];
    uint8_t payload[2] = {0x10, 0x80}; /* origin_addr LE = 0x8010 */
    build_frame(buf, 0x10, CEL_CRSF_TYPE_HEARTBEAT, payload, 2);

    cel_crsf_frame frame;
    TEST_ASSERT_EQUAL_INT(0, cel_crsf_frame_parse(&frame, buf, sizeof(buf)));

    cel_telemetry telem;
    TEST_ASSERT_EQUAL_INT(0, cel_crsf_telemetry_parse(&frame, &telem));
    TEST_ASSERT_EQUAL_UINT(CEL_TELEM_HEARTBEAT, telem.type);
    TEST_ASSERT_EQUAL_UINT16(0x8010, telem.data.heartbeat.origin_addr);
}

void test_parse_airspeed(void) {
    uint8_t buf[32];
    uint8_t payload[2] = {0x00, 0x01}; /* speed LE = 0x0100 = 256 km/h */
    build_frame(buf, 0x08, CEL_CRSF_TYPE_AIRSPEED, payload, 2);

    cel_crsf_frame frame;
    TEST_ASSERT_EQUAL_INT(0, cel_crsf_frame_parse(&frame, buf, sizeof(buf)));

    cel_telemetry telem;
    TEST_ASSERT_EQUAL_INT(0, cel_crsf_telemetry_parse(&frame, &telem));
    TEST_ASSERT_EQUAL_UINT(CEL_TELEM_AIRSPEED, telem.type);
    TEST_ASSERT_EQUAL_UINT16(256, telem.data.airspeed.speed_kmh);
}

void test_parse_unknown_type(void) {
    uint8_t buf[32];
    uint8_t payload[2] = {0x00, 0x01};
    build_frame(buf, 0x08, 0xFF, payload, 2);

    cel_crsf_frame frame;
    TEST_ASSERT_EQUAL_INT(0, cel_crsf_frame_parse(&frame, buf, sizeof(buf)));

    cel_telemetry telem;
    TEST_ASSERT_EQUAL_INT(-1, cel_crsf_telemetry_parse(&frame, &telem));
}

void test_parse_null_args(void) {
    cel_telemetry telem;
    TEST_ASSERT_EQUAL_INT(-1, cel_crsf_telemetry_parse(NULL, &telem));
}

void test_parse_null_out(void) {
    uint8_t buf[32];
    uint8_t payload[2] = {0x10, 0x80};
    build_frame(buf, 0x10, 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_INT(-1, cel_crsf_telemetry_parse(&frame, NULL));
}

int main(void) {
    UNITY_BEGIN();
    RUN_TEST(test_parse_link_stats);
    RUN_TEST(test_parse_link_stats_short);
    RUN_TEST(test_parse_battery);
    RUN_TEST(test_parse_heartbeat);
    RUN_TEST(test_parse_airspeed);
    RUN_TEST(test_parse_unknown_type);
    RUN_TEST(test_parse_null_args);
    RUN_TEST(test_parse_null_out);
    return UNITY_END();
}