// Note: I should probably put resistors between the output shift register and the transistors controlling the DPDT relays


// Include Libraries and Information
#include <LiquidCrystal_I2C.h>


// LCD Setup:
#define I2C_ADDR    0x27
#define LCD_COLUMNS 20
#define LCD_LINES   4
LiquidCrystal_I2C lcd(I2C_ADDR, LCD_COLUMNS, LCD_LINES);


// Pin Definitions:
const byte dataPinOut = 6;      // "DS"
const byte latchPinOut = 7;     // "STCP"
const byte clockPinOut = 8;     // "SHCP"
const byte clockPinIn = 9;      // "CP"
const byte latchPinIn = 10;     // "PL"
const byte dataPinIn = 11;      // "Q7"
const byte allPowerOnPin = 12;  // Toggle power ON to all tracks
const byte allPowerOffPin = 3;  // Toggle power OFF to all tracks
const byte enableProgrammingTrackPin = 13;  // Enable a programming track
const byte rotaryCLKPin = 2;
const byte rotaryDTPin = 4;
const byte rotarySWPin = 5;
const byte leadTrackPowerOutputPin = A0;
const byte leadTrackPowerButtonPin = A1;


// Global Constant Definitions:
const int bitsPerChip = 8;      // The number of bits per shift register.
const int numShiftRegsIn = 1;   // The number of shift registers (for input).
const int numShiftRegsOut = 2;  // The number of shift registers (for output).
const int numBitsIn = bitsPerChip*numShiftRegsIn;     // Set to 8x the number of input shift registers.
const int numBitsOut = bitsPerChip*numShiftRegsOut;   // Set to 8x the number of output shift registers.
const int delayTime = 50;                             // How long to wait between each running of the main loop
// const int toggleAllDelayTime = 250;                   // 
// const int progTrackToggleDelayTime = 100;             // How long to delay when switching to/from being a programming track (power off --> switch --> power on)
// const int debounceDelayTime = 50;                     // How long to wait after a button press (non interrupt/shift register)


#define NUM_SHIFT_REGS_OUT 2
const uint8_t numOfRegisterPinsOut = NUM_SHIFT_REGS_OUT * 8;

const bool showOutArrayDebug = HIGH;      // Controls if the toggleArray is printed to the Serial Monitor for debug/testing (HIGH = Displayed, LOW = Not Displayed)
const bool showLoopTimeDebug = HIGH;      // Controls if the loop time is printed to the Serial Monitor for debug/testing (HIGH = Displayed, LOW = Not Displayed)

const int numTracks = 7;                  // Number of tracks to select from (count starting with 0)



// Global Variable Definitions:
bool enableProgTrack = LOW;                         // Variable to control if a programming track should be enabled (HIGH = Yes, LOW = No)
bool enableProgTrackPrevious = LOW;                 // Variable to store the previous value for if a programming track should be enabled (HIGH = Yes, LOW = No)
bool enableProgTrackSwitchState = HIGH;             // Variable to store the state of the switch to control if a programming trach should be enabled
int progTrackSelection = 0;                         // Variable to store the programming track that is being selected
int progTrackSelectionConfirmed = 0;                // Variable to store active programming track
bool progTrackSelectionUpdate = true;               // Variable to store if the programming track that is being selected has changed
bool progTrackSelectionConfirmedUpdate = true;      // Variable to store if the programming track that is being selected has changed
bool leadTrackPowerState = LOW;                     // Variable to store the power if the power is on or off to the lead track
bool leadTrackToggleButtonState = HIGH;             // Variable to store the state of the button to toggle power to the lead track
bool leadTrackToggleButtonPreviousState = HIGH;     // Variable to store the previous state of the button to toggle power to the lead track
bool leadTrackPowerStateUpdate = true;              // Variable to store if the power state of the lead track has/should change
bool storageTrackPowerStateUpdate = true;           // Variable to store if the power state of one of the storage tracks has changed
bool allPowerOnButtonState = HIGH;                  // Variable to store the state of the button to turn all track power on
bool allPowerOnButtonPreviousState = HIGH;          // Variable to store the previous state of the button to turn all track power on
bool rotaryButtonState = HIGH;                      // Variable to store the state of the rotary encoder's button
bool rotaryButtonPreviousState = HIGH;              // Variable to store the previous state of the rotary encoder's button


