#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 VR_PIN (34) 

#define ADC_READ_A0 analogRead(VR_PIN) //D15 gpio004 esp32

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

#define DHT_PIN  (23) //D23 gpio004 esp32

#define LED_1 (2)     //Pin 
#define LED_2 (4)     //Pin D4

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

#define PIR_PIN (35) // Pin D35

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

#define ADMax (4095)
#define ADMin (0)
#define PWMMax (255)
#define PWMMin (0)
#define PerMin (0) 
#define PerMax (100)

#define DARK (40)
#define DIM  (800)
#define LIGHT (2000)
#define BRIGHT (3200)

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 even = 0;
unsigned long lastime = 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 (usec)
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);
//LiquidCrystal_I2C LCD = LiquidCrystal_I2C(0x3f, 16,2);

#define SCREEN_WIDTH (128)
#define SCREEN_HEIGHT (64)  

#define OLED_RESET (-1)
Adafruit_SSD1306 OLED(SCREEN_WIDTH , SCREEN_HEIGHT, & Wire, OLED_RESET);

/**************************************************************************
// 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 = 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(;;)
  {
  //Sprint("Task 0 count : ");
  //Sprintln(count_0++);
  vTaskDelayUntil(&xLastWakeTime, T0ticks); // set period
  }
vTaskDelete(NULL);
}

void Task1(void *parameter) 
{
  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);
    }
    
  vTaskDelay(T1ticks);
  }
vTaskDelete(NULL);
}

void Task2(void *parameter) 
{
  for(;;)
  {
   int analogValue = ADC_READ_A1;   // 2^12 = 4096
    
    // We'll have a few threshholds, qualitatively determined

    if (analogValue < DARK) // #define DARK (40)
    {
      Sprintln(" => Dark");
    } 
    else if (analogValue < DIM) //#define DIM (800)
    {
      Sprintln(" => Dim");
    } 
    else if (analogValue < LIGHT) //#define LIGHT (2000)
    {
      Sprintln(" => Light");
    } 
    else if (analogValue < 3200)  //#define BRIGHT (3200)
    {
      Sprintln(" => Bright");
    } 
    else 
    {
      Sprintln(" => Very bright");
    }
    /*
     #define ADMax 4095
     #define ADMin 0
     #define PWMMax 255
     #define PWMMin 0
     */
    int AdjustLED = map(analogValue,ADMin,ADMax,PWMMin,PWMMax);

    digitalWrite(LED_2,AdjustLED);

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

void Task3(void *parameter) 
{
  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)="%RH");
  Sprintln("---");

  vTaskDelay(T3ticks);
  }
  vTaskDelete(NULL);
}

void Task4(void *parameter) 
{
  for(;;)
  {
    // A/d averaging

    int AdjustVR = map(ADC_READ_A0,ADMin,ADMax,PerMin,PerMax); //2^12 =4096
    ADval[3] = ADval[2];
    ADval[2] = ADval[1];
    ADval[1] = ADval[0];
    ADval[0] = AdjustVR; //read new analog input
                        //  average of 4 samples
    int adcval = (ADval[3]+ADval[2]+ADval[1]+ADval[0])>>2;

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

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

void Task5(void *parameter) 
{
  for(;;)
    {
    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 *parameter) 
{
  for(;;)
  {
    LCD.setCursor(0,0);
    LCD.print("Temp:" + String(Temp) + "C");
    LCD.setCursor(0,1);
    LCD.print("Humidity:" + String(Humi) + "%");

    if(even)
    {
      // draw a circle
      OLED_Display ();      //OLED Display
    }
    else
    {
      OLED.clearDisplay();
      OLED.setTextColor(WHITE,BLACK);
      OLED.setCursor(2,8);
      OLED.print("distance:" + String(distance) + " Cm");
      OLED.display();
    }

  Sprint("Task 6 count : ");
  Sprintln(count_6++);
  vTaskDelay(T6ticks);
  }
vTaskDelete(NULL);
}

void Task7(void *parameter) 
{
  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(VR_PIN, INPUT);

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

  pinMode(PIR_PIN, INPUT);

  pinMode(LIGHT_SENSOR_PIN, INPUT);

  pinMode(VR_PIN, INPUT);

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

  freertos_init();

  LCD.init();
  LCD.backlight();

  LCD.setCursor(0,0);
  LCD.print("Hello world");

  LCD.setCursor(0,1);
  LCD.print("Mechatronics Eng");

  delay (2000);
  /*******************************************/
    // initialize OLED display with I2C address 0x3C
    if (!OLED.begin(SSD1306_SWITCHCAPVCC, 0x3C)) 
    {
      Serial.println(F("failed to start SSD1306 OLED"));
      while (1);
    }

    delay(2000); // wait two seconds for initializing

    OLED.setCursor(0, 0); // OLED Clear display
    OLED.clearDisplay(); // OLED Clear display
    OLED.setTextColor(WHITE,BLACK);   
    
    OLED.setCursor(2,8);
    OLED.print("Hello world");
    OLED.setCursor(2,16);
    OLED.print("Mechatronics 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);

  // draw a triangle
  OLED.clearDisplay();
  OLED.drawTriangle(30, 15, 0, 60, 60, 60, WHITE);
  OLED.display();
  //delay(1);
  delayMicroseconds(2);

  // fill a triangle
  OLED.clearDisplay();
  OLED.fillTriangle(30, 15, 0, 60, 60, 60, WHITE);
  OLED.display();
  //delay(1);
  delayMicroseconds(2);

  // draw a rectangle
  OLED.clearDisplay();
  OLED.drawRect(0, 15, 60, 40, WHITE);
  OLED.display();
  //delay(1);
  delayMicroseconds(2);

  // fill a rectangle
  OLED.clearDisplay();
  OLED.fillRect(0, 15, 60, 40, WHITE);
  OLED.display();
  //delay(1);
  delayMicroseconds(2);

  // draw a round rectangle
  OLED.clearDisplay();
  OLED.drawRoundRect(0, 15, 60, 40, 8, WHITE);
  OLED.display();
  //delay(1);
  delayMicroseconds(2);

  // fill a round rectangle
  OLED.clearDisplay();
  OLED.fillRoundRect(0, 15, 60, 40, 8, WHITE);
  OLED.display();
  //delay(1);
  delayMicroseconds(2);
}