#include <Servo.h>

int inpRequestEntryPB = 2;
int inpEntryDetection = 3;
int inpRequestExitPB = 4;
int inpExitDetection = 5;
int inpBarrierPositionUp = 6;
int inpBarrierPositionDown = 7;
int outWarningLamp = 8;
int outEntryOkLamp = 9;
int outExitOkLamp = 10;

int SequenceNumber = 0;
int SequenceNumberNext = 0;

int inpRequestEntryPBStatus;
int inpEntryDetectionStatus;
int inpRequestExitPBStatus;
int inpExitDetectionPBStatus;
int inpExitDetectionStatus;
int inpBarrierPositionUpStatus;
int inpBarrierPositionDownStatus;
int gatePosition;

bool debugMessagesOn = true;


Servo servoBarrier;

void setup() {

  // Assign IO
  pinMode(inpRequestEntryPB, INPUT);
  pinMode(inpEntryDetection, INPUT);
  pinMode(inpRequestExitPB, INPUT);
  pinMode(inpExitDetection, INPUT);
  pinMode(inpBarrierPositionUp, INPUT);
  pinMode(inpBarrierPositionDown, INPUT);
  
  pinMode(outWarningLamp, OUTPUT);
  pinMode(outEntryOkLamp, OUTPUT);
  pinMode(outExitOkLamp, OUTPUT);

  // Attach Servo
  servoBarrier.attach(11);


  if (debugMessagesOn){
    Serial.begin(9600);
  }  

}

void loop() {

  // Read Input Statuses
  inpRequestEntryPBStatus = digitalRead(inpRequestEntryPB);
  inpEntryDetectionStatus = digitalRead(inpEntryDetection);
  inpRequestExitPBStatus = digitalRead(inpRequestExitPB);
  inpExitDetectionPBStatus = digitalRead(inpExitDetection);
  inpBarrierPositionUpStatus = digitalRead(inpBarrierPositionUp);
  inpBarrierPositionDownStatus = digitalRead(inpBarrierPositionDown);

  // Update Sequence Numbers if required
  if (SequenceNumber != SequenceNumberNext){
    SequenceNumber = SequenceNumberNext;
  }

  switch (SequenceNumber){

    // Wait for a Request
    case 0:
      if (debugMessagesOn){
        Serial.println("Waiting Request");
      }
        if (inpRequestEntryPBStatus){
          SequenceNumberNext = 1000;
          gatePosition = 90;
        }
        else if (inpRequestExitPBStatus){
          SequenceNumberNext = 2000;
        }
      break;
    
    // Start Opening Gate
    case 1000:
      if (debugMessagesOn){
        Serial.println("Opening Gate");
      }
      if (gatePosition > 0){
        gatePosition = gatePosition - 5;
      }
      servoBarrier.write(gatePosition);
      if (gatePosition%20 == 0){
        digitalWrite(outWarningLamp, HIGH);
      }
      else {
        digitalWrite(outWarningLamp, LOW);
      }
      if ((gatePosition == 0) && (inpBarrierPositionUpStatus)){
        SequenceNumberNext = 1010;
      }
      break;
    
    // Wait Car
    case 1010:
      if (debugMessagesOn){
        Serial.println("Waiting Car");
      }
    digitalWrite(outWarningLamp, LOW);
      digitalWrite(outEntryOkLamp, HIGH);
      break;

    case 2000:

      break;



  }
  delay(100);

}