#include "servo.h"


void ServoController::initServos() {
    HrzSrv.attach(12,  500, 2500);
    delay(50); // Espera 50ms para o motor se mover
    VrtSrv.attach(13,  500, 2500);
}

void ServoController::initMag() {
    while (!mag.begin()) {
        WebMonitor.println("ERRO: Não foi possível iniciar o magnetometro.");
        WebMonitor.println("Tentando novamente em 500ms");
        delay(500);
    }

    if (mag.isQMC()) {
        WebMonitor.println("Iniciando magnetometro QMC5883L");
        
        mag.setRange(QMC5883_RANGE_8GA);
        WebMonitor.print("Range: ");
        WebMonitor.println(mag.getRange());

        mag.setMeasurementMode(QMC5883_CONTINOUS);
        WebMonitor.print("Modo de leitura: ");
        WebMonitor.println(mag.getMeasurementMode());

        mag.setDataRate(QMC5883_DATARATE_200HZ);
        WebMonitor.print("Velocidade: ");
        WebMonitor.println(mag.getDataRate());

        mag.setSamples(QMC5883_SAMPLES_8);
        WebMonitor.print("Sample: ");
        WebMonitor.println(mag.getSamples());

        mag.setDeclinationAngle(DECLINATION);
        WebMonitor.print("Declinação: ");
        WebMonitor.println(DECLINATION);
    } else {
        WebMonitor.println("ERRO: Magnetometro encontrado não é suportado.");
        WebMonitor.println("Suspendendo operação.");
        while (true);
    }
}

void ServoController::updateHeading() {
    mag.getHeadingDegrees();
    sVector_t raw = mag.readRaw();
    float tmp = raw.HeadingDegress;
    
    headingBuffer[bufferPos++] = tmp;
    if (bufferPos > HEADING_BUFFER_LENGTH) {
        bufferPos = 0;
    }

    headingDegrees = 0;
    for (byte i = 0; i < HEADING_BUFFER_LENGTH; i++) {
        headingDegrees += headingBuffer[i];
    }
    headingDegrees /= HEADING_BUFFER_LENGTH;
}

void ServoController::moveToAzAlt() {

}

void ServoController::moveToRADec() {

}