#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 <ESP32Servo.h>        // Servo library for ESP32

Adafruit_FT6206 ctp = Adafruit_FT6206();
#define TFT_CS 15
#define TFT_DC 2
#define TFT_RST -1 // Not connected
Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC, TFT_RST);
#define SLIDER_X 20
#define SLIDER_Y 250
#define SLIDER_WIDTH 200
#define SLIDER_HEIGHT 10
#define SLIDER_COLOR ILI9341_YELLOW
#define SLIDER_HANDLER_COLOR ILI9341_ORANGE
#define SLIDER_HANDLER_WIDTH (SLIDER_WIDTH / 10)
#define SERVO_PIN 5   // Servo control pin
#define SERVO_MIN_ANGLE 0
#define SERVO_MAX_ANGLE 180

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

Servo servo;

int circle1X = 120;
int circle1Y = 120;
int circle1Radius = 50;
int circle2X = 120;
int circle2Y = 120;
int circle2Radius = 35;

void drawSlider() {
  tft.fillRect(SLIDER_X, SLIDER_Y, SLIDER_WIDTH, SLIDER_HEIGHT, SLIDER_COLOR);
}

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);
  drawSlider();
  pinMode(SERVO_PIN, OUTPUT);
  
  drawSliderHandle(map(sliderValue, SERVO_MIN_ANGLE, SERVO_MAX_ANGLE, SLIDER_X, SLIDER_X + SLIDER_WIDTH - SLIDER_HANDLER_WIDTH));
  
  // 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, circle1Radius, ILI9341_LIGHTGREY);
  tft.fillCircle(circle1X, circle1Y, circle2Radius, ILI9341_GREENYELLOW);

  servo.attach(SERVO_PIN);
  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 distance = sqrt(pow(p.x - circle1X, 2) + pow(p.y - circle1Y, 2));
  
  // Check if the touch point is within the gap between the circles
  if (distance > circle2Radius && distance < 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
    updateSliderValue(newSliderValue);
  }
}



void drawSliderHandle(int x) {
  int handlerVisibility = 70;  // Visibility percentage of the handler
  int handlerWidth = SLIDER_HANDLER_WIDTH * handlerVisibility / 100;
  int handlerX = x + (SLIDER_HANDLER_WIDTH - handlerWidth) / 2;
  
  tft.fillRect(SLIDER_X, SLIDER_Y, SLIDER_WIDTH, SLIDER_HEIGHT, SLIDER_COLOR);  // Clear slider background
  tft.fillRect(handlerX, SLIDER_Y, handlerWidth, SLIDER_HEIGHT, SLIDER_HANDLER_COLOR);
}

void updateSliderValue(int newSliderValue) {
  if (newSliderValue != sliderValue) {
    servo.write(newSliderValue);
    drawSliderHandle(map(newSliderValue, SERVO_MIN_ANGLE, SERVO_MAX_ANGLE, SLIDER_X, SLIDER_X + SLIDER_WIDTH - SLIDER_HANDLER_WIDTH));
    sliderValue = newSliderValue;
  }
}