#include <Servo.h>
#include <Adafruit_NeoPixel.h>

// Pin definitions for Neopixels
#define Neopixel1 6
#define Neopixel2 7

// Pin definitions for Servo motors
#define ServoEyeA 11
#define ServoEyeB 10
#define ServoEyelids 9

#define blue_speed 10
#define LED_num 30

// Length of the trailing effect and speed control
#define TRAIL_LEN  7    // Length of the trailing effect
#define SPEED      50   // Speed control (lower value = faster)

// NeoPixel objects for the two strips
Adafruit_NeoPixel rgb1 = Adafruit_NeoPixel(LED_num, Neopixel1, NEO_GRB + NEO_KHZ800);
Adafruit_NeoPixel rgb2 = Adafruit_NeoPixel(LED_num, Neopixel2, NEO_GRB + NEO_KHZ800);

// Servo objects for the two eyes and eyelids
Servo eyeA;
Servo eyeB;
Servo eyelids;

// Timers for various intervals and time tracking
unsigned long strip1MillisTimer, strip2MillisTimer, currentMillis, eyeAMillisTimer, eyeBMillisTimer, eyelidMillisTimer;

// Interval variables for controlling different actions
int eyeAInterval, eyeBInterval, eyelidInterval;

// Arrays to define intervals for different actions
// If the servo movements are not able to reach the full desired range, extend the delay 50 to for example 115:
int EyeA_intervals[] = {1000, 50, 50, 500, 50, 50, 1000, 1500, 50, 1000, 50, 500, 1000, 50, 50, 1500, 50, 1000};
int EyeB_intervals[] = {500, 500, 1500, 1000, 500, 1500, 500, 500, 2000};
int Eyelids_intervals[] = {2000, 115, 115, 115, 3000, 115, 4000, 115, 4000, 115, 6000, 115, 4000, 115, 2000, 115, 4000, 115, 6000, 115, 4000, 115};

void setup() {
  // Attach servo motors to their respective pins
  eyeA.attach(ServoEyeA);
  eyeB.attach(ServoEyeB);
  eyelids.attach(ServoEyelids);

  // Initialize NeoPixel strips
  rgb1.begin();
  rgb2.begin();

  // Set initial positions for servos
  defaultPos();

  // Initialize Serial communication at a baud rate of 9600 (not-necessary)
  Serial.begin(9600);
}

void loop() {
  currentMillis = millis();   // Get current millis for timing

  controlStrip1();    // Control the first LED strip animation
  controlStrip2();    // Control the second LED strip animation

  controlEyeA();      // Control the movement of Eye A
  controlEyeB();      // Control the movement of Eye B
  controlEyelids();   // Control the movement of Eyelids
}

void defaultPos() {
  eyeA.write(90);
  eyeB.write(90);
  eyelids.write(120);
}

// Control the lighting effect on the first NeoPixel strip.
void controlStrip1() {
  // Check if enough time has passed to update the strip
  if (currentMillis - strip1MillisTimer >= SPEED) {
    strip1MillisTimer = currentMillis;

    // Manage the index for creating the trailing effect
    static int endIndex = LED_num - 1; // Start from the rightmost LED

    // Iterate through each LED on the strip
    for (int i = 0; i < LED_num; i++) {
      // Calculate the index for the current pixel with trail effect
      int pixelIndex = (endIndex - i + LED_num) % LED_num;

      // Calculate brightness based on position for trail effect
      int brightness = (i < TRAIL_LEN) ? map(i, 0, TRAIL_LEN - 1, 0, 255) : 0;

      // Set the color and brightness for the pixel
      rgb1.setPixelColor(pixelIndex, rgb1.Color(0, 0, brightness));
    }

    // Display the updated strip
    rgb1.show();

    // Move the ending index for the trailing effect
    endIndex = (endIndex - 1 + LED_num) % LED_num;
  }
}

// Control the lighting effect on the second NeoPixel strip.
void controlStrip2() {
  // Check if enough time has passed to update the strip
  if (currentMillis - strip2MillisTimer >= SPEED) {
    strip2MillisTimer = currentMillis;

    // Manage the index for creating the trailing effect
    static int endIndex = LED_num - 1; // Start from the rightmost LED

    // Iterate through each LED on the strip
    for (int i = 0; i < LED_num; i++) {
      // Calculate the index for the current pixel with trail effect
      int pixelIndex = (endIndex - i + LED_num) % LED_num;

      // Calculate brightness based on position for trail effect
      int brightness = (i < TRAIL_LEN) ? map(i, 0, TRAIL_LEN - 1, 0, 255) : 0;

      // Set the color and brightness for the pixel
      rgb2.setPixelColor(pixelIndex, rgb2.Color(0, 0, brightness));
    }

    // Display the updated strip
    rgb2.show();

    // Move the ending index for the trailing effect
    endIndex = (endIndex - 1 + LED_num) % LED_num;
  }
}

