int pilotLeftPin = 2;
int pilotRightPin = 3;
int resetPin = 4;
int firstPin;
int secondPin;
int counter = 0;
unsigned long currentTime;
unsigned long previousTime;
//unsigned long elapsedTime;
int countDelay = 1000;
int debounce = 70;


void setup() {

pinMode(pilotLeftPin, INPUT_PULLUP);
pinMode(pilotRightPin, INPUT_PULLUP);
pinMode(resetPin, INPUT_PULLUP);
Serial.begin(9600);
}

void loop() {
  sidestick();
}

void sidestick() {

  static unsigned long currentTime;
  static unsigned long previousTime;
  //static uint16_t counter;
  static uint8_t firstPin;
  static uint8_t secondPin;
  

  enum class stickState: uint8_t {
    IDLE,
    COUNTUP,
  };

  static stickState currState = stickState::IDLE;
   
switch (currState) {

  case stickState::IDLE:
    displayState("IDLE state");

    if(digitalRead(pilotLeftPin) == LOW) {
      firstPin = pilotLeftPin;
      secondPin = pilotRightPin;
      previousTime = millis();
      currState = stickState::COUNTUP;
    }
    else if(digitalRead(pilotRightPin) == LOW) {
      firstPin = pilotRightPin;
      secondPin = pilotLeftPin;
      previousTime = millis();
      currState = stickState::COUNTUP;
    }
  break;

  case stickState::COUNTUP:
    displayState("COUNTUP state");
    digitalRead(firstPin);
    digitalRead(secondPin);
    while(digitalRead(firstPin) == LOW) {
      currentTime = millis();
      //Serial.println(previousTime);
      //delay(200);
      //Serial.println(currentTime);
      //delay(200);
      if((currentTime - previousTime) >= countDelay){
        //Serial.println(counter);
        //delay(200);
        counter = counter + 1;
        if(counter > 6) {
          counter = 6;
        }
        Serial.println(counter);
        //delay(200);
        previousTime = millis();
      }
    } 
    while(digitalRead(secondPin) == LOW) {
      currentTime = millis();
      if((currentTime - previousTime) >= countDelay){
        Serial.println(counter);
        counter = counter - 1;
        if(counter < 1) {
          counter = 1;
        }
        previousTime = currentTime;
      }
    }
    
    if(digitalRead(resetPin) == LOW) {
      counter = 0;
      currState = stickState::IDLE;
    }
    break;
  default:
    Serial.println("Default switch case reached - Error");
  }
}

void displayState(String currState) {
  static String prevState = "";

  if (currState != prevState) {
      Serial.println(currState);
      prevState = currState;
  }
}