#include "suit.h"
#include <stdint.h>
#include "unity.h"
#include "HCP_message.h"

void test_encdec_generic_Error() {
    uint8_t buffer_msg_data[MAX_MESSAGE_DATA_SIZE];

    Hcp_message_MotionUpdate msg = {
        .speed = {3, 6, 9},
        .stearing = 0.5,
        .name = "This is a test",
        .heading = 'B',
        .enable = {true, false, true, false},
        .enables = {
            .a = false, .b = true, .c = false
        },
        .dangerLvl = HCP_ENUM_INLINE_MOTION_UPDATE_DANGER_LVL_VALUE_MELTDOWN
    };

    size_t length = toBytes_Hcp_message_MotionUpdate(&msg, buffer_msg_data);

    TEST_ASSERT_EQUAL(length, 58);
    TEST_ASSERT_EQUAL(buffer_msg_data[0], 0x01);

    Hcp_message_MotionUpdate rx_msg;
    size_t parsed_length = 0;
    HCP_MSG_ID msg_id = 0xff;
    HCP_PARSER_RESULT result = fromBytes(&rx_msg, buffer_msg_data, length, &parsed_length, &msg_id);

    TEST_ASSERT_EQUAL(result, HCP_PARSER_RESULT_OK);
    TEST_ASSERT_EQUAL(parsed_length, 58);
    TEST_ASSERT_EQUAL(msg_id, HCP_MESSAGE_MOTION_UPDATE_ID);
        
    TEST_ASSERT_EQUAL(msg.stearing, rx_msg.stearing);
    TEST_ASSERT_EQUAL_STRING(msg.name, rx_msg.name);
    TEST_ASSERT_EQUAL(msg.heading, rx_msg.heading);
    TEST_ASSERT_EQUAL(msg.dangerLvl, rx_msg.dangerLvl);
    
    TEST_ASSERT_EQUAL(msg.speed[0], rx_msg.speed[0]);
    TEST_ASSERT_EQUAL(msg.speed[1], rx_msg.speed[1]);
    TEST_ASSERT_EQUAL(msg.speed[2], rx_msg.speed[2]);
    
    TEST_ASSERT_EQUAL(msg.enable[0], rx_msg.enable[0]);
    TEST_ASSERT_EQUAL(msg.enable[1], rx_msg.enable[1]);
    TEST_ASSERT_EQUAL(msg.enable[2], rx_msg.enable[2]);
    TEST_ASSERT_EQUAL(msg.enable[3], rx_msg.enable[3]);

    TEST_ASSERT_EQUAL(msg.enables.a, rx_msg.enables.a);
    TEST_ASSERT_EQUAL(msg.enables.b, rx_msg.enables.b);
    TEST_ASSERT_EQUAL(msg.enables.c, rx_msg.enables.c);
}

void test_encdec_generic_MotionUpdate() {
    uint8_t buffer_msg_data[MAX_MESSAGE_DATA_SIZE];

    Hcp_message_Error msg = {
        .enables = {
            .aa = true,
            .asd = false,
            .b = true,
            .c = false,
            .test = true
        },
        .recoveryStatus = HCP_ENUM_INLINE_ERROR_RECOVERY_STATUS_VALUE_NO
    };

    size_t length = toBytes_Hcp_message_Error(&msg, buffer_msg_data);

    TEST_ASSERT_EQUAL(length, 5);
    TEST_ASSERT_EQUAL(buffer_msg_data[0], 0x02);

    Hcp_message_Error rx_msg;
    size_t parsed_length = 0;
    HCP_MSG_ID msg_id = 0xff;
    HCP_PARSER_RESULT result = fromBytes(&rx_msg, buffer_msg_data, length, &parsed_length, &msg_id);

    TEST_ASSERT_EQUAL(result, HCP_PARSER_RESULT_OK);
    TEST_ASSERT_EQUAL(parsed_length, 5);
    TEST_ASSERT_EQUAL(msg_id, HCP_MESSAGE_ERROR_ID);

    TEST_ASSERT_EQUAL(rx_msg.enables.aa, msg.enables.aa);
    TEST_ASSERT_EQUAL(rx_msg.enables.asd, msg.enables.asd);
    TEST_ASSERT_EQUAL(rx_msg.enables.b, msg.enables.b);
    TEST_ASSERT_EQUAL(rx_msg.enables.c, msg.enables.c);
    TEST_ASSERT_EQUAL(rx_msg.enables.test, msg.enables.test);

    TEST_ASSERT_EQUAL(rx_msg.recoveryStatus, msg.recoveryStatus);
}