void controlEyeA() {
  // Check if it's time to move to the next interval
  if (currentMillis - eyeAMillisTimer >= EyeA_intervals[eyeAInterval]) {
    // Save the current time
    eyeAMillisTimer = currentMillis;

    // Print the interval number to the serial monitor
    //   Serial.print("Interval 1: ");
    //   Serial.println(eyeAInterval);

    switch (eyeAInterval) { // 0-17 intervals for Eye A
      case 0:
        // Delay: 1000 ms
        // Action: Move Eye A to position 40
        eyeA.write(40);
        break;
      case 1:
        // Delay: 50 ms
        // Action: Move Eye A to position 60
        eyeA.write(60);
        break;
      case 2:
        // Delay: 50 ms
        // Action: Move Eye A to position 30
        eyeA.write(30);
        break;
      case 3:
        // Delay: 500 ms
        // Action: Move Eye A to position 60
        eyeA.write(60);
        break;
      case 4:
        // Delay: 50 ms
        // Action: Move Eye A to position 10
        eyeA.write(10);
        break;
      case 5:
        // Delay: 50 ms
        // Action: Move Eye A to position 20
        eyeA.write(20);
        break;
      case 6:
        // Delay: 1000 ms
        // Action: Move Eye A to position 100
        eyeA.write(100);
        break;
      case 7:
        // Delay: 1500 ms
        // Action: Move Eye A to position 140
        eyeA.write(140);
        break;
      case 8:
        // Delay: 50 ms
        // Action: Move Eye A to position 120
        eyeA.write(120);
        break;
      case 9:
        // Delay: 1000 ms
        // Action: Move Eye A to position 140
        eyeA.write(140);
        break;
      case 10:
        // Delay: 50 ms
        // Action: Move Eye A to position 120
        eyeA.write(120);
        break;
      case 11:
        // Delay: 500 ms
        // Action: Move Eye A to position 170
        eyeA.write(170);
        break;
      case 12:
        // Delay: 1000 ms
        // Action: Move Eye A to position 120
        eyeA.write(120);
        break;
      case 13:
        // Delay: 50 ms
        // Action: Move Eye A to position 140
        eyeA.write(140);
        break;
      case 14:
        // Delay: 50 ms
        // Action: Move Eye A to position 130
        eyeA.write(130);
        break;
      case 15:
        // Delay: 1500 ms
        // Action: Move Eye A to position 50
        eyeA.write(50);
        break;
      case 16:
        // Delay: 50 ms
        // Action: Move Eye A to position 70
        eyeA.write(70);
        break;
      case 17:
        // Delay: 1000 ms
        // Action: Move Eye A to position 90
        eyeA.write(90);
        break;
    }

    // Move to the next interval
    eyeAInterval++;
    if (eyeAInterval >= sizeof(EyeA_intervals) / sizeof(EyeA_intervals[0])) {
      eyeAInterval = 0; // Reset to the beginning if we've reached the end
    }
  }
}

void controlEyeB() {
  // Check if it's time to move to the next interval
  if (currentMillis - eyeBMillisTimer >= EyeB_intervals[eyeBInterval]) {
    eyeBMillisTimer = currentMillis;

    // Print the interval number to the serial monitor
    //    Serial.print("Interval 2: ");
    //   Serial.println(eyeBInterval);

    switch (eyeBInterval) { // 0-8 intervals for Eye B
      case 0:
        // Delay: 500 ms
        // Action: Move Eye B to position 160
        eyeB.write(160);
        break;
      case 1:
        // Delay: 500 ms
        // Action: Move Eye B to position 130
        eyeB.write(130);
        break;
      case 2:
        // Delay: 1500 ms
        // Action: Move Eye B to position 90
        eyeB.write(90);
        break;
      case 3:
        // Delay: 1000 ms
        // Action: Move Eye B to position 60
        eyeB.write(60);
        break;
      case 4:
        // Delay: 500 ms
        // Action: Move Eye B to position 30
        eyeB.write(30);
        break;
      case 5:
        // Delay: 1500 ms
        // Action: Move Eye B to position 80
        eyeB.write(80);
        break;
      case 6:
        // Delay: 500 ms
        // Action: Move Eye B to position 110
        eyeB.write(110);
        break;
      case 7:
        // Delay: 500 ms
        // Action: Move Eye B to position 160
        eyeB.write(160);
        break;
      case 8:
        // Delay: 2000 ms
        // Action: Move Eye B to position 140
        eyeB.write(140);
        break;
    }

    // Move to the next interval
    eyeBInterval++;
    if (eyeBInterval >= sizeof(EyeB_intervals) / sizeof(EyeB_intervals[0])) {
      eyeBInterval = 0; // Reset to the beginning if we've reached the end
    }
  }
}

