/**
 * @file calcAzimuth.cpp
 * @author Alexander Klein (alex@kleiax.de)
 * @brief 
 * @version 0.1
 * @date 2023-09-03
 * 
 * @copyright Copyright (c) 2023
 * 
 */

#include "calcAzimuth.h"

CalcAzimuth::CalcAzimuth(Point point) {
    this->lastChangePoint = point;
    this->currentPosition = point;
    this->loopDelay = 50;
}

void CalcAzimuth::drivingDirectionChange(Point point) {
    if (point.isInit() && point.isValid()) {
        this->directionChangeMode = true;
        this->lastChangePoint = point;
        this->state = State::Invalid;
    }
}

void CalcAzimuth::updateCurrentPosition(Point point) {
    this->currentPosition = point;
    this->positionChanged = true;
}

void CalcAzimuth::run() {
    if (!this->positionChanged)
        return;

    this->positionChanged = false;
    this->updateAzimuth();
}

void CalcAzimuth::updateAzimuth() {
    if (!this->directionChangeMode
        || this->lastChangePoint.distanceTo(this->currentPosition) < 1.0) 
    {
        this->state = State::Invalid;
        this->calcAzimuth = 999;
        return;
    }

    this->calcAzimuth = this->lastChangePoint.courseTo(this->currentPosition);

    // Map point accuracy to State
    if (this->lastChangePoint.getAccuracy() == Point::Accuracy::oneDigOfCM
        || this->currentPosition.getAccuracy() == Point::Accuracy::oneDigOfCM) 
    {
        this->state = State::Good;
    } 
    else if (this->lastChangePoint.getAccuracy() == Point::Accuracy::twoDigOfCM
        || this->currentPosition.getAccuracy() == Point::Accuracy::twoDigOfCM) 
    {
        this->state = State::Ok;
    } 
    else if (this->lastChangePoint.getAccuracy() == Point::Accuracy::threeDigOfCM
        || this->currentPosition.getAccuracy() == Point::Accuracy::threeDigOfCM) 
    {
        this->state = State::Bad;
    } 
    else 
    {
        this->state = State::Invalid;
    }

    // Upgrade quality if the range grows up
    if (this->lastChangePoint.distanceTo(this->currentPosition) > 2.0) {
        switch (this->state) {
            case State::Bad :
                this->state = State::Ok;
                break;
            
            case State::Ok :
                this->state = State::Good;
                break;

            case State::Good :
                this->state = State::Super;
                break;

            default:
                break;
        }
    }
}