// Pin Definitions
const int SS1R = 5;
const int SS2R = 6;
const int FCSR = 7;
const int LUSR = 8;
const int GUS  = 2;
const int GDS  = 3;
const int S7A  = A5;
const int S7B  = A5;
const int S7C  = A3;
const int S7D  = A2;
const int S7E  = A1;
const int S7F  = A0;
const int S7G  = 13;

unsigned long lastDebounceTimeGUS = 0;
unsigned long lastDebounceTimeGDS = 0;
const unsigned long debounceDelay = 500;  // 500 ms debounce delay

void setup() {
  // Initialize pins
  pinMode(SS1R, OUTPUT);
  pinMode(SS2R, OUTPUT);
  pinMode(FCSR, OUTPUT);
  pinMode(LUSR, OUTPUT);
  pinMode(GUS, INPUT_PULLUP);
  pinMode(GDS, INPUT_PULLUP);
  pinMode(S7A, OUTPUT);
  pinMode(S7B, OUTPUT);
  pinMode(S7C, OUTPUT);
  pinMode(S7D, OUTPUT);
  pinMode(S7E, OUTPUT);
  pinMode(S7F, OUTPUT);
  pinMode(S7G, OUTPUT);

  // Start at gear 1
  transitionToG1();
}

int theGear = 1; // Global variable to track the current gear

void loop() {
  unsigned long currentTime = millis();

  // Debounce GUS button to increment gear
  if (digitalRead(GUS) == LOW && (currentTime - lastDebounceTimeGUS > debounceDelay)) {
    theGear++;
    if (theGear >= 4) theGear = 4;
    lastDebounceTimeGUS = currentTime;
  }

  // Debounce GDS button to decrement gear
  if (digitalRead(GDS) == LOW && (currentTime - lastDebounceTimeGDS > debounceDelay)) {
    theGear--;
    if (theGear <= 0) theGear = 1;
    lastDebounceTimeGDS = currentTime;
  }

  static int currentGear = -1;

  // If there is a gear change, execute the corresponding transition
  if (theGear != currentGear) {
    currentGear = theGear;
    switch (theGear) {
      case 1:
        transitionToG1();
        break;
      case 2:
        transitionToG2();
        break;
      case 3:
        transitionToG3();
        break;
      case 4:
        transitionToG4();
        break;
      default:
        Serial.println("Logic error");
        for (;;) ; // Halt if logic error occurs
        break;
    }
  }

  // Removed coil triggering code from ICRS processing
}

void transitionToG1() {
  digitalWrite(SS1R, HIGH);
  digitalWrite(SS2R, HIGH);
  digitalWrite(FCSR, HIGH);
  digitalWrite(LUSR, LOW);
  
  digitalWrite(S7A, LOW);
  digitalWrite(S7B, HIGH);
  digitalWrite(S7C, HIGH);
  digitalWrite(S7D, LOW);
  digitalWrite(S7E, LOW);
  digitalWrite(S7F, LOW);
  digitalWrite(S7G, LOW);
}

void transitionToG2() {
  digitalWrite(SS1R, HIGH);
  digitalWrite(SS2R, LOW);
  digitalWrite(FCSR, HIGH);
  digitalWrite(LUSR, HIGH);
  
  digitalWrite(S7A, HIGH);
  digitalWrite(S7B, HIGH);
  digitalWrite(S7C, LOW);
  digitalWrite(S7D, HIGH);
  digitalWrite(S7E, HIGH);
  digitalWrite(S7F, LOW);
  digitalWrite(S7G, HIGH);
}

void transitionToG3() {
  digitalWrite(SS1R, LOW);
  digitalWrite(SS2R, LOW);
  digitalWrite(FCSR, HIGH);
  digitalWrite(LUSR, HIGH);
  
  digitalWrite(S7A, LOW);
  digitalWrite(S7B, LOW);
  digitalWrite(S7C, LOW);
  digitalWrite(S7D, LOW);
  digitalWrite(S7E, LOW);
  digitalWrite(S7F, LOW);
  digitalWrite(S7G, LOW);
}

void transitionToG4() {
  digitalWrite(SS1R, LOW);
  digitalWrite(SS2R, HIGH);
  digitalWrite(FCSR, LOW);
  digitalWrite(LUSR, HIGH);
  
  digitalWrite(S7A, LOW);
  digitalWrite(S7B, LOW);
  digitalWrite(S7C, LOW);
  digitalWrite(S7D, LOW);
  digitalWrite(S7E, LOW);
  digitalWrite(S7F, LOW);
  digitalWrite(S7G, LOW);
}