#include <Servo.h>

// Pin Definitions
const int TOUCH_PIN = D6; // Connected to your PCB pad
const int SERVO_PIN = D0; // Servo control pin

Servo myServo;

// Threshold for touch detection (adjust based on PCB sensitivity)
const int TOUCH_THRESHOLD = 20000; 

void setup() {
  Serial.begin(115200);
  myServo.attach(SERVO_PIN);
  
  // Initialize servo at 0 degrees
  myServo.write(0);
  
  pinMode(TOUCH_PIN, INPUT);
  Serial.println("System Initialized. Touch the ARDRA pad!");
}

void loop() {
  long touchValue = readCapacitivePin(TOUCH_PIN);

  // If the sensor value exceeds our threshold, trigger the servo
  if (touchValue < TOUCH_THRESHOLD) {
    Serial.print("Touch Detected! Value: ");
    Serial.println(touchValue);
    
    // Perform "one turn" 
    // Note: Standard servos move 0-180. 
    // If it's a 360 continuous servo, this dictates speed/time.
    // Assuming standard 180-degree servo:
    myServo.write(180); 
    delay(1000);        
    myServo.write(0);   
    delay(500);         // Debounce delay
  }
  
  delay(10);
}

// Simple capacitive sensing function for RP2040
long readCapacitivePin(int pinToMeasure) {
  long total = 0;
  for (int i = 0; i < 10; i++) {
    pinMode(pinToMeasure, OUTPUT);
    digitalWrite(pinToMeasure, LOW);
    delayMicroseconds(10);
    
    pinMode(pinToMeasure, INPUT);
    long cycles = 0;
    while (digitalRead(pinToMeasure) == LOW && cycles < 2000) {
      cycles++;
    }
    total += cycles;
  }
  return total;
}