#include <Joystick.h>

int reedPin = 2;
int reversePin = 3;
boolean reedKontakt = false;
boolean reverseKontakt = false;
boolean reedState = false;

long int timeStamp = 0;
long int recentTimeStamp = 0;
long int otherTimeStamp = 0;
int duration = 0;

boolean gateopen = false;

int count = 0;

void setup()
{
  Joystick.begin();
  Serial.begin(9600);
  Serial.println("Trimm dich Rad"); 
  
  pinMode(reedPin, INPUT);
  pinMode(reversePin, INPUT);
}

void loop()
{
  reedKontakt = digitalRead(reedPin);

  otherTimeStamp = millis();
  if((otherTimeStamp - recentTimeStamp) > 600) {
    reedKontakt = !reedState;
    //Serial.println("WORKAROUND");
  }
  else {
  }
  if (reedKontakt != reedState)
  {   
    Serial.println("FEHLER"); 
    if (reedKontakt == HIGH && gateopen)
    {
      timeStamp = millis();
      reverseKontakt = digitalRead(reversePin);
      duration = timeStamp - recentTimeStamp;
      int speed = 0;
      if(duration > 500) {
        speed = 0;
        Joystick.setYAxis(speed);
      }
      else {
        speed = -127 + (duration / 4);
        if(reverseKontakt)
        {
          speed *= -1;
        }
        Joystick.setYAxis(speed);
      }
      
      gateopen = false;
    } else if (reedKontakt == HIGH)
    {
      gateopen = true;
    }
      
    recentTimeStamp = timeStamp;
    reedState = reedKontakt;
    //Serial.println();
    
  }
}