snake.h

#pragma once

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

#include <boost/geometry.hpp>
#include <boost/units/systems/angle/degrees.hpp>
#include <boost/units/systems/si.hpp>
#include <boost/units/systems/si/io.hpp>
#include <boost/units/systems/si/plane_angle.hpp>
#include <boost/units/systems/si/prefixes.hpp>

namespace bg = boost::geometry;
namespace bu = boost::units;

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

using namespace std;

namespace snake {
//=========================================================================
// Geometry stuff.
//=========================================================================

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;
typedef bg::model::box<BoostPoint> BoostBox;

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

struct BoundingBox {
  BoundingBox();

  void clear();

  double width;
  double height;
  double angle;
  BoostPolygon corners;
};

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 = 0, y = 0, z = 0;
  auto alt = in.altitude();
  alt = std::isnan(alt) ? 0 : alt;
  proj.Forward(in.latitude(), in.longitude(), alt, x, y, z);
  out.set<0>(x);
  out.set<1>(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 = 0, lon = 0, alt = 0;
  proj.Reverse(in.get<0>(), in.get<1>(), 0.0, lat, lon, alt);
  out.setLatitude(lat);
  out.setLongitude(lon);
  out.setAltitude(alt);
}

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

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

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

typedef bu::quantity<bu::si::length> Length;
typedef bu::quantity<bu::si::area> Area;
typedef bu::quantity<bu::si::plane_angle> Angle;
typedef bu::quantity<bu::si::plane_angle> Radian;
typedef bu::quantity<bu::degree::plane_angle> Degree;

bool joinedArea(const std::vector<BoostPolygon *> &areas, BoostPolygon &jArea);
bool joinedArea(const BoostPolygon &mArea, const BoostPolygon &sArea,
                const BoostPolygon &corridor, BoostPolygon &jArea,
                std::string &errorString);
bool tiles(const BoostPolygon &area, Length tileHeight, Length tileWidth,
           Area minTileArea, std::vector<BoostPolygon> &tiles,
           BoundingBox &bbox, std::string &errorString);

using Transects = vector<BoostLineString>;
using Progress = vector<int>;
using Route = BoostLineString;

bool transectsFromScenario(Length distance, Length minLength, Angle angle,
                           const BoostPolygon &mArea,
                           const std::vector<BoostPolygon> &tiles,
                           const Progress &p, Transects &t,
                           string &errorString);

struct TransectInfo {
  TransectInfo(size_t n, bool r) : index(n), reversed(r) {}
  size_t index;
  bool reversed;
};
bool route(const BoostPolygon &area, const Transects &transects,
           std::vector<TransectInfo> &transectInfo, Route &route,
           string &errorString);

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

// operator== and operator!= for boost point
namespace boost {
namespace geometry {
namespace model {

bool operator==(snake::BoostPoint p1, snake::BoostPoint p2);
bool operator!=(snake::BoostPoint p1, snake::BoostPoint p2);

} // namespace model
} // namespace geometry
} // namespace boost