snake.cpp

#include <iostream>

#include "snake.h"
#include "clipper/clipper.hpp"
#define CLIPPER_SCALE 10000

using namespace snake_geometry;
using namespace std;

namespace bg = boost::geometry;
namespace trans = bg::strategy::transform;

namespace snake {

Scenario::Scenario() :
    _mAreaBoundingBox(min_bbox_rt{0, 0, 0, BoostPolygon{}})
{

}

bool Scenario::addArea(Area &area)
    {
        if (area.geoPolygon.size() < 3){
            errorString = "Area has less than three vertices.";
            return false;
        }
        if (area.type == MeasurementArea)
            return Scenario::_setMeasurementArea(area);
        else if (area.type == ServiceArea)
            return Scenario::_setServiceArea(area);
        else if (area.type == Corridor)
            return Scenario::_setCorridor(area);
        return false;
}

bool Scenario::defined(double tileWidth, double tileHeight, double minTileArea)
{
    if (!_areas2enu())
        return false;
    if (!_calculateBoundingBox())
        return false;
    if (!_calculateTiles(tileWidth, tileHeight, minTileArea))
        return false;
    if (!_calculateJoinedArea())
        return false;

    return true;
}

bool Scenario::_areas2enu()
{
    if (_measurementArea.geoPolygon.size() > 0){
        _measurementAreaENU.clear();
        for(auto vertex : _measurementArea.geoPolygon) {
                Point3D ENUVertex;
                toENU(_geoOrigin, Point3D{vertex[0], vertex[1], _measurementArea.altitude}, ENUVertex);
                _measurementAreaENU.outer().push_back(BoostPoint{ENUVertex[0], ENUVertex[1]});
        }
        bg::correct(_measurementAreaENU);

        _serviceAreaENU.clear();
        if (_serviceArea.geoPolygon.size() > 0){
            for(auto vertex : _serviceArea.geoPolygon) {
                    Point3D ENUVertex;
                    toENU(_geoOrigin, Point3D{vertex[0], vertex[1], _serviceArea.altitude}, ENUVertex);
                    _serviceAreaENU.outer().push_back(BoostPoint{ENUVertex[0], ENUVertex[1]});
            }
        } else{
            errorString = "Service area has no vertices.";
            return false;
        }
        bg::correct(_serviceAreaENU);
        polygonCenter(_serviceAreaENU, _homePositionENU);

        _corridorENU.clear();
        if (_corridor.geoPolygon.size() > 0){
            for(auto vertex : _corridor.geoPolygon) {
                    Point3D ENUVertex;
                    toENU(_geoOrigin, Point3D{vertex[0], vertex[1], _corridor.altitude}, ENUVertex);
                    _corridorENU.outer().push_back(BoostPoint{ENUVertex[0], ENUVertex[1]});
            }
        }
        bg::correct(_corridorENU);

        return true;
    }

    errorString = "Measurement area has no vertices.";
    return false;
}

bool Scenario::_setMeasurementArea(Area &area)
{
    if (area.geoPolygon.size() <= 0)
        return false;
    GeoPoint2D origin2D = area.geoPolygon[0];
    _geoOrigin = GeoPoint3D{origin2D[0], origin2D[1], 0};
    _measurementArea = area;
    _measurementAreaENU.clear();
    _serviceAreaENU.clear();
    _corridorENU.clear();
    return true;

}

bool Scenario::_setServiceArea(Area &area)
{
    if (area.geoPolygon.size() <= 0)
        return false;
    _serviceArea = area;
    _serviceAreaENU.clear();
    return true;
}

bool Scenario::_setCorridor(Area &area)
{
    if (area.geoPolygon.size() <= 0)
        return false;
    _corridor = area;
    _corridorENU.clear();
    return true;
}

bool Scenario::_calculateBoundingBox()
{
    minimalBoundingBox(_measurementAreaENU, _mAreaBoundingBox);
    return true;
}


/**
 * Devides the (measurement area) bounding  box into tiles and clips it to the measurement area.
 *
 * Devides the (measurement area) bounding  box into tiles of width \p tileWidth and height \p tileHeight.
 * Clips the resulting tiles to the measurement area. Tiles are rejected, if their area is smaller than \p minTileArea.
 * The function assumes that \a _measurementAreaENU and \a _mAreaBoundingBox have correct values. \see \ref Scenario::_areas2enu() and \ref
 * Scenario::_calculateBoundingBox().
 *
 * @param tileWidth The width (>0) of a tile.
 * @param tileHeight The heigth (>0) of a tile.
 * @param minTileArea The minimal area (>0) of a tile.
 *
 * @return Returns true if successful.
 */
bool Scenario::_calculateTiles(double tileWidth, double tileHeight, double minTileArea)
{
    _tilesENU.clear();
    _tileCenterPointsENU.clear();

    if (tileWidth <= 0 || tileHeight <= 0 || minTileArea <= 0) {
        errorString = "Parameters tileWidth, tileHeight, minTileArea must be positive.";
        return false;
    }

    double bbox_width = _mAreaBoundingBox.width;
    double bbox_height = _mAreaBoundingBox.height;

    BoostPoint origin = _mAreaBoundingBox.corners.outer()[0];

    //cout << "Origin: " << origin[0] << " " << origin[1] << endl;
    // Transform _measurementAreaENU polygon to bounding box coordinate system.
    trans::rotate_transformer<boost::geometry::degree, double, 2, 2> rotate(_mAreaBoundingBox.angle*180/M_PI);
    trans::translate_transformer<double, 2, 2> translate(-origin.get<0>(), -origin.get<1>());
    BoostPolygon translated_polygon;
    BoostPolygon rotated_polygon;
    boost::geometry::transform(_measurementAreaENU, translated_polygon, translate);
    boost::geometry::transform(translated_polygon, rotated_polygon, rotate);
    bg::correct(rotated_polygon);

    //cout << bg::wkt<BoostPolygon2D>(rotated_polygon) << endl;

    size_t i_max = ceil(bbox_width/tileWidth);
    size_t j_max = ceil(bbox_height/tileHeight);

    if (i_max < 1 || j_max < 1) {
        errorString = "tileWidth or tileHeight to small.";
        return false;
    }


    trans::rotate_transformer<boost::geometry::degree, double, 2, 2> rotate_back(-_mAreaBoundingBox.angle*180/M_PI);
    trans::translate_transformer<double, 2, 2> translate_back(origin.get<0>(), origin.get<1>());
    for (size_t i = 0; i < i_max; ++i){
        double x_min = tileWidth*i;
        double x_max = x_min + tileWidth;
        for (size_t j = 0; j < j_max; ++j){
            double y_min = tileHeight*j;
            double y_max = y_min + tileHeight;

            BoostPolygon tile_unclipped;
            tile_unclipped.outer().push_back(BoostPoint{x_min, y_min});
            tile_unclipped.outer().push_back(BoostPoint{x_min, y_max});
            tile_unclipped.outer().push_back(BoostPoint{x_max, y_max});
            tile_unclipped.outer().push_back(BoostPoint{x_max, y_min});
            tile_unclipped.outer().push_back(BoostPoint{x_min, y_min});


            std::deque<BoostPolygon> boost_tiles;
            if (!boost::geometry::intersection(tile_unclipped, rotated_polygon, boost_tiles))
                continue;

           for (BoostPolygon t : boost_tiles)
           {
                if (bg::area(t) > minTileArea){
                    // Transform boost_tile to world coordinate system.
                    BoostPolygon rotated_tile;
                    BoostPolygon translated_tile;
                    boost::geometry::transform(t, rotated_tile, rotate_back);
                    boost::geometry::transform(rotated_tile, translated_tile, translate_back);

                    // Store tile and calculate center point.
                    _tilesENU.push_back(translated_tile);
                    BoostPoint tile_center;
                    polygonCenter(translated_tile, tile_center);
                    _tileCenterPointsENU.push_back(tile_center);
                }
           }
        }
    }

    if (_tilesENU.size() < 1){
        errorString = "No tiles calculated. Is the minTileArea parameter large enough?";
        return false;
    }

    return true;
}

bool Scenario::_calculateJoinedArea()
{
    _joinedAreaENU.clear();
    // Measurement area and service area overlapping?
    bool overlapingSerMeas = bg::intersects(_measurementAreaENU, _serviceAreaENU) ? true : false;
    bool corridorValid = _corridorENU.outer().size() > 0 ? true : false;


    // Check if corridor is connecting measurement area and service area.
    bool corridor_is_connection = false;
    if (corridorValid) {
        // Corridor overlaping with measurement area?
        if ( bg::intersects(_corridorENU, _measurementAreaENU) ) {
            // Corridor overlaping with service area?
            if ( bg::intersects(_corridorENU, _serviceAreaENU) )
                corridor_is_connection = true;
        }
    }

    // Are areas joinable?
    std::deque<BoostPolygon> sol;
    BoostPolygon partialArea = _measurementAreaENU;
    if (overlapingSerMeas){
        if(corridor_is_connection){
            bg::union_(partialArea, _corridorENU, sol);
        }
    } else if (corridor_is_connection){
        bg::union_(partialArea, _corridorENU, sol);
    } else {
        errorString = "Areas are not overlapping";
        return false;
    }

    if (sol.size() > 0) {
        partialArea = sol[0];
        sol.clear();
    }

    // Join areas.
    bg::union_(partialArea, _serviceAreaENU, sol);

    if (sol.size() > 0) {
        _joinedAreaENU = sol[0];
    } else {
        return false;
    }

    return true;
}

FlightPlan::FlightPlan()
{

}

bool FlightPlan::generate(double lineDistance, double minTransectLength)
{
    _waypointsENU.clear();    
    _waypoints.clear();
    _arrivalPathIdx.clear();
    _returnPathIdx.clear();

#ifndef NDEBUG
    _PathVertices.clear();
#endif
    auto start = std::chrono::high_resolution_clock::now();
    if (!_generateTransects(lineDistance, minTransectLength))
        return false;
    auto delta = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now() - start);
    cout << endl;
   cout << "Execution time _generateTransects(): " << delta.count() << " ms" << endl;

