geometry.h

#pragma once

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

namespace geometry {
//=========================================================================
// Geometry stuff.
//=========================================================================

// Double geometry.
typedef double FloatType;
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;
typedef std::vector<FLineString> LineStringArray;

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

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>
std::ostream &operator<<(std::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 std::ostream &operator<<<>(std::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;
};

static constexpr int earth_radius = 6371000; // meters (m)
static constexpr double epsilon =
    std::numeric_limits<double>::epsilon(); // meters (m)

template <class GeoPoint1, class GeoPoint2>
void toENU(const GeoPoint1 &origin, const GeoPoint2 &in, FPoint &out) {

  double lat_rad = in.latitude() * M_PI / 180;
  double lon_rad = in.longitude() * M_PI / 180;

  double ref_lon_rad = origin.longitude() * M_PI / 180;
  double ref_lat_rad = origin.latitude() * M_PI / 180;

  double sin_lat = std::sin(lat_rad);
  double cos_lat = std::cos(lat_rad);
  double cos_d_lon = std::cos(lon_rad - ref_lon_rad);

  double ref_sin_lat = std::sin(ref_lat_rad);
  double ref_cos_lat = std::cos(ref_lat_rad);

  double c =
      std::acos(ref_sin_lat * sin_lat + ref_cos_lat * cos_lat * cos_d_lon);
  double k = (std::fabs(c) < epsilon) ? 1.0 : (c / std::sin(c));

  double x = k * cos_lat * std::sin(lon_rad - ref_lon_rad) * earth_radius;
  double y = k * (ref_cos_lat * sin_lat - ref_sin_lat * cos_lat * cos_d_lon) *
             earth_radius;

  out.set<0>(x);
  out.set<1>(y);
}

template <class GeoPoint1, class GeoPoint2, class Point>
void toENU(const GeoPoint1 &origin, const GeoPoint2 &in, Point &out) {

  using namespace std;

  double lat_rad = in.latitude() * M_PI / 180;
  double lon_rad = in.longitude() * M_PI / 180;

  double ref_lon_rad = origin.longitude() * M_PI / 180;
  double ref_lat_rad = origin.latitude() * M_PI / 180;

  double sin_lat = sin(lat_rad);
  double cos_lat = cos(lat_rad);
  double cos_d_lon = cos(lon_rad - ref_lon_rad);

  double ref_sin_lat = sin(ref_lat_rad);
  double ref_cos_lat = cos(ref_lat_rad);

  double c = acos(ref_sin_lat * sin_lat + ref_cos_lat * cos_lat * cos_d_lon);
  double k = (fabs(c) < epsilon) ? 1.0 : (c / sin(c));

  double x = k * cos_lat * sin(lon_rad - ref_lon_rad) * earth_radius;
  double y = k * (ref_cos_lat * sin_lat - ref_sin_lat * cos_lat * cos_d_lon) *
             earth_radius;

  out.setX(x);
  out.setY(y);
}

template <class GeoPoint>
void fromENU(const GeoPoint &origin, const FPoint &in, GeoPoint &out) {

  using namespace std;

  double x_rad = in.get<0>() / earth_radius;
  double y_rad = in.get<1>() / earth_radius;
  double c = sqrt(y_rad * y_rad + x_rad * x_rad);
  double sin_c = sin(c);
  double cos_c = cos(c);

  double ref_lon_rad = origin.longitude() * M_PI / 180;
  double ref_lat_rad = origin.latitude() * M_PI / 180;

  double ref_sin_lat = sin(ref_lat_rad);
  double ref_cos_lat = cos(ref_lat_rad);

  double lat_rad;
  double lon_rad;

  if (fabs(c) > epsilon) {
    lat_rad = asin(cos_c * ref_sin_lat + (y_rad * sin_c * ref_cos_lat) / c);
    lon_rad =
        (ref_lon_rad + atan2(x_rad * sin_c, c * ref_cos_lat * cos_c -
                                                y_rad * ref_sin_lat * sin_c));

  } else {
    lat_rad = ref_lat_rad;
    lon_rad = ref_lon_rad;
  }

  out.setLatitude(lat_rad * 180 / M_PI);
  out.setLongitude(lon_rad * 180 / M_PI);
  out.setAltitude(origin.altitude());
}

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);
} // namespace geometry

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

bool operator==(::geometry::FPoint &p1, ::geometry::FPoint &p2);
bool operator!=(::geometry::FPoint &p1, ::geometry::FPoint &p2);
bool operator==(::geometry::IPoint &p1, ::geometry::IPoint &p2);
bool operator!=(::geometry::IPoint &p1, ::geometry::IPoint &p2);

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