#include "guguji_protocol.h"

#include <math.h>

uint32_t guguji_crc32(const uint8_t *data, size_t length)
{
    uint32_t crc = 0xFFFFFFFFu;
    for (size_t i = 0; i < length; ++i) {
        crc ^= data[i];
        for (int bit = 0; bit < 8; ++bit) {
            const uint32_t mask = -(crc & 1u);
            crc = (crc >> 1) ^ (0xEDB88320u & mask);
        }
    }
    return ~crc;
}

bool guguji_validate_command_packet(const guguji_command_packet_t *packet, size_t length)
{
    if (packet == NULL || length != sizeof(guguji_command_packet_t)) {
        return false;
    }
    if (packet->magic != GUGUJI_COMMAND_MAGIC ||
        packet->version != GUGUJI_PROTOCOL_VERSION ||
        packet->command_type != GUGUJI_COMMAND_TYPE_SETPOINTS ||
        packet->joint_count != GUGUJI_JOINT_COUNT) {
        return false;
    }

    const size_t crc_offset = sizeof(guguji_command_packet_t) - sizeof(uint32_t);
    const uint32_t expected_crc = guguji_crc32((const uint8_t *)packet, crc_offset);
    if (expected_crc != packet->crc32) {
        return false;
    }

    for (int i = 0; i < GUGUJI_JOINT_COUNT; ++i) {
        if (!isfinite(packet->positions_rad[i]) ||
            !isfinite(packet->velocities_rad_s[i]) ||
            !isfinite(packet->kp[i]) ||
            !isfinite(packet->kd[i]) ||
            !isfinite(packet->torque_ff_nm[i])) {
            return false;
        }
    }
    return true;
}

void guguji_finalize_telemetry_packet(guguji_telemetry_packet_t *packet)
{
    packet->magic = GUGUJI_TELEMETRY_MAGIC;
    packet->version = GUGUJI_PROTOCOL_VERSION;
    packet->joint_count = GUGUJI_JOINT_COUNT;
    const size_t crc_offset = sizeof(guguji_telemetry_packet_t) - sizeof(uint32_t);
    packet->crc32 = guguji_crc32((const uint8_t *)packet, crc_offset);
}
