#include <Adafruit_NeoPixel.h>

// 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;
const int NEOPIXEL_PIN = 6;
const int NEOPIXEL_COUNT = 4;

Adafruit_NeoPixel strip = Adafruit_NeoPixel(NEOPIXEL_COUNT, NEOPIXEL_PIN, NEO_GRB + NEO_KHZ800);

bool previousGUSState = HIGH;
bool previousGDSState = HIGH;

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);

  // Initialize NeoPixels
  strip.begin();
  strip.show();

  // Start at G1
  transitionToG1();
}

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

void loop() {
  static int currentState = 1;

  bool currentGUSState = digitalRead(GUS);
  bool currentGDSState = digitalRead(GDS);

  // Check for button press and release for GUS
  if (currentGUSState == LOW && previousGUSState == HIGH) {
    theGear++;
    if (theGear >= 4) theGear = 4;
    lightNeopixels(255, 0, 255, 125); // Purple
  }
  previousGUSState = currentGUSState;

  // Check for button press and release for GDS
  if (currentGDSState == LOW && previousGDSState == HIGH) {
    theGear--;
    if (theGear <= 0) theGear = 1;
    lightNeopixels(0, 128, 128, 125); // Red
  }
  previousGDSState = currentGDSState;

  static int currentGear = -1;

  // 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);
}

void lightNeopixels(uint8_t red, uint8_t green, uint8_t blue, int duration) {
  for (int i = 0; i < NEOPIXEL_COUNT; i++) {
    strip.setPixelColor(i, strip.Color(red, green, blue));
  }
  strip.show();
  delay(duration);
  strip.clear();
  strip.show();
}