    //=======================================
    // Route Transects using Google or-tools.
    //=======================================
    // Offset joined area.
    const BoostPolygon &jArea = _scenario.getJoineAreaENU();
    BoostPolygon jAreaOffset;
    offsetPolygon(jArea, jAreaOffset, detail::offsetConstant);

    // Create vertex list;
    BoostLineString vertices;
    size_t n_t = _transects.size()*2;
    size_t n0  = n_t+1;
    vertices.reserve(n0);
    for (auto lstring : _transects){
        for (auto vertex : lstring){
            vertices.push_back(vertex);
        }
    }
    vertices.push_back(_scenario.getHomePositonENU());

    for (long i=0; i<long(jArea.outer().size())-1; ++i) {
        vertices.push_back(jArea.outer()[i]);
    }
    for (auto ring : jArea.inners()) {
        for (auto vertex : ring)
            vertices.push_back(vertex);
    }
    size_t n1 = vertices.size();
    // Generate routing model.
    RoutingDataModel_t dataModel;
    Matrix<double> connectionGraph(n1, n1);

    start = std::chrono::high_resolution_clock::now();
    _generateRoutingModel(vertices, jAreaOffset, n0, dataModel, connectionGraph);
    delta = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now() - start);
    cout << "Execution time _generateRoutingModel(): " << delta.count() << " ms" << endl;

    // Create Routing Index Manager.
    RoutingIndexManager manager(dataModel.distanceMatrix.getN(), dataModel.numVehicles,
                                dataModel.depot);
    // Create Routing Model.
    RoutingModel routing(manager);

    // Create and register a transit callback.
    const int transit_callback_index = routing.RegisterTransitCallback(
        [&dataModel, &manager](int64 from_index, int64 to_index) -> int64 {
          // Convert from routing variable Index to distance matrix NodeIndex.
          auto from_node = manager.IndexToNode(from_index).value();
          auto to_node = manager.IndexToNode(to_index).value();
          return dataModel.distanceMatrix.get(from_node, to_node);
        });


    // Define Constraints.
    size_t n = _transects.size()*2;
    Solver *solver = routing.solver();
    for (size_t i=0; i<n; i=i+2){
//        auto idx0 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(i));
//        auto idx1 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(i+1));
//        auto cond0 = routing.NextVar(idx0)->IsEqual(idx1);
//        auto cond1 = routing.NextVar(idx1)->IsEqual(idx0);
//        auto c = solver->MakeNonEquality(cond0, cond1);
//        solver->AddConstraint(c);

        // alternative
        auto idx0 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(i));
        auto idx1 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(i+1));
        auto cond0 = routing.NextVar(idx0)->IsEqual(idx1);
        auto cond1 = routing.NextVar(idx1)->IsEqual(idx0);
        vector<IntVar*> conds{cond0, cond1};
        auto c = solver->MakeAllDifferent(conds);
        solver->MakeRejectFilter();
        solver->AddConstraint(c);
    }

    // Define cost of each arc.
    routing.SetArcCostEvaluatorOfAllVehicles(transit_callback_index);


    // Setting first solution heuristic.
    RoutingSearchParameters searchParameters = DefaultRoutingSearchParameters();
    searchParameters.set_first_solution_strategy(
        FirstSolutionStrategy::PATH_CHEAPEST_ARC);
    google::protobuf::Duration *tMax = new google::protobuf::Duration(); // seconds
    tMax->set_seconds(10);
    searchParameters.set_allocated_time_limit(tMax);

    // Solve the problem.
    start = std::chrono::high_resolution_clock::now();
    const Assignment* solution = routing.SolveWithParameters(searchParameters);
    delta = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now() - start);
    cout << "Execution time routing.SolveWithParameters(): " << delta.count() << " ms" << endl;

    if (!solution || solution->Size() <= 1){
        errorString = "Not able to solve the routing problem.";
        return false;
    }

    // Extract waypoints from solution.
    long index = routing.Start(0);
    std::vector<size_t> route;
    route.push_back(manager.IndexToNode(index).value());
    while (!routing.IsEnd(index)){
        index = solution->Value(routing.NextVar(index));
        route.push_back(manager.IndexToNode(index).value());
    }

    // Connect transects
