#include "periph/gpio.h"
#include "periph/uart.h"
#include "include/stepper.h"
#include "include/stepper_params.h"
#include "include/sensor.h"
#include "include/gb_math.h"

#define ENABLE_DEBUG (0)
#include "debug.h"

#define STEP_PIN (stepper->p.step_pin)
#define DIR_PIN (stepper->p.dir_pin)
#define ENABLE_PIN (stepper->p.enable_pin)
#define UART (stepper->p.uart)


static void stepper_event(event_t *_event)
{
    stepper_t *stepper = container_of(_event, stepper_t, event);
    stepper_step(stepper);
    stepper_update(stepper);
}

int stepper_init(stepper_t *stepper, const stepper_params_t *params)
{
    stepper->p = *params;
    event_t event = { .handler = stepper_event };
    stepper->event = event;
    stepper->velocity_setpoint = 0.0;
    stepper->step_count = 0;
    stepper->time_step = 0;
    stepper->step_angle = GB_MATH_TAU / (float)(stepper->p.division * stepper->p.microstep);

    event_timeout_init(&(stepper->event_timeout), stepper->queue, (event_t*)&(stepper->event));

    gpio_init(STEP_PIN, GPIO_OUT);
    gpio_init(DIR_PIN, GPIO_OUT);
    gpio_init(ENABLE_PIN, GPIO_OD);

    stepper_enable(stepper);

    if(uart_init(UART, 38400, NULL, NULL))
        return STEPPER_ERR;

    le_uint32_t data;
    data.u32 = STEPPER_GCONF_VALUES;

    stepper_write(stepper, STEPPER_GCONF_ADDR, &data);
    data.u32 = STEPPER_CHOPCONF_VALUES;
    stepper_write(stepper, STEPPER_CHOPCONF_ADDR, &data);

    //event_post(stepper->queue, &(stepper->event));
    printf("initialized descriptor 0x%08x\n", (unsigned)stepper);
    printf("initialized event 0x%08x\n", (unsigned)&(stepper->event));
    printf("initialized queue 0x%08x\n", (unsigned)stepper->queue);

    return STEPPER_OK;
}

void stepper_set_setpoint(stepper_t *stepper, int16_t count, uint32_t time_step)
{
    //Write the count to the setpoint field.
    stepper->step_setpoint = count;
    //Set direction
    if (stepper->step_setpoint < stepper->step_count)
    {
        stepper_set_direction(stepper, STEPPER_REVERSE);
        //Write the time_step to the time_step field.
        stepper->time_step = time_step/(stepper->step_count - stepper->step_setpoint);
    }
    else 
    {
        stepper_set_direction(stepper, STEPPER_FORWARD);
        //Write the time_step to the time_step field.
        stepper->time_step = time_step/(stepper->step_setpoint - stepper->step_count);
    }
    if (stepper->time_step <= 400){
        stepper->time_step = 400;
    }
    //Call update to set a timeout.
    stepper_update(stepper);
}

void stepper_get_angle(stepper_t *stepper, float *angle)
{
    *angle = stepper->step_count * stepper->step_angle;
    return;
}

int32_t stepper_get_count(stepper_t *stepper, float angle)
{
    return (int32_t) gb_round(angle/stepper->step_angle);
}

void stepper_update(stepper_t *stepper)
{
    // Set thread flags and return if the stepper is at the desired position.
    if(stepper->step_count == stepper->step_setpoint)
    {
        thread_flags_set(stepper->thread, stepper->flags);
        return;
    }
    // Set a new timeout event.
    event_timeout_set(&stepper->event_timeout, stepper->time_step);
}

/*
int stepper_time_step(stepper_t *stepper){
    if (stepper->velocity_setpoint == 0.0) {
        return STEPPER_ERR;
    }
    float step_time = stepper->step_angle / stepper->velocity_setpoint;
    stepper->time_step = (uint32_t)(step_time*1000000.0); 
    return STEPPER_OK;
}
*/

int stepper_step(stepper_t *stepper)
{
    if (!stepper->enable) {
        return STEPPER_DISABLED;
    }
    if (stepper->direction == STEPPER_FORWARD)
    {
        stepper->step_count++;
    }
    else
    {
        stepper->step_count--;
    }
    gpio_toggle(STEP_PIN);
    //printf("Stepper %c @ %i\n", stepper->id, stepper->step_count);
    return STEPPER_OK;
}

uint8_t stepper_crc8(const uint8_t *data, size_t len, uint8_t g_polynom, uint8_t crc){
    for (size_t i=0; i < len; i++)
    {
        uint8_t current_byte = data[i];
        for (size_t i=0; i < 8; i++)
        {
            bool xor = (crc >> 7) ^ (current_byte & 0x01);
            crc = crc <<1;
            crc = xor ? crc ^ g_polynom : crc;
            current_byte = current_byte >> 1;
        }
    }
    return crc;
}

/*
void stepper_set_velocity(stepper_t *stepper, float velocity)
{
    if (velocity > 0)
    {
        stepper->velocity_setpoint = gb_min(velocity,20);//20
        stepper_set_direction(stepper, STEPPER_FORWARD);
    }
    else
    {
        stepper->velocity_setpoint = gb_min(-velocity,20);//20
        stepper_set_direction(stepper, STEPPER_REVERSE);
    }
    stepper_update(stepper);
}
*/

void stepper_set_direction(stepper_t *stepper, stepper_direction_t direction){
    stepper->direction = direction;
    if (direction == STEPPER_REVERSE)
    {
        gpio_set(DIR_PIN);
    }
    else
    {
        gpio_clear(DIR_PIN);
    }
}

void stepper_write(stepper_t *stepper, uint8_t reg, le_uint32_t *data)
{
    uint8_t* message;
    message = (uint8_t*) malloc(8 * sizeof(uint8_t));
    message[0] = STEPPER_UART_SYNC;
    message[1] = STEPPER_UART_SLAVE_ADDR;
    message[2] = reg | 0x80;
    memcpy(&message[3], data->u8, 4);
    message[7] = 0x0;

    uint8_t crc = stepper_crc8(message, 7, 0x07, 0x0);
    message[7] = crc;

    uart_write(UART, message, 8);
/*    printf("\n");
    for (int i = 0; i<8; i++)
    {
           printf("%0.2x",message[i]);
    }
    printf("\n");
    */
    return;
}

void stepper_read(stepper_t *stepper, uint8_t reg)
{
    uint8_t* message;
    message = (uint8_t*) malloc(4 * sizeof(uint8_t));
    message[0] = STEPPER_UART_SYNC;
    message[1] = STEPPER_UART_SLAVE_ADDR;
    message[2] = reg;
    message[3] = stepper_crc8(message, 3, 0x07, 0x0);
    uart_write(UART, message, 4);
    return;
}

void stepper_enable(stepper_t *stepper){
    stepper->enable = true;
    gpio_clear(ENABLE_PIN);
}

void stepper_disable(stepper_t *stepper){
    stepper->enable = false;
    gpio_set(ENABLE_PIN);
}