/*
 * sensor_data.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 "driver.h"
#include "queue.h"
#include "list.h"
#include "stack.h"
#include "kernel.h"
#include "shell.h"
#include "collision_ctrl.h"

struct sensor_data_object {
	bool sys_msg_on;
};

struct sensor_data_object sensor_data_object = {
		.sys_msg_on = false,
};

struct thread_context sensor_data_ctx;
stack_t sensor_data_stack[256];

static void sensor_data_loop(void *arg)
{
	char print_buffer[50];
	while(1) {
		int collision_distance = collision_ctrl_get_distance_mm();
		if(sensor_data_object.sys_msg_on) {
			sprintf(print_buffer, "#SYS:%d\r\n", collision_distance);
			shell_write(print_buffer, strlen(print_buffer));
		}
		sleep_ms(1000);
	}
}

int sensor_data_init(void)
{
	thread_create(
			&sensor_data_ctx,
			sensor_data_stack,
			sizeof(sensor_data_stack) / sizeof(stack_t),
			sensor_data_loop,
			NULL,
			THREAD_PRIO_LOW);

	return 0;
}

int sensor_data_sys_msg(bool state)
{
	sensor_data_object.sys_msg_on = state;

	return 0;
}