#include "GeoUtilities.h"


namespace GeoUtilities {

    QGeoCoordinate toGeo(const QVector3D &point, const QGeoCoordinate &origin)
    {
        using namespace PolygonCalculus;
        double z = point.z();
        double h   = origin.altitude();

        QGeoCoordinate coordinate(toGeo(point.toPointF(), origin));
        coordinate.setAltitude(z + h);
        return coordinate;

    }

    QVector3D toCartesian(const QGeoCoordinate &coordinate, const QGeoCoordinate &origin)
    {

        double h   = coordinate.altitude();
        double hO   = origin.altitude();

        QVector3D point(toCartesian2D(coordinate, origin));
        point.setZ(h - hO);

        return  point;
    }

    QGeoVector toGeo(const QVector3DList &points, const QGeoCoordinate &origin)
    {
        QGeoVector coordinates;
        coordinates.reserve(points.size());
        for (auto point : points)
            coordinates.append(toGeo(point, origin));

        return coordinates;
    }

    QVector3DList toCartesian(const QGeoList &coordinates, const QGeoCoordinate &origin)
    {
        QVector3DList points;
        for (auto coordinate : coordinates )
            points.append(toCartesian(coordinate, origin));

        return points;
    }

    QPointF toCartesian2D(const QGeoCoordinate &point, const QGeoCoordinate &origin)
    {
        double lat = point.latitude()/180*M_PI;
        double lon = point.longitude()/180*M_PI;

        double latO = origin.latitude()/180*M_PI;
        double lonO = origin.longitude()/180*M_PI;

        double dlon = lon-lonO;
        double dlat = lat-latO;


        if (!qFuzzyCompare(dlon, M_PI_2) && !qFuzzyCompare(dlat, M_PI_2))
            return QPointF(/* x */ qTan(dlon)*earthRadius*qCos(latO),
                           /* y */ qTan(dlat)*earthRadius);
        else
            return QPointF(); // singularity occurred (tan(pi/2) = inf)
    }

    QPointFList toCartesian2D(const QGeoList &coordinates, const QGeoCoordinate &origin)
    {
        QPointFList listF;
        listF.reserve(coordinates.size());

        for ( auto coordinate : coordinates)
            listF.append(toCartesian2D(coordinate, origin));

        return listF;
    }

    QGeoCoordinate toGeo(const QPointF &point, const QGeoCoordinate &origin)
    {
        double x = point.x();
        double y = point.y();

        double lat = origin.latitude();
        double lon = origin.longitude();

        //qWarning("%lf %lf %lf %lf", x, y, lat, lon);

        if (!qFuzzyCompare(lat, M_PI_2)) // this could (unlikely) be a problem, replace with different coordinate transformation
            return QGeoCoordinate(/* lat */ qAtan(y/earthRadius)*180/M_PI + lat,
                                  /* lon */ qAtan(x/cos(lat/180*M_PI)/earthRadius)*180/M_PI + lon,
                                  /* alt */ origin.altitude());
        else
            return QGeoCoordinate(); // singularity occurred (1/cos(pi/2) = inf)
    }

    QGeoVector toGeo(const QPointFList &points, const QGeoCoordinate &origin)
    {
        QGeoVector coordinates;
        coordinates.reserve(points.size());
        for ( auto point : points)
            coordinates.append(toGeo(point, origin));

        return coordinates;
    }

    QGeoVector toGeo(const QPointFVector &points, const QGeoCoordinate &origin)
    {
        QGeoVector coordinates;
        coordinates.reserve(points.size());
        for ( auto point : points)
            coordinates.append(toGeo(point, origin));

        return coordinates;
    }

}