#pragma once

#include <QPointF>
#include <QGeoCoordinate>
#include <QVector3D>
#include <QGeoCoordinate>
#include <QtMath>

#include "PolygonCalculus.h"


namespace GeoUtilities {

    typedef QList<QVector3D>        QVector3DList;
    typedef QList<QPointF>          QPointFList;
    typedef QVector<QPointF>        QPointFVector;
    typedef QList<QGeoCoordinate>   QGeoList;
    typedef QVector<QGeoCoordinate> QGeoVector;

    const double earthRadius = 6378137; // meter    

    template <class GeoPoint1, class GeoPoint2, class Point2D>
    void toGeo(const Point2D &in, const GeoPoint1 &origin, GeoPoint2 &out){
        double x = in.x();
        double y = in.y();

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

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

        // this could (unlikely) be a problem, replace with different coordinate transformation
        if (!qFuzzyCompare(lat, M_PI_2)) {
            out.setLatitude(qAtan(y/earthRadius)*180/M_PI + lat);
            out.setLongitude(qAtan(x/cos(lat/180*M_PI)/earthRadius)*180/M_PI + lon);
            out.setAltitude(origin.altitude());
        }
        return;
    }

    template <class GeoPointList, class GeoPoint, class Point2DList>
    void toGeoList(const Point2DList &in, const GeoPoint &origin, GeoPointList &out){
        out.reserve(in.size());

        typedef typename GeoPointList::value_type GeoPoint2;
        GeoPoint2 p;
        for ( auto c : in) {
            toGeo(c, origin, p);
            out.append(p);
        }
    }

    template <class GeoPoint1, class GeoPoint2, class Point2D>
    void toCartesian(const GeoPoint1 &in, const GeoPoint2 &origin, Point2D &out)
    {
        double lat = in.latitude()/180*M_PI;
        double lon = in.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))  {
            out.setX(qTan(dlon)*earthRadius*qCos(latO));
            out.setY(qTan(dlat)*earthRadius);

        }

        return;
    }

    template <class GeoPointList, class GeoPoint, class Point2DList>
    void toCartesianList(const GeoPointList &in, const GeoPoint &origin, Point2DList &out){
        out.reserve(in.size());

        typedef typename Point2DList::value_type Point2D;
        Point2D p;
        for ( auto c : in) {
            toCartesian(c, origin, p);
            out.append(p);
        }

        return;
    }
}