#include <Arduino.h>
#include "DHTesp.h"
#include <Arduino.h>
#include <DHTesp.h>
#include <LiquidCrystal_I2C.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <SPI.h>


#define Sprintln(a) (Serial.println(a))
#define Sprint(a)   (Serial.print(a))
#define ADC_READ_A0 analogRead(34)
#define DHT_PIN  (15) //D15 gpio004 esp32
#define LED_1 (2)     //Pin D2
#define LED_2 (4)     //Pin D4

#define ECHO_ULS_PIN (18)
#define TRIG_ULS_PIN  (19)


unsigned int count_0 = 0; //increment cnt ++
unsigned int count_1 = 0;   
unsigned int count_2 = 0;  
unsigned int count_3 = 0;
unsigned int count_4 = 0;
unsigned int count_5 = 0;  
unsigned int count_6 = 0; 
unsigned int count_7 = 0;


unsigned char Blink_led_1 = 0;  //LED_1 Toggle
unsigned char Blink_led_2 = 0;  //LED_2 Toggle

//----HC-SR04 Ultrasonic Sensor-----
unsigned int duration = 0;
unsigned int distance = 0;
volatile unsigned int Valdur[4]; //Average 4 samples
//Temp Humi//
volatile float Temp =0;
volatile float Humi =0;
volatile unsigned int ADval[4];

// ---- limits for sampling period -----------
const float T0MAX = 1;      // controller (sec)
const float T0MIN = 0.001;
const int T0MSMAX = 1000;   // controller (ms)
const int T0MSMIN = 1;
const int T1MSMAX = 1000;   // serial bulk communication (ms)
const int T1MSMIN = 1;
const int T2MSMAX = 1000;   // OLED
const int T2MSMIN = 1;
const int T3MSMAX = 1000;   // RGB LED
const int T3MSMIN = 1;
const int T4MSMAX = 1000;   // command 
const int T4MSMIN = 1;
const int T5MSMAX = 10000;  // NETPIE freeboard
const int T5MSMIN = 1;
const int T6MSMAX = 20000;  // NETPIE feed
const int T6MSMIN = 1;
const int T7MSMAX = 5000;  // NETPIE feed
const int T7MSMIN = 1;

float T = 0.08;  // sampling period
float tdata = 0; // time data sent to output

int T0_ms, T1_ms, T2_ms, T3_ms, T4_ms, T5_ms, T6_ms, T7_ms;
int T0ticks, T1ticks, T2ticks, T3ticks, T4ticks, T5ticks, T6ticks, T7ticks;
int Tloop_ms = 500;   // loop period

// -----------FreeRTOS handles and flags-----------------
TaskHandle_t xTask0h = NULL, xTask1h = NULL, xTask2h = NULL, xTask3h = NULL, xTask4h = NULL;
TaskHandle_t xTask5h = NULL, xTask6h = NULL, xTask7h = NULL;

QueueHandle_t xQueue_y = NULL, xQueue_u = NULL;

bool xQueueOK;

portMUX_TYPE myMutex = portMUX_INITIALIZER_UNLOCKED;

DHTesp dhtSensor;

LiquidCrystal_I2C LCD = LiquidCrystal_I2C(0x27, 16,2);


/**************************************************************************
// create tasks and queues
  //* Create the task, storing the handle. */
  //xReturned = xTaskCreate(
  //vTaskCode,        /* Function that implements the task. */
  //"NAME",           /* Text name for the task. */
  //STACK_SIZE,       /* Stack size in words, not bytes. */
  //( void * ) 1,     /* Parameter passed into the task. */
  //tskIDLE_PRIORITY, /* Priority at which the task is created. */
  //&xHandle );       /* Used to pass out the created task's handle. */
/***************************************************************************/
void freertos_init(void) 
{

  /*******************************************/
    LCD.init();         // initial LCD on lib
    LCD.backlight();    // Blink LCD backlight on
    LCD.setCursor(0,0); // Clear LCD display
    
    LCD.print("Hello world");
    LCD.setCursor(0,1);
    LCD.print("Mechatronics Eng");

    delay (2000);

  xQueueOK = 0;    // start wit false
  xQueue_y = xQueueCreate(1000,sizeof(float));
  xQueue_u = xQueueCreate(1000,sizeof(float));  
  
  if ((xQueue_y == NULL)|(xQueue_u == NULL)) 
      Serial.println("Error creating the queue");
  else  xQueueOK = 1;
  
  T0_ms = 1000*T;
  T0ticks = pdMS_TO_TICKS(T0_ms);  // period of Task0 in number of ticks
  
  // initialize delays to tasks
  T1_ms = 1000;
  T2_ms = 1000;
  T3_ms = 1000;
  T4_ms = 1000;
  T5_ms = 1000;
  T6_ms = 1000;
  T7_ms = 1000;

  T1ticks = pdMS_TO_TICKS(T1_ms);
  T2ticks = pdMS_TO_TICKS(T2_ms);
  T3ticks = pdMS_TO_TICKS(T3_ms);
  T4ticks = pdMS_TO_TICKS(T4_ms);
  T5ticks = pdMS_TO_TICKS(T5_ms);   
  T6ticks = pdMS_TO_TICKS(T6_ms);    
  T7ticks = pdMS_TO_TICKS(T7_ms);     
   
  xTaskCreatePinnedToCore(Task0, "Task 0", 10000, NULL, 1, &xTask0h, 0);
  xTaskCreatePinnedToCore(Task1, "Task 1", 10000, NULL, 1, &xTask1h, 1); 
  xTaskCreatePinnedToCore(Task4, "Task 2", 10000, NULL, 2, &xTask4h, 1); 
    
   // the following tasks may be suspended if flags are set to 0
  xTaskCreatePinnedToCore(Task2, "Task 3", 10000, NULL, 1, &xTask2h, 0); 
  xTaskCreatePinnedToCore(Task3, "Task 4", 10000, NULL, 2, &xTask3h, 1);   
  xTaskCreatePinnedToCore(Task5, "Task 5", 10000, NULL, 1, &xTask5h, 1);   
  xTaskCreatePinnedToCore(Task6, "Task 6", 10000, NULL, 2, &xTask6h, 1);  
  xTaskCreatePinnedToCore(Task7, "Task 7", 10000, NULL, 1, &xTask7h, 1);

}


