#pragma once

#include "message.h"

#include <stdbool.h>

// Handle for the high level robot interface
struct RobotImp;
typedef struct RobotImp* Robot;

#define FRAGMENT_MESSAGE 1
#define FRAGMENT_RECTANGLE 2
#define FRAGMENT_LINE 3
#define FRAGMENT_MARKER 4

struct Fragment {
    int type;
    union {
        union Message message;
        struct VisionRect rect;
    };
};

int robot_poll(Robot robot, struct Fragment* fragment);

/*
 * Return a handle to the high level robot interface, or NULL on error.
 */
Robot robot_new();

/*
 * Queue initialization tasks before further commands can be sent to the robot.
 * The robot is not guaranteed to be ready until a ready message is popped from
 * the work function.
 *
 * robot: The robot to initialize
 * returns: 0 on success, non-zero on failure
 */
int robot_init(Robot robot);

/*
 * Request the robot to stop. This does not stop the robot immediately. The
 * robot has stopped once the work function returns that there is no more work
 * to do.
 *
 * robot: The robot to stop
 * returns: 0 on success, non-zero on failure
 */
int robot_stop(Robot robot);

/*
 * Set the gimble speed. A packet will be sent to the robot on
 * the next ready tick of the work function.
 *
 * robot: The robot to set the velocity of
 * p: pitch speed
 * y: yaw speed
 * returns: 0 on success, non-zero on failure
 */
int robot_aim(Robot, float p, float y);

/*
 * Set the velocity of the robot chassis. A packet will be sent to the robot on
 * the next ready tick of the work function.
 *
 * robot: The robot to set the velocity of
 * x: strafe velocity
 * y: forward and backward velocity
 * r: rotation velocity
 * returns: 0 on success, non-zero on failure
 */
int robot_drive(Robot, float x, float y, float r);

/*
 * TODO: This is too simplistic for what the lights are capable of.
 * Set the color of all LEDs on the robot. A packet will be sent to the robot
 * on the next ready tick of the work function.
 * 
 * robot: The robot to set LED colors of
 * r: red
 * g: green
 * b: blue
 * returns: 0 on success, non-zero on failure
 */
int robot_led(Robot, unsigned char r, unsigned char g, unsigned char b);

int robot_blast(Robot);

int robot_stream(Robot, bool);

/*
 * If the robot is in ready mode, this makes the robot send a heartbeat in
 * the next tick of the work function. The work function will not do anything
 * else until a heartbeat response is received.
 *
 * A heartbeat must be sent periodically or else the robot will become
 * unresponsive. This function is thread-safe and can be called in a
 * busy-waiting loop, signal handler, or background thread.
 *
 * robot: The robot to send a heartbeat to
 * returns: 0 on success, non-zero on failure
 */
int robot_heartbeat(Robot robot);

/*
 * This does all of the work in small pieces so it returns as quickly as
 * possible. Most of the other functions in this API simply set a flag
 * to signal this function to work. A non-zero value is returned until
 * no more work is left, which would only happen shortly after the stop
 * function is called and any remaining tasks have been worked through.
 *
 * robot: The robot to do periodic work on
 * returns: non-zero until stop function is called
 */
int robot_work(Robot robot);