#ifndef NDEBUG
    _PathVertices = vertices;
#endif

    {
    _waypointsENU.push_back(vertices[route[0]]);
    vector<size_t> pathIdx;
    for (long i=0; i<long(route.size())-1; ++i){
        size_t idx0 = route[i];
        size_t idx1 = route[i+1];
        pathIdx.clear();
        shortestPathFromGraph(connectionGraph, idx0, idx1, pathIdx);
        for (size_t j=1; j<pathIdx.size(); ++j)
            _waypointsENU.push_back(vertices[pathIdx[j]]);
    }
    }

    // Back transform waypoints.
    GeoPoint3D origin{_scenario.getOrigin()};
    for (auto vertex : _waypointsENU) {
        GeoPoint3D geoVertex;
        fromENU(origin, Point3D{vertex.get<0>(), vertex.get<1>(), 0}, geoVertex);
        _waypoints.push_back(GeoPoint2D{geoVertex[0], geoVertex[1]});
    }

    // Reconstruct arrival path;
    int i = 0;
    for (; route[i] >= n_t; ++i)
        _arrivalPathIdx.push_back(i);
    _arrivalPathIdx.push_back(i); // vertex of first transect

    // Reconstruct return path;
    i = route.size()-1;
    for (; route[i] >= n_t; --i)
        _returnPathIdx.push_back(i);
    _returnPathIdx.push_back(i); // vertex of last transect

    return true;
}

