// Pin Definitions
const int SS1R = 2;
const int SS2R = 3;
const int FCSR = 4;
const int LUSR = 5;
const int GUS = 7;
const int GDS = 8;
const int ICRS = 13;
const int COIL1_4 = 9;
const int COIL2_3 = 10;
const int S7A = A4;
const int S7B = A5;
const int S7C = A1;
const int S7D = A0;
const int S7E = 12;
const int S7F = A3;
const int S7G = A2;

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(ICRS, INPUT_PULLUP);
  pinMode(COIL1_4, OUTPUT);
  pinMode(COIL2_3, OUTPUT);
  pinMode(S7A, OUTPUT);
  pinMode(S7B, OUTPUT);
  pinMode(S7C, OUTPUT);
  pinMode(S7D, OUTPUT);
  pinMode(S7E,OUTPUT);
  pinMode(S7F,OUTPUT);
  pinMode(S7G, OUTPUT);
  

  // Start at G1
  transitionToG1();
}

int theGear = 1;    // what gear are we in global

void loop() {
    static int currentState = 1;

    if (digitalRead(GUS) == LOW) {
      theGear++; if (theGear >= 4) theGear = 4;
    }

    if (digitalRead(GDS) == LOW) {
      theGear--; if (theGear <= 0) theGear = 1;
    }

    static int currentGear = -1;    // no, it does not!

// new gear?
    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 (; ; );
        break;
      }
    }

    if (digitalRead(ICRS) == LOW) {
        digitalWrite(COIL1_4, LOW);
        digitalWrite(COIL2_3, LOW);
        while (digitalRead(ICRS) == LOW); // Wait until ICRS goes HIGH
        digitalWrite(COIL1_4, HIGH);
        digitalWrite(COIL2_3, HIGH);
    }
}

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);
    triggerCoils();
}

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

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

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

void triggerCoils() {
    digitalWrite(COIL1_4, LOW);
    digitalWrite(COIL2_3, LOW);
    delay(125);
    digitalWrite(COIL1_4, HIGH);
    digitalWrite(COIL2_3, HIGH);
}