/*
 * shell_commands.c
 *
 *  Created on: Aug 10, 2016
 *      Author: tkl
 */
#include <stddef.h>
#include <stdlib.h>

#include "list.h"
#include "driver.h"
#include "shell.h"
#include "drive_ctrl.h"

static void *drive_turn_right_cb(const char *param);
static void *drive_turn_left_cb(const char *param);
static void *drive_boost_cb(const char *param);
static void *drive_retard_cb(const char *param);
static void *drive_halt_cb(const char *param);
static void *drive_forward_cb(const char *param);
static void *drive_backward_cb(const char *param);

static struct command drive_forward = {
		.command = "f",
		.description = "Start driving forward with preset speed.",
		.command_callback = drive_forward_cb,
};

static struct command drive_backward = {
		.command = "b",
		.description = "Start driving backward with preset speed.",
		.command_callback = drive_backward_cb,
};

static struct command drive_halt = {
		.command = "h",
		.description = "Stop driving.",
		.command_callback = drive_halt_cb,
};

static struct command drive_boost = {
		.command = "+",
		.description = "Boost speed.",
		.command_callback = drive_boost_cb,
};

static struct command drive_retard = {
		.command = "-",
		.description = "Retard speed",
		.command_callback = drive_retard_cb,
};

static struct command drive_turn_left = {
		.command = "l",
		.description = "Turn left.",
		.command_callback = drive_turn_left_cb,
};

static struct command drive_turn_right = {
		.command = "r",
		.description = "Turn right",
		.command_callback = drive_turn_right_cb,
};

int shell_commands_init(void)
{
	shell_add_command(&drive_forward);
	shell_add_command(&drive_backward);
	shell_add_command(&drive_boost);
	shell_add_command(&drive_retard);
	shell_add_command(&drive_halt);
	shell_add_command(&drive_turn_left);
	shell_add_command(&drive_turn_right);

	return 0;
}

static void *drive_turn_right_cb(const char *param)
{
	int speed = drive_ctrl_get_speed_value();
	drive_ctrl_set_speed_left(speed);
	drive_ctrl_set_speed_right(0);

	return NULL;
}

static void *drive_turn_left_cb(const char *param)
{
	int speed = drive_ctrl_get_speed_value();
	drive_ctrl_set_speed_left(0);
	drive_ctrl_set_speed_right(speed);

	return NULL;
}

static void *drive_boost_cb(const char *param)
{
	int speed = drive_ctrl_get_speed_value();
	speed += 10;
	drive_ctrl_set_speed_value(speed);
	drive_ctrl_set_speed_left(speed);
	drive_ctrl_set_speed_right(speed);

	return NULL;
}

static void *drive_retard_cb(const char *param)
{
	int speed = drive_ctrl_get_speed_value();
	speed -= 10;
	drive_ctrl_set_speed_value(speed);
	drive_ctrl_set_speed_left(speed);
	drive_ctrl_set_speed_right(speed);

	return NULL;
}

static void *drive_halt_cb(const char *param)
{
	drive_ctrl_set_speed_left(0);
	drive_ctrl_set_speed_right(0);

	return NULL;
}

static void *drive_forward_cb(const char *param)
{
	int speed = drive_ctrl_get_speed_value();
	if(speed < 0)
		speed = abs(speed);
	drive_ctrl_set_speed_value(speed);
	drive_ctrl_set_speed_left(speed);
	drive_ctrl_set_speed_right(speed);

	return NULL;
}

static void *drive_backward_cb(const char *param)
{
	int speed = drive_ctrl_get_speed_value();
	if(speed > 0)
		speed *= -1;
	drive_ctrl_set_speed_value(speed);
	drive_ctrl_set_speed_left(speed);
	drive_ctrl_set_speed_right(speed);

	return NULL;
}