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

//=========================================================================
// Scenario.
//=========================================================================

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

class Scenario {
public:
  Scenario();

  void setMeasurementArea(const BoostPolygon &area);
  void setServiceArea(const BoostPolygon &area);
  void setCorridor(const BoostPolygon &area);

  const BoundingBox &mAreaBoundingBox() const;

  const BoostPolygon &measurementArea() const;
  const BoostPolygon &serviceArea() const;
  const BoostPolygon &corridor() const;

  BoostPolygon &measurementArea();
  BoostPolygon &serviceArea();
  BoostPolygon &corridor();

  Length tileWidth() const;
  void setTileWidth(Length tileWidth);

  Length tileHeight() const;
  void setTileHeight(Length tileHeight);

  Area minTileArea() const;
  void setMinTileArea(Area minTileArea);

  const BoostPolygon &joinedArea() const;
  const vector<BoostPolygon> &tiles() const;
  const BoostLineString &tileCenterPoints() const;
  const BoundingBox &measurementAreaBBox() const;
  const BoostPoint &homePositon() const;

  bool update();

  string errorString;

private:
  bool _calculateBoundingBox();
  bool _calculateTiles();
  bool _calculateJoinedArea();

  Length _tileWidth;
  Length _tileHeight;
  Area _minTileArea;

  mutable bool _needsUpdate;

  BoostPolygon _mArea;
  BoostPolygon _sArea;
  BoostPolygon _corridor;
  BoostPolygon _jArea;

  BoundingBox _mAreaBoundingBox;
  vector<BoostPolygon> _tiles;
  BoostLineString _tileCenterPoints;
  BoostPoint _homePosition;
};

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

bool joinAreas(const std::vector<BoostPolygon> &areas,
               BoostPolygon &joinedArea);

//========================================================================================
// flight_plan
//========================================================================================

namespace flight_plan {

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

bool transectsFromScenario(Length distance, Length minLength, Angle angle,
                           const Scenario &scenario, const Progress &p,
                           Transects &t, string &errorString);
bool route(const BoostPolygon &area, const Transects &transects,
           Transects &transectsRouted, Route &route, string &errorString);
} // namespace flight_plan

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