// https://wokwi.com/projects/377867353007083521
// https://forum.arduino.cc/t/sliding-game-opener-project/1143078

const int gateOpenPin = 10;         // GPIO2 for gate open
const int gateClosePin = 3;        // GPIO3 for gate close
const int openProximityPin = 19;    // GPIO4 for open proximity sensor
const int closeProximityPin = 2;   // GPIO5 for close proximity sensor
const uint8_t motorControlPinR = 7;    // GPIO6 for right motor control (PWM)
const uint8_t motorControlPinL = 6;    // GPIO7 for left motor control (PWM)
const int keyPadPin = 9;           // GPIO8 for keypad signal

//Varibales for checking pin states
int gateOpen;
int gateClosed;
int openProximity;
int closeProximity;
int keyPad;

// Variables used for motor speed control
const int maxMotorSpeed = 128;        // 50% PWM speed (0-255)
int currentMotorSpeed = 0;
int openChannel = 1;
int closeChannel = 2;

enum motorStates {TO_OPEN, OFF, TO_CLOSE, NONE};    // should we be closing, off or opening with da motor(s)
unsigned char motors;               // the output variable from the P phase

// Variables used for timing
unsigned long gateOpenDuration = 5000; // 45 seconds
unsigned long gateOpenStartTime;
unsigned long startMillis;
unsigned long currentMillis = 0;

//Variables for gate states
enum gateStates {
  OPEN,
  WATING_OPEN,
  CLOSED,
  OPENING,
  CLOSING,
  JAMMED_OPENING,
  JAMMED_CLOSING,
  UNKNOWN,
  NO_STATE,
} state;

#define IDNAME(name) #name
const char* stateNames[] = {
  IDNAME(OPEN), 
  IDNAME(WATING_OPEN), 
  IDNAME(CLOSED),
  IDNAME(OPENING), 
  IDNAME(CLOSING), 
  IDNAME(JAMMED_OPENING), 
  IDNAME(JAMMED_CLOSING), 
  IDNAME(UNKNOWN),
  IDNAME(NO_STATE),
};

enum gateStates gateState;
// enum gateStates prevGateState;

void setup() {
  // Initialize serial communication
  Serial.begin(115200);

  // Initialize pins
  pinMode(gateOpenPin, INPUT_PULLUP);
  pinMode(gateClosePin, INPUT_PULLUP);
  pinMode(openProximityPin, INPUT_PULLUP);
  pinMode(closeProximityPin, INPUT_PULLUP);
  pinMode(keyPadPin, INPUT_PULLUP);
  ledcAttachPin(motorControlPinL, closeChannel);
  ledcAttachPin(motorControlPinR, openChannel);
  ledcSetup(closeChannel, 1000, 8);
  ledcSetup(openChannel, 1000, 8);
  ledcWrite(closeChannel, 0);

// there may be no need for most of this logic in setup

  //Get the current state of the various sensors
  gateOpen = digitalRead( gateOpenPin );
  gateClosed = digitalRead( gateClosePin );
  openProximity = digitalRead( openProximityPin );
  closeProximity = digitalRead( closeProximityPin );
  
  startMillis = millis();

  // determine the initial gate position
  if ( gateOpen == LOW && gateClosed == LOW ) {
    Serial.println(F("ERROR - both limit switches activated"));

    while (1) {
// flash and blink an alarm condition here.
// this could be checked all over the place not just at startup
    }
  }
  else if ( gateOpen == HIGH && gateClosed == HIGH ) {
//    prevGateState = gateState;
    gateState = UNKNOWN;
    Serial.println(F("GATE POSITION UNKNOWN"));
  }
  else if ( gateOpen == LOW ) {
    Serial.println(F("GATE OPEN"));
//    prevGateState = gateState;
    gateState = OPEN;
  }
  else if ( openProximity == LOW ) {
    Serial.println(F("GATE BLOCKED WHILE OPENING"));
//    prevGateState = gateState;
    gateState = JAMMED_OPENING;
  }
  else if ( closeProximity == LOW ) {
    Serial.println(F("GATE BLOCKED WHILE OPENING"));
//    prevGateState = gateState;
    gateState = JAMMED_CLOSING;
  }
  else {
    Serial.println(F("GATE CLOSED"));
//    prevGateState = gateState;
    gateState = CLOSED;
  }
}

// compensate for active lLOW or active HIGH sensor signals
# define GATE_SENSE		LOW
# define PROX_SENSE		LOW
# define BUTTON_SENSE	LOW

unsigned long now;

