size_t in;

size_t out;

size_t bidi;

// Returns true if an edge pointing into here would decrease the

// number of eulerian paths needed.

bool can_end() const {

if (out > in + bidi) {

// There are more out paths than all the possible in paths so this

// must be a start of some path.

return true;

}

if (in > out + bidi) {

// We already have too many paths inward. More would be worse.

return false;

}

// By this point, out - in <= bidi and out - in >= -bidi so

// abs(out-in) <= bidi If number of unmatched bidi edges is odd then

// this can be an end. (bidi - abs(out - in)) % 2 works but we

// can avoid the abs by just addings.

return (bidi + out + in) % 2 == 1;

}

// Returns true if an edge pointing out of here would decrease the

// number of eulerian paths needed.

bool can_start() const {

if (out > in + bidi) {

// There are more out paths than all the possible in paths so this

// can't be the end of some path.

return false;

}

if (in > out + bidi) {

// Too many inward paths so we could use with another outward path.

return true;

}

// By this point, out - in <= bidi and out - in >= -bidi so

// abs(out-in) <= bidi If number of unmatched bidi edges is odd then

// this can be an end. (bidi - abs(out - in)) % 2 works but we

// can avoid the abs by just addings.

return (bidi + out + in) % 2 == 1;

}

const unordered_map<point_type_fp, vector<pair<linestring_type_fp, bool>>>& graph,

point_type_fp start,

const unordered_map<point_type_fp, VertexDegree>& vertex_degrees,

const double g1_speed,

const double up_time,

const double g0_speed,

const double down_time,

