/*

  Program reqs:

  When signal to operate is received detmine if gate is opened or closed.
  Can receive signal from keyPin or phone.
  If open close the gate.
  Operate the PWM motor at 50% until the close hall effect sensor gets signal.
  If closed open the gate.
  Operate the PWM motor at 50% until the open hall effect sensor gets signal.
  If neither open or close close the gate.
  Check the close proximity sensor while closing, if object detected go back to open position.
  Check the open proximity sensor while opening, if object detected go back to close position.
  Automatically close the gate after open for a set ammount of time

*/

// Define PINs for gate control, proximity sensors, and motor control
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 gateClose;
int openProximity;
int closeProximity;
uint8_t motorControlR;
uint8_t motorControlL;
int keyPad;
int prevKeyPad;

// Variables used for motor speed control
const int maxMotorSpeed = 125; 
const int minMotorSpeed = 25;       
int currentMotorSpeed = 25;
int openChannel = 1;
int closeChannel = 2;
const int speedStep = 25;

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

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

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

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);

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

  // determine the initial gate position
  if ( gateOpen == LOW && gateClose == LOW ) {
    Serial.println(F("ERROR - both limit switches activated"));
    while (1);
  }
  else if ( gateOpen == HIGH && gateClose == HIGH ) {
    prevGateState = gateState;
    gateState = UNKNOWN;
    Serial.println(F("GATE POSITION UNKNOWN"));
  }
  else if ( gateOpen == LOW ) {
    Serial.println(F("GATE OPEN"));
    prevGateState = gateState;
    stateChange(OPEN);
  }
  else if ( openProximity == LOW ) {
    Serial.println(F("GATE BLOCKED WHILE OPENING"));
    prevGateState = gateState;
    stateChange(JAMMED_OPENING);
  }
  else if ( closeProximity == LOW ) {
    Serial.println(F("GATE BLOCKED WHILE OPENING"));
    prevGateState = gateState;
    stateChange(JAMMED_CLOSING);
  }
  else {
    Serial.println(F("GATE CLOSED"));
    prevGateState = gateState;
    stateChange(CLOSED);
  }
  startMillis = millis(); 
}

void loop() {
  //Check the sensors
  gateOpen = digitalRead( gateOpenPin );
  gateClose = digitalRead( gateClosePin );
  openProximity = digitalRead( openProximityPin );
  closeProximity = digitalRead( closeProximityPin );
  keyPad = digitalRead(keyPadPin);

  Serial.print("Open pin state: ");
  Serial.println(gateOpen);
  Serial.print("Close pin state: ");
  Serial.println(gateClose);
  Serial.print("Keypad pin state: ");
  Serial.println(digitalRead(keyPadPin));
  Serial.print("Open proximity pin state: ");
  Serial.println(openProximity);
  Serial.print("Close proximity pin state: ");
  Serial.println(closeProximity);
  Serial.print("State of the gate: ");
  Serial.println(stateNames[gateState]);
  
  switch ( gateState ) {
    case UNKNOWN:
      if ( gateOpen == LOW ) {        
        Serial.println(F("GATE OPEN"));
        stateChange(OPEN);
      }
      else if ( gateClose == LOW ) {        
        Serial.println(F("GATE CLOSED"));
        stateChange(CLOSED);
      }
      break;

    case CLOSED:
      if ( gateClose == LOW && keyPad == LOW) {
        stateChange(OPENING);
        Serial.println(F("GATE OPENING"));
        motorControl(closeChannel, 0);
        motorControl(openChannel,minMotorSpeed);
      }
      break;

    case OPENING:
      if (gateOpen == LOW) {
        motorControl(openChannel, 0);
        stateChange(OPEN);
      }
      else {
          if(openProximity == LOW) {
            stateChange(JAMMED_OPENING);
          }   
          else if (currentMotorSpeed < maxMotorSpeed) {              
              //currentMotorSpeed += speedStep;
              currentMotorSpeed++;
              Serial.println(currentMotorSpeed);
              Serial.println("OPENING");
              motorControl(openChannel, currentMotorSpeed);
          }
          else {
              motorControl(openChannel, currentMotorSpeed);
              Serial.println(currentMotorSpeed);
              Serial.println("OPENING");
          }
        }    
      break;

    case JAMMED_OPENING:
      Serial.println(F("OBJECT FOUND WHILE OPENING"));
      currentMotorSpeed = minMotorSpeed;
      stateChange(CLOSING);
      motorControl(openChannel, 0);
      motorControl(closeChannel,currentMotorSpeed);
      break;

    case OPEN:
      if ( gateOpen == LOW ) {
        motorControl(openChannel, 0);
        if (prevGateState == OPENING) delay(gateOpenDuration);
        currentMotorSpeed = minMotorSpeed;
        stateChange(CLOSING);
        Serial.println(F("GATE CLOSING"));
        motorControl(closeChannel,currentMotorSpeed);
        
      }
      break;

    case CLOSING:
      if (gateClose == LOW) {
        currentMotorSpeed = 0;
        motorControl(closeChannel, currentMotorSpeed);
        motorControl(openChannel, currentMotorSpeed);
        stateChange(CLOSED);
      }
      else {
          if(closeProximity == LOW) {
            stateChange(JAMMED_CLOSING);
          }   
          else if (currentMotorSpeed < maxMotorSpeed) {
              //currentMotorSpeed += speedStep;
              currentMotorSpeed++;
              Serial.println(currentMotorSpeed);
              Serial.println("CLOSING");
              motorControl(closeChannel, currentMotorSpeed);
          }
          else {
              motorControl(closeChannel, currentMotorSpeed);
              Serial.println(currentMotorSpeed);
              Serial.println("CLOSING");
          }
        }    
      break;

    case JAMMED_CLOSING:
      Serial.println(F("OBJECT FOUND WHILE CLOSING"));
      currentMotorSpeed = minMotorSpeed;
      stateChange(OPENING);
      motorControl(closeChannel, 0);
      motorControl(openChannel,currentMotorSpeed);     
      break;
  }
}

void stateChange(enum gateStates x){
  prevGateState = gateState;
  gateState = x;
  Serial.print("State of the gate: ");
  Serial.println(stateNames[gateState]);
}

void motorControl(int direction, int speed) {
  if (direction == openChannel) {
    Serial.println("New");
    ledcWrite(direction, speed);
    //delay(1000);
  }

  if (direction == closeChannel) {
    Serial.println("New");
    ledcWrite(direction, speed);
    //delay(1000);
  }

  ledcWrite(direction, speed);
  if(direction == 1)Serial.println("Open");
  else Serial.println("Closed");
  Serial.println(speed);
  //delay(1000);
}