#include <Servo.h>

// Pin definitions
const int led1Pin = 13;
const int led2Pin = 12;
const int servo1Pin = 9;
const int servo2Pin = 10;
const int dataPin = 11;     // DS pin of 74HC595
const int clockPin = 8;     // SHCP pin of 74HC595
const int latchPin = 7;     // STCP pin of 74HC595

// Servo objects
Servo servo1;
Servo servo2;

// Variables for timing
unsigned long previousMillisLED = 0;
unsigned long previousMillisSegment = 0;
unsigned long previousMillisServo1 = 0;
unsigned long previousMillisServo2 = 0;
const long intervalLED = 2000;           // 2 seconds
const long intervalSegment = 2000;       // 2 seconds
const long servoSweepTime = 4000;        // 4 seconds for full sweep
const long servoStopDuration = 2000;     // 2 seconds pause at extremes

// State variables
bool led1State = false;                  // led1 off initially
bool led2State = true;                   // led2 on initially
int segmentCount = 0;                    // 0-9 for 7-segment display

// Servo 1 variables
int servo1Position = 0;                  // 0-180 degrees
bool servo1Increasing = true;            // Direction (true = increasing)
bool servo1Stopped = false;              // Is servo in stopped state
unsigned long servo1StopTime = 0;        // When servo started stopping
bool servo1FirstSweep = true;            // Is this the first sweep

// Servo 2 variables
int servo2Position = 180;                // 180-0 degrees
bool servo2Increasing = false;           // Direction (false = decreasing)
bool servo2Stopped = false;              // Is servo in stopped state
unsigned long servo2StopTime = 0;        // When servo started stopping
bool servo2FirstSweep = true;            // Is this the first sweep

// 7-segment display patterns for 0-9
// For common anode display, 0 means segment is ON, 1 means OFF
const byte segmentPatterns[10] = {
    0b11000000,  // 0: A,B,C,D,E,F ON (G,DP OFF)
    0b11111001,  // 1: B,C ON (A,D,E,F,G,DP OFF)
    0b10100100,  // 2: A,B,D,E,G ON (C,F,DP OFF)
    0b10110000,  // 3: A,B,C,D,G ON (E,F,DP OFF)
    0b10011001,  // 4: B,C,F,G ON (A,D,E,DP OFF)
    0b10010010,  // 5: A,C,D,F,G ON (B,E,DP OFF)
    0b10000010,  // 6: A,C,D,E,F,G ON (B,DP OFF)
    0b11111000,  // 7: A,B,C ON (D,E,F,G,DP OFF)
    0b10000000,  // 8: A,B,C,D,E,F,G ON (DP OFF)
    0b10010000   // 9: A,B,C,D,F,G ON (E,DP OFF)
};

void setup() {
  // Initialize LED pins
  pinMode(led1Pin, OUTPUT);
  pinMode(led2Pin, OUTPUT);
  
  // Initialize 74HC595 pins
  pinMode(dataPin, OUTPUT);
  pinMode(clockPin, OUTPUT);
  pinMode(latchPin, OUTPUT);
  
  // Initialize servos
  servo1.attach(servo1Pin);
  servo2.attach(servo2Pin);
  
  // Set initial states
  digitalWrite(led1Pin, LOW);   // led1 off initially
  digitalWrite(led2Pin, HIGH);  // led2 on initially
  
  servo1.write(0);     // Start servo1 at 0 degrees
  servo2.write(180);   // Start servo2 at 180 degrees
  
  // Display initial number (0) on 7-segment
  updateSegmentDisplay(0);
}

