// ControleEngate.h

#ifndef ControleEngate_h
#define ControleEngate_h

#ifndef Arduino_h
#include "Arduino.h"
#define Arduino_h
#endif

class ControleEngate {
  public:
    ControleEngate(int8_t S1, int8_t S2, int8_t S3, int8_t MA, int8_t MB);
    bool travar();
    bool destravar();
    bool verificarPosicao();

  private:
    const int8_t _S1;
    const int8_t _S2;
    const int8_t _S3;
    const int8_t _MA;
    const int8_t _MB;

    enum EstadoTarefa { TRAVANDO, DESTRAVANDO, PARADO };
    volatile EstadoTarefa _tarefa = PARADO;

    static ControleEngate* instance;
    static void IRAM_ATTR handleInterrupt();
    void parar();
};

#endif

/***********************************************************************/
// ControleEngate.cpp

// #include "ControleEngate.h"

ControleEngate* ControleEngate::instance = nullptr;

ControleEngate::ControleEngate(int8_t S1, int8_t S2, int8_t S3, int8_t MA, int8_t MB)
  : _S1(S1), _S2(S2), _S3(S3), _MA(MA), _MB(MB) {
  pinMode(_S1, INPUT_PULLDOWN);
  pinMode(_S2, INPUT_PULLDOWN);
  pinMode(_S3, INPUT_PULLDOWN);
  pinMode(_MA, OUTPUT);
  pinMode(_MB, OUTPUT);
  instance = this;
}

bool ControleEngate::travar() {
  if (_tarefa == DESTRAVANDO) {
    detachInterrupt(digitalPinToInterrupt(_S2));
  }
  if (!digitalRead(_S3)) {
    // Serial.println("Travando engate...");
    digitalWrite(_MA, HIGH);
    digitalWrite(_MB, LOW);
    _tarefa = TRAVANDO;
    attachInterrupt(digitalPinToInterrupt(_S3), handleInterrupt, RISING);
  } else {
    parar();
    // Serial.println("Engate já travado!");
  }
  if (_tarefa == PARADO) {
    return true;
  } else {
    return false;
  }
}

bool ControleEngate::destravar() {
  if (_tarefa == TRAVANDO) {
    detachInterrupt(digitalPinToInterrupt(_S3));
  }
  if (!digitalRead(_S2)) {
    // Serial.println("Destravando engate...");
    digitalWrite(_MA, LOW);
    digitalWrite(_MB, HIGH);
    _tarefa = DESTRAVANDO;
    attachInterrupt(digitalPinToInterrupt(_S2), handleInterrupt, RISING);
  } else {
    parar();
    // Serial.println("Engate já destravado!");
  }

  if (_tarefa == PARADO) {
    return true;
  } else {
    return false;
  }
}

void IRAM_ATTR ControleEngate::handleInterrupt() {
  if (instance != nullptr) {
    instance->parar();
  }
}

void IRAM_ATTR ControleEngate::parar() {
  digitalWrite(_MA, LOW);
  digitalWrite(_MB, LOW);
  if (_tarefa == DESTRAVANDO) {
    detachInterrupt(digitalPinToInterrupt(_S2));
  } 
  else if (_tarefa == TRAVANDO) {
    detachInterrupt(digitalPinToInterrupt(_S3));
  }
  _tarefa = PARADO;
  // Serial.println("Pronto!");
}

bool ControleEngate::verificarPosicao() {
  if (digitalRead(_S1)) {
    return true;
  } else {
    return false;
  }
}

/***********************************************************************/
// ExemploControleEngate.cpp

#ifndef Arduino_h
#include "Arduino.h"
#define Arduino_h
#endif
// #include "ControleEngate.h"

#define S1_ENGATE 25
#define S2_ENGATE 33
#define S3_ENGATE 32
#define MA_ENGATE 26
#define MB_ENGATE 27

ControleEngate engate(S1_ENGATE, S2_ENGATE, S3_ENGATE, MA_ENGATE, MB_ENGATE);

// Funções para teste
void testeComandos();
void testeContador();

void setup() {
  Serial.begin(115200);
}

void loop() {
  testeComandos();
  testeContador();
}

void testeComandos() {
  static char tarefa = '0';
  static bool sair = false;

  if (Serial.available() > 0) {
    char comando = Serial.read();
    if (comando == 't') {
      Serial.println("Aguardando engate travar...");
      sair = engate.travar();
      tarefa = comando;
    }
    else if (comando == 'd') {
      Serial.println("Aguardando engate destravar...");
      sair = engate.destravar();
      tarefa = comando;
    }
    else if (comando == 'p') {
      bool posicao = engate.verificarPosicao();
      if (posicao == true) {
        Serial.println("Em posição");
      } else {
        Serial.println("Fora de posição");
      }
    }
  }

  if (!sair && tarefa == 't') {
    sair = engate.travar();
  }
  else if (!sair && tarefa == 'd') {
    sair = engate.destravar();
  }
  else if (!sair && (tarefa != 'd' || tarefa != 't')) {
    // Nada pra fazer
  }
  else if (sair && tarefa != '0') {
    Serial.println("Saindo!");
    tarefa = '0';
  }
}

void testeContador() {
  static unsigned long tempoContador = millis();
  static int contador = 0;

  if ((millis() - tempoContador) > 1000) {
    Serial.print("Contador: ");
    Serial.println(contador++);
    tempoContador = millis();
  }
}