/*
 * drive_ctrl.c
 *
 *  Created on: Aug 10, 2016
 *      Author: tkl
 */

#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <stdint.h>

#include "queue.h"
#include "stack.h"
#include "kernel.h"
#include "driver.h"
#include "engine_ctrl.h"

#include "drive_ctrl.h"

int drive_ctrl_init(void)
{
	return engine_ctrl_init();
}

int drive_ctrl_turn_right(void)
{
	drive_ctrl_halt();
	int turn_speed = engine_ctrl_get_turn_speed_value();
	for(int i = 0; i < turn_speed; i++) {
		engine_ctrl_set_speed_left(i);
		engine_ctrl_set_speed_right(-1 * i);
		sleep_ms(10);
	}

	return 0;
}

int drive_ctrl_turn_left(void)
{
	drive_ctrl_halt();
	int turn_speed = engine_ctrl_get_turn_speed_value();
	for(int i = 0; i < turn_speed; i++) {
		engine_ctrl_set_speed_right(i);
		engine_ctrl_set_speed_left(-1 * i);
		sleep_ms(10);
	}

	return 0;
}

int drive_ctrl_boost(void)
{
	int speed = engine_ctrl_get_target_speed_value();
	speed += 10;
	engine_ctrl_set_target_speed_value(speed);
	engine_ctrl_set_speed_left(speed);
	engine_ctrl_set_speed_right(speed);

	return 0;
}

int drive_ctrl_retard(void)
{
	int speed = engine_ctrl_get_target_speed_value();
	speed -= 10;
	engine_ctrl_set_target_speed_value(speed);
	engine_ctrl_set_speed_left(speed);
	engine_ctrl_set_speed_right(speed);

	return 0;
}

int drive_ctrl_halt(void)
{
	int current_speed = engine_ctrl_get_current_speed_value();
	if(current_speed > 0) {
		for(int i = current_speed; i > 0; i--) {
			engine_ctrl_set_current_speed_value(i);
			engine_ctrl_set_speed_left(i);
			engine_ctrl_set_speed_right(i);
			sleep_ms(10);
		}
	}
	else {
		for(int i = current_speed; i < 0; i++) {
			engine_ctrl_set_current_speed_value(i);
			engine_ctrl_set_speed_left(i);
			engine_ctrl_set_speed_right(i);
			sleep_ms(10);
		}
	}

	return 0;
}

int drive_ctrl_forward(void)
{
	int target_speed = engine_ctrl_get_target_speed_value();
	int current_speed = engine_ctrl_get_current_speed_value();
	if(target_speed < 0)
		target_speed = abs(target_speed);
	engine_ctrl_set_target_speed_value(target_speed);
	if(current_speed < target_speed) {
		for(int i = current_speed; i < target_speed; i++) {
			engine_ctrl_set_current_speed_value(i);
			engine_ctrl_set_speed_left(i);
			engine_ctrl_set_speed_right(i);
			sleep_ms(10);
		}
	}
	else {
		for(int i = current_speed; i > target_speed; i--) {
			engine_ctrl_set_current_speed_value(i);
			engine_ctrl_set_speed_left(i);
			engine_ctrl_set_speed_right(i);
			sleep_ms(10);
		}
	}

	return 0;
}

int drive_ctrl_backward(void)
{
	int target_speed = engine_ctrl_get_target_speed_value();
	int current_speed = engine_ctrl_get_current_speed_value();
	if(target_speed > 0)
		target_speed *= -1;
	engine_ctrl_set_target_speed_value(target_speed);
	if(current_speed > target_speed) {
		for(int i = current_speed; i > target_speed; i--) {
			engine_ctrl_set_current_speed_value(i);
			engine_ctrl_set_speed_left(i);
			engine_ctrl_set_speed_right(i);
			sleep_ms(10);
		}
	}
	else {
		for(int i = current_speed; i < target_speed; i++) {
			engine_ctrl_set_current_speed_value(i);
			engine_ctrl_set_speed_left(i);
			engine_ctrl_set_speed_right(i);
			sleep_ms(10);
		}
	}

	return 0;
}