#include <Servo.h>
const byte sensorPin = A0;
const byte sensorThreshold = 800;

enum {OPENING, OPEN, CLOSING, CLOSED} currentState = CLOSED;

const byte gateMotorPin = 9;
const int gateClosedAngle = 2;
const int gateOpenAngle = 90;
const unsigned long gateStepPeriod = 20; // 1° every gateStepPeriod ms when moving
int currentGateAngle = gateClosedAngle;

Servo gateMotor;

unsigned long lastDetectionTime;
const unsigned long openDuration = 7000ul; // 7s in ms

bool personDetected() {
  return analogRead(sensorPin) >= sensorThreshold;
}

void takeOneStepIfNeeded() {
  static unsigned long lastStepTime;
  currentGateAngle = gateMotor.read();
  if (currentState == OPENING) {
    if ((currentGateAngle < gateOpenAngle) && (millis() - lastStepTime >= gateStepPeriod)) {
      gateMotor.write(++currentGateAngle);
      lastStepTime = millis();
    }
  } else if (currentState == CLOSING) {
    if ((currentGateAngle > gateClosedAngle) && (millis() - lastStepTime >= gateStepPeriod)) {
      gateMotor.write(--currentGateAngle);
      lastStepTime = millis();
    }
  }
}

void action() {
  switch (currentState) {
    case OPENING:
      takeOneStepIfNeeded();
      if (currentGateAngle >= gateOpenAngle) {
        currentState = OPEN;
        lastDetectionTime = millis();
      }
      break;

    case OPEN:
      if (personDetected()) {        // Detection whilst opened: reset timeout
        lastDetectionTime = millis();
      } else if (millis() - lastDetectionTime >= openDuration) {          // time's up. Closing
        currentState = CLOSING;
      }
      break;

    case CLOSING:
      if (personDetected()) {        // Detection whilst closing: re-opening
        currentState = OPENING;
      } else {
        takeOneStepIfNeeded();
        if (currentGateAngle <= gateClosedAngle) {
          currentState = CLOSED;
        }
      }
      break;

    case CLOSED:
      if (personDetected()) {        // Detection => opening
        currentState = OPENING;
      }
      break;
  }
}

void setup() {
  gateMotor.write(currentGateAngle);
  gateMotor.attach(gateMotorPin);
}

void loop() {
  action();
}