const int clk165   = 2;    // Zegar dla 74165
const int latch165 = 3;    // Load dla 74165
const int data165  = 4;    // Data z 74165
const int data595  = 5;    // Data do 74595
const int latch595 = 6;    // Latch dla 74595
const int clk595   = 7;    // Zegar dla 74595
const int SPI_MOSI = 8;
const int SPI_MISO = 11;
const int SPI_CLK  = 9;
const int SPI_CS   = 10;
byte lastButtons = 0;
byte leds = 0;

void setup() {
  Serial.begin(115200);
  // Piny dla 74165 (wejście - przyciski)
  pinMode(clk165, OUTPUT);
  pinMode(latch165, OUTPUT);
  pinMode(data165, INPUT);
  // Piny dla 74595 (wyjście - LED)
  pinMode(data595, OUTPUT);
  pinMode(latch595, OUTPUT);
  pinMode(clk595, OUTPUT);
  // Piny SPI
  pinMode(SPI_MOSI, OUTPUT);
  pinMode(SPI_MISO, INPUT);
  pinMode(SPI_CLK, OUTPUT);
  pinMode(SPI_CS, OUTPUT);
  // Stan początkowy
  digitalWrite(latch165, HIGH);
  digitalWrite(latch595, HIGH);
  digitalWrite(clk165, LOW);
  digitalWrite(clk595, LOW);
  digitalWrite(SPI_CS, HIGH);
  digitalWrite(SPI_CLK, LOW);
  digitalWrite(SPI_MOSI, LOW);
  Serial.println("A1 START - MASTER");
}

void syncIO() {
  // 1. LOAD przycisków (74165)
  digitalWrite(latch165, LOW);
  delayMicroseconds(pulseDelay);
  digitalWrite(latch165, HIGH);
  delayMicroseconds(pulseDelay);
  // 2. Odczyt przycisków
  buttons = 0;
  for (int j = 0; j < 8; j++) {
    // Czytaj bit z 165
    if (digitalRead(data165) == HIGH) {
      buttons |= (1 << j);
    }
    // Zegar dla 165 (odczyt następnego bitu)
    digitalWrite(clk165, HIGH);
    delayMicroseconds(pulseDelay);
    digitalWrite(clk165, LOW);
    delayMicroseconds(pulseDelay);
  }
  // 3. Zapisz LED (595)
  digitalWrite(latch595, LOW);
  for (int j = 7; j >= 0; j--) {
    digitalWrite(clk595, LOW);
    digitalWrite(data595, (leds >> j) & 1);
    delayMicroseconds(pulseDelay);
    digitalWrite(clk595, HIGH);
    delayMicroseconds(pulseDelay);
    digitalWrite(clk595, LOW);
  }
  digitalWrite(latch595, HIGH);
}

void sendButtonEvents() {
  byte rising = buttons & ~lastButtons;
  if (rising > 0) {
    for (int j = 0; j < 8; j++) {
      if (bitRead(rising, j)) {
        Serial.print("Przycisk CH: ");
        Serial.println(j);
        // Wysyłamy numer kanału, odbieramy pełny stan wszystkich styków
        byte response = spiTransfer(j);
        // Lustrzane odbicie stanu ze Slave'a
        leds = response;
        Serial.print("Nowy stan LED: ");
        Serial.println(leds, BIN);
      }
    }
  }
  lastButtons = buttons;
}

byte spiTransfer(byte out) {
  byte in = 0;
  digitalWrite(SPI_CS, LOW);
  delayMicroseconds(500);
  Serial.print("SPI SENDING BIN: ");
  Serial.println(out, BIN);
  for (int i = 7; i >= 0; i--) {
    bool bitToSend = (out >> i) & 1;
    digitalWrite(SPI_MOSI, bitToSend);
    delayMicroseconds(100);
    digitalWrite(SPI_CLK, HIGH);
    delayMicroseconds(100);
    bool bitReceived = digitalRead(SPI_MISO);
    if (bitReceived)
      in |= (1 << i);
    // Debug bitów w jednej linii
    Serial.print(bitToSend);
    digitalWrite(SPI_CLK, LOW);
    delayMicroseconds(100);
  }
  digitalWrite(SPI_CS, HIGH);
  // Nowa linia po bitach
  Serial.println();
  return in;
}

