/*
 * Teleporter - Pan/Tilt Servo Steuerung
 * 
 * Empfängt Daten vom Browser über Serial:
 * Format: "pan,tilt\n"
 * Werte: 0-180 (Grad)
 * 
 * pan  = Kopfschütteln (links/rechts)
 * tilt = Nicken (oben/unten)
 * 
 * Hardware:
 * - Pan-Servo  an Pin 9
 * - Tilt-Servo an Pin 10
 */

#include <Servo.h>

// Servo-Objekte
Servo panServo;   // Links/Rechts
Servo tiltServo;  // Oben/Unten

// Pins
const int PAN_PIN = 9;
const int TILT_PIN = 10;

// Aktuelle Positionen
int panPos = 90;
int tiltPos = 90;

// Ziel-Positionen
int panTarget = 90;
int tiltTarget = 90;

// Glättung (0.0 = keine Bewegung, 1.0 = sofort)
const float SMOOTH = 0.15;

// Min/Max Winkel (anpassen je nach Mechanik)
const int PAN_MIN = 30;
const int PAN_MAX = 150;
const int TILT_MIN = 60;
const int TILT_MAX = 120;

void setup() {
  Serial.begin(9600);
  
  panServo.attach(PAN_PIN);
  tiltServo.attach(TILT_PIN);
  
  // Startposition
  panServo.write(90);
  tiltServo.write(90);
  
  Serial.println("Teleporter Pan/Tilt bereit");
  Serial.println("Format: pan,tilt (0-180)");
}

void loop() {
  // Daten lesen
  if (Serial.available() > 0) {
    String data = Serial.readStringUntil('\n');
    data.trim();
    
    // Format: "pan,tilt"
    int commaIndex = data.indexOf(',');
    if (commaIndex > 0) {
      int newPan = data.substring(0, commaIndex).toInt();
      int newTilt = data.substring(commaIndex + 1).toInt();
      
      // Werte begrenzen
      panTarget = constrain(newPan, PAN_MIN, PAN_MAX);
      tiltTarget = constrain(newTilt, TILT_MIN, TILT_MAX);
      
      // Debug
      // Serial.print("Pan: "); Serial.print(panTarget);
      // Serial.print(" Tilt: "); Serial.println(tiltTarget);
    }
  }
  
  // Sanfte Bewegung
  panPos = panPos + (panTarget - panPos) * SMOOTH;
  tiltPos = tiltPos + (tiltTarget - tiltPos) * SMOOTH;
  
  // Servos bewegen
  panServo.write((int)panPos);
  tiltServo.write((int)tiltPos);
  
  delay(20); // ~50 Hz Update-Rate
}