/*
  Arduino Mega program to drive two LEDs, a 7-segment display via a 74HC595, and two servo motors.
  
  - LED1 and LED2 blink alternately. Initially led1 is OFF while led2 is ON.
    Each LED state lasts 2000 ms.

  - The 7-segment display counts from 0 to 9 repeatedly (each number shown for 2000 ms) via a 74HC595 shift register.
    (The seven segment is assumed to be common anode so a LOW output turns on a segment.)

  - Servo1 sweeps from 0° to 180° then back to 0°.
    Servo2 sweeps oppositely (starting from 180° to 0° then back to 180°).
    For each servo, movement from one end to the other takes 2000ms,
    and at each end the servo pauses for 2000ms before reversing (except at the very start).
  
  All tasks are handled in a non-blocking fashion.
*/

#include <Servo.h>

// ----- Pin assignments -----
const int led1Pin = 22;
const int led2Pin = 23;

const int srDataPin  = 24;  // DS of 74HC595
const int srClockPin = 25;  // SHCP of 74HC595
const int srLatchPin = 26;  // STCP of 74HC595

// Servo PWM pins
const int servo1Pin = 44;
const int servo2Pin = 45;

// ----- Seven Segment Digit Patterns (common anode)
// In a common anode display, a LOW (0) lights a segment.
byte digitPatterns[10] = {
  0xC0, // 0 => 0b11000000 (segments A-F on, G off, DP off)
  0xF9, // 1 => 0b11111001
  0xA4, // 2 => 0b10100100
  0xB0, // 3 => 0b10110000
  0x99, // 4 => 0b10011001
  0x92, // 5 => 0b10010010
  0x82, // 6 => 0b10000010
  0xF8, // 7 => 0b11111000
  0x80, // 8 => 0b10000000
  0x90  // 9 => 0b10010000
};

// ----- Global timing variables -----
unsigned long previousLedMillis = 0;
const unsigned long ledInterval = 2000; // 2 seconds

unsigned long previous7SegMillis = 0;
const unsigned long segInterval = 2000; // 2 seconds
int currentDigit = 0;

// ----- Servo objects and state machine variables -----
Servo servo1;
Servo servo2;

// We'll use a simple state machine for each servo with 4 states:
// State 0: Moving toward one end (duration = moveDuration ms)
// State 1: Hold/end pause (duration = holdDuration ms)
// State 2: Moving back (duration = moveDuration ms)
// State 3: Hold/end pause (duration = holdDuration ms)
const unsigned long moveDuration = 2000;
const unsigned long holdDuration = 2000;

// Servo1: starts at 0°, moves up to 180°, hold, then moves back to 0°, hold.
int servo1State = 0;
unsigned long servo1StateStart = 0;
// For moving states record start and target positions:
int servo1StartPos = 0;
int servo1EndPos   = 180;

// Servo2: starts at 180°, moves down to 0°, hold, then moves back to 180°, hold.
int servo2State = 0;
unsigned long servo2StateStart = 0;
int servo2StartPos = 180;
int servo2EndPos   = 0;

// ----- Helper function to update servo position based on a linear interpolation -----
int calculateServoPos(int startPos, int endPos, unsigned long elapsed, unsigned long duration) {
  float fraction = (float)elapsed / duration;
  if(fraction > 1.0) fraction = 1.0;
  int pos = startPos + (int)((endPos - startPos) * fraction);
  return pos;
}

void setup() {
  // Initialize LED pins
  pinMode(led1Pin, OUTPUT);
  pinMode(led2Pin, OUTPUT);
  
  // Start with led1 OFF and led2 ON
  digitalWrite(led1Pin, LOW);
  digitalWrite(led2Pin, HIGH);

  // Initialize digital pins for shift register
  pinMode(srDataPin, OUTPUT);
  pinMode(srClockPin, OUTPUT);
  pinMode(srLatchPin, OUTPUT);
  // Ensure STCP is LOW initially.
  digitalWrite(srLatchPin, LOW);
  
  // Attach servos
  servo1.attach(servo1Pin);
  servo2.attach(servo2Pin);
  
  // Set initial servo positions.
  servo1.write(0);   // servo1 starts at 0°
  servo2.write(180); // servo2 starts at 180°
  servo1StateStart = millis();
  servo2StateStart = millis();
}