void loop() {
  static unsigned long lastTime;
 
  //now = millis();

/* Get the current state of the various sensors
  
  keyPad true if button is being pressed
  gateOpen true if the gate open limit has been reached
  gateClose true if the gate close limit has been reached 
  openProximity true if something is blockage the opening movement
  closeProximity true if something is blockage the closing movement
*/

// INPUT

  keyPad = digitalRead(keyPadPin) == BUTTON_SENSE;
  gateOpen = digitalRead(gateOpenPin) == GATE_SENSE;
  gateClosed = digitalRead(gateClosePin) == GATE_SENSE;
  openProximity = digitalRead(openProximityPin) == PROX_SENSE;
  closeProximity = digitalRead(closeProximityPin) == PROX_SENSE;

// PROCESS - keypad
  getSignal();

// PROCESS limit and jam switches
  static unsigned char lastMotors = NONE;
  motors = OFF; // unless told otherwise by the process phase
  gateFSM();    // process the inputs, develop motors control variable

// OUTPUT - set the motor(s) to the task
//  Serial.println("now set the motor(s) opening, closing or off");
// if you need to, probably really should always in real life but
// we can cut the spam to printing only when a change is made

  if (motors != lastMotors) {
    lastMotors = motors;

    if (motors == TO_OPEN) {
      Serial.println("making the motor(s) opening the gate");
      motorControlO();
    }
    
    if (motors == TO_CLOSE) {
      Serial.println("making the motor(s) closing the gate");
      motorControlC();
    }
    if (motors == OFF) {
      Serial.println("stop the motor(s)");
      motorsOff();
    }
  }

  delay(177);    // poor man's loop throttle and spam mitigation
}

void getSignal() {
  static bool lastKeyPad;
  if (keyPad && ! lastKeyPad) {
    Serial.println("I see you pressed the button.");

// if the gate is open , close it
// if the gate is closed, open it
// if the gate is neither open or closed, open it (default);

// here there might be some JAM action checking

    if (gateState == OPEN) gateState = CLOSING;
    else if (gateState == CLOSED) gateState = OPENING;
    else gateState = OPENING;
  }
  lastKeyPad = keyPad;
}

void gateFSM()
{
  static gateStates lastReport;
  if (gateState != lastReport) {
    Serial.print(" in FSM, state is ");
    Serial.println(stateNames[gateState]);
    lastReport = gateState;
  }

  switch (gateState) {
  case UNKNOWN:
    gateState = OPENING;    // default to open the gate if we have no clue

    break;

  case CLOSED:
    motors = OFF;

    break;

  case OPENING:
    motors = TO_OPEN;
    if (gateOpen) {        
      Serial.println(F("GATE OPEN"));
      gateState = OPEN;
    }
    if (openProximity) {
      Serial.println(F("OBJECT FOUND WHILE OPENING"));
      gateState = JAMMED_OPENING;
    }

    break;

  case JAMMED_OPENING:
    if (!openProximity)
      gateState = CLOSING;

    break;

  case OPEN:
    motors = OFF;
    startMillis = currentMillis;
    gateState = WATING_OPEN;

    break;

  case WATING_OPEN:
    currentMillis = millis();
    Serial.println("Waiting to close");
    Serial.println(currentMillis);
    Serial.println(startMillis);
    Serial.println(currentMillis - startMillis);
    if (currentMillis - startMillis >= gateOpenDuration) {
    gateState = CLOSING;
    }
    
    break;
    

  case CLOSING:
    motors = TO_CLOSE;
    if (gateClosed) {        
      Serial.println(F("GATE CLOSED"));
      gateState = CLOSED;
    }
    if (closeProximity) {
      Serial.println(F("OBJECT FOUND WHILE CLOSING"));
      gateState = JAMMED_CLOSING;
    }
    
    break;

  case JAMMED_CLOSING:
    if (!closeProximity)
      gateState = OPENING;

    break;
  }
}

void motorControlC() 
{
	Serial.println("gate is (still) closing");

  ledcWrite(openChannel, 0);
  ledcWrite(closeChannel, 255);
}

void motorControlO()
{
	Serial.println("gate is (still) opening");

  ledcWrite(openChannel, 255);
  ledcWrite(closeChannel, 0);
}

void motorsOff()
{
  Serial.println("motors off");

  ledcWrite(openChannel, 0);
  ledcWrite(closeChannel, 0);
}

void timer()
{
  currentMillis = millis();
  Serial.println("Waiting to close");
  Serial.println(currentMillis);
  Serial.println(startMillis);
  Serial.println(currentMillis - startMillis);
  if (currentMillis - startMillis >= gateOpenDuration) {
    gateState = CLOSING;
  }
}