#include "U8g2lib.h"
#include <HCSR04.h>
#include "main.h"

U8G2_SSD1306_128X64_NONAME_1_HW_I2C u8g2(U8G2_R0); //[page buffer, size = 128 bytes]

// Funções das tarefas
void vTask_sensor_LCD(void *pvParameters);

//Queues
QueueHandle_t xQueue_distancia_sensor1;
QueueHandle_t xQueue_distancia_sensor2;
QueueHandle_t xQueue_distancia_sensor3;
QueueHandle_t xQueue_distancia_max;
QueueHandle_t xQueue_distancia_min;

portBASE_TYPE xStatus;

void setup() {

  int dist_sensor_1=1;
  int dist_sensor_2=1;
  int dist_sensor_3=1;
  int min_dist = 10;
  int max_dist = 55;

  Serial.begin(115200); // debug only

  // initialize label position data
  u8g2.setFont(u8g2_font_10x20_tf); // gamer digits font - manually generated, only includes digits
  u8g2.setColorIndex(1); // white color
	u8g2.begin();
	u8g2.setBitmapMode(1);

  //Criação das Queues
  xQueue_distancia_sensor1 = xQueueCreate(1, sizeof(int));
  xQueueSendToFront(xQueue_distancia_sensor1, &dist_sensor_1, 0);
  xQueue_distancia_sensor2 = xQueueCreate(1, sizeof(int));
  xQueueSendToFront(xQueue_distancia_sensor2, &dist_sensor_2, 0);
  xQueue_distancia_sensor3 = xQueueCreate(1, sizeof(int));
  xQueueSendToFront(xQueue_distancia_sensor3, &dist_sensor_3, 0);
  xQueue_distancia_max = xQueueCreate(1, sizeof(int));
  xQueueSendToFront(xQueue_distancia_max, &max_dist, 0);
  xQueue_distancia_min = xQueueCreate(1, sizeof(int));
  xQueueSendToFront(xQueue_distancia_min, &min_dist, 0);

	xTaskCreatePinnedToCore(vTask_sensor_LCD, "Task Sensor e LCD", 4096, NULL, 2, NULL, 1);
  
  // Definição dos pinos dos botoes como sendo INPUT_PULLUP e respetivas interrupcoes
  pinMode(BT_1, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(BT_1), vTask_BT1, RISING);  //chama a tarefa vTask_BT1 quando o pino digital BT_1 passa de HIGH para LOW

  pinMode(BT_2, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(BT_2), vTask_BT2, RISING);  //chama a tarefa vTask_BT2 quando o pino digital BT_2 passa de HIGH para LOW

  pinMode(BT_3, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(BT_3), vTask_BT3, RISING);  //chama a tarefa vTask_BT3 quando o pino digital BT_3 passa de HIGH para LOW

  pinMode(BT_4, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(BT_4), vTask_BT4, RISING);  //chama a tarefa vTask_BT4 quando o pino digital BT_4 passa de HIGH para LOW

}