void loop() {
  syncIO();
  sendButtonEvents();
  // Automatyczne odpytywanie o stan (Polling)
  static unsigned long lastPoll = 0;
  if (millis() - lastPoll > 100) {
    // Wysyłamy 0xFF (brak komendy), odbieramy stan styków
    byte response = spiTransfer(0xFF);
    // Ignoruj błędy komunikacji
    if (response != 0xFF) {
      leds = response;
    }
    lastPoll = millis();
  }
  delay(1);
}

const int clk165   = 2;    // Zegar dla 74165
const int latch165 = 3;    // Load dla 74165
const int data165  = 4;    // Data z 74165
const int data595  = 5;    // Data do 74595
const int latch595 = 6;    // Latch dla 74595
const int clk595   = 7;    // Zegar dla 74595
const int SPI_MOSI = 8;
const int SPI_MISO = 11;
const int SPI_CLK  = 9;
const int SPI_CS   = 10;
const int pulseDelay = 2;
byte relays = 0x00;  // Wszystkie wyłączone (HIGH = wyłączony)
byte feedback = 0;
byte spiResponse = 0;
unsigned long relayStartTime[8] = {0};
bool relayActive[8] = {false};

void setup() {
  Serial.begin(115200);
  // Piny dla 74165 (wejście - styki przekaźników)
  pinMode(clk165, OUTPUT);
  pinMode(latch165, OUTPUT);
  pinMode(data165, INPUT);
  // Piny dla 74595 (wyjście - przekaźniki)
  pinMode(data595, OUTPUT);
  pinMode(latch595, OUTPUT);
  pinMode(clk595, OUTPUT);
  // Piny SPI (Slave)
  pinMode(SPI_MOSI, INPUT);
  pinMode(SPI_MISO, OUTPUT);
  pinMode(SPI_CLK, INPUT);
  pinMode(SPI_CS, INPUT);
  // Stan początkowy
  digitalWrite(latch165, HIGH);
  digitalWrite(latch595, HIGH);
  digitalWrite(clk165, LOW);
  digitalWrite(clk595, LOW);
  digitalWrite(SPI_MISO, LOW);
  // Inicjalizacja - wszystkie przekaźniki wyłączone
  updateRelays();
  Serial.println("A2 START - SLAVE");
  Serial.println("Testing SPI pins...");
  Serial.print("CS: ");
  Serial.println(digitalRead(SPI_CS));
  Serial.print("CLK: ");
  Serial.println(digitalRead(SPI_CLK));
  Serial.print("MOSI: ");
  Serial.println(digitalRead(SPI_MOSI));
}

void updateRelays() {
  digitalWrite(latch595, LOW);
  // Zmieniamy kolejność: od 0 do 7 zamiast od 7 do 0
  for (int i = 0; i < 8; i++) {
    digitalWrite(clk595, LOW);
    digitalWrite(data595, (relays >> i) & 1);
    delayMicroseconds(pulseDelay);
    digitalWrite(clk595, HIGH);
    delayMicroseconds(pulseDelay);
    digitalWrite(clk595, LOW);
  }
  digitalWrite(latch595, HIGH);

  Serial.print("Relays state: ");
  Serial.println(relays, BIN);
}

void readFeedback() {
  // LOAD danych do 165
  digitalWrite(latch165, LOW);
  delayMicroseconds(pulseDelay);
  digitalWrite(latch165, HIGH);
  delayMicroseconds(pulseDelay);
  // Odczytaj 8 bitów
  feedback = 0;
  for (int i = 0; i < 8; i++) {
    // Odczytaj bit
    if (digitalRead(data165) == HIGH) {
      feedback |= (1 << i);
    }
    // Zegar dla 165 (kolejny bit)
    digitalWrite(clk165, HIGH);
    delayMicroseconds(pulseDelay);
    digitalWrite(clk165, LOW);
    delayMicroseconds(pulseDelay);
  }
  Serial.print("Feedback state: ");
  Serial.println(feedback, BIN);
}

void pulseRelay(byte ch) {
  if (ch >= 8) return;
  Serial.print("ODPALAM PRZEKAZNIK CH: ");
  Serial.println(ch);
  bitSet(relays, ch);
  updateRelays();
  relayStartTime[ch] = millis();
  relayActive[ch] = true;
}

