#pragma once

#include <vector>
#include <string>
#include <array>

#include "snake_typedefs.h"
#include "snake_geometry.h"

#include <GeographicLib/Geocentric.hpp>
#include <GeographicLib/LocalCartesian.hpp>

using namespace std;

namespace snake {

//=========================================================================
// Geometry stuff.
//=========================================================================
template<class T>
class Matrix{
public:
    Matrix() :
        _elements(0),
        _m(0),
        _n(0),
        _isInitialized(false)
    {
    }
    Matrix(size_t m, size_t n) : Matrix(m, n, T{0}){}
    Matrix(size_t m, size_t n, T value) :
        _elements(m*n),
        _m(m),
        _n(n),
        _isInitialized(true)
    {
        assert((m > 0) || (n > 0));
        _matrix.resize(_elements, value);
    }

    double get(size_t i, size_t j) const
    {
        assert(_isInitialized);
        assert(i < _m);
        assert(j < _n);
        return _matrix[i*_m+j];
    }

    size_t getM() const { return _m;}
    size_t getN() const { return _n;}

    void set(size_t i, size_t j, const T &value)
    {
        assert(_isInitialized);
        assert(i < _m);
        assert(j < _n);
        _matrix[i*_m+j] = value;
    }

    void setDimension(size_t m, size_t n, const T &value)
    {
        assert((m > 0) || (n > 0));
        assert(!_isInitialized);
        _m = m;
        _n = n;
        _elements = n*m;
        _matrix.resize(_elements, value);
        _isInitialized = true;
    }

    void setDimension(size_t m, size_t n)
    {
        setDimension(m, n, T{0});
    }

private:
    size_t _elements;
    size_t _m;
    size_t _n;
    bool   _isInitialized;
    std::vector<T> _matrix;
};

typedef struct {
    double width;
    double height;
    double angle;
    BoostPolygon corners;
}BoundingBox;

template<class GeoPoint>
void toENU(const GeoPoint &origin, const GeoPoint &in, BoostPoint &out)
{
    GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(),
                                    GeographicLib::Constants::WGS84_f());
    GeographicLib::LocalCartesian proj(origin.latitude(),
                                       origin.longitude(),
                                       origin.altitude(),
                                       earth);

    double x, y, z;
    proj.Forward(in.latitude(), in.longitude(), in.altitude(), x, y, z);
    out.set<0>(x);
    out.set<0>(y);
    (void)z;
}
template<class GeoPoint>
void fromENU(const GeoPoint &origin, const BoostPoint &in, GeoPoint &out)
{
    GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(),
                                    GeographicLib::Constants::WGS84_f());
    GeographicLib::LocalCartesian proj(origin.latitude(),
                                       origin.longitude(),
                                       origin.altitude(),
                                       earth);

    double lat, lon, alt;
    proj.Reverse(in.get<0>(), in.get<1>(), 0.0, lat, lon, alt);
    out.setLatitude(lat);
    out.setLongitude(lon);
    out.setAltitude(alt);
}

void polygonCenter(const BoostPolygon   &polygon,
                   BoostPoint           &center);
void minimalBoundingBox(const BoostPolygon  &polygon,
                        BoundingBox         &minBBox);
void offsetPolygon(const BoostPolygon   &polygon,
                   BoostPolygon         &polygonOffset,
                   double               offset);

void graphFromPolygon(const BoostPolygon    &polygon,
                      const BoostLineString &vertices,
                      Matrix<double>        &graph);
void toDistanceMatrix(Matrix<double> &graph);
bool dijkstraAlgorithm(const size_t numElements,
                       size_t startIndex,
                       size_t endIndex,
                       std::vector<size_t> &elementPath,
                       std::function<double (const size_t, const size_t)> distanceDij);
void shortestPathFromGraph(const Matrix<double> &graph,
                           size_t startIndex,
                           size_t endIndex,
                           std::vector<size_t> &pathIdx);

//=========================================================================
// Tile calculation.
//=========================================================================
template<class GeoPoint, template <class, class...> class Container>
void areaToEnu( const GeoPoint              &origin,
                const Container<GeoPoint>   &in,
                BoostPolygon                &out )
{
    for(auto vertex : in) {
            BoostPoint p;
            toENU(origin, vertex, p);
            out.outer().push_back(p);
    }
    bg::correct(_measurementAreaENU);
}

template<class GeoPoint, template <class, class...> class Container>
void areaFromEnu( const GeoPoint              &origin,
                  BoostPolygon                &in,
                  const Container<GeoPoint>   &out )
{
    for(auto vertex : in.outer()) {
            GeoPoint p;
            fromENU(origin, vertex, p);
            out.push_back(p);
    }
}

bool calculateTiles(const BoostPolygon          &mArea,
                    double                      tileWidth,
                    double                      tileHeight,
                    double                      minTileArea,
                    std::vector<BoostPolygon>   &tiles,
                    std::string                 &errorString);
bool joinAreas(const std::vector<BoostPolygon>  &areas,
               BoostPolygon                     &joinedArea);


//=========================================================================
// Waypoint calculation.
//=========================================================================
bool waypoints(const BoostPolygon &mArea, const BoostPolygon &jArea,
               std::vector<BoostPolygon> &tiles,
               BoostPoint &home,
               double lineDistance,
               double minTransectLength, std::vector<BoostPoint>, size_t arrivalPathLength, size_t returnPathLength);

    namespace detail {
        const double offsetConstant = 0.1; // meter, polygon offset to compenstate for numerical inaccurracies.
    }
}