void vTask_sensor_LCD(void *pvParameters){

  int dist_sensor_1=1;
  int dist_sensor_2=2;
  int dist_sensor_3=3;
  int max_dist=55;
  int min_dist=10;
  int dist_step_01=1; // distance for the first tile (there are 4 tiles for each sensor)
  int dist_step_02=2; // distance for the 2nd tile
  int dist_step_03=3; // distance for the 3rd tile
  int dist_step_04=4; // distance for the 4th tile
  
  for(;;){

    xStatus = xQueuePeek(xQueue_distancia_max, &max_dist, 0);
    if (xStatus != pdTRUE)
    Serial.println("Tarefa LOOP - Nao recebeu os dados da queue distancia maxima");
	
    xStatus = xQueuePeek(xQueue_distancia_min, &min_dist, 0);
    if (xStatus != pdTRUE)
    Serial.println("Tarefa LCD - Nao recebeu os dados da queue distancia minima");

    if(distanceSensor1.measureDistanceCm() > 55 ||  distanceSensor1.measureDistanceCm() > max_dist || distanceSensor1.measureDistanceCm() < 0){
      dist_sensor_1 = max_dist;
    } else dist_sensor_1 = distanceSensor1.measureDistanceCm();

    if(distanceSensor2.measureDistanceCm() > 55 ||  distanceSensor2.measureDistanceCm() > max_dist || distanceSensor2.measureDistanceCm() < 0){
      dist_sensor_2 = max_dist;
    } else dist_sensor_2 = distanceSensor2.measureDistanceCm();
    
    if(distanceSensor3.measureDistanceCm() > 55 ||  distanceSensor3.measureDistanceCm() > max_dist || distanceSensor3.measureDistanceCm() < 0){
      dist_sensor_3 = max_dist;
    } else dist_sensor_3 = distanceSensor3.measureDistanceCm();

    xStatus = xQueueOverwrite(xQueue_distancia_sensor1, &dist_sensor_1);
    if (xStatus != pdTRUE)
      Serial.println("Tarefa sensor - Nao enviou dados para a queue distancia sensor 1");

    xStatus = xQueueOverwrite(xQueue_distancia_sensor2, &dist_sensor_2);
    if (xStatus != pdTRUE)
      Serial.println("Tarefa sensor - Nao enviou dados para a queue distancia sensor 2");

    xStatus = xQueueOverwrite(xQueue_distancia_sensor3, &dist_sensor_3);
    if (xStatus != pdTRUE)
      Serial.println("Tarefa sensor - Nao enviou dados para a queue distancia sensor 3");
/*
    Serial.print("distacia sensor 1:");
    Serial.print(dist_sensor_1);
    Serial.println("cm");
  
    Serial.print("distacia sensor 2:");
    Serial.print(dist_sensor_2);
    Serial.println("cm");
   
    Serial.print("distacia sensor 3:");
    Serial.print(dist_sensor_3);
    Serial.println("cm");
    Serial.println(""); 

    delay(1000); 
*/

  u8g2.firstPage();
  do {
   
    u8g2.drawXBMP( 0, 0, 128, 64, bitmap_car_image); // car image on top of the display
    u8g2.drawXBMP( 0, 0, 128, 64, bitmap_sound_on); // sound on indicator - not doing anything for now
    u8g2.drawXBMP( 0, 0, 128, 64, bitmap_unit_cm);    // cm unit indicator - not doing anything for now    


    //SENSOR 1
    if (dist_sensor_1 >= 10) {
        u8g2.drawXBMP(0, 0, 128, 64, bitmap_sensor_01_a_on);
      } else u8g2.drawXBMP(0, 0, 128, 64, bitmap_sensor_01_a_off);

    if (dist_sensor_1 >= 21) {
        u8g2.drawXBMP(0, 0, 128, 64, bitmap_sensor_01_b_on);
      } else u8g2.drawXBMP(0, 0, 128, 64, bitmap_sensor_01_b_off);

    if (dist_sensor_1 >= 32) {
        u8g2.drawXBMP(0, 0, 128, 64, bitmap_sensor_01_c_on);
      } else u8g2.drawXBMP(0, 0, 128, 64, bitmap_sensor_01_c_off);

    if (dist_sensor_1 >= 43) {
        u8g2.drawXBMP(0, 0, 128, 64, bitmap_sensor_01_d_on);
      } else u8g2.drawXBMP(0, 0, 128, 64, bitmap_sensor_01_d_off);



    //SENSOR 2
    if (dist_sensor_2 >= 10) {
        u8g2.drawXBMP(0, 0, 128, 64, bitmap_sensor_02_a_on);
      } else u8g2.drawXBMP(0, 0, 128, 64, bitmap_sensor_02_a_off);

    if (dist_sensor_2 >= 21) {
        u8g2.drawXBMP(0, 0, 128, 64, bitmap_sensor_02_b_on);
      } else u8g2.drawXBMP(0, 0, 128, 64, bitmap_sensor_02_b_off);

    if (dist_sensor_2 >= 32) {
        u8g2.drawXBMP(0, 0, 128, 64, bitmap_sensor_02_c_on);
      } else u8g2.drawXBMP(0, 0, 128, 64, bitmap_sensor_02_c_off);

    if (dist_sensor_2 >= 43) {
        u8g2.drawXBMP(0, 0, 128, 64, bitmap_sensor_02_d_on);
      } else u8g2.drawXBMP(0, 0, 128, 64, bitmap_sensor_02_d_off);


    //SENSOR 3
    if (dist_sensor_3 >= 10) {
        u8g2.drawXBMP(0, 0, 128, 64, bitmap_sensor_03_a_on);
      } else u8g2.drawXBMP(0, 0, 128, 64, bitmap_sensor_03_a_off);

    if (dist_sensor_3 >= 21) {
        u8g2.drawXBMP(0, 0, 128, 64, bitmap_sensor_03_b_on);
      } else u8g2.drawXBMP(0, 0, 128, 64, bitmap_sensor_03_b_off);

    if (dist_sensor_3 >= 32) {
        u8g2.drawXBMP(0, 0, 128, 64, bitmap_sensor_03_c_on);
      } else u8g2.drawXBMP(0, 0, 128, 64, bitmap_sensor_03_c_off);

    if (dist_sensor_3 >= 43) {
        u8g2.drawXBMP(0, 0, 128, 64, bitmap_sensor_03_d_on);
      } else u8g2.drawXBMP(0, 0, 128, 64, bitmap_sensor_03_d_off);
                            
  } while ( u8g2.nextPage() );     
  }
}


