snake.h

#pragma once

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

#include "snake_typedefs.h"

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

using namespace std;

namespace snake {

//=========================================================================
// Geometry stuff.
//=========================================================================
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.
//=========================================================================
class Scenario{
    public:
        Scenario();

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

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

        const BoundingBox &mAreaBoundingBox() const;

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

        double tileWidth() const;
        void setTileWidth(double tileWidth);

        double tileHeight() const;
        void setTileHeight(double tileHeight);

        double minTileArea() const;
        void setMinTileArea(double 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();

        double _tileWidth;
        double _tileHeight;
        double _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);

//========================================================================================
// Flightplan
//========================================================================================

class Flightplan{
public:
    using ScenarioPtr = shared_ptr<Scenario>;
    using ProgressPtr = shared_ptr<vector<int>>;

    Flightplan(ScenarioPtr s, ScenarioPtr p);

    const vector<BoostPoint>   &waypoints(void)      const {return _waypoints;}
    const vector<BoostPoint>   &arrivalPath(void)    const {return _arrivalPath;}
    const vector<BoostPoint>   &returnPath(void)     const {return _returnPath;}

    double lineDistance() const;
    void setLineDistance(double lineDistance);

    double minTransectLengthconst;
    void setMinTransectLength(double minTransectLength);

    ScenarioPtr scenario() const;
    void setScenario(ScenarioPtr &scenario);

    ProgressPtr progress() const;
    void setProgress(ProgressPtr &progress);

    bool update();

    string errorString;

private:
    // Search Filter to speed up routing.SolveWithParameters(...);
    // Found here: http://www.lia.disi.unibo.it/Staff/MicheleLombardi/or-tools-doc/user_manual/manual/ls/ls_filtering.html
    class SearchFilter;
    struct RoutingDataModel;

    bool _generateTransects();
    void _generateRoutingModel(const BoostLineString &vertices,
                               const BoostPolygon &polygonOffset,
                               size_t n0,
                               RoutingDataModel &dataModel,
                               Matrix<double> &graph);
    double                          _lineDistance;
    double                          _minTransectLength;
    shared_ptr<const Scenario>      _pScenario;
    shared_ptr<const vector<int>>   _pProgress;
    vector<BoostPoint>              _waypoints;
    vector<BoostPoint>              _arrivalPath;
    vector<BoostPoint>              _returnPath;
    vector<BoostLineString>         _transects;
};

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