/*

  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;        // 50% PWM speed (0-255)
int currentMotorSpeed = 0;
int openChannel = 1;
int closeChannel = 2;
int speedStep = 25;

// Variables used for timing
const unsigned long gateOperationDuration = 45000; // 45 seconds
unsigned long gateOperationStartTime = 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
  keyPad = digitalRead( keyPadPin );
  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);
  }
}

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]);
  delay(500);
  getSignal();
}

void getSignal() {
  if (keyPad == 0) {
    gateCommand();
  }
}

void gateCommand() {
  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 ) {
        stateChange(OPENING);
        Serial.println(F("GATE OPENING"));
        motorControl(openChannel,currentMotorSpeed);
      }
      break;

    case OPENING:
      if (gateOpen == LOW) {
        stateChange(OPEN);
        motorControl(openChannel, 0);
      }

      if (currentMotorSpeed < maxMotorSpeed) {
        currentMotorSpeed += speedStep;
        motorControl(openChannel, currentMotorSpeed);
        Serial.println(currentMotorSpeed);
      }
      else {
        motorControl(openChannel, currentMotorSpeed);
        Serial.println(currentMotorSpeed);
      }
      break;

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

    case OPEN:
      if ( gateOpen == LOW ) {
        stateChange(CLOSING);
        Serial.println(F("GATE CLOSING"));
        motorControl(closeChannel,currentMotorSpeed);
      }
      break;

    case CLOSING:
      if (gateClose == LOW) {
        stateChange(CLOSED);
        motorControl(closeChannel, 0);
      }

      if (currentMotorSpeed < maxMotorSpeed) {
        currentMotorSpeed += speedStep;
        motorControl(closeChannel, currentMotorSpeed);
        Serial.println(currentMotorSpeed);
      }
      else {
        motorControl(closeChannel, currentMotorSpeed);
        Serial.println(currentMotorSpeed);
      }
      break;

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

void motorControlC() {
  Serial.println("Turn LEDs on.");
  // If your RGB LED turns off instead of on here you should check if the LED is common anode or cathode.
  // If it doesn't fully turn off and is common anode try using 256.
  if (gateOpen == 0) {
    ledcWrite(openChannel, 0);
    ledcWrite(openChannel, maxMotorSpeed);
    Serial.println("Gate Closing");
    stateChange(CLOSING);
    if (closeProximity == LOW) {
        stateChange(JAMMED_CLOSING);
        Serial.println("Object Encountered While Closing");
        delay(1000);
      }
    Serial.println("Gate Closed");
    stateChange(CLOSED);
    delay(1000);
  }
  else {
    ledcWrite(openChannel, 255);
    ledcWrite(closeChannel, 255);
  }
  Serial.println("Turn LEDs off.");
  ledcWrite(openChannel, 0);
  ledcWrite(closeChannel, 0);
}

void motorControlO() {
  Serial.println("Turn LEDs on.");
  // If your RGB LED turns off instead of on here you should check if the LED is common anode or cathode.
  // If it doesn't fully turn off and is common anode try using 256.
  if (gateClose == 0) {
    ledcWrite(closeChannel, 0);
    ledcWrite(closeChannel, maxMotorSpeed);
    Serial.println("Gate Opening");
    if (openProximity == LOW) {
        stateChange(JAMMED_OPENING);
        Serial.println("Object Encountered While Opening");
        delay(1000);
      }
    Serial.println("Gate Open");
    stateChange(OPEN);
  }
  else {
    ledcWrite(openChannel, 255);
    ledcWrite(closeChannel, 255);
  }
  Serial.println("Turn LEDs off.");
  ledcWrite(openChannel, 0);
  ledcWrite(closeChannel, 0);
}

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) {
    stateChange(OPEN);
    Serial.println("New");
    
  }

  if (direction == closeChannel) {
    stateChange(CLOSED);
    Serial.println("New");
    
  }

  ledcWrite(direction, speed);
  Serial.println(direction);
  Serial.println(speed);
  delay(2000);
}