#include <stdio.h>
#include <stdlib.h>

#include "tb6612fng.h"
#include "periph/pwm.h"
#include "periph/gpio.h"
#include "timex.h" /* for US_PER_SEC */
#include "xtimer.h"

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

#ifndef MOTOR_FREQUENCY
#define MOTOR_FREQUENCY       (1000U)
#endif

#ifndef MOTOR_RESOLUTION
#define MOTOR_RESOLUTION      MOTOR_FREQUENCY
#endif

int motor_init(motor_t *dev, pwm_t pwm, int pwm_channel, gpio_t in1, gpio_t in2, gpio_t stby, int offset)
{
    int actual_frequency;

    actual_frequency = pwm_init(pwm, PWM_LEFT, MOTOR_FREQUENCY, MOTOR_RESOLUTION);

    DEBUG("servo: requested %d hz, got %d hz\n", MOTOR_FREQUENCY, actual_frequency);

    if (actual_frequency < 0) {
        /* PWM error */
        return -1;
    }

    dev->device = pwm;
    dev->channel = pwm_channel;
    dev->in1 = in1;
    dev->in2 = in2;
    dev->stby = stby;
    dev->offset = offset;

    if(gpio_init(dev->in1,GPIO_OUT) < 0) {return -1;}
    if(gpio_init(dev->in2,GPIO_OUT) < 0){return -1;}
    if(gpio_init(dev->stby,GPIO_OUT) < 0){return -1;}

    return 0;
}


void motor_fw(const motor_t *dev, uint16_t speed){
	gpio_set(dev->in1);
	gpio_clear(dev->in2);
	pwm_set(dev->device,dev->channel, speed);
}
void motor_rev(const motor_t *dev, uint16_t speed){
	gpio_clear(dev->in1);
	gpio_set(dev->in2);
	pwm_set(dev->device,dev->channel,speed);
}

void motor_drive(const motor_t *dev, uint16_t speed, int direction){
        gpio_set(dev->stby);
        speed = speed * dev->offset;
	if(direction > 0) motor_fw(dev,speed);
        else motor_rev(dev,speed);
}

void motor_brake(const motor_t *dev1, motor_t *dev2){
	gpio_set(dev1->in1);
	gpio_set(dev1->in2);
	pwm_poweroff(dev1->device);

	gpio_set(dev2->in1);
	gpio_set(dev2->in2);
	pwm_poweroff(dev2->device);
}

void motor_forward(const motor_t *dev1, const motor_t *dev2, uint16_t speed){
	motor_drive(dev1,speed,1);
        motor_drive(dev2,speed,1);
}
void motor_back(const motor_t *dev1, const motor_t *dev2, uint16_t speed){
	motor_drive(dev1,speed,-1);
        motor_drive(dev2,speed,-1);
}
void motor_left(const motor_t *left, const motor_t *right, uint16_t speed){
	motor_drive(left,speed/2,-1);
        motor_drive(right,speed/2,1);

}
void motor_right(const motor_t *left,const motor_t *right, uint16_t speed){
	motor_drive(left,speed/2,1);
        motor_drive(right,speed/2,-1);
}

void motor_stby(const motor_t *dev){
	gpio_clear(dev->stby);
}