#include "Wire.h"

#define I2C_Freq 100000
#define SDA_0 9
#define SCL_0 10

#define SDA_pin_Lolin 12
#define SCL_pin_Lolin 14

#define WIFI_SSID "ZOG-521"
#define WIFI_PASSWORD "111"

#define SPEAKER_PIN 34
#define NOTE_A4  440


#define BMP180_ADDRESS 0x77
#define SCREEN_ADDRESS 0x3C
#define SCREEN2_ADDRESS 0x3D
#define AHT10_ADDRESS 0x38
#define DS1307_ADDRESS 0x68
#define MPU6050_ADDRESS 0x69

bool sensor_bmp85_present = false;
bool sensor_aht10_present = false;
bool display_1306_present = false;
bool display2_1306_present = false;
bool sensor_ds1307_present = false;
bool sensor_mpu6050_present = false;
bool ps_speaker_present = true;

TwoWire I2C_0 = TwoWire(0);

#include "SPI.h"
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
#define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin)
//#define SCREEN_ADDRESS 0x3C ///< See datasheet for Address; 0x3D for 128x64, 0x3C for 128x32
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &I2C_0, OLED_RESET);
Adafruit_SSD1306 display2(SCREEN_WIDTH, SCREEN_HEIGHT, &I2C_0, OLED_RESET);

#include "Adafruit_ILI9341.h"

#define STMPE_CS 6  //SCK
#define SD_CS    0  //wtf?
#define TFT_CS 2
#define TFT_DC 3
#define TFT_RST 19
//Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC);
//Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC, TFT_MOSI, TFT_CLK, TFT_RST, TFT_MISO);
Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC, MOSI, SCK, TFT_RST);

int inputPin = 13;               // choose the input pin (for PIR sensor)
int pirState = LOW;             // we start, assuming no motion detected
int val = 0;                    // variable for reading the pin status




//#define NO_HARDWARE_PIN_SUPPORT
//#define FASTLED_RMT_MAX_CHANNELS 1
//#define FASTLED_RMT_SERIAL_DEBUG 1

//#define FASTLED_ESP32_FLASH_LOCK 1
#define FASTLED_RMT_BUILTIN_DRIVER 1

#include <FastLED.h>
// How many leds in your strip?
#define NUM_LEDS 8
#define LED_TYPE    WS2812
#define COLOR_ORDER GRB
#define NUM_LEDS2 8
#define LED_TYPE2    WS2812
#define COLOR_ORDER2 GRB
#define FASTLED2_DATA_PIN 27
#define FASTLED_DATA_PIN 26

// Define the arrays of leds
CRGB leds[NUM_LEDS];
///CRGB leds2[NUM_LEDS2];
float xByGyro, yByGyro, zByGyro;      // Global variables for the rotation by gyro
// Set the origin in the middle of the display
const int xOrigin = 64;
const int yOrigin = 32;

//const float viewDistance = 150.0;     // higher for less perspective, lower for more.

const int freq = 5000;
//const int ledChannel = 0;
const int resolution = 8;



unsigned long BarLastRenderTime = 0;

#include <ShiftRegister74HC595.h>
#define DATA_PIN 18  // Pin connected to DS of 74HC595
#define LATCH_PIN 5  // Pin connected to STCP of 74HC595
#define CLOCK_PIN 19 // Pin connected to SHCP of 74HC595

// create a global shift register object
// parameters: <number of shift registers> (data pin, clock pin, latch pin)
ShiftRegister74HC595<2> sr(DATA_PIN, CLOCK_PIN, LATCH_PIN);
#define NUM_SHIFT_REGS 2

const uint8_t numOfRegisterPins = NUM_SHIFT_REGS * 8;

bool registers[numOfRegisterPins];


#include "RTClib.h"

RTC_DS1307 rtc;

char daysOfTheWeek[7][12] = {"Sunday", "Monday", "Tuesday", "Wednesday", "Thursday", "Friday", "Saturday"};
//==============================================================
void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);

  Serial.println("Hello, ESP32! HAHA LOL!");


  Serial.println("Chip");
  chip_info();


 //Serial.println("Wire reinit? why?");
  //   Wire.begin();
