/**
 * @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->navigation->getNTRIPClient()->setAutoReconnect(true);
        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() {
    if (ControlPadButton::isControlPadButtonPressed(this->input, ControlPadButton::PadButton::Action)) {
        this->status = this->navigation->addCurrentPosToRoute();
        if (this->status == Navigation::Status::Updated) {
            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;
}