#include <Adafruit_GFX.h>      // Core graphics library
#include <SPI.h>               // This is needed for display
#include <Adafruit_ILI9341.h>
#include <Wire.h>              // This is needed for FT6206
#include <Adafruit_FT6206.h>
#include <Servo.h>        // Servo library for ESP32

Adafruit_FT6206 ctp = Adafruit_FT6206();
#define TFT_CS 10
#define TFT_DC 53
#define TFT_RST -1 // Not connected
Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC, TFT_RST);

#define SLIDER_WIDTH 200
#define SLIDER_HANDLER_COLOR ILI9341_ORANGE
#define SLIDER_HANDLER_WIDTH (SLIDER_WIDTH / 13)
#define SERVO_PIN 5   // Servo control pin
#define SERVO_MIN_ANGLE 0
#define SERVO_MAX_ANGLE 180
#define LED_PIN 13    // Pin connected to the LED

int sliderValue = SERVO_MIN_ANGLE;  // Current slider value
int previousSliderX = 0;  // Previous slider X position

Servo servo;
int ledOn = LOW;

int circle1X = 120;
int circle1Y = 100;
int circle1Radius = 50;
int circle2X = 120;
int circle2Y = 100;
int circle2Radius = 35;
int circle3X = 120;
int circle3Y = 100;
int circle3Radius = 60;

void setup() {
  Serial.begin(115200);
  Serial.println(F("Cap Touch Paint!"));
  tft.begin();
  
  if (!ctp.begin(40)) {  // Pass in 'sensitivity' coefficient
    Serial.println("Couldn't start FT6206 touchscreen controller");
    while (1);
  }
  
  Serial.println("Capacitive touchscreen started");
  tft.fillScreen(ILI9341_BLACK);
  tft.setCursor(50, 0);
  tft.setTextColor(ILI9341_GREEN);
  tft.setTextSize(2);
  tft.println("Smart Knob");

  pinMode(SERVO_PIN, OUTPUT);
  pinMode(LED_PIN, OUTPUT);

  // Draw two circles
  tft.drawCircle(circle1X, circle1Y, circle1Radius, ILI9341_RED);  // Outer circle
  tft.drawCircle(circle1X, circle1Y, circle2Radius, ILI9341_RED);  // Inner circle

  tft.fillCircle(circle1X, circle1Y, circle3Radius, ILI9341_CYAN);
  tft.fillCircle(circle1X, circle1Y, circle1Radius, ILI9341_LIGHTGREY);
  tft.fillCircle(circle1X, circle1Y, circle2Radius, ILI9341_GREENYELLOW);
  tft.fillCircle(77, 100, SLIDER_HANDLER_WIDTH / 2, SLIDER_HANDLER_COLOR);

  servo.attach(SERVO_PIN);
  servo.write(0);
  // servo.setPeriodHertz(50);  // Set servo PWM frequency
}

void loop() {
  // Wait for a touch
  if (!ctp.touched()) {
    delay(10); // Speeds up the simulation
    return;
  }
  
  TS_Point p = ctp.getPoint();
  p.x = map(p.x, 0, 240, 240, 0);
  p.y = map(p.y, 0, 320, 320, 0);
  // Serial.print("(");
  // Serial.print(p.x);
  // Serial.print(", ");
  // Serial.print(p.y);
  // Serial.println(")");
  
  int dist_C12 = sqrt(pow(p.x - circle1X, 2) + pow(p.y - circle1Y, 2));
  int dist_C31 = sqrt(pow(p.x - circle3X, 2) + pow(p.y - circle3Y, 2));

  // Check if the touch point is within the LED region
  if (dist_C31 > circle1Radius && dist_C31 < circle3Radius) {
    unsigned long currentMillis = millis();
    
    static unsigned long lastDebounceTime = 0;
    unsigned long debounceDelay = 200;

    if (currentMillis - lastDebounceTime >= debounceDelay) {
      lastDebounceTime = currentMillis;
      
      Serial.println("LED region touched");
      ledOn = !ledOn;
      digitalWrite(LED_PIN, ledOn);
    }
  } 
  else {
    Serial.println("Touch occurred outside the LED region");
  }

  // Check if the touch point is within the gap between the circles
  if (dist_C12 > circle2Radius && dist_C12 < circle1Radius) {
    // Calculate the angle of the touch point relative to the center of the circles
    int angle = atan2(p.y - circle1Y, p.x - circle1X) * 180 / PI;


    // Map the angle to the servo range
    int newSliderValue = map(angle, -180, 180, SERVO_MIN_ANGLE, SERVO_MAX_ANGLE);
  
    // Update the servo position if it changed
    if (newSliderValue != sliderValue) {
      servo.write(newSliderValue); 

      // Clear the previous angle value by filling it with the background color
      tft.fillRect(100, 92, 40, 15, ILI9341_GREENYELLOW);
      // Print the new angle value
      tft.setCursor(100, 92);
      tft.setTextColor(ILI9341_BLACK);
      tft.setTextSize(2);
      tft.println(newSliderValue);
      Serial.println(newSliderValue);

      // Clear the previous handler position by filling it with the background color
      int previousHandlerX = circle1X + (circle1Radius - SLIDER_HANDLER_WIDTH / 2) * cos(previousSliderX * PI / 180);
      int previousHandlerY = circle1Y + (circle1Radius - SLIDER_HANDLER_WIDTH / 2) * sin(previousSliderX * PI / 180);
      tft.fillCircle(previousHandlerX, previousHandlerY, SLIDER_HANDLER_WIDTH / 2, ILI9341_LIGHTGREY);

      // Calculate the position of the handler on the semi-circular path
      int handlerX = circle1X + (circle1Radius - SLIDER_HANDLER_WIDTH / 2) * cos(angle * PI / 180);
      int handlerY = circle1Y + (circle1Radius - SLIDER_HANDLER_WIDTH / 2) * sin(angle * PI / 180);

      // Draw the handler
      tft.fillCircle(handlerX, handlerY, SLIDER_HANDLER_WIDTH / 2, SLIDER_HANDLER_COLOR);

      // Update the previousSliderX variable
      previousSliderX = angle;
      sliderValue = newSliderValue;
    }

  }
}