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

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


#define STEP_PIN (dev->p.step_pin)
#define DIR_PIN (dev->p.dir_pin)
#define UART (dev->p.uart)

int stepper_init(stepper_t *dev, const stepper_params_t *params)
{
    assert(dev && params);

    dev->p = *params;

    gpio_init(STEP_PIN, GPIO_OUT);

    uart_init(UART, 38400, NULL, NULL);

    le_uint32_t data;
    data.u32 = STEPPER_GCONF_VALUES;

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


    return STEPPER_OK;
}


int stepper_step(stepper_t *dev, stepper_direction_t *direction)
{
    assert(direction);
    gpio_toggle(STEP_PIN);

    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;
}
    


int stepper_write(stepper_t *dev, uint8_t reg, le_uint32_t *data)
{
    uint8_t* message;
    uint32_t be_data;
    be_data = byteorder_swapl(data->u32);
    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;

    printf(data);

    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 STEPPER_OK;
}

int stepper_read(stepper_t *dev, 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);
}