#ifndef CONTROLLER_H
#define CONTROLLER_H

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


#ifndef SERVO_RAISED
#define SERVO_RAISED (500U)
#endif
#ifndef SERVO_LOWERED
#define SERVO_LOWERED (750U)
#endif

#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;
    servo_t *servo;
    bool lifted;
    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);

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

/**
 * @brief lower the marker position
 *
 * @param[in] control    struct describing the controller
 */
void controller_lower_marker(controller_t *control);

/**
 * @brief raise the marker position
 *
 * @param[in] control    struct describing the controller
 */
void controller_raise_marker(controller_t *control);


#ifdef __cplusplus
}
#endif


#endif /* CONTROLLER_H */