void vTask_BT1(void) {

static unsigned long debounceMais = millis();
int max_dist = 55;
int min_dist = 10;

  if((millis()-debounceMais) > 1000){

    xStatus = xQueuePeekFromISR(xQueue_distancia_max, &max_dist);
    if (xStatus != pdTRUE)
      Serial.println("Tarefa BT1 - Nao recebeu os dados da queue distancia maxima");  

    xStatus = xQueuePeekFromISR(xQueue_distancia_min, &min_dist);
    if (xStatus != pdTRUE)
      Serial.println("Tarefa BT1 - Nao recebeu os dados da queue distancia minima");

    if (max_dist < 56 && max_dist > min_dist) {
      max_dist = max_dist + 5;
     } else {
      max_dist = 55;
      }

    xStatus = xQueueOverwrite(xQueue_distancia_max, &max_dist);
    if (xStatus != pdTRUE)
      Serial.println("Tarefa BT1 - Nao enviou dados para a queue distancia maxima");

  Serial.print("distancia max");
  Serial.print(max_dist);
  Serial.println("cm");
  Serial.println();
    }
  }
void vTask_BT2(void) {

static unsigned long debounceMenos = millis();
int max_dist = 55;
int min_dist=10;

  if((millis()-debounceMenos) > 1000){

    xStatus = xQueuePeekFromISR(xQueue_distancia_max, &max_dist);
    if (xStatus != pdTRUE)
      Serial.println("Tarefa BT2 - Nao recebeu os dados da queue distancia maxima");

    xStatus = xQueuePeekFromISR(xQueue_distancia_min, &min_dist);
    if (xStatus != pdTRUE)
      Serial.println("Tarefa BT2 - Nao recebeu os dados da queue distancia minima"); 

    if (max_dist <= 55 && max_dist > min_dist) {
      max_dist = max_dist - 5;
    } else {
      max_dist = 55;
     }

    xStatus = xQueueOverwrite(xQueue_distancia_max, &max_dist);
    if (xStatus != pdTRUE)
      Serial.println("Tarefa BT2 - Nao enviou dados para a queue distancia maxima");

  Serial.print("distancia max");
  Serial.print(max_dist);
  Serial.println("cm");
  Serial.println();
    }
  }