void test_encdec_generic_Error_invalid_id() {
    uint8_t buffer_msg_data[MAX_MESSAGE_DATA_SIZE];

    Hcp_message_Error msg = {
        .enables = {
            .aa = true,
            .asd = false,
            .b = true,
            .c = false,
            .test = true
        },
        .recoveryStatus = HCP_ENUM_INLINE_ERROR_RECOVERY_STATUS_VALUE_NO
    };

    size_t length = toBytes_Hcp_message_Error(&msg, buffer_msg_data);

    TEST_ASSERT_EQUAL(length, 5);
    TEST_ASSERT_EQUAL(buffer_msg_data[0], 0x02);

    // Mofiy data
    buffer_msg_data[0] = 0xf2;

    Hcp_message_Error rx_msg;
    size_t parsed_length = 0;
    HCP_MSG_ID msg_id = 0xff;
    HCP_PARSER_RESULT result = fromBytes(&rx_msg, buffer_msg_data, length, &parsed_length, &msg_id);

    TEST_ASSERT_EQUAL(parsed_length, 0);
    TEST_ASSERT_EQUAL(result, HCP_PARSER_RESULT_ERROR_INVALID_ID);
    TEST_ASSERT_EQUAL(msg_id, 0xff);
}

void test_encdec_generic_Error_invalid_size() {
    uint8_t buffer_msg_data[MAX_MESSAGE_DATA_SIZE];

    Hcp_message_Error msg = {
        .enables = {
            .aa = true,
            .asd = false,
            .b = true,
            .c = false,
            .test = true
        },
        .recoveryStatus = HCP_ENUM_INLINE_ERROR_RECOVERY_STATUS_VALUE_NO
    };

    size_t length = toBytes_Hcp_message_Error(&msg, buffer_msg_data);

    TEST_ASSERT_EQUAL(length, 5);
    TEST_ASSERT_EQUAL(buffer_msg_data[0], 0x02);

    // Mofiy data
    length = 2;

    Hcp_message_Error rx_msg;
    size_t parsed_length = 0;
    HCP_MSG_ID msg_id = 0xff;
    HCP_PARSER_RESULT result = fromBytes(&rx_msg, buffer_msg_data, length, &parsed_length, &msg_id);

    TEST_ASSERT_EQUAL(parsed_length, 0);
    TEST_ASSERT_EQUAL(result, HCP_PARSER_RESULT_ERROR_INVALID_SIZE);
    TEST_ASSERT_EQUAL(msg_id, 0xff);
}

void test_encdec_generic_Error_large_size() {
    uint8_t buffer_msg_data[MAX_MESSAGE_DATA_SIZE];

    Hcp_message_Error msg = {
        .enables = {
            .aa = true,
            .asd = false,
            .b = true,
            .c = false,
            .test = true
        },
        .recoveryStatus = HCP_ENUM_INLINE_ERROR_RECOVERY_STATUS_VALUE_NO
    };

    size_t length = toBytes_Hcp_message_Error(&msg, buffer_msg_data);

    TEST_ASSERT_EQUAL(length, 5);
    TEST_ASSERT_EQUAL(buffer_msg_data[0], 0x02);

    length += 5;

    Hcp_message_Error rx_msg;
    size_t parsed_length = 0;
    HCP_MSG_ID msg_id = 0xff;
    HCP_PARSER_RESULT result = fromBytes(&rx_msg, buffer_msg_data, length, &parsed_length, &msg_id);

    TEST_ASSERT_EQUAL(result, HCP_PARSER_RESULT_OK);
    TEST_ASSERT_EQUAL(parsed_length, 5);
    TEST_ASSERT_EQUAL(msg_id, HCP_MESSAGE_ERROR_ID);

    TEST_ASSERT_EQUAL(rx_msg.enables.aa, msg.enables.aa);
    TEST_ASSERT_EQUAL(rx_msg.enables.asd, msg.enables.asd);
    TEST_ASSERT_EQUAL(rx_msg.enables.b, msg.enables.b);
    TEST_ASSERT_EQUAL(rx_msg.enables.c, msg.enables.c);
    TEST_ASSERT_EQUAL(rx_msg.enables.test, msg.enables.test);

    TEST_ASSERT_EQUAL(rx_msg.recoveryStatus, msg.recoveryStatus);
}

void test_encdec_generic_Error_invalid_crc() {
    uint8_t buffer_msg_data[MAX_MESSAGE_DATA_SIZE];

    Hcp_message_Error msg = {
        .enables = {
            .aa = true,
            .asd = false,
            .b = true,
            .c = false,
            .test = true
        },
        .recoveryStatus = HCP_ENUM_INLINE_ERROR_RECOVERY_STATUS_VALUE_NO
    };

    size_t length = toBytes_Hcp_message_Error(&msg, buffer_msg_data);

    TEST_ASSERT_EQUAL(length, 5);
    TEST_ASSERT_EQUAL(buffer_msg_data[0], 0x02);

    // Mofiy data
    buffer_msg_data[3] = 0xff;

    Hcp_message_Error rx_msg;
    size_t parsed_length = 0;
    HCP_MSG_ID msg_id = 0xff;
    HCP_PARSER_RESULT result = fromBytes(&rx_msg, buffer_msg_data, length, &parsed_length, &msg_id);

    TEST_ASSERT_EQUAL(parsed_length, 0);
    TEST_ASSERT_EQUAL(result, HCP_PARSER_RESULT_ERROR_INVALID_CRC);
    TEST_ASSERT_EQUAL(msg_id, 0xff);
}