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

#include "WGS84toCartesian.hpp"

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