/*
 * main.c
 *
 *  Created on: Aug 2, 2016
 *      Author: tkl
 */

#include <stdbool.h>

#include "driver.h"
#include "board.h"
#include "stack.h"
#include "queue.h"
#include "kernel.h"
#include "driver.h"
#include "list.h"
#include "shell.h"

struct engine_ctrl {
	const struct driver *pwm;
	const struct driver *enable;
};

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


static struct engine_ctrl right_forward = {
		.pwm = &pwm_1,
		.enable = &gpio_c1,
};

static struct engine_ctrl right_backward = {
		.pwm = &pwm_2,
		.enable = &gpio_c0,
};

static struct engine_ctrl left_forward = {
		.pwm = &pwm_3,
		.enable = &gpio_c3,
};

static struct engine_ctrl left_backward = {
		.pwm = &pwm_4,
		.enable = &gpio_c2,
};

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

#define TH_STACK_SIZE  256
stack_t th_stack[TH_STACK_SIZE];
struct thread_context th_ctx;
static void th_func(void *arg)
{
	unsigned int duty = 0;

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

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

	/* enable enable pins */
	write(drive_ctrl.left_forward->enable, "1", 1);
	write(drive_ctrl.left_backward->enable, "1", 1);
	write(drive_ctrl.right_forward->enable, "1", 1);
	write(drive_ctrl.right_backward->enable, "1", 1);
	while(1) {
		duty = 0;
		ioctl(drive_ctrl.left_backward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
		ioctl(drive_ctrl.right_backward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
		for(duty = 0; duty < 100; duty++) {
			ioctl(drive_ctrl.left_forward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
			ioctl(drive_ctrl.right_forward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
			sleep_ms(100);
		}
		duty = 0;
		ioctl(drive_ctrl.left_forward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
		ioctl(drive_ctrl.right_forward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
		sleep_ms(100);
		for(duty = 0; duty < 100; duty++) {
			ioctl(drive_ctrl.left_backward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
			ioctl(drive_ctrl.right_backward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
			sleep_ms(100);
		}
		sleep_ms(100);
	}
}

int main(void)
{
	thread_create(&th_ctx, th_stack, TH_STACK_SIZE, th_func, NULL, THREAD_PRIO_LOW);
	schedule_start();

	return 0;
}