#include <Encoder.h>
#include <Servo.h>
#include "SPI.h"
#include "Adafruit_GFX.h"
#include "Adafruit_ILI9341.h"

#define TFT_DC 9 // TFT Data/Command pin
#define TFT_CS 10 // TFT Chip Select pin
Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC);

const byte encoderPinA = 2; // OutputA digital pin 2
const byte encoderPinB = 3; // OutputB digital pin 3
volatile int count = 0;
int protectedCount = 0;
int previousCount = 0;

#define readA bitRead(PIND, 2) // Faster than digitalRead()
#define readB bitRead(PIND, 3) // Faster than digitalRead()

#define buttonPin 4

Encoder myEncoder(encoderPinA, encoderPinB);
long previousValue = 0;
int buttonState = HIGH; // HIGH when not pressed

Servo servos[5]; // Array to hold 5 servo objects
int servoPins[] = {5, 6, 7, 8, A0};
int servoangles[] = {0, 0, 0, 0, 0}; // Initial angles for the 5 servos
int currentServoIndex = 0; // Index to track current servo
int p_encoderValue = 0;

void setup() {
    tft.begin();
    tft.fillScreen(ILI9341_BLACK); // Clear screen

    pinMode(buttonPin, INPUT_PULLUP);
    pinMode(encoderPinA, INPUT_PULLUP);
    pinMode(encoderPinB, INPUT_PULLUP);

    attachInterrupt(digitalPinToInterrupt(encoderPinA), isrA, CHANGE);
    attachInterrupt(digitalPinToInterrupt(encoderPinB), isrB, CHANGE);

    for (int i = 0; i < 5; i++) {
        servos[i].attach(servoPins[i]); // Attach each servo to its pin
        servos[i].write(90); // Set initial angle (90 degrees) for each servo
    }

    Serial.begin(9600);
}

void loop() {
    tft.fillRect(20, 50, 200, 20, ILI9341_BLACK); 
    tft.setCursor(20, 50);
    tft.setTextColor(ILI9341_YELLOW);
    tft.setTextSize(2);
    tft.print("Servo Angle: ");
    tft.print(servoangles[0]);

    tft.fillRect(20, 80, 200, 20, ILI9341_BLACK); 
    tft.setCursor(20, 80);
    tft.setTextColor(ILI9341_YELLOW);
    tft.setTextSize(2);
    tft.print("Servo Angle: ");
    tft.print(servoangles[1]);

    tft.fillRect(20, 110, 200, 20, ILI9341_BLACK); 
    tft.setCursor(20, 110);
    tft.setTextColor(ILI9341_YELLOW);
    tft.setTextSize(2);
    tft.print("Servo Angle: ");
    tft.print(servoangles[2]);

    tft.fillRect(20, 140, 200, 20, ILI9341_BLACK); 
    tft.setCursor(20, 140);
    tft.setTextColor(ILI9341_YELLOW);
    tft.setTextSize(2);
    tft.print("Servo Angle: ");
    tft.print(servoangles[3]);

    tft.fillRect(20, 170, 200, 20, ILI9341_BLACK); 
    tft.setCursor(20, 170);
    tft.setTextColor(ILI9341_YELLOW);
    tft.setTextSize(2);
    tft.print("Servo Angle: ");
    tft.print(servoangles[4]);

    noInterrupts();
    protectedCount = count;
    interrupts();
    
    if (protectedCount != previousCount) {
        // Serial.println(protectedCount);
    }
    previousCount = protectedCount;

    int encoderValue = protectedCount;
    
    // Read button state
    buttonState = digitalRead(buttonPin);
    
    // Check if button is pressed (buttonState is LOW due to INPUT_PULLUP)
    if (buttonState == LOW) {
        Serial.println("Button Pressed!");
        count = 0;
        // Move to the next servo for the next button press
        currentServoIndex++;
        if (currentServoIndex >= 5) {
            currentServoIndex = 0; // Reset to the first servo
        }
    }
    
    // Calculate direction of rotation
    int direction = encoderValue - p_encoderValue > 0 ? 1 : -1;
    
    // Change angle for current servo
    if (encoderValue != p_encoderValue) {
        servoangles[currentServoIndex] += direction * map(abs(encoderValue - p_encoderValue), 0, 100, 0, 180); // Map encoder value to servo angle (0-180)
    }
    servos[currentServoIndex].write(servoangles[currentServoIndex]);
    Serial.print("Servo ");
    Serial.print(currentServoIndex + 1);
    Serial.print(" Angle: ");
    Serial.println(servoangles[currentServoIndex]);
    p_encoderValue = encoderValue;

    delay(50); // Optional delay to debounce and reduce loop frequency
}

void isrA() {
    if (readB != readA) {
        count++;
    } else {
        count--;
    }
}

void isrB() {
    if (readA == readB) {
        count++;
    } else {
        count--;
    }
}