#ifndef CONTROLLER_H
#define CONTROLLER_H

#include "event.h"
#include "gb_math.h"
#include "stepper.h"
#include "path.h"
#include "angle.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 {
    stepper_t *stepper_a;
    stepper_t *stepper_b;
    uint32_t time_step;
    path_t *path;
    thread_flags_t flags;
} controller_t;

/**
 * @brief Initialize controller
 *
 * @param[out] control    struct describing the controller
 */
int controller_init(controller_t *control);

/**
 * @brief execute main loop of the controller
 *
 * @param[in] control    struct describing the controller
 */
int controller_loop(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);


/**
 * @brief home both stepper motors
 *
 * @param[in] control    struct describing the controller
 */
int controller_home(controller_t *control);




#ifdef __cplusplus
}
#endif


#endif /* CONTROLLER_H */