#define STOP_ON_PIN 18 //PC1

#define OVERCURRENT_PIN1 3
#define OVERCURRENT_PIN2 2

#define INH1_PIN 9 // PB1
#define IN1_PIN 21 // PE3 („1” zamykanie, „0”- otwieranie: ustawienie kierunku, a włączenie sygnałem INH)
#define INH2_PIN 11 //1 // PD1
#define IN2_PIN 10 // PD0

volatile bool brakeSignal = false;
volatile bool openSignal = false;
volatile bool leftOvercurrentSignal = false;
volatile bool rightOvercurrentSignal = false;
volatile bool leftBrakeActivated = false;
volatile bool rightBrakeActivated = false;
volatile bool leftOpeningActivated = false;
volatile bool rightOpeningActivated = false;



float calipersOpeningTime = 5000.0;
float calipersClosingTime = 5000.0;
float minimumCaliperOpeningTime = 1000;
volatile unsigned long brakeStartingTime = 0;
volatile unsigned long openingStartingTime = 0;


void leftOvercurrentISR() {
  Serial.println("leftOvercurrentISR");
  leftOvercurrentSignal = true;
  // Stop the task if it's running
  if (leftBrakeActivated && millis() > (brakeStartingTime + minimumCaliperOpeningTime)) {
    digitalWrite(INH1_PIN, LOW); // Disable the output pin
    leftBrakeActivated = false;   
    Serial.println("Overcurrent while braking");
  }
  if (leftOpeningActivated && millis() > (openingStartingTime + minimumCaliperOpeningTime)) {
    digitalWrite(INH1_PIN, LOW); // Disable the output pin
    leftOpeningActivated = false;
    Serial.println("Overcurrent while opening");
  }
  leftOvercurrentSignal = false;
  // Handle overcurrent interrupt here if necessary
}

void rightOvercurrentISR() {
  Serial.println("rightOvercurrentISR");
  rightOvercurrentSignal = true;
  // Stop the task if it's running
  if (rightBrakeActivated && millis() > (brakeStartingTime + minimumCaliperOpeningTime)) {
    digitalWrite(INH2_PIN, LOW); // Disable the output pin
    rightBrakeActivated = false;   
    Serial.println("Overcurrent while braking");
  }
  if (rightOpeningActivated && millis() > (openingStartingTime + minimumCaliperOpeningTime)) {
    digitalWrite(INH2_PIN, LOW); // Disable the output pin
    rightOpeningActivated = false;
    Serial.println("Overcurrent while opening");
  }
  rightOvercurrentSignal = false;
  // Handle overcurrent interrupt here if necessary
}

void brakeInterruptISR() {
  Serial.println("brake ISR");
  if (digitalRead(STOP_ON_PIN) == HIGH) {
    Serial.println("Brake HIGH");
    brakeSignal = true;
    openSignal = false;
  } else {
    Serial.println("Brake LOW");
    brakeSignal = false;
    openSignal = true;
  }
}

void setup() {

  Serial.begin(115200);


  pinMode(STOP_ON_PIN, INPUT_PULLUP);
  pinMode(INH1_PIN, OUTPUT);
  pinMode(IN1_PIN, OUTPUT);
  pinMode(INH2_PIN, OUTPUT);
  pinMode(IN2_PIN, OUTPUT);
  pinMode(OVERCURRENT_PIN1, INPUT);
  pinMode(OVERCURRENT_PIN2, INPUT);

  digitalWrite(INH1_PIN, LOW);
  digitalWrite(INH2_PIN, LOW);
  
  attachInterrupt(digitalPinToInterrupt(OVERCURRENT_PIN1), leftOvercurrentISR, RISING);
  attachInterrupt(digitalPinToInterrupt(OVERCURRENT_PIN2), rightOvercurrentISR, RISING);
  attachInterrupt(digitalPinToInterrupt(STOP_ON_PIN), brakeInterruptISR, CHANGE);

}

void loop() {

  if (leftBrakeActivated && (millis() - brakeStartingTime) >= calipersClosingTime) {
    leftBrakeActivated = false;
    Serial.println("Left braking timeout");
    digitalWrite(INH1_PIN, LOW);    
  }
  if (rightBrakeActivated && (millis() - brakeStartingTime) >= calipersClosingTime) {
    rightBrakeActivated = false;
    Serial.println("Right braking timeout");
    digitalWrite(INH2_PIN, LOW);    
  }

  if (leftOpeningActivated && (millis() - openingStartingTime) >= calipersOpeningTime) {
    leftOpeningActivated = false;
    Serial.println("Left opening timeout");
    digitalWrite(INH1_PIN, LOW);    
  }
  if (rightOpeningActivated && (millis() - openingStartingTime) >= calipersOpeningTime) {
    rightOpeningActivated = false;
    Serial.println("Right opening timeout");
    digitalWrite(INH2_PIN, LOW);    
  }
  

  if (brakeSignal) {
    brakeSignal = false;
    leftBrakeActivated = true;
    rightBrakeActivated = true;
    brakeStartingTime = millis();
    digitalWrite(IN1_PIN, HIGH);
    digitalWrite(INH1_PIN, HIGH);
    digitalWrite(IN2_PIN, HIGH);
    digitalWrite(INH2_PIN, HIGH);
    Serial.println("Brake signal detected");    
  }
  if (openSignal) {
    openSignal = false;
    leftOpeningActivated = true;
    rightOpeningActivated = true;
    openingStartingTime = millis();
    digitalWrite(IN1_PIN, LOW);
    digitalWrite(INH1_PIN, HIGH);
    digitalWrite(IN2_PIN, LOW);
    digitalWrite(INH2_PIN, HIGH);
    Serial.println("Opening signal detected");    
  }
  // if (openSignal && brakeActivated) {
  //   openSignal = false;
  //   Serial.println("Open signal detected");
  //   if(!brakeActivated) {
  //     brakeActivated = false;
  //   }
  // }
  delay(100); // Adjust delay as per your requirements
}