void Task0( void * parameter)
{
  TickType_t xLastWakeTime;
  xLastWakeTime = xTaskGetTickCount();

  for(;;)
  {

    /********************************/
    StatePrevious = StateCurrent;       // set current time
    StateCurrent = digitalRead(PIR_PIN);// Read current state with PIR Sensor

   // if(!StatePrevious && StateCurrent)
    if(StatePrevious==LOW && StateCurrent == HIGH)
    {
      Sprintln("Motion detected");
      digitalWrite(LED_1,HIGH);
    }
    else if(StatePrevious==HIGH && StateCurrent == LOW)
    {
      Sprintln("Motion stopped !");
      digitalWrite(LED_1,LOW);
    }
  }

  vTaskDelete(NULL);
}

void Task1(void *parameter) 
{
  for(;;)
  {
    Blink_led_1 ^= 1;
    digitalWrite(LED_1,Blink_led_1);

    Sprint("Task 1 count : ");
    Sprintln(count_0 ++);

    vTaskDelay(T1ticks/ portTICK_PERIOD_MS);
  }
  vTaskDelete(NULL);
}
void Task2(void *parameter) 
{
  for(;;)
  {
    Blink_led_2 ^= 1;
    digitalWrite(LED_2,Blink_led_2);

    Sprint("Task 2 count : ");
    Sprintln(count_1 ++);

    vTaskDelay(T2ticks*2/ portTICK_PERIOD_MS);
  }
  vTaskDelete(NULL);
}

void Task3( void * parameter)
{
  for(;;)
  {
    TempAndHumidity data = dhtSensor.getTempAndHumidity();
    
    Temp = data.temperature;
    Humi = data.humidity;

    Sprintln("Temp: " + String(data.temperature, 2) + "C");
    Sprintln("Humidity: " + String(data.humidity, 1) + "%");
    Sprintln("---");

    vTaskDelay(T3ticks);
  }
  vTaskDelete(NULL);
}
void Task4( void * parameter)
{
  for(;;)
  {
    int AdjustVR = map(ADC_READ_A0,0,4095,0,100);

    ADval[3] = ADval[2];
    ADval[2] = ADval[1];
    ADval[1] = ADval[0];
    ADval[0] = AdjustVR;

    int adcval = (ADval[3]+ADval[2]+ADval[1]+ADval[0])>>2;

    Sprint("Task 4 AdjustVR : ");
    Sprint(AdjustVR);
    Sprintln(" %");

    vTaskDelay(T4ticks);
  }
  vTaskDelete(NULL);
}

void Task5(void *parameter) 
{
  for(;;)
  {

    Sprint("Task 5 count : ");
    Sprintln(count_5 ++);

    vTaskDelay(T5ticks/ portTICK_PERIOD_MS);
  }
  vTaskDelete(NULL);
}

void Task6(void *parameter) 
{
  for(;;)
  {
    //  LED.clear();
    LCD.setCursor(0,0);
    LCD.print("Temp:" + String(Temp) + " C");
    LCD.setCursor(0,1);
    LCD.print("Humidity: " + String(Humi) + "%");

    LED.print("Temp:" + String(Temp) + " C");
    LCD.setCursor(0,1);
    LCD.print("Mechatronics Eng");

    Sprint("Task 6 count : ");
    Sprintln(count_6 ++);

    vTaskDelay(T6ticks/ portTICK_PERIOD_MS);
  }
  vTaskDelete(NULL);
}

void Task7(void *parameter) 
{
  for(;;)
  {
    digitalWrite(TRIG_ULS_PIN, LOW);
    delayMicroseconds(2);

    digitalWrite(TRIG_ULS_PIN, HIGH);
    delayMicroseconds(10);

    digitalWrite(TRIG_ULS_PIN, LOW);
    
    duration = pulseIn(ECHO_ULS_PIN, HIGH);
    // Moving average of 4 samples
    Valdur[3] = Valdur[2];
    Valdur[2] = Valdur[1];
    Valdur[1] = Valdur[0];
    Valdur[0] = duration;   // read new analog input
                            // average of 4 samples
    int ValDuration = (Valdur[3]+Valdur[2]+Valdur[1]+Valdur[0])>>2;
                                      // V = 340 m/s {V = 0.034 cm/us}
                                      // T = distance(S) / speed(V)
                                      // S = S/V = T * 0.34/2 
   distance= ValDuration*0.034/2;                      
   
   Sprint("Distance : ");
    Sprint(distance);
    Sprintln(" cm");
   
  }
  vTaskDelete(NULL);
}

void setup()
{
  Serial.begin(9600);

  pinMode(LED_1, OUTPUT);
  pinMode(LED_2, OUTPUT);
  
  

 

  pinMode(ECHO_ULS_PIN, INPUT);
  pinMode(TRIG_ULS_PIN, OUTPUT);


  

  pinMode(ADC_READ_A0, INPUT);
  
  dhtSensor.setup(DHT_PIN, DHTesp::DHT22);

  freertos_init();
}

void loop()
{ 
  
}