// Include drivers and libraries
#include "DHT.h"
#include "ESP32Servo.h"
#include "HX711.h"
#include <stdarg.h>

// Hardware Config
#define LED_PIN 4
#define LED_PIN2 8
#define DHTPIN 14
#define servoPin 35
#define loadcellpin_DT 41
#define loadcellpin_SCK 40
#define DHTTYPE DHT22

// Program settings
const int TEMP_TRIGGER=26;
const int loadcellCalibrationFactor=420;
const float target_force = 10; //Newtons

// Global variables
float loadcell_force = 0;
int servo_target_position = 0; //Set servo initial position

// Instantiate classes
DHT dht(DHTPIN, DHTTYPE);
Servo servo;
HX711 scale;
TaskHandle_t MainLoop_Core0_handle;
TaskHandle_t MainLoop_Core1_handle;

// Init timer as "timer"
hw_timer_t *timer = NULL;
volatile SemaphoreHandle_t timerSemaphore;
portMUX_TYPE timerMux = portMUX_INITIALIZER_UNLOCKED;
volatile uint32_t isrCounter = 0;
volatile uint32_t lastIsrAt = 0;
volatile bool LED2STATE = LOW;
int loopcount_core0 = 0;
int loopcount_core1 = 0;
int looptimercount = 0;

// Define ISR (interrupt service routine) "onTimer"
void ARDUINO_ISR_ATTR onTimer() {
  // Increment the counter and set the time of ISR
  isrCounter = isrCounter + 1;
  lastIsrAt = millis();
  
  xSemaphoreGiveFromISR(timerSemaphore, NULL); // Give a semaphore that we can check in the loop
  // It is safe to use digitalRead/Write here if you want to toggle an output
  LED2STATE = !LED2STATE;
  digitalWrite(LED_PIN2, LED2STATE);
  
  Serial.print("onTimer count: ");  Serial.print(isrCounter); 
  //Serial.print(" (at "); Serial.print(lastIsrAt); Serial.println(" ms)");
  portENTER_CRITICAL_ISR(&timerMux);
  update_temp();
  portEXIT_CRITICAL_ISR(&timerMux);
}

void setup() {
  Serial.println("Setup");
  pinMode(LED_PIN, OUTPUT);
  pinMode(LED_PIN2, OUTPUT);
  Serial.begin(9600);
  Serial.println(F("DHTxx test!"));
  dht.begin();
  Serial.println("Initializing the servo");
  servo.attach(servoPin, 500, 2400);
  Serial.println("Initializing the scale");
  scale.begin(loadcellpin_DT, loadcellpin_SCK);

  // Interrupt timer:
    // Create semaphore to inform us when the timer has fired
    // Set timer frequency to 1Mhz
    // Attach onTimer function to our timer.
    // Set alarm to call onTimer function every 2 second (in microseconds), repeating forever.
  timerSemaphore = xSemaphoreCreateBinary();
  timer = timerBegin(1000000);
  timerAttachInterrupt(timer, &onTimer);
  timerAlarm(timer, 2000000, true, 0);

  // Multithreading:
  //create a task that will be executed in the MainLoop_Core0() function, with priority 1 and executed on core 0
  xTaskCreatePinnedToCore(
                    MainLoop_Core0,   /* Task function. */
                    "MainLoop_Core0_handle",     /* name of task. */
                    10000,       /* Stack size of task */
                    NULL,        /* parameter of the task */
                    1,           /* priority of the task */
                    &MainLoop_Core0_handle,      /* Task handle to keep track of created task */
                    0);          /* pin task to core 0 */                  
  delay(500); 

  //create a task that will be executed in the MainLoop_Core1() function, with priority 1 and executed on core 1
  xTaskCreatePinnedToCore(
                    MainLoop_Core1,   /* Task function. */
                    "MainLoop_Core1_handle",     /* name of task. */
                    10000,       /* Stack size of task */
                    NULL,        /* parameter of the task */
                    1,           /* priority of the task */
                    &MainLoop_Core1_handle,      /* Task handle to keep track of created task */
                    1);          /* pin task to core 1 */
    delay(500); 

}

//MainLoop_Core0: blinks an LED every 1000 ms
void MainLoop_Core0( void * pvParameters ){
  myPrint(1, "test");
  for(;;){
    loopcount_core0 = loopcount_core0+1;
    Serial.print("Main Loop running on core "); Serial.print(xPortGetCoreID()); Serial.print(", Loop count: "); Serial.println(loopcount_core0); 
    delay(500);

    //update_loadcell_data();
    //update_servo_position();

    // If Timer has fired (happens once after each interrupt)
    if (xSemaphoreTake(timerSemaphore, 0) == pdTRUE) {
      looptimercount = looptimercount + 1;
      Serial.print("in-loop timer count: "); Serial.print(looptimercount); Serial.print("\n");
      xSemaphoreGive(timerSemaphore);
    }
  } 
}

//MainLoop_Core1: blinks an LED every 700 ms
void MainLoop_Core1( void * pvParameters ){
  delay(200); // Delay to offset activity of the two cores to reduce collisions when writing to serial
  for(;;){
    loopcount_core1 = loopcount_core1+1;
    Serial.print("Main Loop running on core "); Serial.print(xPortGetCoreID()); Serial.print(", Loop count: "); Serial.println(loopcount_core1);
    delay(500);
  } 
}

void loop() {
}

void update_loadcell_data(){
  loadcell_force = scale.get_units()/loadcellCalibrationFactor;
  //Serial.print("Loadcell force: "); Serial.print(loadcell_force, 1); Serial.print("\n");
}

void update_servo_position(){
  if (loadcell_force < target_force){
    servo_target_position = servo_target_position + 2;
    servo.write(servo_target_position);
  }
}

void update_temp(){
  Serial.println("\n-----Temp Data------");
  // Reading temperature or humidity takes about 250 milliseconds!
  // Sensor readings may also be up to 2 seconds 'old' (its a very slow sensor)
  float h = dht.readHumidity(); 
  float t = dht.readTemperature();      // Read temperature as Celsius (default)
  float f = dht.readTemperature(true);  // Read temperature as Fahrenheit

  //delay(2000);
  // Check if any reads failed and exit early (to try again).
  if (isnan(h) || isnan(t) || isnan(f)) {
    Serial.println(F("Failed to read from DHT sensor!"));
    return;
  }

  float hif = dht.computeHeatIndex(f, h);         // Compute heat index in Fahrenheit (the default)
  float hic = dht.computeHeatIndex(t, h, false);  // Compute heat index in Celsius (isFahreheit = false)

  Serial.print(F("Temperature: ")); Serial.print(t);    Serial.print(F("°C\n"));
  Serial.print(F("Humidity: "));    Serial.print(h);    Serial.print(F("%\n"));
  Serial.print(F("Heat index: "));  Serial.print(hic);  Serial.print(F("°C\n"));
  //Serial.println("---------------------");
  (t > TEMP_TRIGGER) ? digitalWrite(LED_PIN, HIGH) : digitalWrite(LED_PIN, LOW);
}

void myPrint(int count, ...){
  va_list args;
  va_start(args, count); 
  for(int j = 0; j < count; j++){
    Serial.print(va_arg(args, char));
  }
  va_end(args);
}