snake.h

#pragma once

#include <array>
#include <atomic>
#include <functional>
#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>

#include "QGCQGeoCoordinate.h"
#include "QmlObjectListModel.h"

using namespace std;

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

// Double geometry.
using FloatType = double;
typedef bg::model::point<double, 2, bg::cs::cartesian> FPoint;
typedef bg::model::polygon<FPoint> FPolygon;
typedef bg::model::linestring<FPoint> FLineString;
typedef bg::model::box<FPoint> FBox;

// Integer geometry.
using IntType = long long;
using IPoint = bg::model::point<IntType, 2, bg::cs::cartesian>;
using IPolygon = bg::model::polygon<IPoint>;
using IRing = bg::model::ring<IPoint>;
using ILineString = bg::model::linestring<IPoint>;

FPoint int2Float(const IPoint &ip);
FPoint int2Float(const IPoint &ip, IntType scale);
IPoint float2Int(const FPoint &ip);
IPoint float2Int(const FPoint &ip, IntType scale);

template <class T> class Matrix;

template <class DataType>
ostream &operator<<(ostream &os, const Matrix<DataType> &matrix) {
  for (std::size_t i = 0; i < matrix.m(); ++i) {
    for (std::size_t j = 0; j < matrix.n(); ++j) {
      os << "(" << i << "," << j << "):" << matrix(i, j) << std::endl;
    }
  }
  return os;
}

// Matrix
template <class DataType> class Matrix {
public:
  using value_type = DataType;
  Matrix(std::size_t m, std::size_t n) : _m(m), _n(n) { _matrix.resize(m * n); }
  Matrix(std::size_t m, std::size_t n, DataType value) : _m(m), _n(n) {
    _matrix.resize(m * n, value);
  }

  DataType &operator()(std::size_t i, std::size_t j) {
    assert(i < _m);
    assert(j < _n);
    return _matrix[i * _m + j];
  }

  const DataType &operator()(std::size_t i, std::size_t j) const {
    assert(i < _m);
    assert(j < _n);
    return _matrix[i * _m + j];
  }

  std::size_t m() const { return _n; }
  std::size_t n() const { return _n; }

  friend ostream &operator<<<>(ostream &os, const Matrix<DataType> &dt);

private:
  std::vector<DataType> _matrix;
  const std::size_t _m;
  const std::size_t _n;
}; // Matrix

struct BoundingBox {
  BoundingBox();

  void clear();

  double width;
  double height;
  double angle;
  FPolygon corners;
};

template <class GeoPoint1, class GeoPoint2>
void toENU(const GeoPoint1 &origin, const GeoPoint2 &in, FPoint &out) {
  static GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(),
                                         GeographicLib::Constants::WGS84_f());
  GeographicLib::LocalCartesian proj(origin.latitude(), origin.longitude(), 0,
                                     earth);

  double x = 0, y = 0, z = 0;
  proj.Forward(in.latitude(), in.longitude(), 0, x, y, z);
  out.set<0>(x);
  out.set<1>(y);
  (void)z;
}

template <class GeoPoint1, class GeoPoint2, class Point>
void toENU(const GeoPoint1 &origin, const GeoPoint2 &in, Point &out) {
  static GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(),
                                         GeographicLib::Constants::WGS84_f());
  GeographicLib::LocalCartesian proj(origin.latitude(), origin.longitude(), 0,
                                     earth);

  double x = 0, y = 0, z = 0;
  proj.Forward(in.latitude(), in.longitude(), 0, x, y, z);
  out.setX(x);
  out.setY(y);
  (void)z;
}

template <class GeoPoint>
void fromENU(const GeoPoint &origin, const FPoint &in, GeoPoint &out) {
  static GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(),
                                         GeographicLib::Constants::WGS84_f());
  GeographicLib::LocalCartesian proj(origin.latitude(), origin.longitude(), 0,
                                     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, class Container1, class Container2>
void areaToEnu(const GeoPoint &origin, const Container1 &in, Container2 &out) {
  for (auto &vertex : in) {
    typename Container2::value_type p;
    toENU(origin, vertex, p);
    out.push_back(p);
  }
}

template <class GeoPoint, class Container>
void areaToEnu(const GeoPoint &origin, const Container &in, FPolygon &out) {
  for (auto &vertex : in) {
    FPoint p;
    toENU(origin, vertex, p);
    out.outer().push_back(p);
  }
  bg::correct(out);
}

template <class GeoPoint>
void areaToEnu(const GeoPoint &origin, QmlObjectListModel &in, FPolygon &out) {
  FPolygon buffer;
  for (int i = 0; i < in.count(); ++i) {
    auto vertex = in.value<const QGCQGeoCoordinate *>(i);
    if (vertex != nullptr) {
      FPoint p;
      toENU(origin, vertex->coordinate(), p);
      buffer.outer().push_back(p);
    } else {
      return;
    }
  }
  bg::correct(buffer);
  out = std::move(buffer);
}

template <class GeoPoint, class Container1, class Container2>
void areaFromEnu(const GeoPoint &origin, const Container1 &in,
                 Container2 &out) {
  for (auto &vertex : in) {
    typename Container2::value_type p;
    fromENU(origin, vertex, p);
    out.push_back(p);
  }
}

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

void polygonCenter(const FPolygon &polygon, FPoint &center);
bool minimalBoundingBox(const FPolygon &polygon, BoundingBox &minBBox);
void offsetPolygon(const FPolygon &polygon, FPolygon &polygonOffset,
                   double offset);
void graphFromPolygon(const FPolygon &polygon, const FLineString &vertices,
                      Matrix<double> &graph);
bool toDistanceMatrix(Matrix<double> &graph);
bool dijkstraAlgorithm(size_t numElements, size_t startIndex, size_t endIndex,
                       std::vector<size_t> &elementPath, double &length,
                       std::function<double(size_t, size_t)> distanceDij);

bool shortestPathFromGraph(const Matrix<double> &graph, const size_t startIndex,
                           const 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<FPolygon *> &areas, FPolygon &jArea);
bool joinedArea(const FPolygon &mArea, const FPolygon &sArea,
                const FPolygon &corridor, FPolygon &jArea,
                std::string &errorString);
bool tiles(const FPolygon &area, Length tileHeight, Length tileWidth,
           Area minTileArea, std::vector<FPolygon> &tiles, BoundingBox &bbox,
           std::string &errorString);

using Transects = vector<FLineString>;
using Progress = vector<int>;

bool transectsFromScenario(Length distance, Length minLength, Angle angle,
                           const FPolygon &mArea,
                           const std::vector<FPolygon> &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;
};
struct Route {
  FLineString path;
  std::vector<TransectInfo> info;
};
using Solution =
    std::vector<Route>; // Every route corresponds to one run/vehicle
struct RouteParameter {
  RouteParameter()
      : numSolutionsPerRun(1), numRuns(1), minNumTransectsPerRun(5),
        stop([] { return false; }) {}
  std::size_t numSolutionsPerRun;
  std::size_t numRuns;
  std::size_t minNumTransectsPerRun;
  std::function<bool(void)> stop;
  mutable std::string errorString;
};
bool route(const FPolygon &area, const Transects &transects,
           std::vector<Solution> &solutionVector,
           const RouteParameter &par = RouteParameter());

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::FPoint &p1, snake::FPoint &p2);
bool operator!=(snake::FPoint &p1, snake::FPoint &p2);
bool operator==(snake::IPoint &p1, snake::IPoint &p2);
bool operator!=(snake::IPoint &p1, snake::IPoint &p2);

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