void handleRelayTimeouts() {
  bool changed = false;
  for (int i = 0; i < 8; i++) {
    if (relayActive[i] && (millis() - relayStartTime[i] >= 1000)) {
      bitClear(relays, i);
      relayActive[i] = false;
      changed = true;
      Serial.print("Auto OFF Relay CH: ");
      Serial.println(i);
    }
  }
  if (changed) updateRelays();
}

byte spiTransferSlave() {
  byte in = 0;
  // Nie dajemy tu Serial.print, bo spowolnimy procesor!
  for (int i = 7; i >= 0; i--) {
    // Najpierw wystawiamy MISO
    digitalWrite(SPI_MISO, (spiResponse >> i) & 1);
    // Czekamy na CLK
    while (digitalRead(SPI_CLK) == LOW) {
      if (digitalRead(SPI_CS) == HIGH)
        return in;
    }
    // Czytamy MOSI
    if (digitalRead(SPI_MOSI) == HIGH)
      in |= (1 << i);
    // Czekamy na koniec impulsu zegara
    while (digitalRead(SPI_CLK) == HIGH);
  }
  return in;
}

void loop() {
  if (digitalRead(SPI_CS) == LOW) {
    // Przygotuj dane do wysłania
    spiResponse = feedback;
    byte raw = spiTransferSlave();
    // Jeśli Master wysłał numer kanału (a nie 0xFF)
    if (raw < 8) {
      pulseRelay(raw);
    }
  }
  static unsigned long lastUpdate = 0;
  if (millis() - lastUpdate > 50) {
    readFeedback();
    handleRelayTimeouts();
    lastUpdate = millis();
  }
}

#include <SoftwareSerial.h>
SoftwareSerial LINK(8, 9); // RX, TX
const int clk165 = 2;
const int latch165 = 3;
const int data165 = 4;
const int data595 = 5;
const int latch595 = 6;
const int clk595 = 7;

byte buttonStates = 0;
byte lastButtonStates = 0;
byte ledStates = 0;
byte feedbackStates = 0;

void setup() {
  Serial.begin(9600);
  LINK.begin(9600);/*
  pinMode(clk165, OUTPUT);
  pinMode(latch165, OUTPUT);
  pinMode(data165, INPUT);
  pinMode(data595, OUTPUT);
  pinMode(latch595, OUTPUT);
  pinMode(clk595, OUTPUT);
  pinMode(txPin, OUTPUT);
  pinMode(rxPin, INPUT);
  digitalWrite(txPin, HIGH);
  digitalWrite(clk165, LOW);
  digitalWrite(latch165, HIGH);
  Serial.println("A1");
  //updateLEDs();*/
}

void loop() {
  /*
    byte pressed = readButtons();
    if (pressed) {
      Serial.print("PRESS: ");
      Serial.println(pressed, HEX);
      // cały bajt
      LINK.write(pressed);
    }
    delay(50);*/
}

// ODCZYT 74165
byte readButtons() {
  digitalWrite(latch165, LOW);
  delayMicroseconds(5);
  digitalWrite(latch165, HIGH);
  delayMicroseconds(5);
  byte newStates = 0;
  for (int i = 0; i < 8; i++) {
    if (digitalRead(data165) == HIGH) {
      newStates |= (1 << (7 - i));
    }
    digitalWrite(clk165, HIGH);
    delayMicroseconds(5);
    digitalWrite(clk165, LOW);
    delayMicroseconds(2);
  }
  byte pressed = newStates & ~buttonStates;
  buttonStates = newStates;
  return pressed;
}

void handlePress(byte pressed) {
  // DEBUG
  Serial.print("PRESS: ");
  for (int i = 7; i >= 0; i--) {
    Serial.print((pressed >> i) & 1);
  }
  Serial.print(" HEX: 0x");
  Serial.println(pressed, HEX);
  // WYSYŁANIE
  for (int i = 0; i < 8; i++) {
    if (pressed & (1 << i)) {
      // UART / SoftwareSerial / Serial1
      LINK.write(i);
    }
  }
}