//  I2C_0.begin(SDA_0, SCL_0, I2C_Freq);






  Serial.println("Init HC595 register");

  pinMode(DATA_PIN, OUTPUT);
  pinMode(CLOCK_PIN, OUTPUT);
  pinMode(LATCH_PIN, OUTPUT);
  clearRegisters();
  writeRegisters();


  Serial.println("Test HC595 register");
  for (int dot = 0; dot < numOfRegisterPins; dot++) {
    setRegisterPin(dot, true);
    writeRegisters();
    delay(20);
    setRegisterPin(dot, false);
  }
  writeRegisters();


 
  //pinMode(DATA_PIN, OUTPUT);
  //pinMode(CLOCK_PIN, OUTPUT);
  //pinMode(LATCH_PIN, OUTPUT);
  //clearRegisters();
  //writeRegisters();

//  Serial.println("Chip");
//  chip_info();



 
  Serial.println("FastLED init");

  //FastLED.addLeds<NEOPIXEL, FASTLED_DATA_PIN>(leds, NUM_LEDS);  // GRB ordering is assumed
  FastLED.addLeds<WS2812B, FASTLED_DATA_PIN, GRB>(leds, NUM_LEDS);
//  FastLED.addLeds<WS2812B, FASTLED2_DATA_PIN, GRB>(leds2, NUM_LEDS2);
//  FastLED.show(); 
  //FastLED2.addLeds<WS2812B, FASTLED2_DATA_PIN, GRB>(leds2, NUM_LEDS2);
  //FastLED.addLeds<LED_TYPE, FASTLED_DATA_PIN, COLOR_ORDER>(leds, NUM_LEDS);
  delay(100);

  FastLED.setBrightness(77);

  Serial.println("Test indexing leds");
  

  leds[0] = CRGB(255, 4, 4);
  leds[1] = CRGB(255, 87, 4);
  leds[2] = CRGB(251, 239, 5);
  leds[3] = CRGB(4, 251, 16);
  leds[4] = CRGB(6, 223, 243);
  leds[5] = CRGB(18, 1, 246);
  leds[6] = CRGB(109, 4, 246);
  leds[7] = CRGB::White;
  
  FastLED.delay(20);
  FastLED.show();
  
  leds[0] = CRGB(0, 0, 0);
  leds[1] = CRGB(0, 0, 0);
  leds[2] = CRGB(0, 0, 0);
  leds[3] = CRGB(0, 0, 0);
  leds[4] = CRGB(0, 0, 0);
  leds[5] = CRGB(0, 0, 0);
  leds[6] = CRGB(0, 0, 0);
  leds[7] = CRGB(0, 0, 0);
  
  FastLED.delay(20);
  FastLED.show();
  delay(100);












  for (int dot = 0; dot < NUM_LEDS; dot++) {
    leds[dot] = CRGB::Blue;
 //   leds2[dot] = CRGB::Blue;  
    FastLED.delay(20);
    FastLED.show(); 
   // delay(20);
    // clear this led for the next time around the loop
    leds[dot] = CRGB::Black;
  //  leds2[dot] = CRGB::Black;
    FastLED.delay(20);
    FastLED.show(); 
    }









  
/* 
  leds[0] = CRGB(255, 4, 4);
  leds[1] = CRGB(255, 87, 4);
  leds[2] = CRGB(251, 239, 5);
  leds[3] = CRGB(4, 251, 16);
  leds[4] = CRGB(6, 223, 243);
  leds[5] = CRGB(18, 1, 246);
  leds[6] = CRGB(109, 4, 246);
  leds[7] = CRGB::White;

  leds2[0] = CRGB(255, 4, 4);
  leds2[1] = CRGB(255, 87, 4);
  leds2[2] = CRGB(251, 239, 5);
  leds2[3] = CRGB(4, 251, 16);
  leds2[4] = CRGB(6, 223, 243);
  leds2[5] = CRGB(18, 1, 246);
  leds2[6] = CRGB(109, 4, 246);
  leds2[7] = CRGB::White;
  FastLED.show();
  //  delay(1500);
  leds[0] = CRGB(0, 0, 0);
  leds[1] = CRGB(0, 0, 0);
  leds[2] = CRGB(0, 0, 0);
  leds[3] = CRGB(0, 0, 0);
  leds[4] = CRGB(0, 0, 0);
  leds[5] = CRGB(0, 0, 0);
  leds[6] = CRGB(0, 0, 0);
  leds[7] = CRGB(0, 0, 0);

  leds2[0] = CRGB(0, 0, 0);
  leds2[1] = CRGB(0, 0, 0);
  leds2[2] = CRGB(0, 0, 0);
  leds2[3] = CRGB(0, 0, 0);
  leds2[4] = CRGB(0, 0, 0);
  leds2[5] = CRGB(0, 0, 0);
  leds2[6] = CRGB(0, 0, 0);
  leds2[7] = CRGB(0, 0, 0);
  FastLED.show();
  delay(100);
 */


