#include <Servo.h>

const int servo11Pin = 11; // Main Clutch
const int servo10Pin = 10; // Auxiliary/Throttle
const int triggerButtonPin = 7; 
const int resetButtonPin = 8;

Servo servo11;
Servo servo10;

bool isLatched = false;

void setup() {
  servo11.attach(servo11Pin);
  servo10.attach(servo10Pin);
  servo11.write(0);
  servo10.write(0);
  
  pinMode(triggerButtonPin, INPUT_PULLUP);
  pinMode(resetButtonPin, INPUT_PULLUP);
}

void loop() {
  int triggerState = digitalRead(triggerButtonPin);
  int resetState = digitalRead(resetButtonPin);

  // 1. DISENGAGE (Instant Pull-in)
  if (triggerState == LOW && !isLatched) {
    servo11.write(90);    
    isLatched = true;     
  }

  // 2. NON-LINEAR ENGAGEMENT (Smooth Release)
  if (resetState == LOW && isLatched) {
    
    for (int pos = 90; pos >= 0; pos -= 1) {
      servo11.write(pos);
      
      // Calculate Servo 10 position (0 to 10 degrees)
      int s10Pos = map(pos, 90, 0, 0, 10);
      servo10.write(s10Pos);

      // --- NON-LINEAR DELAY LOGIC ---
      int currentDelay;
      
      if (pos > 60) {
        currentDelay = 10;  // Fast retract (Dead space)
      } 
      else if (pos <= 60 && pos > 15) {
        currentDelay = 60;  // Very slow (The Friction Zone / Biting Point)
      } 
      else {
        currentDelay = 15;  // Moderate (Full engagement finish)
      }
      
      delay(currentDelay); 
    }
    
    // 3. SECONDARY RESET (Servo 10 returns to home)
    for (int pos = 10; pos >= 0; pos -= 1) {
      servo10.write(pos);
      delay(30);
    }

    isLatched = false;    
  }
}