#include <stdio.h>

#include "cpu.h"
#include "board.h"
#include "xtimer.h"
#include "periph/pwm.h"
#include "periph/gpio.h"
#include "robotcar.h"

#define PWM        PWM_DEV(0)

#define MOTOR1_CHANNEL     0
#define MOTOR1_IN1     GPIO_PIN(PORT_E,0)
#define MOTOR1_IN2     GPIO_PIN(PORT_E,2)
#define MOTOR1_ECD     GPIO_PIN(PORT_A,8)

#define MOTOR2_CHANNEL     1
#define MOTOR2_IN1     GPIO_PIN(PORT_E,1)
#define MOTOR2_IN2     GPIO_PIN(PORT_E,3)
#define MOTOR2_ECD     GPIO_PIN(PORT_A,9)

#define STDBY	GPIO_PIN(PORT_D,4)

static robot_t motor_1;
static robot_t motor_2;

static void motor1_cb(void *arg)
{
  robot_step_counter(&motor_1);
}

static void motor2_cb(void *arg)
{
  robot_step_counter(&motor_2);
}

int main(void)
{
    int res;
    puts("\nRIOT dc motor test");
    puts("Connect an dc motor or scope to PWM_0 channel 0/1 to see anything");

    res = robot_init(&motor_1,PWM,MOTOR1_CHANNEL,MOTOR1_IN1,MOTOR1_IN1,STDBY,MOTOR1_ECD);
    if (res < 0) {
        puts("Errors while initializing motor");
        return -1;
    }

  res = robot_init(&motor_2,PWM,MOTOR2_CHANNEL,MOTOR2_IN2,MOTOR2_IN2,STDBY,MOTOR2_ECD);
    if (res < 0) {
        puts("Errors while initializing motor");
        return -1;
    }
    puts("all motors initialized.");

   if (gpio_init_int(MOTOR1_ECD,GPIO_IN, GPIO_RISING, motor1_cb,
                      (void *)MOTOR1_ECD) < 0) {
        puts("Error while initializing MOTOR1_ECD as external interrupt\n");
        return 1;
    }
    
    if (gpio_init_int(MOTOR2_ECD,GPIO_IN, GPIO_RISING, motor2_cb,
                      (void *)MOTOR2_ECD) < 0) {
        puts("Error while initializing MOTOR2_ECD as external interrupt\n");
        return 1;
    }

    printf("motor interrupt initialized successfuly\n");

    while (1) {
    
    gpio_clear(LED3_PIN);
    gpio_clear(LED2_PIN);
    gpio_clear(LED1_PIN);
    gpio_clear(LED0_PIN);

    robot_move_forward(&motor_1,&motor_2,400,8);
    gpio_clear(LED3_PIN);
    gpio_clear(LED2_PIN);
    gpio_clear(LED1_PIN);
    gpio_clear(LED0_PIN);

    xtimer_sleep(2);
    robot_move_reverse(&motor_1,&motor_2,400,11);
    gpio_clear(LED3_PIN);
    gpio_clear(LED2_PIN);
    gpio_clear(LED1_PIN);
    gpio_clear(LED0_PIN);

    xtimer_sleep(2);
    robot_spin_right(&motor_1,&motor_2,400,6);
    gpio_clear(LED3_PIN);
    gpio_clear(LED2_PIN);
    gpio_clear(LED1_PIN);
    gpio_clear(LED0_PIN);
    xtimer_sleep(2);
    robot_spin_left(&motor_1,&motor_2,400,5);
    gpio_clear(LED3_PIN);
    gpio_clear(LED2_PIN);
    gpio_clear(LED1_PIN);
    gpio_clear(LED0_PIN);

    xtimer_sleep(2);
    }

    return 0;
}