#include "deadends.h"

#include <maps/libs/geolib/include/units_literals.h>
#include <maps/libs/log8/include/log8.h>

using namespace maps::geolib3::literals;

namespace maps::mrc::gen_targets {

namespace {

bool turnIs180UTurn(const geolib3::Direction2 direction1, const geolib3::Direction2 direction2)
{
    auto angle = geolib3::angleBetween(direction1, direction2);
    if (angle > geolib3::PI - 0.2_rad) {
        return true;
    } else {
        return false;
    }
}

// Removes maneuvers between edges if at least one of two edges has
// fc > maxAllowedFc
Edges removeManeuversWithProhibitedFc(Edges edges,
                                      const std::vector<int>& allowedFc)
{
    int maxAllowedFc = *std::max_element(allowedFc.begin(), allowedFc.end());

    for (auto& it : edges) {
        Edge& edge = it.second;

        for (int i = edge.outEdges.size() - 1; i >= 0; i--) {
            if (edge.fc > maxAllowedFc
                || edges.at(edge.outEdges[i]).fc > maxAllowedFc)
            {
                edge.outEdges.erase(edge.outEdges.begin() + i);
            }
        }

        for (int i = edge.inEdges.size() - 1; i >= 0; i--) {
            if (edge.fc > maxAllowedFc
                || edges.at(edge.inEdges[i]).fc > maxAllowedFc)
            {
                edge.inEdges.erase(edge.inEdges.begin() + i);
            }
        }
    }
    return edges;
}

Edges removeUturnsOutsideCrossroads(Edges edges)
{
    for (auto& it : edges) {
        Edge& edge = it.second;
        if (edge.outEdges.size() != 2) {
            continue;
        }

        size_t reverseEdgeIndex;
        if (turnIs180UTurn(edge.outgoingDirection(),
                           edges.at(edge.outEdges[0]).incomingDirection())){
            reverseEdgeIndex = 0;
        } else if (turnIs180UTurn(edge.outgoingDirection(),
                                  edges.at(edge.outEdges[1]).incomingDirection())) {
            reverseEdgeIndex = 1;
        } else {
            continue;
        }

        Edge& reverseEdge = edges.at(edge.outEdges[reverseEdgeIndex]);
        if (reverseEdge.inEdges.size() != 2) {
            continue;
        }

        edge.outEdges.erase(edge.outEdges.begin() + reverseEdgeIndex);
        reverseEdge.inEdges.erase(std::find(reverseEdge.inEdges.begin(),
                                            reverseEdge.inEdges.end(),
                                            edge.id));
    }
    return edges;
}

void addDeadEnd(const Edges& edges,
                const Edge& edgeBeforeDeadEnd,
                const Edge& edgeAfterDeadEnd,
                std::unordered_set<EdgeId>& deadEndEdges)
{
    Edge curEdge = edgeBeforeDeadEnd;
    deadEndEdges.insert(curEdge.id);
    while (curEdge.inEdges.size() == 1
           && edges.at(curEdge.inEdges[0]).outEdges.size() == 1
           && !deadEndEdges.count(curEdge.inEdges[0]))
    {
        curEdge = edges.at(curEdge.inEdges[0]);
        deadEndEdges.insert(curEdge.id);
    }

    curEdge = edgeAfterDeadEnd;
    deadEndEdges.insert(curEdge.id);
    while (curEdge.outEdges.size() == 1
           && edges.at(curEdge.outEdges[0]).inEdges.size() == 1
           && !deadEndEdges.count(curEdge.outEdges[0]))
    {
        curEdge = edges.at(curEdge.outEdges[0]);
        deadEndEdges.insert(curEdge.id);
    }
}

} // anonymous namespace

std::unordered_set<EdgeId> getDeadEnds(const RoadNetworkData& roadNetwork,
                                       const std::vector<int>& allowedFc)
{
    Edges edges = removeManeuversWithProhibitedFc(roadNetwork.getEdges(),
                                                  allowedFc);
    edges = removeUturnsOutsideCrossroads(edges);
    std::unordered_set<EdgeId> deadEndEdges;

    for (const auto& it : edges) {
        const Edge& edge = it.second;
        if (edge.outEdges.size() != 1) {
            continue;
        }
        const Edge& outEdge = edges.at(edge.outEdges[0]);
        if (outEdge.inEdges.size() == 1
            && turnIs180UTurn(edge.outgoingDirection(),
                              outEdge.incomingDirection())) {
            addDeadEnd(edges, edge, outEdge, deadEndEdges);
        }
    }

    return deadEndEdges;
}

} // namespace maps::mrc::gen_targets
