/**
 * @file captureRoute.cpp
 * @author Alexander Klein (alex@kleiax.de)
 * @brief Contains the implementation of the class CaptureRoute
 * @version 0.1
 * @date 2022-02-15
 * 
 * @copyright Copyright (c) 2022
 * 
 */

#include "driveModi/Modi/CaptureRoute/captureRoute.h"

CaptureRoute::CaptureRoute(MoveControl* moveControl, const ControlPadInput *input, Navigation* navigation) 
    : ManualControl(moveControl, input) {
        this->navigation = navigation;
        this->navigation->getRoute()->clear();
        this->routeInfo = navigation->getRouteInfo();
}

CaptureRoute::~CaptureRoute() {
    this->navigation->getNTRIPClient()->setActivated(false);
}

void CaptureRoute::loop() {
    ManualControl::loop();

    if (millis() - this->lastMillis < this->delay) {
        return;
    }
    this->runCaptureRoute();
    this->lastMillis = millis();
}

void CaptureRoute::runCaptureRoute() {
    this->checkNtrip();

    if (this->state != NtripState::Enabled)
        return;

    if (ControlPadButton::isControlPadButtonPressed(this->input, ControlPadButton::PadButton::Action)) {
        if (this->navigation->addCurrentPosToRoute()) {
            this->lastSavedPoint = this->navigation->getCurrentPosition();
            this->routeInfo = navigation->getRouteInfo();
            this->updateDisplay = true;
        }
    }
}

double CaptureRoute::getDistanceToLastPoint() {
    if (!this->lastSavedPoint.isInit())
        return 0;
    return this->lastSavedPoint.distanceTo(this->navigation->getCurrentPosition());
}

bool CaptureRoute::shouldUpdate() {
    if (this->updateDisplay) {
        this->updateDisplay = false;
        return true;
    }
    return false;
}

void CaptureRoute::checkNtrip() {
    NTRIPClient* client = this->navigation->getNTRIPClient();
    NTRIPClientStates state = client->getClientState();
    if (state == NTRIPClientStates::notAvailable) {
        this->state = NtripState::NoNtrip;
    } else if (state == NTRIPClientStates::pushData) {
        this->ntripReconnectAttemps = 0;
        this->state = NtripState::Enabled;
    } else if (state ==NTRIPClientStates::wait) {
        this->state = NtripState::Waiting;
        if (millis() - this->reconnectNtripLastMillis < this->reconnectNtripDelayMillis)
            return;

        if (this->ntripReconnectAttemps > this->ntripReconnectMaxAttemps) {
            this->state = NtripState::NoNtrip;
            return;
        }

        this->reconnectNtripLastMillis = millis();
        client->setActivated(true);
        this->ntripReconnectAttemps++;
    }
}