//============================================================================
// Two-tasks implementation
// Urgent task = liner
// Slow task = uturn
// Deterministic communication version:
// - Since Fast task (liner) has priority, it is in charge
//   to manage deterministic communication 
//=============================================================================
#include <Arduino_FreeRTOS.h>
#include <Arduino.h>
#include "code/FreeRTOS_Utils.h"
#include "code/BatCar.h"
#include "liner.h"
#include "uturn.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)
//-------------------------------------------------------------

// - liner is executed RATIO times often than uturn 
// - step counts modulo RATIO 
// - uturn_clock is true when step=0
#define RATIO 2 
int step = 0;
bool uturn_clock;

//TODO: declare here the variables necessary for the task communication
// uturn -> liner
// double-buffer

//Where uturn writes MUST BE WELL INITIALIZED
bool buff_do_turn = false;
int buff_mlt = 0;
int buff_mrt = 0;
//Where liner reads:
bool do_turn;
int mlt;
int mrt;

// liner -> uturn variables,
// initialization not necessary (priority)
bool running, ww, wb, bw, bb;

//TODO: define here the output functions of liner task...
//... to BatCar
void uturn_O_buzzer(bool v) { BatCar.set_buzzer(!v); }
//... to liner (bufferized)
void uturn_O_do_turn(bool v) { buff_do_turn=v; }
void uturn_O_mlt(int v) { buff_mlt=v; }
void uturn_O_mrt(int v) { buff_mrt=v; }

//TODO: define here the output functions of liner task
//... to BatCar
void liner_O_motor_left(int s){ BatCar.set_left_speed(s); }
void liner_O_motor_right(int s){ BatCar.set_right_speed(s); }
void liner_O_red_light(bool v){ BatCar.set_red_light(v); }
void liner_O_yellow_light(bool v){ BatCar.set_yellow_light(v); }
void liner_O_green_light(bool v){ BatCar.set_green_light(v); }
//... to uturn
void liner_O_running(bool v){ running = v; }
void liner_O_ww(bool v){ if (uturn_clock) ww = v; }
void liner_O_wb(bool v){ if (uturn_clock) wb = v; }
void liner_O_bw(bool v){ if (uturn_clock) bw = v; }
void liner_O_bb(bool v){ if (uturn_clock) bb = v; }

//-------------------------------------------------------------
// FreeRTOS tasks
// - 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 FAST_PERIOD 1 
void liner_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(;;){
		uturn_clock = (step == 0);
		step = (step+1)%RATIO;

   	//TODO: here, sample (read) necessary input from the SBC
   	//      and 'send' it the lustre program
   	liner_I_k1(BatCar.button_pressed());
   	liner_I_sensor_left(BatCar.line_sensor_left());
   	liner_I_sensor_right(BatCar.line_sensor_right());
   	//TODO: here, sample (read) necessary input from uturn task
   	//      and 'send' it the lustre program
		if (uturn_clock){
			//swap buffers
			do_turn = buff_do_turn;
			mlt = buff_mlt;
			mrt = buff_mrt;
		}
   	liner_I_do_turn(do_turn);
   	liner_I_ml_turn(mlt);
   	liner_I_mr_turn(mrt);

   	//TODO: here, call the lustre step
   	liner_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, FAST_PERIOD);

	}
}

#define SLOW_PERIOD (FAST_PERIOD*RATIO) 
void uturn_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
   	uturn_I_odist(BatCar.ultrasonic_sensor_distance());
   	//TODO: here, sample (read) necessary input from liner task
   	//      and 'send' it the lustre program
		uturn_I_running(running);
		uturn_I_ww(ww);
		uturn_I_wb(wb);
		uturn_I_bw(bw);
		uturn_I_bb(bb);

   	//TODO: here, call the lustre step
   	uturn_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, SLOW_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
   liner_reset();
   uturn_reset();

	//CREATE  TASK
	//TODO: create liner task with suitable priority
	xTaskCreate(
		*liner_task,    // pointer to task code (void *f(void*))
		"BatCar Liner 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, must be the highest
		NULL  //Where to store the created handler, not used
	);
	//TODO: create uturn task with suitable priority
	xTaskCreate(
		*uturn_task,    // pointer to task code (void *f(void*))
		"BatCar Uturn 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
		2,    //Priority, must be the lowest
		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
}