/*
 * collision_ctrl.c
 *
 *  Created on: Aug 30, 2016
 *      Author: tkl
 */

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

#include "board_devices.h"
#include "queue.h"
#include "list.h"
#include "stack.h"
#include "kernel.h"
#include "driver.h"
#include "shell.h"
#include "drive_ctrl.h"
#include "engine_ctrl.h"

#pragma pack(push)
#pragma pack(1)
struct collision_ctrl_object {
	int distance_mm;
};
#pragma pack(pop)

struct collision_ctrl_object collision_ctrl_object;
struct thread_context collision_ctrl_ctx;
stack_t collision_ctrl_stack[256];

static void collision_ctrl_loop(void *arg)
{
	drv_open(&pwm5_c2);	/* trigger pwm */
	drv_open(&pwm2_c4); /* read pwm */
	drv_open(&rng);
	while(1) {
		int pulse_ns = drv_ioctl(&pwm2_c4, IOCTL_PWM_GET_PULSE_WIDTH_NS, NULL);
		collision_ctrl_object.distance_mm = 343 * pulse_ns / 1000 / 2;
		if(collision_ctrl_object.distance_mm < 300) {
			char print_buffer[50];
			if(engine_ctrl_get_current_speed_value() != 0) {
				drive_ctrl_halt();
				sprintf(print_buffer, "collision detected (%dmm)...\r\n", collision_ctrl_object.distance_mm);
				shell_write(print_buffer, strlen(print_buffer));
				sprintf(print_buffer, "going back...\r\n");
				shell_write(print_buffer, strlen(print_buffer));
				drive_ctrl_backward();
				sleep_ms(500);
				drive_ctrl_halt();

				sprintf(print_buffer, "start turning...\r\n");
				shell_write(print_buffer, strlen(print_buffer));
				drive_ctrl_turn_left();
				sleep_ms(drv_read(&rng, Null, 0) % 3500 + 500); // rand between 500 and 4000
				drive_ctrl_halt();

				sprintf(print_buffer, "restart forward...\r\n");
				shell_write(print_buffer, strlen(print_buffer));
				drive_ctrl_forward();
			}
			else {
				sprintf(print_buffer, "collision detected while not driving 8(\r\n");
				shell_write(print_buffer, strlen(print_buffer));
			}
		}
		sleep_ms(1);
	}
}

int collision_ctrl_init(void)
{
	thread_create(
			&collision_ctrl_ctx,
			collision_ctrl_stack,
			sizeof(collision_ctrl_stack) / sizeof(stack_t),
			collision_ctrl_loop,
			NULL,
			THREAD_PRIO_LOW);

	return 0;
}

int collision_ctrl_get_distance_mm(void)
{
	return collision_ctrl_object.distance_mm;
}