/*   leds[0] = CRGB(237, 245, 8);
  FastLED.show();
  sr.setAllHigh(); // set all pins HIGH
  delay(500);

  sr.setAllLow(); // set all pins LOW
  delay(500);

  leds[0] = CRGB::Black;
  FastLED.show(); */


 Serial.println("PIR Sensor pin");

  //for pir sensor
  pinMode(inputPin, INPUT);

  //ps speaker
  Serial.println("Speaker pin");

  pinMode(SPEAKER_PIN, OUTPUT);

 //ledcSetup(ledChannel, freq, resolution);
  
  ledcAttachPin(SPEAKER_PIN, 1); // assign RGB led pins to channels
//  ledcAttachPin(ledG, 2);
//  ledcAttachPin(ledB, 3);

//  ledcAttachPin(ledWarm, 5);
//  ledcAttachPin(ledCool, 4);

  // Initialize channels 
  // channels 0-15, resolution 1-16 bits, freq limits depend on resolution
  // ledcSetup(uint8_t channel, uint32_t freq, uint8_t resolution_bits);
  ledcSetup(1, freq, resolution); // 12 kHz PWM, 8-bit resolution
//  ledcSetup(2, freq, resolution);
//  ledcSetup(3, freq, resolution);

//  ledcSetup(4, freq, resolution);
//  ledcSetup(5, freq, resolution);



  Serial.println("Speaker beep");



  tone(SPEAKER_PIN, NOTE_A4);
  delay(250);
  noTone(SPEAKER_PIN);






  // Turn the LED on, then pause
  leds[0] = CRGB(26, 180, 241);
  FastLED.show();

  Serial.println("Test monitor Logo");
  tft.begin();

  tft.setCursor(46, 120);
  tft.setTextColor(ILI9341_RED);
  tft.setTextSize(3);
  tft.println("LAB515");