bool FlightPlan::_generateTransects(double lineDistance, double minTransectLength)
{
    _transects.clear();
    if (_scenario.getTilesENU().size() != _progress.size()){
        errorString = "Number of tiles is not equal to progress array length";
        return false;
    }

    // Calculate processed tiles (_progress[i] == 100) and subtract them from measurement area.
    size_t num_tiles = _progress.size();
    vector<BoostPolygon> processedTiles;
    {
        const auto &tiles = _scenario.getTilesENU();
        for (size_t i=0; i<num_tiles; ++i) {
            if (_progress[i] == 100){
                processedTiles.push_back(tiles[i]);
            }
        }

        if (processedTiles.size() == num_tiles)
            return true;
    }

    // Convert measurement area and tiles to clipper path.
    ClipperLib::Path mAreaClipper;
    for ( auto vertex : _scenario.getMeasurementAreaENU().outer() ){
        mAreaClipper.push_back(ClipperLib::IntPoint{static_cast<ClipperLib::cInt>(vertex.get<0>()*CLIPPER_SCALE),
                                                    static_cast<ClipperLib::cInt>(vertex.get<1>()*CLIPPER_SCALE)});
    }
    vector<ClipperLib::Path> processedTilesClipper;
    for (auto t : processedTiles){
        ClipperLib::Path path;
        for (auto vertex : t.outer()){
            path.push_back(ClipperLib::IntPoint{static_cast<ClipperLib::cInt>(vertex.get<0>()*CLIPPER_SCALE),
                                                static_cast<ClipperLib::cInt>(vertex.get<1>()*CLIPPER_SCALE)});
        }
        processedTilesClipper.push_back(path);
    }

    const min_bbox_rt &bbox     = _scenario.getMeasurementAreaBBoxENU();
    double alpha                = bbox.angle;
    double x0                   = bbox.corners.outer()[0].get<0>();
    double y0                   = bbox.corners.outer()[0].get<1>();
    double bboxWidth            = bbox.width;
    double bboxHeight           = bbox.height;
    double delta                = detail::offsetConstant;

    size_t num_t = int(ceil((bboxHeight + 2*delta)/lineDistance)); // number of transects
    vector<double> yCoords;
    yCoords.reserve(num_t);
    double y = -delta;
    for (size_t i=0; i < num_t; ++i) {
        yCoords.push_back(y);
        y += lineDistance;
    }


    // Generate transects and convert them to clipper path.
    trans::rotate_transformer<boost::geometry::degree, double, 2, 2> rotate_back(-alpha*180/M_PI);
    trans::translate_transformer<double, 2, 2> translate_back(x0, y0);
    vector<ClipperLib::Path> transectsClipper;
    transectsClipper.reserve(num_t);
    for (size_t i=0; i < num_t; ++i) {
        // calculate transect
        BoostPoint v1{-delta, yCoords[i]};
        BoostPoint v2{bboxWidth+delta, yCoords[i]};
        BoostLineString transect;
        transect.push_back(v1);
        transect.push_back(v2);

        // transform back
        BoostLineString temp_transect;
        bg::transform(transect, temp_transect, rotate_back);
        transect.clear();
        bg::transform(temp_transect, transect, translate_back);


        ClipperLib::IntPoint c1{static_cast<ClipperLib::cInt>(transect[0].get<0>()*CLIPPER_SCALE),
                                static_cast<ClipperLib::cInt>(transect[0].get<1>()*CLIPPER_SCALE)};
        ClipperLib::IntPoint c2{static_cast<ClipperLib::cInt>(transect[1].get<0>()*CLIPPER_SCALE),
                                static_cast<ClipperLib::cInt>(transect[1].get<1>()*CLIPPER_SCALE)};
        ClipperLib::Path path{c1, c2};
        transectsClipper.push_back(path);
    }

    // Perform clipping.
    // Clip transects to measurement area.
    ClipperLib::Clipper clipper;
    clipper.AddPath(mAreaClipper, ClipperLib::ptClip, true);
    clipper.AddPaths(transectsClipper, ClipperLib::ptSubject, false);
    ClipperLib::PolyTree clippedTransecsPolyTree1;
    clipper.Execute(ClipperLib::ctIntersection, clippedTransecsPolyTree1, ClipperLib::pftNonZero, ClipperLib::pftNonZero);

    // Subtract holes (tiles with measurement_progress == 100) from transects.
    clipper.Clear();
    for (auto child : clippedTransecsPolyTree1.Childs)
        clipper.AddPath(child->Contour, ClipperLib::ptSubject, false);
    clipper.AddPaths(processedTilesClipper, ClipperLib::ptClip, true);
    ClipperLib::PolyTree clippedTransecsPolyTree2;
    clipper.Execute(ClipperLib::ctDifference, clippedTransecsPolyTree2, ClipperLib::pftNonZero, ClipperLib::pftNonZero);

    // Extract transects from  PolyTree and convert them to BoostLineString
    for (auto child : clippedTransecsPolyTree2.Childs){
        ClipperLib::Path clipperTransect = child->Contour;
        BoostPoint v1{static_cast<double>(clipperTransect[0].X)/CLIPPER_SCALE,
                      static_cast<double>(clipperTransect[0].Y)/CLIPPER_SCALE};
        BoostPoint v2{static_cast<double>(clipperTransect[1].X)/CLIPPER_SCALE,
                      static_cast<double>(clipperTransect[1].Y)/CLIPPER_SCALE};

        BoostLineString transect{v1, v2};
        if (bg::length(transect) >= minTransectLength)
            _transects.push_back(transect);

    }

    if (_transects.size() < 1)
        return false;

    return true;
}

