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

#include "route.h"

double Point::distanceTo(const Point &point) const {
    // 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 lat = (this->lat + point.lat) / 2 * ROUTE_DEGREE_TO_RADIANT;
    double dy = ROUTE_DISTANCE_BETWEEN_LATITUDE * (this->lat - point.lat);
    double dx = ROUTE_DISTANCE_BETWEEN_LATITUDE * cos(lat) * (this->lon - point.lon);

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

double Point::courseTo(const Point &point) const {
    double phi = log( tan(point.lat * ROUTE_DEGREE_TO_RADIANT / 2 + ROUTE_PI / 4) / tan(this->lat * ROUTE_DEGREE_TO_RADIANT / 2 + ROUTE_PI / 4) );
    double lon = (this->lon * ROUTE_DEGREE_TO_RADIANT - point.lon * ROUTE_DEGREE_TO_RADIANT);

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

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

Route::Route() {

}

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

Point Route::startRoute() {
    if (this->points.size() < 1)
        return Point(0, 0);
    this->it = this->points.begin();
    this->currentPoint = 1;
    return *this->it;
}

Point Route::getNextPoint() {
    if (this->it != this->points.end()) {
        this->it++;
        this->currentPoint++;
        return *this->it;
    }
    else
        return Point(0, 0);
}

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