#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_ILI9341.h>
#include <ESP32Servo.h>

// ---------------- TFT ----------------
#define TFT_DC   2
#define TFT_RST  4
#define TFT_CS   15

Adafruit_ILI9341 tft(TFT_CS, TFT_DC, TFT_RST);

// ---------------- SWITCH ----------------
#define SW1 32
#define SW2 33
#define SW3 25
#define SW4 26
#define SW5 27

// ---------------- 74HC595 ----------------
#define SR_DATA  14
#define SR_CLOCK 16
#define SR_LATCH 17

// ---------------- LED NAMES (Q0–Q7) ----------------
#define LED1 0   // S1 green
#define LED2 1   // S1 red
#define LED3 2   // S1 orange
#define LED4 3   // S2 green
#define LED5 4   // S2 red
#define LED6 5   // S2 orange
#define LED7 6   // T1 blue
#define LED8 7   // T1 white

// ---------------- SERVO ----------------
Servo s1;
Servo s2;
Servo s3;
Servo s4;

// ---------------- SHIFT REGISTER ----------------

byte srState = 0;

void srWrite() {
  digitalWrite(SR_LATCH, LOW);
  shiftOut(SR_DATA, SR_CLOCK, MSBFIRST, srState);
  digitalWrite(SR_LATCH, HIGH);
}

void setLED(int bit, bool on) {
  if (on) srState |=  (1 << bit);
  else    srState &= ~(1 << bit);
  srWrite();
}

// ---------------- TFT SEGMENTS ----------------

void clearSegment(int line, int segment) {
  int y = 50 + line * 40;
  int x1 = 100 + segment * 22;
  int x2 = x1 + 20;

  tft.drawLine(x1, y, x2, y, ILI9341_BLACK);
  tft.drawLine(x1, y - 10, x2, y + 10, ILI9341_BLACK);
  tft.drawLine(x1, y + 10, x2, y - 10, ILI9341_BLACK);
}

void drawSegment(int line, int segment, int type) {
  clearSegment(line, segment);

  int y = 50 + line * 40;
  int x1 = 100 + segment * 22;
  int x2 = x1 + 20;

  if (type == 0) tft.drawLine(x1, y, x2, y, ILI9341_WHITE);
  else if (type == 1) tft.drawLine(x1, y - 10, x2, y + 10, ILI9341_WHITE);
  else if (type == 2) tft.drawLine(x1, y + 10, x2, y - 10, ILI9341_WHITE);
}

// ---------------- DOTS ----------------

void drawDot(int x, int y, uint16_t color) {
  tft.fillCircle(x, y, 12, color);
}

void updateDots() {

  // GÓRNA LINIA (LED1, LED2, LED3)
  drawDot(30, 50,  (srState & (1 << LED1)) ? ILI9341_GREEN  : ILI9341_BLACK);
  drawDot(60, 50,  (srState & (1 << LED2)) ? ILI9341_RED    : ILI9341_BLACK);
  drawDot(90, 50,  (srState & (1 << LED3)) ? ILI9341_ORANGE : ILI9341_BLACK);

  // ŚRODKOWA LINIA (LED4, LED5, LED6)
  drawDot(30, 90,  (srState & (1 << LED4)) ? ILI9341_GREEN  : ILI9341_BLACK);
  drawDot(60, 90,  (srState & (1 << LED5)) ? ILI9341_RED    : ILI9341_BLACK);
  drawDot(90, 90,  (srState & (1 << LED6)) ? ILI9341_ORANGE : ILI9341_BLACK);

  // DOLNA LINIA (czarna, LED7, LED8)
  drawDot(30, 130, ILI9341_BLACK);
  drawDot(60, 130, (srState & (1 << LED8)) ? ILI9341_BLUE   : ILI9341_BLACK);
  drawDot(90, 130, (srState & (1 << LED7)) ? ILI9341_WHITE  : ILI9341_BLACK);
}

void drawLines() {
  tft.fillScreen(ILI9341_BLACK);

  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 10; j++) {
      drawSegment(i, j, 0);
    }
  }
}

// ---------------- MODES ----------------
int mode1 = 0;
int mode2 = 0;
int mode3 = 0;