bool storageTrackPowerButtonStates[numTracks+1];    // Array to store the states of the storage track power toggle buttons
bool storageTrackPowerButtonPreviusStates[numTracks+1];   // Array to store the previous states of the storage track power toggle buttons

bool storageTrackPowerStates[numTracks+1];          // Array to store the power states of the storage tracks
bool progTrackBoolArray[numTracks+1];               // Array to store the programming track status of the storage tracks

bool combinedOutputArray[numBitsOut];               // Array to store the values to be output to the shift register(s)


// Debug Configuration:
const bool serialPrintDebug = false;                 // Should debug information be printed to the serial monitor



void setup() {
  // Set pinModes:
  pinMode(dataPinOut, OUTPUT);
  pinMode(latchPinOut, OUTPUT);
  pinMode(clockPinOut, OUTPUT);
  pinMode(clockPinIn, OUTPUT);
  pinMode(latchPinIn, OUTPUT);
  pinMode(dataPinIn, INPUT);
  pinMode(allPowerOnPin, INPUT_PULLUP);
  pinMode(allPowerOffPin, INPUT_PULLUP);
  pinMode(enableProgrammingTrackPin, INPUT_PULLUP);
  pinMode(rotaryCLKPin,INPUT_PULLUP);
  pinMode(rotaryDTPin, INPUT);
  pinMode(rotarySWPin, INPUT_PULLUP);
  pinMode(leadTrackPowerOutputPin, OUTPUT);
  pinMode(leadTrackPowerButtonPin, INPUT_PULLUP);

  // Attach Interrupt(s):
  attachInterrupt(digitalPinToInterrupt(rotaryCLKPin), rotaryISR, FALLING);
  attachInterrupt(digitalPinToInterrupt(allPowerOffPin), allPowerOffISR, FALLING);


  // Begin Serial Communication
  if (serialPrintDebug == true) {
    Serial.begin(115200);
    Serial.println("Printing debug information to serial monitor.");
  }


  // Pre-set array values:
  for (int i=0; i<=numTracks; i++) {
    storageTrackPowerStates[i] = LOW;               // Pre-set array for storage track power states
    progTrackBoolArray[i] = LOW;                    // Pre-set array for programming track selection

    storageTrackPowerButtonStates[i] = LOW;         // Pre-set array for state of storage track power state toggle buttons
    storageTrackPowerButtonPreviusStates[i] = LOW;  // Pre-set array for state of previous storage track power state toggle buttons

    combinedOutputArray[i] = LOW;                   // Pre-set array for output to the shift registers
    combinedOutputArray[i+bitsPerChip] = LOW;
  }


  // Setup LCD:
  lcd.init();
  lcd.backlight();
  lcd.clear();

  lcd.setCursor(0,0);
  lcd.print("TRK PWR:");
  lcd.setCursor(0,1);
  lcd.print("ACTIVE PROG TRK:");
  lcd.setCursor(0,2);
  lcd.print("SELECT PROG TRK:");

  if (digitalRead(enableProgrammingTrackPin) == LOW) {
    lcd.setCursor(16,1);
    lcd.print(" ");
    lcd.print(progTrackSelectionConfirmed);
    lcd.print(" ");
  } else if (digitalRead(enableProgrammingTrackPin) == HIGH) {
    lcd.setCursor(16,1);
    lcd.print(" -- ");
  }
  lcd.setCursor(16,2);
  lcd.print(" ");
  lcd.print(progTrackSelection);
  lcd.print(" ");
  lcd.setCursor(0,3);
  lcd.print("LEAD TRK PWR:");
  lcd.setCursor(14,3);
  if (leadTrackPowerState==LOW) {
    lcd.print("OFF");
  } else if (leadTrackPowerState==HIGH) {
    lcd.print("ON ");
  }
  
}

