#include <Servo.h>

// Define LED pins
const int led1Pin = 22;
const int led2Pin = 23;
const int led3Pin = 24;

// Define shift register pins for sevseg1 (tens digit)
const int sr1DataPin  = 40;
const int sr1ClockPin = 41;
const int sr1LatchPin = 42;

// Define shift register pins for sevseg2 (units digit)
const int sr2DataPin  = 43;
const int sr2ClockPin = 44;
const int sr2LatchPin = 45;

// Define servo pins
const int servo1Pin = 9;
const int servo2Pin = 10;
const int servo3Pin = 11;

// Create Servo objects
Servo servo1;
Servo servo2;
Servo servo3;

// Global counter variable (0-99)
int counter = 0;

// Digit patterns for common-anode 7-seg displays (active LOW)
// Bit order: Q0->A, Q1->B, Q2->C, Q3->D, Q4->E, Q5->F, Q6->G, Q7->DP
byte digitPatterns[10] = {
  0b11000000,  // 0: A B C D E F on, G off, DP off
  0b11111001,  // 1: B C on
  0b10100100,  // 2
  0b10110000,  // 3
  0b10011001,  // 4
  0b10010010,  // 5
  0b10000010,  // 6
  0b11111000,  // 7
  0b10000000,  // 8
  0b10010000   // 9
};

void setup() {
  // Initialize LED pins
  pinMode(led1Pin, OUTPUT);
  pinMode(led2Pin, OUTPUT);
  pinMode(led3Pin, OUTPUT);
  
  // Initialize shift register control pins for sevseg1
  pinMode(sr1DataPin, OUTPUT);
  pinMode(sr1ClockPin, OUTPUT);
  pinMode(sr1LatchPin, OUTPUT);
  
  // Initialize shift register control pins for sevseg2
  pinMode(sr2DataPin, OUTPUT);
  pinMode(sr2ClockPin, OUTPUT);
  pinMode(sr2LatchPin, OUTPUT);
  
  // Initialize servos
  servo1.attach(servo1Pin);
  servo2.attach(servo2Pin);
  servo3.attach(servo3Pin);
  
  // Initialization: set counter to 0, LEDs off, servos at 0 degrees
  counter = 0;
  digitalWrite(led1Pin, LOW);
  digitalWrite(led2Pin, LOW);
  digitalWrite(led3Pin, LOW);
  
  servo1.write(0);
  servo2.write(0);
  servo3.write(0);
  
  // Update seven segment displays to "00"
  updateSevenSegmentDisplays(0);
}

void loop() {
  // Increment counter every 2 seconds
  counter++;
  if(counter > 99) {
    counter = 0;
  }
  
  // Display update: update seven-seg displays
  updateSevenSegmentDisplays(counter);
  
  // Update LED indicators based on the units digit of counter
  int ones = counter % 10;
  if(ones >= 0 && ones <= 2){
    digitalWrite(led1Pin, HIGH);
    digitalWrite(led2Pin, LOW);
    digitalWrite(led3Pin, LOW);
  }
  else if(ones >= 3 && ones <= 5){
    digitalWrite(led1Pin, LOW);
    digitalWrite(led2Pin, HIGH);
    digitalWrite(led3Pin, LOW);
  }
  else if(ones >= 6 && ones <= 9){
    digitalWrite(led1Pin, LOW);
    digitalWrite(led2Pin, LOW);
    digitalWrite(led3Pin, HIGH);
  }
  
  // Update servos based on the tens digit of counter
  int tens = counter / 10;
  if(tens >= 0 && tens <= 2) {
    servo1.write(30);
    servo2.write(60);
    servo3.write(90);
  }
  else if(tens >= 3 && tens <= 5) {
    servo1.write(60);
    servo2.write(90);
    servo3.write(120);
  }
  else if(tens >= 6 && tens <= 9) {
    servo1.write(90);
    servo2.write(120);
    servo3.write(150);
  }
  
  delay(2000); // Wait 2 seconds before next update
}

// Function to update both 7-segment displays using shift registers.
void updateSevenSegmentDisplays(int number) {
  int tensDigit = number / 10;
  int onesDigit = number % 10;
  
  // Get the segment patterns for tens and ones digits.
  byte tensPattern = digitPatterns[tensDigit];
  byte onesPattern = digitPatterns[onesDigit];
  
  // Update sevseg1 (tens digit) using sr1 shift register
  updateShiftRegister(sr1DataPin, sr1ClockPin, sr1LatchPin, tensPattern);
  
  // Small delay to allow shift register latch to settle
  delay(5);
  
  // Update sevseg2 (ones digit) using sr2 shift register
  updateShiftRegister(sr2DataPin, sr2ClockPin, sr2LatchPin, onesPattern);
}

// Function to send one byte to a shift register.
void updateShiftRegister(int dataPin, int clockPin, int latchPin, byte data) {
  digitalWrite(latchPin, LOW);   // Begin latching
  // Use built-in shiftOut function; using MSBFIRST.
  shiftOut(dataPin, clockPin, MSBFIRST, data);
  digitalWrite(latchPin, HIGH);  // Latch the data (update outputs)
}