#include <LiquidCrystal.h>
#include <Arduino_FreeRTOS.h>
#include <queue.h>

struct distance{
  float cm;
};
QueueHandle_t my_queue;

//Display
LiquidCrystal lcd(12, 11, 10, 9, 8, 7);
//Sets pin 2 to measure the high pulse length to get the distance
#define ECHO_PIN 2
//Sets pin 3 to send a pulse to start the measurement
#define TRIG_PIN 3
//Sets pin A0 as the input for the buzzer
int buzzer = A0;

void setup() {
  Serial.begin(9600);
  pinMode(13, OUTPUT);
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  my_queue = xQueueCreate(1, sizeof(struct distance));
  if(my_queue == NULL){
    Serial.println("Queue cannot be created!");
  }
  
  xTaskCreate(lcd_task,"LCD Task",128,NULL,3,NULL);
  xTaskCreate(read_dist,"Read Distance Task",128,NULL,2,NULL);
  xTaskCreate(output_task,"Output Task",128,NULL,2,NULL);
}

void loop(){
  //Empty loop
}

//Task that sets up the lcd and deletes itself after it's complete
void lcd_task(void *pvParameters){
  lcd.begin(16, 2);
  while(1){
    lcd.print("Dist.= ");
    lcd.setCursor(14,0);
    lcd.print("cm");
    vTaskDelete(NULL);
  }
}

//Read distance task
void read_dist(void *pvParameters){
  struct distance x;
  int data;
  TickType_t xLastWakeTime = xTaskGetTickCount();
  for(;;){
    //Pulses sent to the TRIG_PIN and ECHO_PIN for the measurement process
    digitalWrite(TRIG_PIN, LOW);
    delayMicroseconds(2);
    digitalWrite(TRIG_PIN, HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIG_PIN, LOW);
    data = pulseIn(ECHO_PIN, HIGH);
    x.cm = data/58; //Data is converted into centimeters
    xQueueSend(my_queue,&x,portMAX_DELAY); //Value x is sent to the queue
    vTaskDelayUntil(&xLastWakeTime,(1000/portTICK_PERIOD_MS));
  }
}

//Output task that lights LED, sounds the buzzer, and prints the distance
void output_task(void *pvParameters){
  struct distance x;
  while(1){
    //Task receives the value of x fromt the queue
    if(xQueueReceive(my_queue,&x,portMAX_DELAY)==pdPASS){
      bool withinRange = x.cm < 50;
      //Lights up LED if object is within range
      digitalWrite(LED_BUILTIN, withinRange);
      //Triggers the buzzer if object is within range
      if(withinRange == true)
      {
        PORTC = 0b01111111;
        tone(A0, 262, 250);
      }
      else
      {
        PORTC = 0b10000000;
        noTone(buzzer);
      }
      lcd.setCursor(7,0);
      lcd.print(x.cm,2);
    }
  }
}