void loop() { 
  // Run Code At The Beginning Of The Loop:
  readInputs();


  // Interpret Inputs:
  interpretInputs();


  if (leadTrackPowerStateUpdate == true && serialPrintDebug == true) {
    Serial.println(leadTrackPowerState);
  }
  if ((storageTrackPowerStateUpdate == true || progTrackSelectionConfirmedUpdate == true) && serialPrintDebug == true) {
    for (int i=0; i<numBitsOut-1; i++) {
      Serial.print(combinedOutputArray[i]);
      Serial.print(" ");
    }
    Serial.println(combinedOutputArray[numBitsOut-1]);

    // for (int i=0; i<numBitsIn-1; i++) {
    //   Serial.print(progTrackBoolArray[i]);
    //   Serial.print(" ");
    // }
    // Serial.println(progTrackBoolArray[numBitsIn-1]);
  }
 
  // Update LCD As Needed:

  if (storageTrackPowerStateUpdate==true) {       // Update display of storage track power states
    lcd.setCursor(9,0);
    for (int i=0; i<=numTracks; i++) {
      lcd.print(storageTrackPowerStates[i]);
    }
  }
  if (progTrackSelectionConfirmedUpdate==true) {           // Update display of active programming track
    lcd.setCursor(17,1);
    if (enableProgTrack == HIGH) {
      if (progTrackSelectionConfirmed < 10) {
        lcd.print(progTrackSelectionConfirmed);
        lcd.print("  ");
      } else if (progTrackSelectionConfirmed >=10 && progTrackSelectionConfirmed < 100) {
        lcd.print(progTrackSelectionConfirmed);
        lcd.print(" ");
      }
    } else if (enableProgTrack == LOW) {
      lcd.print("-- ");
    }
  }
  if (progTrackSelectionUpdate==true) {           // Update display of programming track selection
    lcd.setCursor(17,2);
    if (progTrackSelection < 10) {
      lcd.print(progTrackSelection);
      lcd.print("  ");
    } else if (progTrackSelection >=10 && progTrackSelection < 100) {
      lcd.print(progTrackSelection);
      lcd.print(" ");
    }
  }
  if (leadTrackPowerStateUpdate==true) {          // Update display of the lead track power state
    lcd.setCursor(14,3);
    if (leadTrackPowerState==LOW) {
      lcd.print("OFF");
    } else if (leadTrackPowerState==HIGH) {
      lcd.print("ON ");
    }
  }
  // Serial.print(progTrackSelection);
  // Serial.println(progTrackSelectionConfirmed);

  // Write the outputs
  writeOutputs();

  // Run Code At The End Of The Loop:
  storageTrackPowerStateUpdate = false;
  progTrackSelectionConfirmedUpdate = false;
  progTrackSelectionUpdate = false;
  leadTrackPowerStateUpdate = false;

  delay(delayTime);
}


// ISRs: (Interrupt Service Routines)
void rotaryISR() {                                              // ISR to run if the rotary decoder has been turned
  byte dtPinStatus = digitalRead(rotaryDTPin);
  if (dtPinStatus == HIGH) {                        // Clockwise
    progTrackSelection = progTrackSelection + 1;
  }
  if (dtPinStatus == LOW) {                         // Counterclockwise
    progTrackSelection = progTrackSelection - 1;
  }
  // Check for selection limits:
  if (progTrackSelection > numTracks) {             // If we have exceeded the number of tracks controlled
    progTrackSelection = 0;
  }
  if (progTrackSelection < 0) {
    progTrackSelection = numTracks;
  }
  progTrackSelectionUpdate = true;
}

void allPowerOffISR() {
  leadTrackPowerState = LOW;
  // leadTrackToggleButtonState = LOW;
  // leadTrackToggleButtonPreviousState = LOW;
  // digitalWrite(leadTrackPowerOutputPin, LOW);
  leadTrackPowerStateUpdate = true;

  for (int i=0; i<numBitsIn; i++) {
    storageTrackPowerStates[i] = LOW;
    combinedOutputArray[i+numBitsIn] = storageTrackPowerStates[i];
  }
  storageTrackPowerStateUpdate = true;
  writeOutputs();
}


