//============================================================================
// Template for periodic lustre SBC controller
// system tick = default = (theory) 15ms = (actually) 17ms
// FUNCTION_PERIOD is in system tick, i.e.
// FUNCTION_PERIOD=2 => real-time period = 30ms (35ms in practice)
//=============================================================================
#include <Arduino_FreeRTOS.h>
#include <Arduino.h>
#include "code/FreeRTOS_Utils.h"
#include "code/BatCar.h"
#include "control.h"

//-------------------------------------------------------------
// Utils
//-------------------------------------------------------------
// Macros to ease debug
#define PVAR(X) Serial.print(#X"="); Serial.print(X); Serial.print(' ');
#define PVARLN(X) Serial.print(#X"="); Serial.println(X);

//-------------------------------------------------------------
// Interface with lustre code (output functions)
//-------------------------------------------------------------

//TODO: define here the lustre output functions

void control_O_red_light(bool v){
   BatCar.set_red_light(v);
}
void control_O_yellow_light(bool v){
   BatCar.set_yellow_light(v);
}
void control_O_green_light(bool v){
   BatCar.set_green_light(v);
}
void control_O_motor_left(int s){
   BatCar.set_left_speed(s);
}
void control_O_motor_right(int s){
   BatCar.set_right_speed(s);
}
//warning! false=LOW=ON, true=HIGH=OFF
void control_O_buzzer(bool v) {
   BatCar.set_buzzer(!v);
}

//-------------------------------------------------------------
// FreeRTOS task
// - Expected profile: void vATaskFunction(void *pvParameters)
//   pvParameters = user client-data, not used here
// - this is typically a never-endind procedure. i.e. 
//   reactive loop is implemented here
// - Real-time parameters:
//   * OFFSET: a delay before starting
//   * PERIOD: the delay between 2 steps
//   * N.b. delays are in system tick (i.e. 15ms) 
//-------------------------------------------------------------
#define OFFSET 5 
#define PERIOD 1 
void control_task(void* _dummy){

	//TODO: (REAL TIME) periodic timer initialization
	// Initialize xLastWakeTime with current ABSOLUTE time.
	TickType_t xLastWakeTime;
   xLastWakeTime = xTaskGetTickCount();

	//TODO: (REAL TIME) Wait offest
	vTaskDelayUntil(&xLastWakeTime, OFFSET);

	// Core of the task = infinite control loop
	for(;;){

   	//TODO: here, sample (read) necessary input from the SBC
   	//      and 'send' it the lustre program
   	control_I_k1(BatCar.button_pressed());
   	control_I_sensor_left(BatCar.line_sensor_left());
   	control_I_sensor_right(BatCar.line_sensor_right());
   	control_I_odist(BatCar.ultrasonic_sensor_distance());

   	//TODO: here, call the lustre step
   	control_step();

   	//N.b. noting to do for outputs, they are sent
   	//to the lustre program via the _O_ procedures

		//TODO: (REAL TIME) 'sleep' until next peiod
		vTaskDelayUntil( &xLastWakeTime, PERIOD);

	}
}

//-------------------------------------------------------------
// Arduino setup and loop core
//-------------------------------------------------------------

void setup()
{
	Serial.begin(9600);     // Set Serial baud rate 9600


   // INIT SmartBatCar
   //TODO: put here the necessary initializations (see code/BatCar.h)
   BatCar.init_motors();
   BatCar.init_button();
   BatCar.init_buzzer();
   BatCar.init_lights();
   BatCar.init_line_sensors();
   BatCar.init_ultrasonic_sensor();

   //INIT LUSTRE CODE
   //TODO: put here the initialization of lustre program
   control_reset();

	//CREATE  TASK
	//TODO: fill with suitable parameters, and un-comment
	xTaskCreate(
		*control_task,    // pointer to task code (void *f(void*))
		"My BatCar Task", // name of the task (const char*) put what you want
		configMINIMAL_STACK_SIZE, // stack size, use this default value constant
		NULL, //Parameter to pass to control_task, not used
		1,    //Priority, not relevant for a single task
		NULL  //Where to store the created handler, not used
	);

	//TODO: start the FreeRTOS scheduler
	Serial.println("START TASKS");
	vTaskStartScheduler();

	//Dummy infinite loop => unreachable
	for (;;) ;
}

void loop()
{
	//empty: unreachable
}