byte readUART() {
  delayMicroseconds(1500); // Czekaj na środek start bitu
  byte result = 0;
  for (int i = 0; i < 8; i++) {
    delayMicroseconds(104); // 9600 baud = 104µs per bit
    if (digitalRead(rxPin) == HIGH) {
      result |= (1 << i);
    }
  }
  delayMicroseconds(104); // Stop bit
  return result;
}

void sendUART(byte data) {
  digitalWrite(txPin, LOW); // Start bit
  delayMicroseconds(104);
  for (int i = 0; i < 8; i++) {
    // LSB first
    digitalWrite(txPin, (data >> i) & 0x01);
    delayMicroseconds(104);
  }
  // Stop bit
  digitalWrite(txPin, HIGH);
  delayMicroseconds(104);
}

void checkButtonPress() {
  static unsigned long lastPress = 0;
  byte pressed = buttonStates & ~lastButtonStates;
  if (pressed != 0 && millis() - lastPress > 300) {
    for (int i = 0; i < 8; i++) {
      if (bitRead(pressed, i)) {
        //Serial.print("SEND: ");
        //Serial.println(i);
        sendUART(1 << i); // Wyślij bit kanału
        lastPress = millis();
        delay(200);
        break;
      }
    }
  }
  lastButtonStates = buttonStates;
}

void updateLEDs() {
  Serial.print("LEDS: ");
  Serial.println(feedbackStates, BIN);
  digitalWrite(latch595, LOW);
  // Wolne LSB first
  for (int i = 0; i < 8; i++) {
    bool bitValue = (feedbackStates >> i) & 0x01;
    digitalWrite(data595, bitValue);
    delayMicroseconds(50);
    digitalWrite(clk595, HIGH);
    delayMicroseconds(100);
    digitalWrite(clk595, LOW);
    delayMicroseconds(50);
  }
  digitalWrite(latch595, HIGH);
}
















byte readUART() {
  delayMicroseconds(1500);
  byte result = 0;
  
  for (int i = 0; i < 8; i++) {
    delayMicroseconds(104);
    if (digitalRead(rxPin) == HIGH) {
      result |= (1 << i);
    }
  }
  
  delayMicroseconds(104);
  return result;
}

void sendUART(byte data) {
  digitalWrite(txPin, LOW);
  delayMicroseconds(104);
  
  for (int i = 0; i < 8; i++) {
    digitalWrite(txPin, (data >> i) & 0x01);
    delayMicroseconds(104);
  }
  
  digitalWrite(txPin, HIGH);
  delayMicroseconds(104);
}

void simulateFeedback() {
  static unsigned long lastSend = 0;
  if (millis() - lastSend > 200) {
    lastSend = millis();
    
    byte simulatedFB = 0;
    unsigned long now = millis();
    
    for (int i = 0; i < 8; i++) {
      if (bitRead(relayStates, i) && relayTimers[i] != 0) {
        if (now - relayTimers[i] < 900) {
          simulatedFB |= (1 << i);
        }
      }
    }
    
    if (simulatedFB != feedbackStates) {
      feedbackStates = simulatedFB;
      Serial.print("SEND FB: ");
      Serial.println(feedbackStates, BIN);
      sendUART(feedbackStates);
    }
  }
}

void manageRelayTimers() {
  unsigned long currentTime = millis();
  
  for (int i = 0; i < 8; i++) {
    if (bitRead(relayStates, i) && relayTimers[i] != 0) {
      if (currentTime - relayTimers[i] >= 1000) {
        bitClear(relayStates, i);
        relayTimers[i] = 0;
        Serial.print("OFF: ");
        Serial.println(i);
        updateRelays();
      }
    }
  }
}

void updateRelays() {
  Serial.print("RELAYS: ");
  Serial.println(relayStates, BIN);
  
  digitalWrite(latch595, LOW);
  
  for (int i = 0; i < 8; i++) {
    bool bitValue = (relayStates >> i) & 0x01;
    digitalWrite(data595, bitValue);
    delayMicroseconds(50);
    digitalWrite(clk595, HIGH);
    delayMicroseconds(100);
    digitalWrite(clk595, LOW);
    delayMicroseconds(50);
  }
  
  digitalWrite(latch595, HIGH);
}