/* 

  leds[0] = CRGB(15, 18, 246);
  FastLED.show();
  sr.setAllHigh(); // set all pins HIGH
  delay(500);

  sr.setAllLow(); // set all pins LOW
  delay(500);
 */


  leds[0] = CRGB(157, 22, 246);
  FastLED.show();
  Serial.println("Scan i2c bus");
  scani2c();

  // render_ua();

  if (sensor_ds1307_present) {
    leds[1] = CRGB(248, 236, 6);
    FastLED.show();

    if (! rtc.begin(&I2C_0)) {
      leds[1] = CRGB(136, 3, 3);
      FastLED.show();

      Serial.println("Failed ds1307 clock initialization");
      Serial.println("TODO: and what we gone do now?!");
      // for (;;); // Don't proceed, loop forever

    } else {
      Serial.println("ds1307 init...");
      leds[1] = CRGB(56, 136, 3);
      FastLED.show();

      DateTime now = rtc.now();
      String TimeString = "";

     //buffer can be defined using following combinations:
     //hh - the hour with a leading zero (00 to 23)
     //mm - the minute with a leading zero (00 to 59)
     //ss - the whole second with a leading zero where applicable (00 to 59)
     //YYYY - the year as four digit number
     //YY - the year as two digit number (00-99)
     //MMM - the abbreviated English month name ('Jan' to 'Dec')
     //DD - the day as number with a leading zero (01 to 31)
     //DDD - the abbreviated English day name ('Mon' to 'Sun')

      char buf1[] = "hh:mm:ss";
      Serial.println(now.toString(buf1));

      char buf2[] = "YYMMDD-hh:mm:ss";
      Serial.println(now.toString(buf2));

      char buf3[] = "Today is DDD, MMM DD YYYY";
      Serial.println(now.toString(buf3));

      char buf4[] = "MM-DD-YYYY";
      Serial.println(now.toString(buf4));
      TimeString = now.toString(buf1); 



      Serial.print("Current time: ");
      Serial.print(now.year(), DEC);
      Serial.print('/');
      Serial.print(now.month(), DEC);
      Serial.print('/');
      Serial.print(now.day(), DEC);
      Serial.print(" (");
      Serial.print(daysOfTheWeek[now.dayOfTheWeek()]);
      Serial.print(") ");
      Serial.print(now.hour(), DEC);
      Serial.print(':');
      Serial.print(now.minute(), DEC);
      Serial.print(':');
      Serial.print(now.second(), DEC);
      Serial.println();

    tft.setCursor(0, 200);
    tft.setTextColor(ILI9341_WHITE);
    tft.setTextSize(4);
    tft.println(TimeString);



    }
  }

  if (display2_1306_present) {
    leds[2] = CRGB(248, 236, 6);
    FastLED.show();
    // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
    if (!display2.begin(SSD1306_SWITCHCAPVCC, SCREEN2_ADDRESS)) {
      leds[2] = CRGB(136, 3, 3);
      FastLED.show();
      Serial.println("Failed display SSD1306 initialization");
      Serial.println("TODO: and what we gone do now?!");
      for (;;); // Don't proceed, loop forever

    } else {
      Serial.println("SSD1306 init...");
      leds[2] = CRGB(56, 136, 3);
      FastLED.show();
      display2.clearDisplay();
      display2.setRotation(2);

      //  //     render_icons(false);
      display2.display();

      //delay(1000);
      BarLastRenderTime = micros();
      render_ua2();
    }

  }  else {
    Serial.println("SSD1306 not found! Skip it...");
  }

  if (display_1306_present) {
    leds[3] = CRGB(248, 236, 6);
    FastLED.show();
    // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
    if (!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
      leds[3] = CRGB(136, 3, 3);
      FastLED.show();
      Serial.println("Failed display SSD1306 initialization");
      Serial.println("TODO: and what we gone do now?!");
      for (;;); // Don't proceed, loop forever

    } else {
      Serial.println("SSD1306 init...");
      leds[3] = CRGB(56, 136, 3);
      FastLED.show();
      display.clearDisplay();
      display.setRotation(2);

      //  //     render_icons(false);
      display.display();

      //delay(1000);
      BarLastRenderTime = micros();
      render_ua();
    }

  }  else {
    Serial.println("SSD1306 not found! Skip it...");
  }







  if (sensor_mpu6050_present) {
    leds[4] = CRGB(248, 236, 6);
    FastLED.show();
    // Initialize the MPU-6050 and test if it is connected.
    I2C_0.beginTransmission( MPU6050_ADDRESS);
    I2C_0.write( 0x6B);                           // PWR_MGMT_1 register
    I2C_0.write( 0);                              // set to zero (wakes up the MPU-6050)
    auto error = I2C_0.endTransmission();
    if ( error != 0)
    {
      Serial.println(F( "Error, MPU-6050 init failed"));
      leds[4] = CRGB(136, 3, 3);
      FastLED.show();
      // for(;;);                                   // halt the sketch if error encountered
    } else {
      leds[4] = CRGB(56, 136, 3);
      FastLED.show();
    }




  } else {
    Serial.println("MPU-6050 not found! Skip it...");
    leds[4] = CRGB(207, 114, 7);
    FastLED.show();
  }








  Serial.println("Going ");
  // Now turn the LED off, then pause
  leds[0] = CRGB(61, 190, 14);
  FastLED.show();
  // delay(500);

/*   sr.setAllHigh(); // set all pins HIGH
  delay(500);

  sr.setAllLow(); // set all pins LOW
  delay(500); */


  Serial.println("Setup is done");
  // Now turn the LED off, then pause
  leds[0] = CRGB::Black;
  FastLED.show();

  Serial.print("MOSI: ");
  Serial.println(MOSI);
  Serial.print("MISO: ");
  Serial.println(MISO);
  Serial.print("SCK: ");
  Serial.println(SCK);
  Serial.print("SS: ");
  Serial.println(SS);

}

