#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 (25)

#define SWITCH_S1 (25)
#define BUTTON_START digitalRead(SWITCH_S1)==1

#define DHT_PIN  (23) //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)

#define PIR_PIN (35)

#define LIGHT_SENSOR_PIN (32)
#define ADC_READ_A1 analogRead(LIGHT_SENSOR_PIN)


#define ADMax (4095)
#define ADMin (0)
#define PWMMin (0)
#define PWMMAX (255)
#define Dark (40)
#define Dim (800)
#define Light (2000)
#define Bright (3200)

#define SEREEN_WIDTH (128)
#define SEREEN_HEIGHT (64)

#define OLED_RESET
Adafruit_SSD1306 OLED(SEREEN_WIDTH,SEREEN_HEIGHT, & Wire OLED_RESET);


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
unsigned int lastime =0;
unsigned int even =0;

volatile float Temp = 0;
volatile float Humi = 0;
volatile unsigned int ADval[4];

unsigned int duration = 0;
unsigned int distance = 0;
volatile unsigned int Valdur[4];

unsigned int StateCurrent=0;
unsigned int StatePrevious=0;

// ---- 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) 
{
  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 = 500;
  T2_ms = 500;
  T3_ms = 1000;
  T4_ms = 500;
  T5_ms = 500;
  T6_ms = 500;
  T7_ms = 500;

  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(;;)
  {
    vTaskDelayUntil(&xLastWakeTime,T0ticks);
  }

  vTaskDelete(NULL);

}

void task1(void *paremeter)
{
  for(;;)
  {
    StatePrevious = StateCurrent;
    StatePrevious = digitalRead(PIR_PIN);
    

    if(StatePrevious==LOW && StateCurrent==HIGH)
    {
      Sprintln("Motion detectet");
      digitalWrite(LED_1, HIGH);
    }
    else if(StatePrevious==HIGH && StateCurrent==LOW)
    {
      Sprintln("Motion detectet");
      digitalWrite(LED_1, LOW);
    }
    vTaskDelay(T1ticks);
  }
  vTaskDelete(NULL);
}

void task2(void *paremeter)
{
  for(;;)
  {
    int analogValue = ADC_READ_A1;   // 2^12 = 4096
    /////

    // We'll have a few threshholds, qualitatively determined
    if (analogValue < 40) 
    {
      Sprintln(" => Dark");
    } 
    else if (analogValue < 800) 
    {
      Sprintln(" => Dim");
    } 
    else if (analogValue < 2000) 
    {
      Sprintln(" => Light");
    } 
    else if (analogValue < 3200) 
    {
      Sprintln(" => Bright");
    } 
    else 
    {
      Sprintln(" => Very bright");
    }
    
    int AdjustLED = map(analogValue,0,4095,0,255);    
    digitalWrite(LED_2,AdjustLED);

    vTaskDelay(T2ticks);
  }
  vTaskDelete(NULL);
}

void task3(void *paremeter)
{
  for(;;)
 {
 //Sprint("Task 3 count : ");
 //Sprintln(count_3 ++);
 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 *paremeter)
{
  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(adcval);
   Sprint("%");

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

void task5(void *paremeter)
{
  for(;;)
 {
    //Sprint("Task 5 count : ");
    //(count_5 ++);

    if(BUTTON_START)
    {
      long now = millis();
      if(now - lastime > 5)
      {
        lastime = 0;

        if(even ==0)
        {
          even = 1;
        }
        else if (even ==1)
        {
          even = 0;
        }
      }
    }
 
    vTaskDelay(T5ticks);
 }
vTaskDelete(NULL);
}

void task6(void *paremeter)
{

  for(;;)
 {
   
    LCD.setCursor(0,0);
  LCD.print("Temp:" + String(Temp) + " c");
  LCD.setCursor(0,1);
  LCD.print("Humidity" + String(Humi) + " %");

  if(even)
  {
    OLED.Display();
  }
  else
  {
    OLED.clearDisplay();
    OLED.setTextColor(WHITE,BLCK);
    OLED.setCursor(2,8);
    OLED.print("distance:"+ String(distance) + "Cm");
  }
    vTaskDelay(T6ticks);
 }
vTaskDelete(NULL);
}

void task7(void *paremeter)
{
  for(;;)
 {
    digitalWrite(TRIG_ULS_PIN, LOW);   //Clear triger signal
    delayMicroseconds(2);

    digitalWrite(TRIG_ULS_PIN, HIGH);  //send triger signal
    delayMicroseconds(10);

    digitalWrite(TRIG_ULS_PIN, LOW);  //Clear triger signal

    duration = pulseIn(ECHO_ULS_PIN, HIGH);   //Responding signal data
    // 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.034 /2 
    distance= ValDuration*0.034/2;    //Gain Duration*0.034/2

    Sprint("Distance : ");
    Sprint(distance);
    Sprintln(" cm");

    vTaskDelay(T7ticks);
 }
vTaskDelete(NULL);
}

void setup()
{
  Serial.begin(9600);
  pinMode(LED_1, OUTPUT);
  pinMode(LED_2, OUTPUT);
  pinMode(SWITCH_S1, INPUT);
  pinMode(ADC_READ_A0, INPUT);

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

  pinMode(PIR_PIN, OUTPUT);
  pinMode(LIGHT_SENSOR_PIN, INPUT);

  dhtSensor.setup(DHT_PIN, DHTesp::DHT22);

  freertos_init();
  LCD.init();
  LCD.backlight();
  LCD.setCursor(0,0);
  LCD.print("Hellow world");
  LCD.setCursor(0,1);
  LCD.print("ice");
  delay(2000);
  
  if (!OLED.begin(SSD1306_SWITCHAPVCC, 0x3C))
  {
    Serial.println(F("failed to start SSD1306 OLED"));
    while(1);
  }
  delay(2000);
  OLED.setCursor(0,0);
  OLED.clearDisplay();
  OLED.setTextColor(WHITE,BLCK);
  
  OLED.setCursor(2,8);
  OLED.print("Hellow world");
  OLED.setCursor(2,16);
  OLED.print("Mechatroincs Engineering");
  OLED.Display();
}


void loop() 
{
}
void OLED_Display(void)
{
  // draw a circle
  OLED.clearDisplay();
  OLED.drawCircle(20, 35, 20, WHITE);
  OLED.display();
  //delay(1);
  delayMicroseconds(2);

  // fill a circle
  OLED.clearDisplay();
  OLED.fillCircle(20, 35, 20, WHITE);
  OLED.display();
  //delay(1);
  delayMicroseconds(2);