void FlightPlan::_generateRoutingModel(const BoostLineString &vertices,
                                       const BoostPolygon &polygonOffset,
                                       size_t n0,
                                       FlightPlan::RoutingDataModel_t &dataModel,
                                       Matrix<double> &graph)
{
    auto start = std::chrono::high_resolution_clock::now();
    graphFromPolygon(polygonOffset, vertices, graph);
    auto delta = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now()-start);
    cout << "Execution time graphFromPolygon(): " << delta.count() << " ms" << endl;
//    cout << endl;
//    for (size_t i=0; i<graph.size(); ++i){
//        vector<double> &row = graph[i];
//        for (size_t j=0; j<row.size();++j){
//            cout << "(" << i << "," << j << "):" << row[j] << " ";
//        }
//        cout << endl;
//    }
//    cout << endl;
    Matrix<double> distanceMatrix(graph);
    start = std::chrono::high_resolution_clock::now();
    toDistanceMatrix(distanceMatrix);
    delta = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now()-start);
    cout << "Execution time toDistanceMatrix(): " << delta.count() << " ms" << endl;

    dataModel.distanceMatrix.setDimension(n0, n0);
    for (size_t i=0; i<n0; ++i){
        dataModel.distanceMatrix.set(i, i, 0);
        for (size_t j=i+1; j<n0; ++j){
            dataModel.distanceMatrix.set(i, j, int64_t(distanceMatrix.get(i, j)*CLIPPER_SCALE));
            dataModel.distanceMatrix.set(j, i, int64_t(distanceMatrix.get(i, j)*CLIPPER_SCALE));
        }
    }

    dataModel.numVehicles   = 1;
    dataModel.depot         = n0-1;
}

}