#pragma once

#include <stdbool.h>
#include <stddef.h>
#include <stdint.h>

#define GUGUJI_JOINT_COUNT 8
#define GUGUJI_PROTOCOL_VERSION 1

#define GUGUJI_COMMAND_MAGIC 0x43475547u
#define GUGUJI_TELEMETRY_MAGIC 0x54475547u

#define GUGUJI_COMMAND_TYPE_SETPOINTS 1

#define GUGUJI_COMMAND_FLAG_ARM (1u << 0)
#define GUGUJI_COMMAND_FLAG_DISARM (1u << 1)
#define GUGUJI_COMMAND_FLAG_HOLD (1u << 2)
#define GUGUJI_COMMAND_FLAG_ESTOP (1u << 3)
#define GUGUJI_COMMAND_FLAG_CLEAR_FAULTS (1u << 4)

#define GUGUJI_FAULT_MOTOR_MASK 0x000000FFu
#define GUGUJI_FAULT_FEEDBACK_TIMEOUT_MASK 0x0000FF00u
#define GUGUJI_FAULT_IMU_OFFLINE (1u << 16)
#define GUGUJI_FAULT_MAG_OFFLINE (1u << 17)

typedef enum {
    GUGUJI_STATE_DISARMED = 0,
    GUGUJI_STATE_ARMED = 1,
    GUGUJI_STATE_ESTOP = 2,
    GUGUJI_STATE_TIMEOUT = 3,
} guguji_robot_state_t;

typedef struct __attribute__((packed)) {
    uint32_t magic;
    uint8_t version;
    uint8_t command_type;
    uint16_t joint_count;
    uint32_t sequence;
    uint32_t monotonic_ms;
    uint32_t flags;
    float positions_rad[GUGUJI_JOINT_COUNT];
    float velocities_rad_s[GUGUJI_JOINT_COUNT];
    float kp[GUGUJI_JOINT_COUNT];
    float kd[GUGUJI_JOINT_COUNT];
    float torque_ff_nm[GUGUJI_JOINT_COUNT];
    uint32_t crc32;
} guguji_command_packet_t;

typedef struct __attribute__((packed)) {
    uint32_t magic;
    uint8_t version;
    uint8_t robot_state;
    uint16_t joint_count;
    uint32_t sequence;
    uint32_t uptime_ms;
    uint32_t last_command_age_ms;
    uint32_t fault_mask;
    float joint_position_rad[GUGUJI_JOINT_COUNT];
    float joint_velocity_rad_s[GUGUJI_JOINT_COUNT];
    float joint_torque_nm[GUGUJI_JOINT_COUNT];
    float motor_temperature_c[GUGUJI_JOINT_COUNT];
    float accel_m_s2[3];
    float gyro_rad_s[3];
    float mag_u_t[3];
    float rpy_rad[3];
    uint32_t crc32;
} guguji_telemetry_packet_t;

uint32_t guguji_crc32(const uint8_t *data, size_t length);
bool guguji_validate_command_packet(const guguji_command_packet_t *packet, size_t length);
void guguji_finalize_telemetry_packet(guguji_telemetry_packet_t *packet);
