#include "servo.h"


void ServoController::initServos() {
    HrzSrv.attach(12,  500, 2500);
    delay(100);
    VrtSrv.attach(14,  500, 2500);
    delay(100);

    HrzSrv.write(0);
    delay(100);
    VrtSrv.write(90);
    delay(100);
}

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();
    headingDegrees = raw.HeadingDegress;

    WebMonitor.println(headingDegrees);
}

void ServoController::moveToAzAlt(float azimuth, float altitude) {
    digitalWrite(15, 0);

    azimuth -= headingDegrees;

    if (azimuth < 0) {
        azimuth += 360;
    }

    if (altitude <= 0) {
        return;
    }

    if (azimuth <= 180) {
        HrzSrv.write(180-azimuth);
        delay(100);
        VrtSrv.write(altitude + 90);
        delay(100);
    } else {
        azimuth += 90;

        if (azimuth > 360) {
            azimuth -= 360;
        }

        HrzSrv.write(azimuth-270);
        delay(100);
        VrtSrv.write(altitude + 90);
        delay(100);
    }

    WebMonitor.println(azimuth);
    WebMonitor.println(altitude);
    digitalWrite(15, 1);
}
