/*Include Libs*/
#include <DHT.h>
#include <ESP32Servo.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

/*GPIO Def*/
#define cb_dht 23
#define cb_mq2 4
#define coi_quat 19
#define echo_sr04 34               
#define trig_sr04 25
#define mo_cua 26

Servo servo_mo_cua;
LiquidCrystal_I2C lcd(0x27, 16, 2);

//==============Task Def=============
void TaskReadDHT(void *pvParam);
void TaskLCD(void *pvParam);

//============QUEUE==========
QueueHandle_t QueueTemp, QueueHumi, QueueMQ2, QueueSR04;

TaskHandle_t DHTTask, MQ2Task, SR04Task, LCDTask;

void setup() {
  Serial.begin(115200);
  pinMode(coi_quat, OUTPUT);
  digitalWrite(coi_quat, LOW); 
  pinMode(cb_dht, INPUT);   
  pinMode(cb_mq2, INPUT);
  pinMode(trig_sr04, OUTPUT);
  pinMode(echo_sr04, INPUT);
  lcd.init();                    
  lcd.backlight(); 
  servo_mo_cua.attach(mo_cua);
  
  lcd.setCursor(0, 0);
  lcd.print("Wait a few sec!!");
  Serial.println("Wait a few seconds for the system to boot!");
  delay(5000);
  lcd.clear();

  QueueTemp = xQueueCreate(1, sizeof(float));
  QueueHumi = xQueueCreate(1, sizeof(float));
  QueueMQ2 = xQueueCreate(1, sizeof(int));
  QueueSR04 = xQueueCreate(1, sizeof(int));
  
  xTaskCreatePinnedToCore(TaskReadDHT
                          , "ReadDHT"
                          , 1024*2
                          , NULL
                          , 2
                          , &DHTTask
                          , ARDUINO_RUNNING_CORE);

  xTaskCreatePinnedToCore(TaskReadMQ2
                          , "ReadMQ2"
                          , 1024*2
                          , NULL
                          , 2
                          , &MQ2Task
                          , ARDUINO_RUNNING_CORE);

  xTaskCreatePinnedToCore(TaskReadSR04
                          , "ReadSR04"
                          , 1024*2
                          , NULL
                          , 2
                          , &SR04Task
                          , ARDUINO_RUNNING_CORE);

  xTaskCreatePinnedToCore(TaskLCD
                          , "LCD"
                          , 1024*2
                          , NULL
                          , 3
                          , &LCDTask
                          , ARDUINO_RUNNING_CORE);
}

void loop() {}

void TaskReadDHT (void *pvParam){
  float oldtemp = 0;
  float oldhumi = 0;
  float humi, temp;

  DHT dht(cb_dht, DHT22);
  dht.begin();

  for (;;){
    temp = dht.readTemperature();
    humi = dht.readHumidity();

    if (isnan(temp) || isnan(humi)){
      Serial.println("DHT22 Failed to Read!!!");
      temp = oldtemp;
      humi = oldhumi;
    }
    else {
      oldtemp = temp;
      oldhumi = humi;
    }

    xQueueOverwrite(QueueTemp, &temp);
    xQueueOverwrite(QueueHumi, &humi);

    Serial.println("Temp: " + String(temp) + "C");
    Serial.println("Humi: " + String(humi) + "%");

    vTaskDelay(1000 / portTICK_PERIOD_MS);
  }
}

void TaskReadMQ2 (void *pvParam){
  int gas_val = 0;

  for(;;){
    gas_val = analogRead(cb_mq2);

    xQueueOverwrite(QueueMQ2, &gas_val);

    Serial.println("Gas Value: " + String(gas_val));

    vTaskDelay(1000 / portTICK_PERIOD_MS);
  }
}

void TaskReadSR04 (void *pvParam){
  int sr04_val = 0;

  for(;;){
    digitalWrite(trig_sr04, LOW);
    delayMicroseconds(2);
    digitalWrite(trig_sr04, HIGH);
    delayMicroseconds(2);
    digitalWrite(trig_sr04, LOW);
    sr04_val = pulseIn(echo_sr04, HIGH) / 58.2;
    
    xQueueOverwrite(QueueSR04, &sr04_val);

    Serial.println("HC-SR04 Value: " + String(sr04_val));
    Serial.println("==============================");
    vTaskDelay(1000 / portTICK_PERIOD_MS);
  }
}

void TaskLCD (void *pvParam){
  static float temp, humi;
  static int gas_val, sr04_val;

  for (;;){    
    xQueuePeek(QueueTemp, &temp, portMAX_DELAY);
    lcd.setCursor(0, 0);
    lcd.print("T:" + String(temp, 1) + "C");

    xQueuePeek(QueueHumi, &humi, portMAX_DELAY);
    lcd.setCursor(9, 0);
    lcd.print("H:" + String(humi, 1) + "%");

    xQueuePeek(QueueMQ2, &gas_val, portMAX_DELAY);
    lcd.setCursor(0, 1);
    lcd.print("Gas:" + String(gas_val));

    xQueuePeek(QueueSR04, &sr04_val, portMAX_DELAY);
    lcd.setCursor(9, 1);
    lcd.print("SR:" + String(sr04_val));

    vTaskDelay(1000 / portTICK_PERIOD_MS);
    lcd.clear();
  }
}