#ifndef CONTROLLER_H
#define CONTROLLER_H

#include "event.h"
#include "gb_math.h"
#include "stepper.h"
#include "path.h"


#define VEL_DIV 1024

#ifdef __cplusplus
extern "C" {
#endif

enum {
    CONTROLLER_OK = 0,
    CONTROLLER_ERR = -1,
    CONTROLLER_NO_PATH = -2,
    CONTROLLER_STATISFIED = -3,
};

typedef struct {
    event_t event;
    event_timeout_t event_timeout;
    event_queue_t *queue;
    stepper_t *stepper_a;
    stepper_t *stepper_b;
    float arm_a;
    float arm_b;
    float angle1_setpoint;
    float angle2_setpoint;
    uint32_t time_step;
    gbVec2 setpoint;
    path_t *path;
    float error;
} controller_t;

int controller_init(controller_t *control);

void controller_update(event_t *event);

int controller_setpoint(controller_t *control, gbVec2 *point);

void controller_inverse_kinematics(controller_t *control, gbVec2 *point, float *angle1, float *angle2);

void controller_current_position(controller_t *control, gbVec2 *point);

int controller_calculate_velocity(controller_t *control);

int controller_sub_setpoint(controller_t *control, gbVec2 *setpoint, float distance);

int controller_home(controller_t *control);




#ifdef __cplusplus
}
#endif


#endif /* CONTROLLER_H */