#ifndef STEPPER_H
#define STEPPER_H

#include <stdint.h>
#include "periph/gpio.h"
#include "periph/uart.h"
#include "sensor.h"
#include "byteorder.h"

#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include "event.h"
#include "event/timeout.h"

#ifdef __cplusplus
extern "C" {
#endif

#define STEPPER_PARAM_VEL_MAX (25)
#define STEPPER_PARAM_ACC_MAX (200)
#define STEPPER_PARAM_VELOCITY_DEFAULT (0)
#define STEPPER_PARAM_MICROSTEP_DEFAULT (16)
#define STEPPER_PARAM_DIVISION_DEFAULT (200)
#define STEPPER_PARAM_ANGLE_DEFAULT (0)
#define STEPPER_PARAM_HOMING_ANGLE_DEFAULT (20)

#define STEPPER_UART_SLAVE_ADDR (0x0)
#define STEPPER_UART_SYNC       (0x05)
#define STEPPER_UART_REG_GCONF  (0x00)
#define STEPPER_UART_REG_GSTAT  (0x01)
#define STEPPER_UART_REG_IFCNT  (0x02)
#define STEPPER_UART_REG_SLAVECONF (0x03)

#define STEPPER_GCONF_BIT_I_SCALE_ANALOG (0x0 << 0)
#define STEPPER_GCONF_BIT_INTERNAL_RSENSE (0x1 << 1) // 
#define STEPPER_GCONF_BIT_SPREADCYCLE (0x1 << 2) // enable spreadcycle
#define STEPPER_GCONF_BIT_SHAFT (0x0 << 3) // Non inverse
#define STEPPER_GCONF_BIT_PDN_DISABLE (0x1 << 6) // Set to 1 when using UART
#define STEPPER_GCONF_BIT_MSTEP_SELECT (0x1 << 7)

    // default datagram => 05 ff 00 00 00 01 01 bb
#define STEPPER_GCONF_ADDR (0x00) // 0x00
#define STEPPER_GCONF_VALUES (0xC1000000UL) //(0x83800000UL) // 000001C1 set bit 0,6,7,8. 6: PDN disable is require for UART operation.

    // default datagram => 05 ff 6c 17 01 00 53 3e
#define STEPPER_CHOPCONF_ADDR (0x6c) // 6C
#define STEPPER_CHOPCONF_VALUES (0xB5810134UL)//(0xCA00004CUL) // 32000053 set dedge=0x1, intpol=0x1, mres=0x2, hstrt=0x5, toff=0x3. others are 0x0.
//#define STEPPER_CHOPCONF_VALUES [0x32,0x00,0x00,0x53] // set dedge=0x1, intpol=0x1, mres=0x2, hstrt=0x5, toff=0x3. others are 0x0.



/**
 * @brief   Status and error codes
 */
    enum {
        STEPPER_OK  =  0,
        STEPPER_ERR = -1,
        STEPPER_DISABLED = -2,
    };

    /**
     * @brief   Direction for stepper
     */
    typedef enum {
        STEPPER_FORWARD = 0x0,
        STEPPER_REVERSE = 0x1,
    } stepper_direction_t;

/**
 * @brief   Data structure holding the stepperice parameters needed for initialization
 */
typedef struct {
    gpio_t step_pin;
    gpio_t dir_pin;
    gpio_t enable_pin;
    uint8_t uart;
    uint8_t vel_max;
    uint8_t acc_max;
    uint8_t microstep;
    uint16_t division;
    float angle;
} stepper_params_t;

/**
 * @brief   Device Descriptor for Stepper controller
 */
typedef struct {
    stepper_params_t p;
    float velocity_setpoint;
    stepper_direction_t direction;
    int16_t step_count;
    int16_t step_setpoint;
    uint32_t time_step;
    float step_angle;
    bool enable;
    event_t event;
    event_timeout_t event_timeout;
    event_queue_t *queue;
    thread_t *thread;
    thread_flags_t flags;
    char id;
} stepper_t;

typedef struct {
    event_queue_t* queue;
    sensor_t* sensor;
} stepper_init_t;

/**
 * @brief Initialize the stepper
 *
 * @param[out] stepper  struct describing the stepper
 * @param[in]  params   static configuration parameters of stepper
 *
 * @return STEPPER_OK on success
 * @return STEPPER_ERR on error
 */
int stepper_init(stepper_t *stepper, const stepper_params_t *params);

/**
 * @brief Instruct the stepper to move to setpoint
 *
 * @param[in] stepper   struct describing the stepper
 * @param[in] count     stepcount setpoint
 * @param[in] time_step time span in which to complete the move
 */
void stepper_set_setpoint(stepper_t *stepper, int16_t count, uint32_t time_step);

//void stepper_set_velocity(stepper_t *stepper, float velocity);

/**
 * @brief Change rotation direction of stepper
 *
 * @param[in] stepper   struct describing the stepper
 * @param[in] direction move direction of stepper
 */
void stepper_set_direction(stepper_t *stepper, stepper_direction_t direction);

//int stepper_time_step(stepper_t *stepper);

int32_t stepper_get_count(stepper_t *stepper, float angle);

/**
 * @brief Update next action.
 * Schedules a new event or when the set point is reached, signal the controller.
 *
 * @param[in] stepper   struct describing the stepper
 */
void stepper_update(stepper_t *stepper);

/**
 * @brief Return the current angle in radiansa
 *
 * @param[in] stepper   struct describing the stepper
 * @param[out] angle    angle in radians
 */
void stepper_get_angle(stepper_t *stepper, float *angle);

/** 
 * @brief Move stepper by a single step.
 * 
 * @param[in] stepper   struct describing the stepper
 */
int stepper_step(stepper_t *stepper);

/**
 * @brief write to stepper register
 *
 * @param[in] stepper   struct describing the stepper
 * @param[in] reg       register adress
 * @param[in] data      data to write in register
 */
void stepper_write(stepper_t *stepper, uint8_t reg, le_uint32_t *data);

/**
 * @brief trigger read from stepper register
 *
 * @param[in] stepper   struct describing the stepper
 * @param[in] reg       register adress
 *
 * @note This function does not parse the answer from the stepper. Use a logic analyser for that.
 */
void stepper_read(stepper_t *stepper, uint8_t reg);

/**
 * @brief enable stepper motor
 *
 * @param[in] stepper   struct describing the stepper
 */
void stepper_enable(stepper_t *stepper);

/**
 * @brief disable stepper motor
 *
 * @param[in] stepper   struct describing the stepper
 */
void stepper_disable(stepper_t *stepper);

#ifdef __cplusplus
}
#endif

#endif /* STEPPER_H */