#pragma once
#include <vector>
#include <array>
#include <boost/geometry.hpp>
#include <bits/stl_set.h>

namespace bg = boost::geometry;

namespace snake_geometry {

typedef std::array<double, 3>   Point2D;
typedef std::array<double, 3>   Point3D;
typedef std::array<double, 3>   GeoPoint3D;
typedef std::array<double, 2>   GeoPoint2D;
typedef std::vector<Point2D>    Point2DList;
typedef std::vector<Point3D>    Point3DList;
typedef std::vector<GeoPoint2D> GeoPoint2DList;
typedef std::vector<GeoPoint3D> GeoPoint3DList;

typedef bg::model::point<double, 2, bg::cs::cartesian>  BoostPoint;
//typedef std::vector<BoostPoint>                        BoostPointList;
typedef bg::model::polygon<BoostPoint>                  BoostPolygon;
typedef bg::model::linestring<BoostPoint>               BoostLineString;
typedef std::vector<std::vector<int64_t>>               Int64Matrix;


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);
        if (!_isInitialized){
            _m = m;
            _n = n;
            _elements = n*m;
            _matrix.resize(_elements, value);
        }
    }

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

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;
}min_bbox_rt;

void toENU(const GeoPoint3D &WGS84Reference, const GeoPoint3D &WGS84Position, Point3D &ENUPosition);
void fromENU(const Point3D &WGS84Reference, const Point3D &ENUPosition, GeoPoint3D &WGS84Position);

void polygonCenter(const BoostPolygon &polygon, BoostPoint &center);
void minimalBoundingBox(const BoostPolygon &polygon, min_bbox_rt &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);


void rotateDeg(const Point2DList &point_list, Point2DList &rotated_point_list, double degree);
void rotateRad(const Point2DList &point_list, Point2DList &rotated_point_list, double rad);

bool isClockwise(const Point2DList &point_list);

void toBoost(const Point2D &point, BoostPoint &boost_point);
void toBoost(const Point2DList &point_list, BoostPolygon &boost_polygon);

void fromBoost(const BoostPoint &boost_point, Point2D &point);
void fromBoost(const BoostPolygon &boost_polygon, Point2DList &point_list);
}