/* Definicje pinów
  Pin definitions */
// 40 Button, 10 column, 4 row, 5 x 74HC165
// Wejścia przycisków (Serial Load)
// Button input
const int REG_SL = 2;
// Zegar dla przycisków (CK - Clock)
// Input clock
const int REG_CK = 3;
// Dane wejściowe przycisków (DI - Data In)
// Input data
const int REG_DI = 4;

// 40 Relay, 5 x 74HC595
// Wyjście sterujące przekaźnikami (Latch Pin)
// Relay control output
const int latchPin = 5;
// Zegar dla przekaźników
// Relay output clock
const int clockPin = 6;
// Dane wyjściowe przekaźników
// Relay output data
const int dataPin = 7;

// 40 Feedback Relay, 5 x 74HC165
// Feedback przekaźników
// Feedback state input
const int REG_SL2 = 8;
// Zegar stanu Feedback
// Feedback state clock
const int REG_CK2 = 9;
// Dane stanu Feedback
// Feedback state data
const int REG_DI2 = 10;

// 40 LED, 5 x 74HC595
// Wyjście sterujące diodami LED
// LED control output
const int latchPin2 = 11;
// Zegar wyjściowy LED
// LED output clock
const int clockPin2 = 12;
// Dane wyjściowe LED
// LED output data
const int dataPin2 = 13;

byte buttonStates[5];     // Stany przycisków (5 bajtów)
byte relayStates[5];      // Stany przekaźników (5 bajtów)
byte feedbackStates[5];   // Stany feedbacku przekaźników (5 bajtów)
byte ledStates[5];        // Stany diod LED (5 bajtów)
byte prevLedStates[5];    // Poprzednie stany LED
byte prevFeedbackStates[5]; // Poprzednie stany feedbacku

const int pulseDelay = 2; // Opóźnienie zegara
const unsigned long relayOnTime = 500; // Czas włączenia przekaźnika (1 sekunda)
unsigned long relayTimers[40]; // Tablica czasów włączenia przekaźników

void setup() {
  Serial.begin(9600);
  pinMode(REG_SL, OUTPUT);
  pinMode(REG_CK, OUTPUT);
  pinMode(REG_DI, INPUT);
  pinMode(latchPin, OUTPUT);
  pinMode(clockPin, OUTPUT);
  pinMode(dataPin, OUTPUT);
  pinMode(REG_SL2, OUTPUT);
  pinMode(REG_CK2, OUTPUT);
  pinMode(REG_DI2, INPUT);
  pinMode(latchPin2, OUTPUT);
  pinMode(clockPin2, OUTPUT);
  pinMode(dataPin2, OUTPUT);
  //Serial.println("READY");
}

void loop() {
  ReadButtons();
  HandleButtons();
  ReadFeedback();
  UpdateRelays();
  UpdateLEDs();
  delay(50);
}

// Funkcje dla pierwszego Arduino (odczyt przycisków i wysyłanie poleceń)
void ReadButtons() {
  digitalWrite(REG_SL, LOW);
  delayMicroseconds(pulseDelay);
  digitalWrite(REG_SL, HIGH);
  delayMicroseconds(pulseDelay);

  for (int i = 0; i < 5; i++) {
    byte currentState = ReadOne165(REG_DI, REG_CK);
    buttonStates[i] = currentState;
  }
}

void HandleButtons() {
  for (int i = 0; i < 5; i++) {
    for (int bit = 0; bit < 8; bit++) {
      if (buttonStates[i] & (1 << bit)) {
        int buttonNumber = (bit + 1) + (i * 8);
        //Serial.print("Button ");
        Serial.print(buttonNumber);
        //Serial.println(" pressed.");
        sendRelayCommand(buttonNumber, true); // Wyślij polecenie do drugiego Arduino
        turnRelay(buttonNumber, true); // Włącz przekaźnik na pierwszym Arduino
        ledStates[i] ^= (1 << bit); // Przełącz stan LED
        UpdateLEDs(); // Aktualizacja LED
      }
    }
  }
}

void sendRelayCommand(int relayNumber, bool state) {
  //Serial.print("RELAY: ");
  Serial.print(relayNumber);
  //Serial.println(state ? " ON" : " OFF");
}

// Funkcje dla drugiego Arduino (sterowanie przekaźnikami)
void turnRelay(int relayNumber, bool state) {
  int byteIndex = (relayNumber - 1) / 8;
  int bitIndex = (relayNumber - 1) % 8;

  if (state) {
    bitSet(relayStates[byteIndex], bitIndex);
    relayTimers[relayNumber - 1] = millis();
  } else {
    bitClear(relayStates[byteIndex], bitIndex);
    relayTimers[relayNumber - 1] = 0;
  }

  digitalWrite(latchPin, LOW);
  for (int i = 4; i >= 0; i--) {
    shiftOut(dataPin, clockPin, MSBFIRST, relayStates[i]);
  }
  digitalWrite(latchPin, HIGH);

  //Serial.print("Relay ");
  Serial.print(relayNumber);
  //Serial.println(state ? " ON." : " OFF.");
}

void UpdateRelays() {
  for (int i = 0; i < 40; i++) {
    if (relayTimers[i] != 0 && millis() - relayTimers[i] >= relayOnTime) {
      turnRelay(i + 1, false);
    }
  }
}

// Funkcje wspólne (odczyt feedbacku i aktualizacja LED)
void ReadFeedback() {
  digitalWrite(REG_SL2, LOW);
  delayMicroseconds(pulseDelay);
  digitalWrite(REG_SL2, HIGH);
  delayMicroseconds(pulseDelay);

  for (int i = 0; i < 5; i++) {
    byte currentState = ReadOne165(REG_DI2, REG_CK2);
    if (currentState != feedbackStates[i]) {
      feedbackStates[i] = currentState;
      UpdateLEDs();
    }
  }
}

void UpdateLEDs() {
  if (memcmp(ledStates, prevLedStates, 5) != 0) { // Sprawdź, czy stan LED się zmienił
    digitalWrite(latchPin2, LOW);
    for (int i = 4; i >= 0; i--) {
      shiftOut(dataPin2, clockPin2, MSBFIRST, ledStates[i]);
    }
    digitalWrite(latchPin2, HIGH);
    for (int i = 0; i < 40; i++) {
      int byteIndex = i / 8;
      int bitIndex = i % 8;
      if (bitRead(ledStates[byteIndex], bitIndex) != bitRead(prevLedStates[byteIndex], bitIndex)) { // Sprawdź, czy stan LED się zmienił
        //Serial.print("LED nr ");
        Serial.print(i + 1);
        //Serial.println(bitRead(ledStates[byteIndex], bitIndex) ? " ON" : " OFF"); // Wyświetl tylko zmienione LEDy
      }
    }
    memcpy(prevLedStates, ledStates, 5); // Aktualizuj poprzedni stan LED
  }
}

// Funkcja pomocnicza
byte ReadOne165(int dataPin, int clockPin) {
  byte ret = 0;
  for (int i = 0; i < 8; i++) {
    ret = (ret << 1) | digitalRead(dataPin);
    digitalWrite(clockPin, HIGH);
    delayMicroseconds(pulseDelay);
    digitalWrite(clockPin, LOW);
  }
  return ret;
}