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

#include <stdlib.h>

#include "driver.h"
#include "board_devices.h"
#include "engines.h"

#include "drive_ctrl.h"

struct drive_ctrl {
	struct engine_ctrl *left_forward;
	struct engine_ctrl *left_backward;
	struct engine_ctrl *right_forward;
	struct engine_ctrl *right_backward;
};

struct drive_ctrl_object {
	unsigned int target_speed;
	unsigned int turn_speed;
	unsigned int current_speed;
};

static struct drive_ctrl drive_ctrl = {
		.left_forward = &left_forward,
		.left_backward = &left_backward,
		.right_forward = &right_forward,
		.right_backward = &right_backward,
};

static struct drive_ctrl_object drive_ctrl_object = {
		.target_speed = 80,	/* % */
		.turn_speed = 40,
		.current_speed = 0,	/* % */
};

int drive_ctrl_init(void)
{
	unsigned int duty = 0;

	/* open enable pins */
	drv_open(drive_ctrl.left_forward->enable);
	drv_write(drive_ctrl.left_forward->enable, "0", 1);
	drv_open(drive_ctrl.left_backward->enable);
	drv_write(drive_ctrl.left_backward->enable, "0", 1);
	drv_open(drive_ctrl.right_forward->enable);
	drv_write(drive_ctrl.right_forward->enable, "0", 1);
	drv_open(drive_ctrl.right_backward->enable);
	drv_write(drive_ctrl.right_backward->enable, "0", 1);

	/* open pwm's */
	drv_open(drive_ctrl.left_backward->pwm);
	drv_ioctl(drive_ctrl.left_backward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
	drv_open(drive_ctrl.left_forward->pwm);
	drv_ioctl(drive_ctrl.left_forward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
	drv_open(drive_ctrl.right_backward->pwm);
	drv_ioctl(drive_ctrl.right_backward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
	drv_open(drive_ctrl.right_forward->pwm);
	drv_ioctl(drive_ctrl.right_forward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);

	/* enable enable pins */
	drv_write(drive_ctrl.left_forward->enable, "1", 1);
	drv_write(drive_ctrl.left_backward->enable, "1", 1);
	drv_write(drive_ctrl.right_forward->enable, "1", 1);
	drv_write(drive_ctrl.right_backward->enable, "1", 1);

	return 0;
}

static unsigned int calc_speed_front(int speed_percent)
{
	unsigned int speed_front = 0;
	if(speed_percent > 0) {
		if(speed_percent > 100)
			speed_front = 100;
		else
			speed_front = (unsigned int) speed_percent;
	}
	return speed_front;
}

static unsigned int calc_speed_back(int speed_percent)
{
	unsigned int speed_back = 0;
	if(speed_percent < 0) {
		if(speed_percent < -100)
			speed_back = 100;
		else
			speed_back = (unsigned int) abs(speed_percent);
	}
	return speed_back;
}

int drive_ctrl_set_speed_left(int speed_percent)
{
	unsigned int front_speed = calc_speed_front(speed_percent);
	unsigned int back_speed = calc_speed_back(speed_percent);
	int ret = drv_ioctl(drive_ctrl.right_forward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&front_speed);
	ret |= drv_ioctl(drive_ctrl.left_backward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&back_speed);

	return ret;
}

int drive_ctrl_set_speed_right(int speed_percent)
{
	unsigned int front_speed = calc_speed_front(speed_percent);
	unsigned int back_speed = calc_speed_back(speed_percent);
	int ret = drv_ioctl(drive_ctrl.left_forward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&front_speed);
	ret |= drv_ioctl(drive_ctrl.right_backward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&back_speed);

	return ret;
}

int drive_ctrl_set_target_speed_value(int speed_percent)
{
	drive_ctrl_object.target_speed = speed_percent;

	return 0;
}

int drive_ctrl_get_target_speed_value(void)
{
	return drive_ctrl_object.target_speed;
}

int drive_ctrl_set_current_speed_value(int speed_percent)
{
	drive_ctrl_object.current_speed = speed_percent;

	return 0;
}

int drive_ctrl_get_current_speed_value(void)
{
	return drive_ctrl_object.current_speed;
}

int drive_ctrl_set_turn_speed_value(int speed_percent)
{
	drive_ctrl_object.turn_speed = speed_percent;

	return 0;
}

int drive_ctrl_get_turn_speed_value(void)
{
	return drive_ctrl_object.turn_speed;
}