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

#include "list.h"
#include "driver.h"
#include "shell.h"
#include "queue.h"
#include "stack.h"
#include "kernel.h"
#include "drive_ctrl.h"
#include "version.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 void *app_version(const char *param);

static struct command cmd_app_version = {
		.command = "version",
		.description = "Get application version.",
		.command_callback = app_version,
};

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);
	shell_add_command(&cmd_app_version);

	return 0;
}

static void *drive_turn_right_cb(const char *param)
{
	drive_halt_cb(NULL);
	int turn_speed = drive_ctrl_get_turn_speed_value();
	for(int i = 0; i < turn_speed; i++) {
		drive_ctrl_set_speed_left(i);
		drive_ctrl_set_speed_right(-1 * i);
		sleep_ms(10);
	}

	return NULL;
}

static void *drive_turn_left_cb(const char *param)
{
	drive_halt_cb(NULL);
	int turn_speed = drive_ctrl_get_turn_speed_value();
	for(int i = 0; i < turn_speed; i++) {
		drive_ctrl_set_speed_right(i);
		drive_ctrl_set_speed_left(-1 * i);
		sleep_ms(10);
	}

	return NULL;
}

static void *drive_boost_cb(const char *param)
{
	int speed = drive_ctrl_get_target_speed_value();
	speed += 10;
	drive_ctrl_set_target_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_target_speed_value();
	speed -= 10;
	drive_ctrl_set_target_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)
{
	int current_speed = drive_ctrl_get_current_speed_value();
	if(current_speed > 0) {
		for(int i = current_speed; i > 0; i--) {
			drive_ctrl_set_current_speed_value(i);
			drive_ctrl_set_speed_left(i);
			drive_ctrl_set_speed_right(i);
			sleep_ms(10);
		}
	}
	else {
		for(int i = current_speed; i < 0; i++) {
			drive_ctrl_set_current_speed_value(i);
			drive_ctrl_set_speed_left(i);
			drive_ctrl_set_speed_right(i);
			sleep_ms(10);
		}
	}

	return NULL;
}

static void *drive_forward_cb(const char *param)
{
	int target_speed = drive_ctrl_get_target_speed_value();
	int current_speed = drive_ctrl_get_current_speed_value();
	if(target_speed < 0)
		target_speed = abs(target_speed);
	drive_ctrl_set_target_speed_value(target_speed);
	if(current_speed < target_speed) {
		for(int i = current_speed; i < target_speed; i++) {
			drive_ctrl_set_current_speed_value(i);
			drive_ctrl_set_speed_left(i);
			drive_ctrl_set_speed_right(i);
			sleep_ms(10);
		}
	}
	else {
		for(int i = current_speed; i > target_speed; i--) {
			drive_ctrl_set_current_speed_value(i);
			drive_ctrl_set_speed_left(i);
			drive_ctrl_set_speed_right(i);
			sleep_ms(10);
		}
	}

	return NULL;
}

static void *drive_backward_cb(const char *param)
{
	int target_speed = drive_ctrl_get_target_speed_value();
	int current_speed = drive_ctrl_get_current_speed_value();
	if(target_speed > 0)
		target_speed *= -1;
	drive_ctrl_set_target_speed_value(target_speed);
	if(current_speed > target_speed) {
		for(int i = current_speed; i > target_speed; i--) {
			drive_ctrl_set_current_speed_value(i);
			drive_ctrl_set_speed_left(i);
			drive_ctrl_set_speed_right(i);
			sleep_ms(10);
		}
	}
	else {
		for(int i = current_speed; i < target_speed; i++) {
			drive_ctrl_set_current_speed_value(i);
			drive_ctrl_set_speed_left(i);
			drive_ctrl_set_speed_right(i);
			sleep_ms(10);
		}
	}

	return NULL;
}

static void *app_version(const char *param)
{
#if 0
	char *greeter = "engine_control version: ";

	drv_write(shell_object.shell_device, greeter, strlen(greeter));
	drv_write(shell_object.shell_device, MAJOR_VERSION, strlen(MAJOR_VERSION));
	drv_write(shell_object.shell_device, ".", 1);
	drv_write(shell_object.shell_device, MINOR_VERSION, strlen(MINOR_VERSION));
	drv_write(shell_object.shell_device, ".", 1);
	drv_write(shell_object.shell_device, BUILD_NUMBER, strlen(BUILD_NUMBER));
#endif
	return NULL;
}