/**************************************************************************
* NAME: 	   tasks_main.c												  *
*																		  *
* AUTHOR: 	   Jonas Holmberg											  *
*                                                        				  *
* PURPOSE: 	   Implement all the functions that will be called when		  *
* 			   executing a task in the scheduler.						  *
*																		  *
* INFORMATION: Holds the function implementations for the individual tasks*
* 			   that are invoked when a task is executed in the scheduler. *
* 			   Each task needs to have an associated function that has to *
* 			   be invoked when it is chosen as the task to run.       	  *
* 			   Additionally optional event driven task functions must be  *
* 			   implemented here as well. This file will include different *
* 			   drivers meaning that the task functions could jump around  *
* 			   into other files before finishing its execution.           *
* 			                                                              *
* GLOBAL VARIABLES:                                                       *
* Variable        Type          Description                               *
* --------        ----          -----------                               *
***************************************************************************/

#include <stdint.h>
#include <stdbool.h>

#include "Scheduler/scheduler.h"
#include "Scheduler/tasks.h"
#include "stm32f4xx_revo.h"

/* Drivers */
#include "drivers/led.h"
#include "drivers/adc.h"
#include "drivers/motors.h"
#include "drivers/pwm.h"
#include "drivers/system_clock.h"
#include "config/eeprom.h"
#include "config/cli.h"
#include "drivers/sbus.h"



void systemTaskGyroPid(void)
{
	//Read gyro and update PID and finally update the motors. The most important task in the system
}

void systemTaskAccelerometer(void)
{
	//update the accelerometer data
	uint8_t c = 97;
	usart_transmit(&cliUsart, &c, 1, 1000000000);
}

void systemTaskAttitude(void)
{

}

void systemTaskRx(void)
{
	//Interpret commands to the vehicle
	sbus_read();
	sbusFrame_s frame = sbusChannelData;
}

bool systemTaskRxCheck(uint32_t currentDeltaTime)
{
	//This task is what is controlling the event activation of the systemTaskRx
	//check if there is anything that has be received.

	return sbus_frame_available();
}

void systemTaskRxCli(void)
{
	/* Check if CLI should be activated */
	if (cliShouldRun() == true)
		cliRun();
}

bool systemTaskRxCliCheck(uint32_t currentDeltaTime)
{
	/* First check if any value has been sent to the cli usart.
	 * We dont care about the delta time for this since if this
	 * has received something we should always check we dont care about
	 * the loop times. */
	return cliHasMessage();

	return false;
}

void systemTaskSerial(void)
{
	uint8_t c = 115;
	usart_transmit(&cliUsart, &c, 1, 1000000000);
}

void systemTaskBattery(void)
{
	//Keep track of the battery level of the system
	uint8_t c = 98;
	usart_transmit(&cliUsart, &c, 1, 1000000000);
}

void systemTaskBaro(void)
{
	//Obtain the barometer data
}

void systemTaskCompass(void)
{
	//Obtain compass data
}

void systemTaskGps(void)
{
	//Obtain gps data
}

void systemTaskSonar(void)
{
	//obtain sonar data
}

void systemTaskAltitude(void)
{
	//Keep track of the vehicles current altitude, based on some sensor. In this case either barometer or sonar
}

void systemTaskBeeper(void)
{

}

/* TO BE USED ONLY WHEN TESTING/DEBUGIN TASK FUNCTIONALLITY, DONT USE WHEN RUNNING THE REAL SYSTEM!!!!!!!!!! */
#ifdef USE_DEBUG_TASKS

void systemTaskDebug_1(void)
{
	//ledToggle(Led0_PIN, Led0_GPIO_PORT);
	clock_delay_ms(8);
	//ledToggle(Led0_PIN, Led0_GPIO_PORT);
}

void systemTaskDebug_2(void)
{
	//ledToggle(Led1, Led1_GPIO_PORT);
	//clock_delay_ms(15);
	clock_delay_ms(8);
	//ledToggle(Led1, Led1_GPIO_PORT);
}

void systemTaskDebug_3(void)
{
	//ledToggle(GPIO_PIN_0, GPIOA);
	//clock_delay_ms(20);
	clock_delay_ms(8);
	//ledToggle(GPIO_PIN_0, GPIOA);
}

#endif	/* End USE_DEBUG_TASKS */