#include <stdio.h>
#include <stdint.h>

#include "assert.h"
#include "periph/gpio.h"
#include "timex.h"
#include "roomba.h"
#include "board.h"

#define RADIUS_STRAIGHT 0x8000


void motors_drive(int16_t speed, int16_t radius)
{
    uint8_t data[4];
    if ((speed > 500) || (speed < (0 - 500)) || (radius > 2000) || (radius < (0 - 2000)))
        motors_stop();
    data[0] = ((uint16_t)speed) & 0xFF;
    data[1] = (((uint16_t)speed) & 0xFF00) >> 8;
    if ((radius == 0) && (speed != 0))
        radius = RADIUS_STRAIGHT;
    data[2] = ((uint16_t)radius) & 0xFF;
    data[3] = (((uint16_t)radius) & 0xFF00) >> 8;
    roomba_cmd(ROOMBA_DRIVE, data, 4);
}

void motors_stop(void)
{
    motors_drive(0, 0);
    motors_clean_set(0,0,0);
}

void motors_spin(int cw)
{
    if(cw)
        motors_drive(10, -1);
    else
        motors_drive(10, 1);
}


void motors_clean_set(int brush, int vac, int side)
{
    uint8_t data;
    data = ((!!brush) << 2) | ((!!vac) << 1) | (!!side);
    roomba_cmd(ROOMBA_MOTORS, &data, 1);
}