#include <Adafruit_GFX.h>
#include <Adafruit_ILI9341.h>
#include <Wire.h>
#include <Adafruit_FT6206.h>
#include <Servo.h>
#include <Encoder.h>

// Define connections for ILI9341 display
#define TFT_CS 10     // Chip Select pin
#define TFT_DC 9      // Data/Command pin

// Initialize display, touch controller, servo, and encoder
Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC);
Adafruit_FT6206 ctp = Adafruit_FT6206();
Servo myServo;
Encoder myEncoder(2, 3);  // Rotary encoder pins (adjust as needed)

int servoPin = 5;         // Servo motor control pin
int servoAngle = 90;      // Initial servo angle
int lastServoAngle = -1;  // For angle change detection
long encoderPosition = 0; // Track rotary encoder position

enum Mode { TOUCH, MANUAL, SWING };
Mode currentMode = TOUCH;
unsigned long swingDelay = 15;   // Delay for servo swing speed (controls speed in Swing mode)
int swingDirection = 1;          // Swing forward initially
unsigned long lastSwingUpdate = 0;
int stepSize = 1;               // Step size for servo in Manual mode (controls precision)

void setup() {
  Serial.begin(115200);
  
  tft.begin();
  tft.fillScreen(ILI9341_BLACK);
  
  // Initialize the touchscreen
  if (!ctp.begin(10)) {
    Serial.println("Couldn't start FT6206 touchscreen controller");
    while (1);
  }
  Serial.println("Capacitive touchscreen started");

  // Initialize servo and set it to initial position
  myServo.attach(servoPin);
  myServo.write(servoAngle);

  drawUI();               // Draw UI elements on screen
  updateDisplay();        // Show initial angle and mode
}

void loop() {
  // Touchscreen-based mode selection
  if (ctp.touched()) {
    TS_Point p = ctp.getPoint();
    p.x = map(p.x, 0, 240, 240, 0);
    p.y = map(p.y, 0, 320, 320, 0);

    // Detect mode change based on touch location
    if (p.y >= 20 && p.y <= 60) {
      if (p.x >= 20 && p.x < 80) {
        currentMode = TOUCH;
      } else if (p.x >= 90 && p.x < 150) {
        currentMode = MANUAL;
        encoderPosition = servoAngle;  // Sync encoder position with servo angle
        myEncoder.write(encoderPosition * 4); // Adjust for encoder steps
      } else if (p.x >= 160 && p.x <= 220) {
        currentMode = SWING;
      }
      updateDisplay();
    }

    // Touch mode: control servo angle with slider
    if (currentMode == TOUCH && p.y >= 200 && p.y <= 250) {
      servoAngle = map(p.x, 20, 220, 0, 180);
      if (servoAngle != lastServoAngle) {
        myServo.write(servoAngle);
        lastServoAngle = servoAngle;
        updateDisplay();
      }
    }

    // Swing mode: control speed with the slider
    if (currentMode == SWING && p.y >= 200 && p.y <= 250) {
      swingDelay = map(p.x, 20, 220, 5, 100);  // Adjust delay based on slider position
      updateDisplay();
    }

    // Manual mode: control step size with the slider
    if (currentMode == MANUAL && p.y >= 200 && p.y <= 250) {
      stepSize = map(p.x, 20, 220, 1, 10);  // Adjust step size based on slider position
      updateDisplay();
    }
  }

  // Manual mode: control servo angle with rotary encoder
  if (currentMode == MANUAL) {
    long newEncoderPosition = myEncoder.read() / 4;
    if (newEncoderPosition != encoderPosition) {
      encoderPosition = newEncoderPosition;
      servoAngle = constrain(encoderPosition * stepSize, 0, 180);  // Apply step size to the movement
      myServo.write(servoAngle);
      updateDisplay();
    }
  }

  // Swing mode: servo sweeps back and forth from 0 to 180
  if (currentMode == SWING) {
    if (millis() - lastSwingUpdate > swingDelay) {
      lastSwingUpdate = millis();
      servoAngle += swingDirection;
      if (servoAngle >= 180 || servoAngle <= 0) {
        swingDirection = -swingDirection; // Reverse direction
      }
      myServo.write(servoAngle);
      updateDisplay();
    }
  }
}

void drawUI() {
  // Draw mode selection buttons
  tft.fillRect(20, 20, 60, 40, ILI9341_BLUE);
  tft.fillRect(90, 20, 60, 40, ILI9341_GREEN);
  tft.fillRect(160, 20, 60, 40, ILI9341_RED);
  tft.setCursor(30, 30);
  tft.setTextColor(ILI9341_WHITE); tft.setTextSize(1);
  tft.print("Touch");
  tft.setCursor(100, 30);
  tft.print("Manual");
  tft.setCursor(170, 30);
  tft.print("Swing");

  // Draw slider bar for touch mode
  tft.fillRect(20, 220, 200, 20, ILI9341_WHITE);
  tft.drawRect(20, 220, 200, 20, ILI9341_BLACK);
}

void updateDisplay() {
  // Clear previous text area
  tft.fillRect(20, 100, 200, 40, ILI9341_BLACK);

  // Display current mode
  tft.setCursor(20, 100);
  tft.setTextColor(ILI9341_YELLOW);
  tft.setTextSize(2);
  tft.print("Mode: ");
  if (currentMode == TOUCH) tft.print("Touch");
  else if (currentMode == MANUAL) tft.print("Manual");
  else if (currentMode == SWING) tft.print("Swing");

  // Display current servo angle
  tft.fillRect(20, 150, 200, 40, ILI9341_BLACK);
  tft.setCursor(20, 150);
  tft.setTextColor(ILI9341_GREEN);
  tft.setTextSize(2);
  tft.print("Angle: ");
  tft.print(servoAngle);

  // Update slider indicator if in touch mode
  if (currentMode == TOUCH) {
    drawUI(); // Redraw slider area
    int sliderPosX = map(servoAngle, 0, 180, 20, 220);
    tft.fillRect(sliderPosX - 5, 220, 10, 20, ILI9341_RED);
  }

  // Display swing speed or step size depending on mode
  if (currentMode == SWING) {
    tft.setCursor(20, 200);
    tft.setTextColor(ILI9341_WHITE);
    tft.setTextSize(1);
    tft.print("Speed: ");
    tft.print(100 - swingDelay); // Show speed as 100 - swingDelay
  }

  if (currentMode == MANUAL) {
    tft.setCursor(20, 200);
    tft.setTextColor(ILI9341_WHITE);
    tft.setTextSize(1);
    tft.print("Steps: ");
    tft.print(stepSize); // Display step size in Manual mode
  }
}