Bachelorarbeit-Rover/lib/Navigation/route.cpp

/**
 * @file route.cpp
 * @author Alexander Klein (alex@kleiax.de)
 * @brief Implements the class Route and Point
 * @version 0.1
 * @date 2022-01-31
 * 
 * @copyright Copyright (c) 2022
 * 
 */

#include "route.h"

Point::Point(double lat, double lon, uint32_t horizontalAccuracy) {
    this->coordinates.lat = lat;
    this->coordinates.lon = lon; 
    this->init(horizontalAccuracy);
}

Point::Point(int32_t lat, int32_t lon, uint32_t horizontalAccuracy) {
    this->coordinates.lat = lat / 10000000.0;
    this->coordinates.lon = lon / 10000000.0;
    this->init(horizontalAccuracy);
}

Point::Point(Coordinates coords, uint32_t horizontalAccuracy) {
    this->coordinates = coords;
    this->init(horizontalAccuracy);
}

Point::Point(Coordinates coords, bool imported) {
    this->coordinates = coords;
    if (imported)
        this->init(UINT32_MAX);
    else
        this->init(0);
}

Point::Point() {
    this->coordinates.lat = 0;
    this->coordinates.lon = 0; 
    this->init(0);
}

bool Point::operator==(const Point& rhs) const {
    return this->coordinates == rhs.getCoordinates();
}

    // distance = sqrt(dx * dx + dy * dy)
    // mit distance: Entfernung in km 
    // dx = 111.3 * cos(lat) * (lon1 - lon2)
    // lat = (lat1 + lat2) / 2 * 0.01745
    // dy = 111.3 * (lat1 - lat2)
    // lat1, lat2, lon1, lon2: Breite, Länge in Grad
double Point::distanceTo(const Coordinates& point) const {
    Coordinates begin = this->coordinates;
    Coordinates end = point;

    double lat = (begin.lat + end.lat) / 2 * ROUTE_DEGREE_TO_RADIANT;
    double dy = ROUTE_DISTANCE_BETWEEN_LATITUDE * (begin.lat - end.lat);
    double dx = ROUTE_DISTANCE_BETWEEN_LATITUDE * cos(lat) * (begin.lon - end.lon);

    return sqrt(dx * dx + dy * dy);
}

double Point::distanceTo(const Point &point) const {
    return this->distanceTo(point.getCoordinates());
}

int16_t Point::courseTo(const Coordinates& point) const {
    Coordinates begin = this->coordinates;
    Coordinates end = point;

    double phi = log( tan(end.lat * ROUTE_DEGREE_TO_RADIANT / 2 + ROUTE_PI / 4) / tan(begin.lat * ROUTE_DEGREE_TO_RADIANT / 2 + ROUTE_PI / 4) );
    double lon = (begin.lon * ROUTE_DEGREE_TO_RADIANT - end.lon * ROUTE_DEGREE_TO_RADIANT);

    int16_t res = (int16_t) atan2(lon, phi) / ROUTE_DEGREE_TO_RADIANT;

    // if (res < 0)
    //     res += 360; 
    
    return res;
}

int16_t Point::courseTo(const Point &point) const {
    return this->courseTo(point.getCoordinates());
}

void Point::init(uint32_t horizontalAccuracy) {
    this->creationTime = millis();

    if (horizontalAccuracy == UINT32_MAX)
        this->accuracy = PointAccuracy::imported;
    else if (horizontalAccuracy > 9999)
        this->accuracy = PointAccuracy::fourDigOfCM;
    else if (horizontalAccuracy > 999)
        this->accuracy = PointAccuracy::threeDigOfCM;
    else if (horizontalAccuracy > 99)
        this->accuracy = PointAccuracy::twoDigOfCM;
    else if (horizontalAccuracy > 1)
        this->accuracy = PointAccuracy::oneDigOfCM;
    else 
        this->accuracy = PointAccuracy::none;
        
}

Route::Route() {
    
}

void Route::addPointToRoute(Point point) {
    this->points.push_back(point);
}

void Route::clear() {
    this->points.clear();
    this->currentPoint = 0;
    this->started = false;
}

Point Route::startRoute() {
    if (this->points.size() < 1) {
        this->started = false;
        return Point();
    }

    this->it = this->points.begin();
    this->currentPoint = 1;
    this->started = true;
    return *this->it;
}

Point Route::endRoute() {
    if (this->points.size() < 1) {
        this->started = false;
        return Point();
    }

    this->it = this->points.end();
    // TODO: Understand why i have to decrement the iterator first to get realy the last element.
    this->it--;
    this->currentPoint = this->points.size();
    this->started = true;
    return *this->it;
}

Point Route::getNextPoint() {
    if (!this->started)
        return Point();

    if (this->it != this->points.end()) {
        this->it++;
        this->currentPoint++;
        return *this->it;
    // } else if (this->it == this->points.end() && this->currentPoint != this->points.size()) {
    //     this->currentPoint++;
    //     return *this->it;
    } else
        return Point();
}

Point Route::getPreviousPoint() {
    if (!this->started)
        return Point();
    
    if (this->it != this->points.begin()) {
        this->it--;
        this->currentPoint--;
        return *this->it;
    } else {
        return Point();
    }
}

RouteInfo Route::getRouteInfo() {
    RouteInfo info;
    info.totalPoints = this->points.size();
    info.currentPoint = this->currentPoint;
    return info;
}