//============================================================================
// This program is no complete, track the 'TODO' comments
//=============================================================================
#include <Arduino.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);
}

//-------------------------------------------------------------
// 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();

   Serial.println("START PROGRAM");
}

//A global step counter, 
unsigned long int step=0;

//TODO: Real-time support.
//      somewhere in procedure loop, put some code
//      that will make the behavior periodic,
//      with period close to PERIOD 
//      (PERIOD is in ms)
const int PERIOD = 20;

void loop()
{

   //TODO: Real-time support
   //      put some code here (if needed)
	unsigned long t0 = millis();

	//step counter, just for info...
   step ++;

   //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 support
   //      put some code here (if needed) 

   //get final time
   unsigned long t1 = millis();
   unsigned int diff = t1 - t0;

   if(diff > PERIOD)
      Serial.println("NOT REAL TIME !");
   else
      delay(PERIOD - diff); // wait
}