void loop() {
  unsigned long currentMillis = millis();
  
  // ----- LED blinking (alternating every 2 seconds) -----
  if(currentMillis - previousLedMillis >= ledInterval) {
    previousLedMillis = currentMillis;
    // Read current state of led2 (if led2 is ON, led1 is OFF and vice versa)
    if(digitalRead(led2Pin) == HIGH) {
      digitalWrite(led2Pin, LOW);
      digitalWrite(led1Pin, HIGH);
    } else {
      digitalWrite(led2Pin, HIGH);
      digitalWrite(led1Pin, LOW);
    }
  }
  
  // ----- 7-Segment Counting (update every 2 seconds) -----
  if(currentMillis - previous7SegMillis >= segInterval) {
    previous7SegMillis = currentMillis;
    // Send the pattern (using shiftOut) to the 74HC595 so that the seven-seg displays currentDigit.
    digitalWrite(srLatchPin, LOW); // Begin latch cycle
    // The built-in shiftOut writes bit by bit, here we use MSBFIRST.
    shiftOut(srDataPin, srClockPin, MSBFIRST, digitPatterns[currentDigit]);
    digitalWrite(srLatchPin, HIGH); // Latch the values to output
    // Increment digit and roll over after 9.
    currentDigit = (currentDigit + 1) % 10;
  }
  
  // ----- Servo1 State Machine (0->180->0) -----
  switch(servo1State) {
    case 0: { // Moving from low to high (0 to 180)
      unsigned long elapsed = currentMillis - servo1StateStart;
      if(elapsed < moveDuration) {
        int pos = calculateServoPos(0, 180, elapsed, moveDuration);
        servo1.write(pos);
      } else {
        servo1.write(180);
        // Transition to hold state at top
        servo1State = 1;
        servo1StateStart = currentMillis;
      }
      break;
    }
    case 1: { // Holding at 180°
      if(currentMillis - servo1StateStart >= holdDuration) {
        // Transition to moving down: set new start & end.
        servo1State = 2;
        servo1StateStart = currentMillis;
      }
      break;
    }
    case 2: { // Moving from high to low (180 to 0)
      unsigned long elapsed = currentMillis - servo1StateStart;
      if(elapsed < moveDuration) {
        int pos = calculateServoPos(180, 0, elapsed, moveDuration);
        servo1.write(pos);
      } else {
        servo1.write(0);
        // Transition to hold state at 0°
        servo1State = 3;
        servo1StateStart = currentMillis;
      }
      break;
    }
    case 3: { // Holding at 0°
      if(currentMillis - servo1StateStart >= holdDuration) {
        // Transition to moving up again.
        servo1State = 0;
        servo1StateStart = currentMillis;
      }
      break;
    }
  }
  
  // ----- Servo2 State Machine (opposite: 180->0->180) -----
  switch(servo2State) {
    case 0: { // Moving from high to low (180 to 0)
      unsigned long elapsed = currentMillis - servo2StateStart;
      if(elapsed < moveDuration) {
        int pos = calculateServoPos(180, 0, elapsed, moveDuration);
        servo2.write(pos);
      } else {
        servo2.write(0);
        // Transition to hold state at 0°
        servo2State = 1;
        servo2StateStart = currentMillis;
      }
      break;
    }
    case 1: { // Holding at 0°
      if(currentMillis - servo2StateStart >= holdDuration) {
        // Transition to moving up.
        servo2State = 2;
        servo2StateStart = currentMillis;
      }
      break;
    }
    case 2: { // Moving from low to high (0 to 180)
      unsigned long elapsed = currentMillis - servo2StateStart;
      if(elapsed < moveDuration) {
        int pos = calculateServoPos(0, 180, elapsed, moveDuration);
        servo2.write(pos);
      } else {
        servo2.write(180);
        // Transition to hold state at 180°
        servo2State = 3;
        servo2StateStart = currentMillis;
      }
      break;
    }
    case 3: { // Holding at 180°
      if(currentMillis - servo2StateStart >= holdDuration) {
        // Transition to moving down again.
        servo2State = 0;
        servo2StateStart = currentMillis;
      }
      break;
    }
  }
  
  // A very short delay so we don't overwhelm the CPU.
  delay(5);
}