void vTask_BT3(void) {

static unsigned long debounceMais = millis();
int max_dist = 55;
int min_dist = 10;

  if((millis()-debounceMais) > 1000){

    xStatus = xQueuePeekFromISR(xQueue_distancia_min, &min_dist);
    if (xStatus != pdTRUE)
      Serial.println("Tarefa BT3 - Nao recebeu os dados da queue distancia minima");

    xStatus = xQueuePeekFromISR(xQueue_distancia_max, &max_dist);
    if (xStatus != pdTRUE)
      Serial.println("Tarefa BT3 - Nao recebeu os dados da queue distancia maxima");

    if (min_dist >= 5 && min_dist < max_dist) {
      min_dist = min_dist + 5;

    } else {
      min_dist = 10;
    
    }

  xStatus = xQueueOverwrite(xQueue_distancia_min, &min_dist);
    if (xStatus != pdTRUE)
      Serial.println("Tarefa BT3 - Nao enviou dados para a queue distancia minima");

  Serial.print("distancia min");
  Serial.print(min_dist);
  Serial.println("cm");
  }
}

void vTask_BT4(void) {

static unsigned long debounceMenos = millis();
int max_dist = 55;
int min_dist = 10;

  if((millis()-debounceMenos) > 1000){

    xStatus = xQueuePeekFromISR(xQueue_distancia_min, &min_dist);
    if (xStatus != pdTRUE)
      Serial.println("Tarefa BT4 - Nao recebeu os dados da queue distancia minima");

    xStatus = xQueuePeekFromISR(xQueue_distancia_max, &max_dist);
    if (xStatus != pdTRUE)
      Serial.println("Tarefa BT4 - Nao recebeu os dados da queue distancia maxima");

    if (min_dist > 5 && min_dist < max_dist) {
      min_dist = min_dist - 5;
    
    } else {
      min_dist = 10;
    
    }

  xStatus = xQueueOverwrite(xQueue_distancia_min, &min_dist);
    if (xStatus != pdTRUE)
      Serial.println("Tarefa BT4 - Nao enviou dados para a queue distancia minima");

  Serial.print("distancia min");
  Serial.print(min_dist);
  Serial.println("cm");
  }
}



void loop() {

  int dist_sensor_1=1;
  int dist_sensor_2=2;
  int dist_sensor_3=3;
  int max_dist=55;
  int min_dist=10;

  xStatus = xQueuePeek(xQueue_distancia_sensor1, &dist_sensor_1, 0);
    if (xStatus != pdTRUE)
    Serial.println("Tarefa LOOP - Nao recebeu os dados da queue distancia sensor 1");

    xStatus = xQueuePeek(xQueue_distancia_sensor2, &dist_sensor_2, 0);
    if (xStatus != pdTRUE)
    Serial.println("Tarefa LOOP - Nao recebeu os dados da queue distancia sensor 2");

    xStatus = xQueuePeek(xQueue_distancia_sensor3, &dist_sensor_3, 0);
    if (xStatus != pdTRUE)
    Serial.println("Tarefa LOOP - Nao recebeu os dados da queue distancia sensor 3");

    xStatus = xQueuePeek(xQueue_distancia_max, &max_dist, 0);
    if (xStatus != pdTRUE)
    Serial.println("Tarefa LOOP - Nao recebeu os dados da queue distancia maxima");
	
    xStatus = xQueuePeek(xQueue_distancia_min, &min_dist, 0);
    if (xStatus != pdTRUE)
    Serial.println("Tarefa LCD - Nao recebeu os dados da queue distancia minima");

  Serial.print("distacia sensor 1:");
    Serial.print(dist_sensor_1);
    Serial.println("cm");
  
    Serial.print("distacia sensor 2:");
    Serial.print(dist_sensor_2);
    Serial.println("cm");
   
    Serial.print("distacia sensor 3:");
    Serial.print(dist_sensor_3);
    Serial.println("cm");
    Serial.println(""); 

    delay(1000);

}