/**
 * @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"
#include "captureRoute.h"

CaptureRoute::~CaptureRoute()
{
    delete this->navigation;

    if (static_cast<bool>(this->sensorData->getNtripClient()))
    {
        this->sensorData->getNtripClient()->setActivated(false);
    }
}

void CaptureRoute::run()
{
    ManualControl::run();
    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;
        }
    }
}

void CaptureRoute::afterActivate()
{
    this->navigation = new Navigation(this->sensorData);
    this->navigation->getRoute()->clear();
    this->sensorData->getNtripClient()->setAutoReconnect(true);
    this->routeInfo = navigation->getRouteInfo();
}

double CaptureRoute::getDistanceToLastPoint() const
{
    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;
}