void readInputs() {           // Read all of the non interrupt and non-rotational inputs
  // Read buttons on shift register:
  digitalWrite(latchPinIn, LOW);
  digitalWrite(latchPinIn, HIGH);
  for (int i=0; i<numBitsIn; i++) {
    storageTrackPowerButtonPreviusStates[i] = storageTrackPowerButtonStates[i];
  }
  for (int i=0; i<numShiftRegsIn; i++) {
    for (int j=(bitsPerChip-1); j>=0; j--) {
      int indexVar = (bitsPerChip*i) + j;
      int bit = digitalRead(dataPinIn);
      if (bit == HIGH) {
        storageTrackPowerButtonStates[indexVar] = LOW;
      } else {
        storageTrackPowerButtonStates[indexVar] = HIGH;
      }
      digitalWrite(clockPinIn, HIGH);
      digitalWrite(clockPinIn, LOW);
    }
  }


  // Read others:
  enableProgTrackSwitchState = digitalRead(enableProgrammingTrackPin);  // Read switch to control if a programming track should be enabled

  rotaryButtonPreviousState = rotaryButtonState;                        // Update button state for button on rotary encoder
  rotaryButtonState = digitalRead(rotarySWPin);

  allPowerOnButtonPreviousState = allPowerOnButtonState;                // Update button state for button to turn on power to all tracks
  allPowerOnButtonState = digitalRead(allPowerOnPin);

  leadTrackToggleButtonPreviousState = leadTrackToggleButtonState;      // Update button states for toggling lead track power
  leadTrackToggleButtonState = digitalRead(leadTrackPowerButtonPin);
}


void interpretInputs() {      // Interpret/handle/translate the inputs as necessary
  // Lead track power state
  if (leadTrackToggleButtonState == LOW && leadTrackToggleButtonPreviousState == HIGH) {
    leadTrackPowerState = !leadTrackPowerState;
    leadTrackPowerStateUpdate = true;
  } 
  // Storage track power states
  bool hasChange = false;
  for (int i=0; i<numBitsIn; i++) {
    if (storageTrackPowerButtonPreviusStates[i]==LOW && storageTrackPowerButtonStates[i]==HIGH) {
      hasChange = true;
    }
  }
  if (hasChange==true) {
    storageTrackPowerStateUpdate = true;
    for (int i=0; i<numBitsIn; i++) {
      if (storageTrackPowerButtonPreviusStates[i]==LOW && storageTrackPowerButtonStates[i]==HIGH) {
        storageTrackPowerStates[i] = !storageTrackPowerStates[i];
      }
      combinedOutputArray[i+numBitsIn] = storageTrackPowerStates[i];
    }
  }
  // rotaryButtonState
  if (rotaryButtonPreviousState == HIGH && rotaryButtonState == LOW) {
    progTrackSelectionConfirmedUpdate = true;
    progTrackSelectionConfirmed = progTrackSelection;
  }
  // enableProgTrack
  enableProgTrackPrevious = enableProgTrack;
  if (enableProgTrackSwitchState==LOW) {
    enableProgTrack = HIGH;
  } else if (enableProgTrackSwitchState==HIGH) {
    enableProgTrack = LOW;
  }
  if (enableProgTrackPrevious != enableProgTrack) {
    progTrackSelectionConfirmedUpdate = true;
  }
  if (enableProgTrack == LOW) {
    for (int i=0; i<=numTracks; i++) {
      progTrackBoolArray[i] = LOW;
      combinedOutputArray[i] = LOW;
    }
  } else if (enableProgTrack == HIGH) {
    for (int i=0; i<=numTracks; i++) {
      progTrackBoolArray[i] = LOW;
      combinedOutputArray[i] = LOW;
    }
    progTrackBoolArray[progTrackSelectionConfirmed] = HIGH;
    combinedOutputArray[progTrackSelectionConfirmed] = HIGH;
  }
  // allPowerOnButtonState
  if (allPowerOnButtonPreviousState==HIGH && allPowerOnButtonState==LOW) {
    storageTrackPowerStateUpdate = true;
    leadTrackPowerStateUpdate = true;
    leadTrackPowerState = HIGH;
    for (int i=0; i<numBitsIn; i++) {
      storageTrackPowerStates[i] = HIGH;
      combinedOutputArray[i+numBitsIn] = storageTrackPowerStates[i];
    }
  }
  
}

void writeOutputs() {
  if (leadTrackPowerStateUpdate==true) {
    digitalWrite(leadTrackPowerOutputPin, leadTrackPowerState);
  }
  if ((storageTrackPowerStateUpdate==true) || (progTrackSelectionConfirmedUpdate==true)) {
    digitalWrite(latchPinOut, LOW);
    for (int i=numOfRegisterPinsOut - 1; i>=0; i--) {
      digitalWrite(clockPinOut, LOW);
      digitalWrite(dataPinOut, combinedOutputArray[i]);
      digitalWrite(clockPinOut, HIGH);
    }
    digitalWrite(latchPinOut, HIGH);
  }
}