void controlEyelids() {
  // Check if it's time to move to the next interval
  if (currentMillis - eyelidMillisTimer >= Eyelids_intervals[eyelidInterval]) {
    eyelidMillisTimer = currentMillis;

    // Print the interval number to the serial monitor
    // Serial.print("Interval 3: ");
    // Serial.println(eyelidInterval);

    switch (eyelidInterval) { // 0-21 intervals for Eyelids
      case 0:
        // Delay: 2000 ms
        // Action: Move Eyelids to closed position (0)
        eyelids.write(0);
        break;
      case 1:
        // Delay: 115 ms
        // Action: Move Eyelids to partially closed position (120)
        eyelids.write(120);
        break;
      case 2:
        // Delay: 115 ms
        // Action: Move Eyelids to closed position (0)
        eyelids.write(0);
        break;
      case 3:
        // Delay: 115 ms
        // Action: Move Eyelids to partially closed position (120)
        eyelids.write(120);
        break;
      case 4:
        // Delay: 3000 ms
        // Action: Move Eyelids to closed position (0)
        eyelids.write(0);
        break;
      case 5:
        // Delay: 115 ms
        // Action: Move Eyelids to partially closed position (120)
        eyelids.write(120);
        break;
      case 6:
        // Delay: 4000 ms
        // Action: Move Eyelids to closed position (0)
        eyelids.write(0);
        break;
      case 7:
        // Delay: 115 ms
        // Action: Move Eyelids to partially closed position (120)
        eyelids.write(120);
        break;
      case 8:
        // Delay: 4000 ms
        // Action: Move Eyelids to closed position (0)
        eyelids.write(0);
        break;
      case 9:
        // Delay: 115 ms
        // Action: Move Eyelids to partially closed position (120)
        eyelids.write(120);
        break;
      case 10:
        // Delay: 6000 ms
        // Action: Move Eyelids to closed position (0)
        eyelids.write(0);
        break;
      case 11:
        // Delay: 115 ms
        // Action: Move Eyelids to partially closed position (120)
        eyelids.write(120);
        break;
      case 12:
        // Delay: 4000 ms
        // Action: Move Eyelids to closed position (0)
        eyelids.write(0);
        break;
      case 13:
        // Delay: 115 ms
        // Action: Move Eyelids to partially closed position (120)
        eyelids.write(120);
        break;
      case 14:
        // Delay: 2000 ms
        // Action: Move Eyelids to closed position (0)
        eyelids.write(0);
        break;
      case 15:
        // Delay: 115 ms
        // Action: Move Eyelids to partially closed position (120)
        eyelids.write(120);
        break;
      case 16:
        // Delay: 4000 ms
        // Action: Move Eyelids to closed position (0)
        eyelids.write(0);
        break;
      case 17:
        // Delay: 115 ms
        // Action: Move Eyelids to partially closed position (120)
        eyelids.write(120);
        break;
      case 18:
        // Delay: 6000 ms
        // Action: Move Eyelids to closed position (0)
        eyelids.write(0);
        break;
      case 19:
        // Delay: 115 ms
        // Action: Move Eyelids to partially closed position (120)
        eyelids.write(120);
        break;
      case 20:
        // Delay: 4000 ms
        // Action: Move Eyelids to closed position (0)
        eyelids.write(0);
        break;
      case 21:
        // Delay: 6000 ms
        // Action: Move Eyelids to partially closed position (120)
        eyelids.write(120);
        break;
    }

    // Move to the next interval
    eyelidInterval++;
    if (eyelidInterval >= sizeof(Eyelids_intervals) / sizeof(Eyelids_intervals[0])) {
      eyelidInterval = 0; // Reset to the beginning if we've reached the end
    }
  }
}