/**
 * Claude's Eyes - Servo Control Implementation
 *
 * Pan/Tilt camera mount using standard hobby servos
 */

#include "servo_control.h"
#include "config.h"

// Global instance
ServoControl Servos;

ServoControl::ServoControl()
    : _initialized(false)
    , _attached(false)
    , _isMoving(false)
    , _currentPan(SERVO_PAN_CENTER)
    , _currentTilt(SERVO_TILT_CENTER)
    , _targetPan(SERVO_PAN_CENTER)
    , _targetTilt(SERVO_TILT_CENTER)
    , _lastStepTime(0)
{
}

bool ServoControl::begin() {
#if !ENABLE_SERVOS
    Serial.println("[Servos] Disabled in config");
    return false;
#endif

    // Allow allocation of all timers for servo library
    ESP32PWM::allocateTimer(0);
    ESP32PWM::allocateTimer(1);
    ESP32PWM::allocateTimer(2);
    ESP32PWM::allocateTimer(3);

    // Attach servos
    _panServo.setPeriodHertz(50);  // Standard 50Hz servo
    _tiltServo.setPeriodHertz(50);

    attach();

    // Move to center position
    center();

    _initialized = true;
    Serial.println("[Servos] Initialized successfully");
    return true;
}

void ServoControl::attach() {
    if (_attached) return;

    _panServo.attach(SERVO_PAN_PIN, 500, 2400);   // Min/max pulse width
    _tiltServo.attach(SERVO_TILT_PIN, 500, 2400);

    _attached = true;
    Serial.println("[Servos] Attached");
}

void ServoControl::detach() {
    if (!_attached) return;

    _panServo.detach();
    _tiltServo.detach();

    _attached = false;
    Serial.println("[Servos] Detached");
}

void ServoControl::look(LookDirection direction, bool smooth) {
    int targetPan = _currentPan;
    int targetTilt = _currentTilt;

    switch (direction) {
        case LOOK_CENTER:
            targetPan = SERVO_PAN_CENTER;
            targetTilt = SERVO_TILT_CENTER;
            break;

        case LOOK_LEFT:
            targetPan = SERVO_PAN_MAX;  // Look left (servo rotates right)
            break;

        case LOOK_RIGHT:
            targetPan = SERVO_PAN_MIN;  // Look right (servo rotates left)
            break;

        case LOOK_UP:
            targetTilt = SERVO_TILT_MAX;
            break;

        case LOOK_DOWN:
            targetTilt = SERVO_TILT_MIN;
            break;

        default:
            break;
    }

    setPosition(targetPan, targetTilt, smooth);
}

void ServoControl::setPosition(int pan, int tilt, bool smooth) {
    if (!_initialized) {
        Serial.println("[Servos] Not initialized");
        return;
    }

    // Ensure servos are attached
    if (!_attached) {
        attach();
    }

    // Clamp to valid ranges
    _targetPan = clampPan(pan);
    _targetTilt = clampTilt(tilt);

    Serial.printf("[Servos] Target position: pan=%d, tilt=%d\n", _targetPan, _targetTilt);

    if (smooth) {
        // Smooth movement will be handled in update()
        _isMoving = true;
        _lastStepTime = millis();
    } else {
        // Immediate movement
        _currentPan = _targetPan;
        _currentTilt = _targetTilt;
        _panServo.write(_currentPan);
        _tiltServo.write(_currentTilt);
        _isMoving = false;
    }
}

void ServoControl::center() {
    setPosition(SERVO_PAN_CENTER, SERVO_TILT_CENTER, false);
}

void ServoControl::update() {
    if (!_isMoving) return;

    unsigned long now = millis();
    if (now - _lastStepTime < SERVO_STEP_DELAY_MS) {
        return;
    }
    _lastStepTime = now;

    bool panDone = false;
    bool tiltDone = false;

    // Move pan servo
    if (_currentPan < _targetPan) {
        _currentPan = min(_currentPan + SERVO_STEP_SIZE, _targetPan);
        _panServo.write(_currentPan);
    } else if (_currentPan > _targetPan) {
        _currentPan = max(_currentPan - SERVO_STEP_SIZE, _targetPan);
        _panServo.write(_currentPan);
    } else {
        panDone = true;
    }

    // Move tilt servo
    if (_currentTilt < _targetTilt) {
        _currentTilt = min(_currentTilt + SERVO_STEP_SIZE, _targetTilt);
        _tiltServo.write(_currentTilt);
    } else if (_currentTilt > _targetTilt) {
        _currentTilt = max(_currentTilt - SERVO_STEP_SIZE, _targetTilt);
        _tiltServo.write(_currentTilt);
    } else {
        tiltDone = true;
    }

    // Check if movement complete
    if (panDone && tiltDone) {
        _isMoving = false;
        Serial.printf("[Servos] Movement complete: pan=%d, tilt=%d\n",
                      _currentPan, _currentTilt);
    }
}

int ServoControl::clampPan(int angle) {
    return constrain(angle, SERVO_PAN_MIN, SERVO_PAN_MAX);
}

int ServoControl::clampTilt(int angle) {
    return constrain(angle, SERVO_TILT_MIN, SERVO_TILT_MAX);
}