#include <Servo.h>

// Define pins for LEDs
const int led1Pin = 8;
const int led2Pin = 9;
const int led3Pin = 10;

// Define pins for servos
const int servo1Pin = 2;
const int servo2Pin = 3;
const int servo3Pin = 4;

// Define pins for shift register 1 (tens digit)
const int sr1DataPin = 11;
const int sr1ClockPin = 12;
const int sr1LatchPin = 13;

// Define pins for shift register 2 (units digit)
const int sr2DataPin = 5;
const int sr2ClockPin = 6;
const int sr2LatchPin = 7;

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

// Variables to keep track of time and display count
unsigned long previousMillis = 0;
const long interval = 2000;  // 2 seconds
int displayCount = 0;

// 7-segment display patterns for digits 0-9 (common anode: 0 = ON, 1 = OFF)
// Bit order: A B C D E F G DP (from LSB to MSB)
const byte digitPatterns[10] = {
  B00000011,  // 0: All segments except G and DP are ON
  B10011111,  // 1: Only B and C are ON
  B00100101,  // 2: A, B, D, E, G are ON
  B00001101,  // 3: A, B, C, D, G are ON
  B10011001,  // 4: B, C, F, G are ON
  B01001001,  // 5: A, C, D, F, G are ON
  B01000001,  // 6: A, C, D, E, F, G are ON
  B00011111,  // 7: A, B, C are ON
  B00000001,  // 8: All segments except DP are ON
  B00001001   // 9: A, B, C, D, F, G are ON
};

void setup() {
  // Initialize LED pins as outputs
  pinMode(led1Pin, OUTPUT);
  pinMode(led2Pin, OUTPUT);
  pinMode(led3Pin, OUTPUT);
  
  // Initialize shift register pins as outputs
  pinMode(sr1DataPin, OUTPUT);
  pinMode(sr1ClockPin, OUTPUT);
  pinMode(sr1LatchPin, OUTPUT);
  pinMode(sr2DataPin, OUTPUT);
  pinMode(sr2ClockPin, OUTPUT);
  pinMode(sr2LatchPin, OUTPUT);
  
  // Attach servos to pins
  servo1.attach(servo1Pin);
  servo2.attach(servo2Pin);
  servo3.attach(servo3Pin);
  
  // Initialize system state
  resetSystem();
}

void loop() {
  unsigned long currentMillis = millis();
  
  // Check if 2 seconds have passed
  if (currentMillis - previousMillis >= interval) {
    previousMillis = currentMillis;
    
    // Increment counter
    displayCount++;
    if (displayCount > 99) {
      displayCount = 0;
    }
    
    // Update system state
    updateDisplay();
    updateLEDs();
    updateServos();
  }
}

void resetSystem() {
  // Initialize counter to 0
  displayCount = 0;
  
  // Turn all LEDs off
  digitalWrite(led1Pin, LOW);
  digitalWrite(led2Pin, LOW);
  digitalWrite(led3Pin, LOW);
  
  // Set all servos to 0 degrees
  servo1.write(0);
  servo2.write(0);
  servo3.write(0);
  
  // Display "00" on 7-segment displays
  updateDisplay();
  updateLEDs();
}

void updateDisplay() {
  int tens = displayCount / 10;
  int units = displayCount % 10;
  
  // Update tens digit display
  digitalWrite(sr1LatchPin, LOW);
  shiftOut(sr1DataPin, sr1ClockPin, MSBFIRST, digitPatterns[tens]);
  digitalWrite(sr1LatchPin, HIGH);
  
  // Update units digit display
  digitalWrite(sr2LatchPin, LOW);
  shiftOut(sr2DataPin, sr2ClockPin, MSBFIRST, digitPatterns[units]);
  digitalWrite(sr2LatchPin, HIGH);
}

void updateLEDs() {
  int units = displayCount % 10;
  
  // Turn off all LEDs first
  digitalWrite(led1Pin, LOW);
  digitalWrite(led2Pin, LOW);
  digitalWrite(led3Pin, LOW);
  
  // Turn on appropriate LED based on units digit
  if (units >= 0 && units <= 2) {
    digitalWrite(led1Pin, HIGH);
  } else if (units >= 3 && units <= 5) {
    digitalWrite(led2Pin, HIGH);
  } else { // units >= 6 && units <= 9
    digitalWrite(led3Pin, HIGH);
  }
}

void updateServos() {
  int tens = displayCount / 10;
  
  // Set servo angles based on tens digit
  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 { // tens >= 6 && tens <= 9
    servo1.write(90);
    servo2.write(120);
    servo3.write(150);
  }
}