// MACRO-START * MACRO-START * MACRO-START * MACRO-START * MACRO-START * MACRO-START *
// Take it for granted at the moment scroll down to void setup
// start of macros dbg and dbgi
#define dbg(myFixedText, variableName) \
  Serial.print( F(#myFixedText " "  #variableName"=") ); \
  Serial.println(variableName);
// usage: dbg("1:my fixed text",myVariable);
// myVariable can be any variable or expression that is defined in scope

#define dbgi(myFixedText, variableName,timeInterval) \
  do { \
    static unsigned long intervalStartTime; \
    if ( millis() - intervalStartTime >= timeInterval ){ \
      intervalStartTime = millis(); \
      Serial.print( F(#myFixedText " "  #variableName"=") ); \
      Serial.println(variableName); \
    } \
  } while (false);
// usage: dbgi("2:my fixed text",myVariable,1000);
// myVariable can be any variable or expression that is defined in scope
// third parameter is the time in milliseconds that must pass by until the next time a
// Serial.print is executed
// end of macros dbg and dbgi
// print only once when value has changed
#define dbgc(myFixedText, variableName) \
  do { \
    static long lastState; \
    if ( lastState != variableName ){ \
      Serial.print( F(#myFixedText " "  #variableName" changed from ") ); \
      Serial.print(lastState); \
      Serial.print( F(" to ") ); \
      Serial.println(variableName); \
      lastState = variableName; \
    } \
  } while (false);
// MACRO-END * MACRO-END * MACRO-END * MACRO-END * MACRO-END * MACRO-END * MACRO-END *

#include <Servo.h>

Servo servo;
const int analogInPin = A0;
int val = 0;
const byte servoPin = 9;
int angle = 90;
int targetAngle = 90;

unsigned long MyTestTimer = 0;                   // Timer-variables MUST be of type unsigned long
const byte    OnBoard_LED = 2;
unsigned long ServoTimer;
unsigned long myWaitTimer;


// for easy identifiying which code-Version is flashed
void PrintFileNameDateTime() {
  Serial.println( F("Code running comes from file ") );
  Serial.println( F(__FILE__) );
  Serial.print( F("  compiled ") );
  Serial.print( F(__DATE__) );
  Serial.print( F(" ") );
  Serial.println( F(__TIME__) );
}


// easy to use helper-function for non-blocking timing
boolean TimePeriodIsOver (unsigned long &startOfPeriod, unsigned long TimePeriod) {
  unsigned long currentMillis  = millis();
  if ( currentMillis - startOfPeriod >= TimePeriod ) {
    // more time than TimePeriod has elapsed since last time if-condition was true
    startOfPeriod = currentMillis; // a new period starts right here so set new starttime
    return true;
  }
  else return false;            // actual TimePeriod is NOT yet over
}


void BlinkHeartBeatLED(int IO_Pin, int BlinkPeriod) {
  static unsigned long MyBlinkTimer;
  pinMode(IO_Pin, OUTPUT);

  if ( TimePeriodIsOver(MyBlinkTimer, BlinkPeriod) ) {
    digitalWrite(IO_Pin, !digitalRead(IO_Pin) );
  }
}

// constnats fo rthe different modes of operation
const byte sm_CheckForFigure  = 0;
const byte sm_openGate        = 1;
const byte sm_waitSomeSeconds = 2;
const byte sm_closeGate       = 3;

// the variable for the mode of operation
byte myState = sm_CheckForFigure;


// move servo slowy in a non.blocking way
void moveServoSlow(byte targetAngle, unsigned long waitingPeriod) {
  int  deltaAngle;
  static unsigned long ServoTimer;

  // dbgc("m0",angle);
  if (angle != targetAngle) {
    if (angle > targetAngle) {
      deltaAngle = -1;
    }

    if (angle < targetAngle) {
      deltaAngle = 1;
    }
    if ( TimePeriodIsOver(ServoTimer, waitingPeriod) ) {
      angle += deltaAngle;
      dbgi("a+da",angle,1000);
      servo.write(angle);
    }
  }

}


void myStateMachine() {
  static unsigned long TimeToWait;
  dbgc("State:",myState);
  val = analogRead(analogInPin);   // This reads the value of the sensor to see if an object is detected then saves in the val variable.
  dbgi("above switch",val,1000);

  switch (myState) {

    case sm_CheckForFigure:
      if (val < 600) {
        Serial.println("figure detected start opening gate");
        targetAngle = 75;
        ServoTimer = millis();
        myState = sm_openGate;
      }
      break; // immidiately jump down to End-of-Switch


    case sm_openGate:
      moveServoSlow(targetAngle, 20);
      dbgc("openG",angle);
      if (angle == targetAngle) {
        Serial.println("gate opened start waiting");
        myWaitTimer = millis();
        myState = sm_waitSomeSeconds;
      }
      break; // immidiately jump down to End-of-Switch

    case sm_waitSomeSeconds:
      TimeToWait = (7000 - (millis() - myWaitTimer) ) / 1000; // print seconds left until closing starts
      dbgi("wait",TimeToWait,1000);

      if (val < 600) { // check for figure
        myWaitTimer = millis(); // if figure ist detected reset timer to start new
      }
      if ( TimePeriodIsOver(myWaitTimer, 7000) ) { // check if waiting time is over
        Serial.println("Waiting-Time over start closing gate");
        targetAngle = 180;
        ServoTimer = millis();
        myState = sm_closeGate;
      }
      break; // immidiately jump down to End-of-Switch

    case sm_closeGate:
      moveServoSlow(targetAngle, 20);
      if (angle == targetAngle) {
        Serial.println("gate closed check for figure");
        myState = sm_CheckForFigure;
      }
      break; // immidiately jump down to End-of-Switch
  } // End-of-Switch
}


void setup() {
  Serial.begin(115200);
  Serial.println("Setup-Start");
  PrintFileNameDateTime();
  servo.attach(servoPin);
  angle = 90;
  targetAngle = 90;
}


void loop() {
  BlinkHeartBeatLED(OnBoard_LED, 250);

  myStateMachine();
}