void loop() {
  unsigned long currentMillis = millis();
  
  // Update LEDs
  if (currentMillis - previousMillisLED >= intervalLED) {
    previousMillisLED = currentMillis;
    
    // Toggle LED states
    led1State = !led1State;
    led2State = !led2State;
    
    digitalWrite(led1Pin, led1State ? HIGH : LOW);
    digitalWrite(led2Pin, led2State ? HIGH : LOW);
  }
  
  // Update 7-segment display
  if (currentMillis - previousMillisSegment >= intervalSegment) {
    previousMillisSegment = currentMillis;
    
    // Increment count and wrap around after 9
    segmentCount = (segmentCount + 1) % 10;
    
    // Update the display
    updateSegmentDisplay(segmentCount);
  }
  
  // Update servo1
  updateServo1(currentMillis);
  
  // Update servo2
  updateServo2(currentMillis);
}

void updateSegmentDisplay(int number) {
  // Ensure number is in valid range
  if (number < 0 || number > 9) return;
  
  // Pull latch low to start data transfer
  digitalWrite(latchPin, LOW);
  
  // Shift out the data
  shiftOut(dataPin, clockPin, MSBFIRST, segmentPatterns[number]);
  
  // Pull latch high to update display
  digitalWrite(latchPin, HIGH);
}

void updateServo1(unsigned long currentMillis) {
  // If servo is in stop phase
  if (servo1Stopped) {
    if (currentMillis - servo1StopTime >= servoStopDuration) {
      // Resume motion after stop duration
      servo1Stopped = false;
    }
    return;  // Don't move while stopped
  }
  
  // Calculate step delay to complete 180 degrees in 4 seconds (4000ms)
  unsigned long stepDelay = servoSweepTime / 180;  // Time per degree
  
  // Check if it's time to move
  if (currentMillis - previousMillisServo1 >= stepDelay) {
    previousMillisServo1 = currentMillis;
    
    // Update position based on direction
    if (servo1Increasing) {
      servo1Position++;
      if (servo1Position >= 180) {
        servo1Position = 180;
        servo1Increasing = false;
        
        if (!servo1FirstSweep) {
          // Stop at 180 degrees (unless it's the first sweep)
          servo1Stopped = true;
          servo1StopTime = currentMillis;
        } else {
          servo1FirstSweep = false;  // No longer the first sweep
        }
      }
    } else {
      servo1Position--;
      if (servo1Position <= 0) {
        servo1Position = 0;
        servo1Increasing = true;
        
        if (!servo1FirstSweep) {
          // Stop at 0 degrees (unless it's the first sweep)
          servo1Stopped = true;
          servo1StopTime = currentMillis;
        } else {
          servo1FirstSweep = false;  // No longer the first sweep
        }
      }
    }
    
    // Update servo position
    servo1.write(servo1Position);
  }
}

void updateServo2(unsigned long currentMillis) {
  // If servo is in stop phase
  if (servo2Stopped) {
    if (currentMillis - servo2StopTime >= servoStopDuration) {
      // Resume motion after stop duration
      servo2Stopped = false;
    }
    return;  // Don't move while stopped
  }
  
  // Calculate step delay to complete 180 degrees in 4 seconds (4000ms)
  unsigned long stepDelay = servoSweepTime / 180;  // Time per degree
  
  // Check if it's time to move
  if (currentMillis - previousMillisServo2 >= stepDelay) {
    previousMillisServo2 = currentMillis;
    
    // Update position based on direction
    if (servo2Increasing) {
      servo2Position++;
      if (servo2Position >= 180) {
        servo2Position = 180;
        servo2Increasing = false;
        
        if (!servo2FirstSweep) {
          // Stop at 180 degrees (unless it's the first sweep)
          servo2Stopped = true;
          servo2StopTime = currentMillis;
        } else {
          servo2FirstSweep = false;  // No longer the first sweep
        }
      }
    } else {
      servo2Position--;
      if (servo2Position <= 0) {
        servo2Position = 0;
        servo2Increasing = true;
        
        if (!servo2FirstSweep) {
          // Stop at 0 degrees (unless it's the first sweep)
          servo2Stopped = true;
          servo2StopTime = currentMillis;
        } else {
          servo2FirstSweep = false;  // No longer the first sweep
        }
      }
    }
    
    // Update servo position
    servo2.write(servo2Position);
  }
}