bool servo12 = false;
bool servo34 = false;

unsigned long blinkTimer = 0;
bool blinkState = false;

// ---------------- SETUP ----------------

void setup() {
  tft.begin();
  tft.setRotation(1);
  drawLines();

  pinMode(SW1, INPUT_PULLUP);
  pinMode(SW2, INPUT_PULLUP);
  pinMode(SW3, INPUT_PULLUP);
  pinMode(SW4, INPUT_PULLUP);
  pinMode(SW5, INPUT_PULLUP);

  pinMode(SR_DATA, OUTPUT);
  pinMode(SR_CLOCK, OUTPUT);
  pinMode(SR_LATCH, OUTPUT);
  srWrite();

  s1.attach(12);
  s2.attach(13);
  s3.attach(5);
  s4.attach(21);

  s1.write(0);
  s2.write(0);
  s3.write(0);
  s4.write(0);

  // STAN STARTOWY LED
  setLED(LED2, true);
  setLED(LED5, true);
  setLED(LED8, true);
}

// ---------------- LOOP ----------------

void loop() {

  // ---------------- MODE 3 (Ms1/Ms2) ----------------
  if (!digitalRead(SW5)) {
    mode3 = !mode3;
    delay(250);
  }

  if (mode3 == 0) {
    setLED(LED8, true);
    setLED(LED7, false);
  } else {
    setLED(LED8, false);
    setLED(LED7, true);
  }

  // ---------------- SERVO 1+2 ----------------
  if (!digitalRead(SW1)) {
    servo12 = !servo12;
    servo34 = false;
    delay(250);
  }

  if (servo12) {
    s1.write(45);
    s2.write(45);
    s3.write(0);
    s4.write(0);

    drawSegment(0, 3, 1);
    drawSegment(1, 4, 1);

  } else {
    s1.write(0);
    s2.write(0);

    clearSegment(0, 3);
    clearSegment(1, 4);
  }

  // ---------------- SERVO 3+4 ----------------
  if (!digitalRead(SW2)) {
    servo34 = !servo34;
    servo12 = false;
    delay(250);
  }

  if (servo34) {
    s3.write(45);
    s4.write(45);
    s1.write(0);
    s2.write(0);

    drawSegment(1, 5, 2);
    drawSegment(2, 4, 2);

  } else {
    s3.write(0);
    s4.write(0);

    clearSegment(1, 5);
    clearSegment(2, 4);
  }

  // ---------------- MODE 1 (S1/S2/S10/S11) ----------------
  if (!digitalRead(SW3)) {
    mode1 = (mode1 + 1) % 4;
    delay(250);
  }

  switch (mode1) {
    case 0: // S1 = red
      setLED(LED1, false);
      setLED(LED2, true);
      setLED(LED3, false);
      break;

    case 1: // S2 = green
      setLED(LED1, true);
      setLED(LED2, false);
      setLED(LED3, false);
      break;

    case 2: // S10 = green + orange
      setLED(LED1, true);
      setLED(LED2, false);
      setLED(LED3, true);
      break;

    case 3: // S11 = green blink + orange
      setLED(LED3, true);
      if (millis() - blinkTimer > 250) {
        blinkTimer = millis();
        blinkState = !blinkState;
      }
      setLED(LED1, blinkState);
      break;
  }

  // ---------------- MODE 2 (S1/S2/S10/S11) ----------------
  if (!digitalRead(SW4)) {
    mode2 = (mode2 + 1) % 4;
    delay(250);
  }

  switch (mode2) {
    case 0:
      setLED(LED4, false);
      setLED(LED5, true);
      setLED(LED6, false);
      break;

    case 1:
      setLED(LED4, true);
      setLED(LED5, false);
      setLED(LED6, false);
      break;

    case 2:
      setLED(LED4, true);
      setLED(LED5, false);
      setLED(LED6, true);
      break;

    case 3:
      setLED(LED6, true);
      if (millis() - blinkTimer > 250) {
        blinkTimer = millis();
        blinkState = !blinkState;
      }
      setLED(LED4, blinkState);
      break;
  }

  // ---------------- UPDATE DOTS ----------------
  updateDots();
}