//bool toggle = false;




//================================================================================
void loop() {
  leds[0] = CRGB(16, 222, 205);
  FastLED.show();
 


  //TODO: make RGB led indication as timeslots and setup default loop for leds blinking
  //    like music, 4 tackts and light "notes" that shown current state

  val = digitalRead(inputPin);  // read input value
  if (val == HIGH) {
    int pitch = NOTE_A4;

    leds[0] = CRGB(228, 90, 16);
    FastLED.show();

     if (ps_speaker_present) {
    tone(SPEAKER_PIN, pitch);
    delay(5);
    noTone(SPEAKER_PIN);
  }
    // check if the input is HIGH
//    digitalWrite(ledPin, HIGH);  // turn LED ON
    if (pirState == LOW) {
      // we have just turned on

      Serial.println("Motion detected!");
      // We only want to print on the output change, not state
      pirState = HIGH;
    }
  } else {
//    digitalWrite(ledPin, LOW); // turn LED OFF
    if (pirState == HIGH) {
      // we have just turned of
      leds[0] = CRGB::Black;
      FastLED.show();
      Serial.println("Motion ended!");
      // We only want to print on the output change, not state
      pirState = LOW;
    }
  }


  // put your main code here, to run repeatedly:
  delay(10); // this speeds up the simulation
  leds[0] = CRGB::Black;
  FastLED.show();
}

void chip_info() {
  // Get chip info
  uint32_t chipId = 0;
  String chipModel = ESP.getChipModel();

  for (int i = 0; i < 17; i = i + 8) {
    chipId |= ((ESP.getEfuseMac() >> (40 - i)) & 0xff) << i;
  }

  Serial.printf("ESP32 Chip model = %s Rev %d\n", ESP.getChipModel(), ESP.getChipRevision());
  Serial.printf("This chip has %d cores\n", ESP.getChipCores());
  Serial.printf("Flash size: %d bytes\n", spi_flash_get_chip_size());

  Serial.print("Chip ID: "); Serial.println(chipId);
  Serial.print("Chip Model: "); Serial.println(chipModel);

  //  Wire.setPins(SDA_pin, SCL_pin);


  //Init wire nogodryg

  if (chipModel == "ESP32-C3") {
    Serial.println("Detected ESP32-C3 ID? Uses 9 and 10 pins");
//    I2C_0.setPins(SDA_0, SCL_0);
    I2C_0.begin(SDA_0, SCL_0, I2C_Freq);

  } else if (chipModel == "ESP32-D0WDQ6") {
    Serial.print("Detected Lolin32 lite ID? Uses ");
    Serial.print(SDA_pin_Lolin);
    Serial.print(" and ");
    Serial.print(SCL_pin_Lolin);
    Serial.println(" pins");
    
    I2C_0.setPins(SDA_pin_Lolin, SCL_pin_Lolin);
    I2C_0.begin(SDA_pin_Lolin, SCL_pin_Lolin, I2C_Freq);
  } else {
    Serial.print("Default ESP32C3 Use 9 and 10 pins");
    I2C_0.begin(SDA_0, SCL_0, I2C_Freq);

  //  I2C_0.setPins(SDA_0, SCL_0);
  }

}