const double in_per_sec) {

if (!vertex_degrees.at(start).can_start()) {

// Starting from here isn't useful.

return {0, {}};

}

// unordered_map from vertex to the best-so-far distance to get to the

// vertex, along with the edge that gets you there. The edge

// might be reversed.

unordered_map<point_type_fp, pair<long double, pair<linestring_type_fp, bool>>> distances;

distances[start] = make_pair(0, make_pair(linestring_type_fp{}, true));

priority_queue<pair<long double, point_type_fp>,

vector<pair<long double, point_type_fp>>,

greater<pair<long double, point_type_fp>>> to_search;

set<point_type_fp> done;

to_search.push(make_pair(0, start));

while (to_search.size() > 0) {

auto current_vertex = to_search.top().second;

to_search.pop();

if (start != current_vertex &&

vertex_degrees.count(current_vertex) > 0 &&

vertex_degrees.at(current_vertex).can_end()) {

// Found the nearest solution. Return the edges in the right

// order with the right directionality.

vector<pair<linestring_type_fp, bool>> reverse_path;

for (auto v = current_vertex; v != start;) {

const auto& e = distances.at(v).second;

reverse_path.emplace_back(e);

if (e.second && v == e.first.front()) {

// Bidi edge and it was reversed.

std::reverse(reverse_path.back().first.begin(), reverse_path.back().first.end());

}

v = reverse_path.back().first.front();

}

std::reverse(reverse_path.begin(), reverse_path.end());

return {distances[current_vertex].first, reverse_path};

}

if (done.count(current_vertex)) {

continue; // We already completed this one.

}

for (const auto& edge : graph.at(current_vertex)) {

// Get end that isn't the current_vertex.

point_type_fp new_vertex = edge.first.back();

if (edge.second && current_vertex == new_vertex) {

// Reversible and this was the wrong end.

new_vertex = edge.first.front();

}

if (done.count(new_vertex)) {

continue;

}

long double new_distance = distances[current_vertex].first + bg::length(edge.first);

const auto max_manhattan = max(abs(new_vertex.x() - start.x()), abs(new_vertex.y() - start.y()));

double time_with_backtrack = new_distance / g1_speed;

double time_without_backtrack = up_time + max_manhattan / g0_speed + down_time;

double time_saved = time_without_backtrack - time_with_backtrack;

if (time_saved < 0 || new_distance / time_saved > in_per_sec) {

continue; // This is already too far away to be useful.

}

const auto old_distance_pair = distances.find(new_vertex);

if (old_distance_pair == distances.cend() || old_distance_pair->second.first > new_distance) {

distances[new_vertex] = make_pair(new_distance, edge);

}

to_search.push(make_pair(distances[new_vertex].first, new_vertex));

}

done.insert(current_vertex);

}

return {0, {}};

const vector<pair<linestring_type_fp, bool>>& paths,

const double g1_speed, const double up_time, const double g0_speed, const double down_time,

const double in_per_sec) {

if (in_per_sec == 0) {

return {};

}

// Convert the paths structure to a unordered_map from vector to edges that

// meet there.

unordered_map<point_type_fp, vector<pair<linestring_type_fp, bool>>> graph;

// Find the in, out, and bidi degree of all vertices. We can't just

// use the graph because we will modify this as we go.

unordered_map<point_type_fp, VertexDegree> vertex_degrees;

for (const auto& ls : paths) {

graph[ls.first.front()].push_back(ls);

graph[ls.first.back()]; // Create this element even if there are no edges from it.

if (ls.second) {

// bi-directional

graph[ls.first.back()].push_back(ls);

vertex_degrees[ls.first.front()].bidi++;

vertex_degrees[ls.first.back()].bidi++;

} else {

// directional

vertex_degrees[ls.first.front()].out++;

vertex_degrees[ls.first.back()].in++;

}

}

vector<pair<linestring_type_fp, bool>> backtracks;

// best_backtracks stores the total length, the start, the end,

// and a list of paths to take to get there, in the right order

// and with the right directionality.

vector<pair<long double,

vector<pair<linestring_type_fp, bool>>>> best_backtracks;

// For each odd-degree vertex, find the nearest odd-degree vertex

// using the distance function on the edge.

for (const auto& v : vertex_degrees) {

// Get the length and path to the nearest element.

// find_nearest_vertex returns 0 if there is none that is close

// enough or if the start vertex is not can_start(), that is, it

// has so many paths out already that it shouldn't get anymore.

auto length_and_path = find_nearest_vertex(graph, v.first, vertex_degrees, g1_speed, up_time, g0_speed, down_time, in_per_sec);

if (length_and_path.first > 0) {

best_backtracks.push_back(length_and_path);

}

}

// Now sort so that the shortest backtracks are first.

make_heap(best_backtracks.begin(), best_backtracks.end(), greater<>());

// Select backtracks one-at-a-time, best first. Every time that a

// backtrack is used, we may need to recalculate the nearest vertex.

// Because adding a backtrack can only possibly remove the can_start

// or can_end, there is no possibility that the new search will find

// an even shorter backtrack so we don't need to start from the

// beginning.

while (best_backtracks.size() > 0) {

const auto& i = best_backtracks.cbegin();

if (vertex_degrees[i->second.front().first.front()].can_start() &&

vertex_degrees[i->second.back().first.back()].can_end()) {

for (const auto& p : i->second) {

backtracks.emplace_back(p);

}

if (i->second.front().second) {

// Start is reversible.

vertex_degrees[i->second.front().first.front()].bidi++;

} else {

// Start is not reversible.

vertex_degrees[i->second.front().first.front()].out++;

}

if (i->second.back().second) {

// End is reversible.

vertex_degrees[i->second.back().first.back()].bidi++;

} else {

// End is not reversible.

vertex_degrees[i->second.back().first.back()].in++;

}

}

// Because this vertex used to have a backtrack, it might still

// have one so look for it.

auto length_and_path = find_nearest_vertex(

graph, i->second.front().first.front(), vertex_degrees, g1_speed,

up_time, g0_speed, down_time, in_per_sec);

// Now we can remove the used one and perhaps put a new one instead.

pop_heap(best_backtracks.begin(), best_backtracks.end(), greater<>());

best_backtracks.pop_back();

if (length_and_path.first > 0) {

best_backtracks.push_back(length_and_path);

push_heap(best_backtracks.begin(), best_backtracks.end(), greater<>());

}

}

return backtracks;