/*
 * Copyright (C) 2015 Freie Universität Berlin
 *
 * This file is subject to the terms and conditions of the GNU Lesser
 * General Public License v2.1. See the file LICENSE in the top level
 * directory for more details.
 */

/**
 * @ingroup     examples
 * @{
 *
 * @file
 * @brief       Demonstrating the sending and receiving of UDP data
 *
 * @author      Hauke Petersen <hauke.petersen@fu-berlin.de>
 *
 * @}
 */

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

#include "kernel_types.h"
#include "msg.h"
#include "cpu.h"
#include "board.h"
#include "xtimer.h"
#include "periph/pwm.h"
#include "periph/gpio.h"
#include "tb6612fng.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 MOTOR2_CHANNEL     1
#define MOTOR2_IN1     GPIO_PIN(PORT_E,1)
#define MOTOR2_IN2     GPIO_PIN(PORT_E,3)

#define STBY	GPIO_PIN(PORT_D,4)
 
kernel_pid_t server, client;
motor_t left;
motor_t right;

//define data type to send
typedef union
{
  uint32_t msg;
  struct 
  {
    uint16_t cmd; 
    uint16_t speed; 
  };

}word;

static void send_cmd(uint32_t c)
{
    msg_t msg;
    msg.content.value = c;
    msg_send(&msg, client);
}

int motor_cmd(int argc, char **argv)
{
    word data; 
    if (argc < 2) {
        printf("usage: %s [run|stop]\n", argv[0]);
        return 1;
    }
    if (strcmp(argv[1], "run") == 0) {
        uint32_t vitesse = 400;
        if (argc < 3) {
            printf("usage: %s run [forward|back|right|left] [<duty cycle>]\n",
                   argv[0]);
            return 1;
        }
        if(strcmp(argv[2], "forward") == 0){
          data.cmd = 0;     
        }
        else if(strcmp(argv[2], "back") == 0){
          data.cmd = 1;     
        }
        else if(strcmp(argv[2], "right") == 0){
          data.cmd = 2;     
        }
        else if(strcmp(argv[2], "left") == 0){
          data.cmd = 3;     
        }
        if (argc > 3) {
            vitesse = atoi(argv[3]);
        }
        data.speed = vitesse;
        send_cmd(data.msg);
    }

    else if(strcmp(argv[1], "stop") == 0){
    if (argc < 3) {
            printf("usage: %s stop [brake|stdby]\n",
                   argv[0]);
            return 1;
        }
      if(strcmp(argv[2], "brake") == 0){
          data.cmd = 4;     
        }
      else if(strcmp(argv[2], "stdby") == 0){
          data.cmd = 5;     
        }
     data.speed = 0;
     send_cmd(data.msg);
    }
  else {
        puts("error: invalid command");
    }
     
    return 0;
}

void send_motor(uint32_t msg)
{
 if((msg&0xffff) == 0) {motor_forward(&left,&right,(msg>>16));}
 if((msg&0xffff) == 1) {motor_back(&left,&right,(msg>>16));}
 if((msg&0xffff) == 2) {motor_right(&left,&right,(msg>>16));}
 if((msg&0xffff) == 4) {motor_left(&left,&right,(msg>>16));}
 if((msg&0xffff) == 4) {motor_brake(&left,&right);}
 if((msg&0xffff) == 5) {motor_stby(&left);}
}

int init_motor(motor_t *dev1, motor_t *dev2)
{
 int res;
 
    res = motor_init(dev1,PWM,MOTOR1_CHANNEL,MOTOR1_IN1,MOTOR1_IN2,STBY);
    if (res < 0) {
        puts("Errors while initializing motor");
        return -1;
    }
    puts("motor 1 initialized.");

   res = motor_init(dev2,PWM,MOTOR2_CHANNEL,MOTOR2_IN1,MOTOR2_IN2,STBY);
    if (res < 0) {
        puts("Errors while initializing motor");
        return -1;
    }
    puts("motor 2 initialized.");
 return 0;
}