void render_ua2() {
  //     display.fillRect(0, 32, 64, 64, SSD1306_INVERSE);
  display2.fillRect(10, 42, 30, 12, SSD1306_INVERSE);
  //   display.drawTriangle(0,0,10,0,5,7,SSD1306_INVERSE);
  display2.display();
  display2.setTextSize(1); // Draw 2X-scale text
  display2.setTextColor(SSD1306_INVERSE);
  display2.setCursor(10, 56);
  display2.println(F("FUCK RUZZIA"));
  display2.setTextColor(SSD1306_WHITE);
  display2.setCursor(65, 56);
  display2.println(F("@2023"));
  display2.setCursor(10, 10);
  //     display.println(F("MyDeliriumDevice01"));
  //     display.println(F("LAB515-© RїЯ"));
  display2.setCursor(10, 20);
  display2.println(F("Searching SSID: "));
  display2.setCursor(10, 30);
  display2.println(F(WIFI_SSID));
  display2.display();
  //   delay(333);
  //   display.startscrollleft(0, 127);
  //   delay(2000);
  //   display.stopscroll();
}
void render_ua() {
  //     display.fillRect(0, 32, 64, 64, SSD1306_INVERSE);
  display.fillRect(10, 42, 30, 12, SSD1306_INVERSE);
  //   display.drawTriangle(0,0,10,0,5,7,SSD1306_INVERSE);
  display.display();
  display.setTextSize(1); // Draw 2X-scale text
  display.setTextColor(SSD1306_INVERSE);
  display.setCursor(29, 56);
  display.println(F("LAB515"));
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(65, 56);
  display.println(F("@2023"));
  display.setCursor(10, 10);
  //     display.println(F("MyDeliriumDevice01"));
  //     display.println(F("LAB515-© RїЯ"));
  display.setCursor(10, 20);
  display.println(F("Searching SSID: "));
  display.setCursor(10, 30);
  display.println(F(WIFI_SSID));
  display.display();
  //   delay(333);
  //   display.startscrollleft(0, 127);
  //   delay(2000);
  //   display.stopscroll();
}
void checki2c(uint8_t adresok) {

  switch (adresok) {
    case BMP180_ADDRESS:
      sensor_bmp85_present = true;
      Serial.printf("I2C BMP180 sensor is found at 0x%02X\n", adresok);
      break;
    case SCREEN_ADDRESS:
      display_1306_present = true;
      Serial.printf("I2C Display 1306 is found at 0x%02X\n", adresok);
      leds[3] = CRGB(248, 115, 6);
      FastLED.show();
      break;
    case SCREEN2_ADDRESS:
      display2_1306_present = true;
      Serial.printf("I2C Display 1306 is found at 0x%02X\n", adresok);
      leds[2] = CRGB(248, 115, 6);
      FastLED.show();
      break;
    case AHT10_ADDRESS:
      sensor_aht10_present = true;
      Serial.printf("I2C AHT10 sensor is found at 0x%02X\n", adresok);
      break;
    case MPU6050_ADDRESS:
      sensor_mpu6050_present = true;
      Serial.printf("I2C MPU6050 sensor is found at 0x%02X\n", adresok);
      leds[4] = CRGB(248, 115, 6);
      FastLED.show();
      break;
    case DS1307_ADDRESS:
      sensor_ds1307_present = true;
      Serial.printf("I2C DS1307 clock is found at 0x%02X\n", adresok);
      leds[1] = CRGB(248, 115, 6);
      FastLED.show();
      break;
    case 4:

      break;
    case 5:
      break;
    default:
      Serial.printf("I2C device found at address 0x%02X\n", adresok);
      break;
  }
}

void scani2c() {
  byte error, address;
  int nDevices = 0;

  //clean flags before scan
  sensor_bmp85_present = false;
  sensor_aht10_present = false;
  display_1306_present = false;
  sensor_mpu6050_present = false;


  Serial.println("Scanning for I2C devices...");
  for (address = 0x01; address < 0x7f; address++) {
    //    Serial.print(">");
    I2C_0.beginTransmission(address);
    error = I2C_0.endTransmission();
    if (error == 0) {
      Serial.printf("I2C device found at address 0x%02X\n", address);
      checki2c(address);
      nDevices++;
    } else if (error != 2) {
      Serial.printf("Error %d at address 0x%02X\n", error, address);
    }
  }
  if (nDevices == 0) {
    Serial.println("No I2C devices found");
  }
}
void clearRegisters() {
  // Reset all register pins
  for (int i = numOfRegisterPins - 1; i >= 0; i--) {
    registers[i] = LOW;
  }
}

void setRegisterPin(int index, int value) {
  // Set an individual pin HIGH or LOW
  registers[index] = value;
}

void writeRegisters() {
  // Set and display registers
  digitalWrite(LATCH_PIN, LOW);

  for (int i = numOfRegisterPins - 1; i >= 0; i--) {
    digitalWrite(CLOCK_PIN, LOW);
    digitalWrite(DATA_PIN, registers[i]);
    digitalWrite(CLOCK_PIN, HIGH);
  }

  digitalWrite(LATCH_PIN, HIGH);
}