/*
 * 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 "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)

robot_t motor_1;
robot_t motor_2;

kernel_pid_t server, client, robot_pid;

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

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

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

int robot_cmd(int argc, char **argv)
{
  word data;
  uint32_t speed,steps;
 
  if (argc < 2)
    {
      printf("usage: %s [move|spin]\n", argv[0]);
      return 1;
    }
  if (strcmp(argv[1], "move") == 0)
    {
      
      if (argc < 5)
	{
	  printf("usage: %s move [<forward|reverse>] <speed[300-999]> <distance[1-254] in cm> \n",argv[0]);
	  return 1;
	}
      if(strcmp(argv[2], "forward") == 0)
	{
	  data.cmd = 0;     
	}
      else if(strcmp(argv[2], "reverse") == 0)
	{
          data.cmd = 1;     
        }
       else
    	{
         puts("error: invalid command");
         return 1;
    	}
      speed = atoi(argv[3]);
      steps = atoi(argv[4]);
      data.speed = speed;
      data.step = steps;
      send_cmd(data.msg);
    }
   else if (strcmp(argv[1], "spin") == 0)
    {
      if (argc < 5)
	{
	  printf("usage: %s spin [<right|left>] <speed[0-1000]> <distance[0-254] in cm> \n",argv[0]);
	  return 1;
	}
      
      if(strcmp(argv[2], "right") == 0)
	{
	  data.cmd = 2;     
	}
      else if(strcmp(argv[2], "left") == 0)
	{
          data.cmd = 3;     
        }
       else
    	{
         puts("error: invalid command");
         return 1;
    	}
      speed = atoi(argv[3]);
      steps = atoi(argv[4]);
      data.speed = speed;
      data.step = steps;
      send_cmd(data.msg);
    }
   else
    {
      puts("error: invalid command");
    }
  
  return 0;
}

void send_to_robot(uint32_t msg)
{
  if((msg&0xff) == 0) {robot_move_forward(&motor_1,&motor_2,(msg>>16),cm_to_steps((msg>>8)&0xff));}
  if((msg&0xff) == 1) {robot_move_reverse(&motor_1,&motor_2,(msg>>16),cm_to_steps((msg>>8)&0xff));}
  if((msg&0xff) == 2) {robot_spin_right(&motor_1,&motor_2,(msg>>16),cm_to_steps((msg>>8)&0xff));}
  if((msg&0xff) == 3) {robot_spin_left(&motor_1,&motor_2,(msg>>16),cm_to_steps((msg>>8)&0xff));}
}

int init_robot(robot_t *dev1, robot_t *dev2)
{
    int res;
     
    res = robot_init(&motor_1,PWM,MOTOR1_CHANNEL,MOTOR1_IN1,MOTOR1_IN2,STDBY,MOTOR1_ECD,1);
    if (res < 0) {
        puts("Errors while initializing motor");
        return 1;
    }

   res = robot_init(&motor_2,PWM,MOTOR2_CHANNEL,MOTOR2_IN1,MOTOR2_IN2,STDBY,MOTOR2_ECD,2);
    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;
    }

    puts("motor interrupt initialized successfuly\n");
   return 0;
}