#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

#define brakePin  4
#define motorOn 25
#define motorOff 26
#define overcurrentPin 5

volatile bool brakeSignal = false;
volatile bool openSignal = false;
volatile bool overcurrentSignal = false;
volatile bool brakeTaskRunning = false;
volatile bool openTaskRunning = false;
volatile bool brakeActivated = false;

float calipersOpeningTime = 5000.0;
float calipersClosingTime = 5000.0;
float minimumCaliperOpeningTime = 1000;
int breakTaskStartingTime = 0;
int openTaskStartingTime = 0;

TaskHandle_t taskHandle;

void startBreakingTask() {
  // Start a task to handle the brake signal
  xTaskCreate(closeCalipersTaskFunction, "BrakeTask", 4096, NULL, 1, &taskHandle);
  Serial.println("Breaking task started");
  breakTaskStartingTime = millis();
}

void startOpeningTask() {
  // Start a task to handle the brake signal
  xTaskCreate(openCalipersTaskFunction, "OpenTask", 4096, NULL, 1, &taskHandle);
  Serial.println("Opening task started");  
  openTaskStartingTime = millis();
}

void closeCalipersTaskFunction(void* parameter) {
  delay(10);
  Serial.println("Starting parking task");
  brakeTaskRunning = true;
  digitalWrite(motorOn, HIGH); // Enable the output pin
  Serial.println("Waiting for calipers");
  delay(int(calipersOpeningTime)); // Adjust time interval as per your requirements
  digitalWrite(motorOn, LOW); // Disable the output pin
  brakeTaskRunning = false;
  Serial.println("Stopping parking task");
  vTaskDelete(NULL);
}

void openCalipersTaskFunction(void* parameter) {
  delay(10);
  Serial.println("Starting opening task");  
  openTaskRunning = true;
  digitalWrite(motorOn, LOW); // Enable the output pin
  digitalWrite(motorOff, HIGH); // Enable the output pin
  Serial.println("Waiting for calipers to open");
  delay(int(calipersClosingTime)); // Adjust time interval as per your requirements
  digitalWrite(motorOff, LOW); // Enable the output pin
  openTaskRunning = false;
  Serial.println("Stopping opening task");
  vTaskDelete(NULL);
}


void overcurrentISR() {
  overcurrentSignal = true;
  // Stop the task if it's running
  if (brakeTaskRunning && millis() > (breakTaskStartingTime + minimumCaliperOpeningTime)) {
    vTaskDelete(taskHandle);
    digitalWrite(motorOn, LOW); // Disable the output pin
    brakeTaskRunning = false;
    Serial.println("Overcurrent while braking");
  }
  if (openTaskRunning && millis() > (openTaskStartingTime + minimumCaliperOpeningTime)) {
    vTaskDelete(taskHandle);
    digitalWrite(motorOff, LOW); // Disable the output pin
    openTaskRunning = false;
    Serial.println("Overcurrent while opening");
  }
  overcurrentSignal = false;
  // Handle overcurrent interrupt here if necessary
}

void setup() {

  Serial.begin(115200);

  uint32_t result = 0; // Initialize the result to zero
  
  for (int i = 0; i < 16; i++) { // Loop from 0 to 15
    result = 0;
    // Shift the current loop iteration value (i) into bits 8-11 of the result
    result |= ((uint32_t)i << 8);
    
    // Convert decimal value 111 to binary and insert it into bits 23-30 of the result
    uint32_t binary111 = 0b11101111; // Binary representation of decimal 111 with added 1 in front
    result |= (binary111 << 23);
    
    
    // Print the result for demonstration purposes (optional)
    Serial.print("Iteration ");
    Serial.print(i);
    Serial.print(": ");
    Serial.print(i, BIN);
    Serial.print(", ");
    Serial.print(result, HEX);
    Serial.print(", ");
    Serial.println(result, BIN);
    
    // Delay for visualization purposes (optional)
    delay(100);
  }


  pinMode(brakePin, INPUT_PULLDOWN);
  pinMode(motorOn, OUTPUT);
  pinMode(motorOff, OUTPUT);
  pinMode(overcurrentPin, INPUT_PULLDOWN);

  // attachInterrupt(digitalPinToInterrupt(brakePin), brakeISR, RISING);
  // attachInterrupt(digitalPinToInterrupt(brakePin), openISR, FALLING);
  attachInterrupt(digitalPinToInterrupt(overcurrentPin), overcurrentISR, RISING);

  Serial.println("[APP] Free memory: " + String(esp_get_free_heap_size()) + " bytes");
}

void loop() {
  
  if (digitalRead(brakePin) == HIGH) {
    brakeSignal = true;
    openSignal = false;
  } else {
    brakeSignal = false;
    openSignal = true;
  }

  if (brakeSignal && !brakeActivated) {
    brakeSignal = false;
    brakeActivated = true;
    Serial.println("Brake signal detected");
    startBreakingTask();
    Serial.println("[APP] Free memory: " + String(esp_get_free_heap_size()) + " bytes");
  }
  if (openSignal && brakeActivated) {
    openSignal = false;
    Serial.println("Open signal detected");
    if(!brakeTaskRunning) {
      startOpeningTask();
      brakeActivated = false;
    }
  }
  delay(100); // Adjust delay as per your requirements
}