#include "extractor/guidance/toolkit.hpp"
#include "extractor/guidance/turn_lane_matcher.hpp"
#include "util/guidance/toolkit.hpp"

#include <boost/assert.hpp>
#include <boost/numeric/conversion/cast.hpp>

#include <functional>

namespace osrm
{
namespace extractor
{
namespace guidance
{
namespace lanes
{

// Translate Turn Tags into a Matching Direction Modifier
DirectionModifier::Enum getMatchingModifier(const TurnLaneType::Mask &tag)
{
    const constexpr TurnLaneType::Mask tag_by_modifier[] = {TurnLaneType::uturn,
                                                            TurnLaneType::sharp_right,
                                                            TurnLaneType::right,
                                                            TurnLaneType::slight_right,
                                                            TurnLaneType::straight,
                                                            TurnLaneType::slight_left,
                                                            TurnLaneType::left,
                                                            TurnLaneType::sharp_left,
                                                            TurnLaneType::merge_to_left,
                                                            TurnLaneType::merge_to_right};
    const auto index =
        std::distance(tag_by_modifier, std::find(tag_by_modifier, tag_by_modifier + 10, tag));

    BOOST_ASSERT(index <= 10);

    const constexpr DirectionModifier::Enum modifiers[11] = {
        DirectionModifier::UTurn,
        DirectionModifier::SharpRight,
        DirectionModifier::Right,
        DirectionModifier::SlightRight,
        DirectionModifier::Straight,
        DirectionModifier::SlightLeft,
        DirectionModifier::Left,
        DirectionModifier::SharpLeft,
        DirectionModifier::Straight,
        DirectionModifier::Straight,
        DirectionModifier::UTurn}; // fallback for invalid tags

    return modifiers[index];
}

// check whether a match of a given tag and a turn instruction can be seen as valid
bool isValidMatch(const TurnLaneType::Mask &tag, const TurnInstruction instruction)
{
    using util::guidance::hasLeftModifier;
    using util::guidance::hasRightModifier;
    const auto isMirroredModifier = [](const TurnInstruction instruction) {
        return instruction.type == TurnType::Merge;
    };

    if (tag == TurnLaneType::uturn)
    {
        return hasLeftModifier(instruction) ||
               instruction.direction_modifier == DirectionModifier::UTurn;
    }
    else if (tag == TurnLaneType::sharp_right || tag == TurnLaneType::right ||
             tag == TurnLaneType::slight_right)
    {
        if (isMirroredModifier(instruction))
            return hasLeftModifier(instruction);
        else
            // needs to be adjusted for left side driving
            return leavesRoundabout(instruction) || hasRightModifier(instruction);
    }
    else if (tag == TurnLaneType::straight)
    {
        return instruction.direction_modifier == DirectionModifier::Straight ||
               instruction.type == TurnType::Suppressed || instruction.type == TurnType::NewName ||
               instruction.type == TurnType::StayOnRoundabout || entersRoundabout(instruction) ||
               (instruction.type ==
                    TurnType::Fork && // Forks can be experienced, even for straight segments
                (instruction.direction_modifier == DirectionModifier::SlightLeft ||
                 instruction.direction_modifier == DirectionModifier::SlightRight)) ||
               (instruction.type ==
                    TurnType::Continue && // Forks can be experienced, even for straight segments
                (instruction.direction_modifier == DirectionModifier::SlightLeft ||
                 instruction.direction_modifier == DirectionModifier::SlightRight)) ||
               instruction.type == TurnType::UseLane;
    }
    else if (tag == TurnLaneType::slight_left || tag == TurnLaneType::left ||
             tag == TurnLaneType::sharp_left)
    {
        if (isMirroredModifier(instruction))
            return hasRightModifier(instruction);
        else
        {
            // Needs to be fixed for left side driving
            return (instruction.type == TurnType::StayOnRoundabout) || hasLeftModifier(instruction);
        }
    }
    return false;
}

// Every tag is somewhat idealized in form of the expected angle. A through lane should go straight
// (or follow a 180 degree turn angle between in/out segments.) The following function tries to find
// the best possible match for every tag in a given intersection, considering a few corner cases
// introduced to OSRM handling u-turns
typename Intersection::const_iterator findBestMatch(const TurnLaneType::Mask &tag,
                                                    const Intersection &intersection)
{
    const constexpr double idealized_turn_angles[] = {0, 35, 90, 135, 180, 225, 270, 315};
    const auto idealized_angle = idealized_turn_angles[getMatchingModifier(tag)];
    return std::min_element(
        intersection.begin(),
        intersection.end(),
        [idealized_angle, &tag](const ConnectedRoad &lhs, const ConnectedRoad &rhs) {
            // prefer valid matches
            if (isValidMatch(tag, lhs.turn.instruction) != isValidMatch(tag, rhs.turn.instruction))
                return isValidMatch(tag, lhs.turn.instruction);
            // if the entry allowed flags don't match, we select the one with
            // entry allowed set to true
            if (lhs.entry_allowed != rhs.entry_allowed)
                return lhs.entry_allowed;

            return angularDeviation(idealized_angle, lhs.turn.angle) <
                   angularDeviation(idealized_angle, rhs.turn.angle);
        });
}

// Reverse is a special case, because it requires access to the leftmost tag. It has its own
// matching function as a result of that. The leftmost tag is required, since u-turns are disabled
// by default in OSRM. Therefor we cannot check whether a turn is allowed, since it could be
// possible that it is forbidden. In addition, the best u-turn angle does not necessarily represent
// the u-turn, since it could be a sharp-left turn instead on a road with a middle island.
typename Intersection::const_iterator
findBestMatchForReverse(const TurnLaneType::Mask &neighbor_tag, const Intersection &intersection)
{
    const auto neighbor_itr = findBestMatch(neighbor_tag, intersection);
    if (neighbor_itr + 1 == intersection.cend())
        return intersection.begin();

    const constexpr double idealized_turn_angles[] = {0, 35, 90, 135, 180, 225, 270, 315};
    const TurnLaneType::Mask tag = TurnLaneType::uturn;
    const auto idealized_angle = idealized_turn_angles[getMatchingModifier(tag)];
    return std::min_element(
        intersection.begin() + std::distance(intersection.begin(), neighbor_itr),
        intersection.end(),
        [idealized_angle, &tag](const ConnectedRoad &lhs, const ConnectedRoad &rhs) {
            // prefer valid matches
            if (isValidMatch(tag, lhs.turn.instruction) != isValidMatch(tag, rhs.turn.instruction))
                return isValidMatch(tag, lhs.turn.instruction);
            // if the entry allowed flags don't match, we select the one with
            // entry allowed set to true
            if (lhs.entry_allowed != rhs.entry_allowed)
                return lhs.entry_allowed;

            return angularDeviation(idealized_angle, lhs.turn.angle) <
                   angularDeviation(idealized_angle, rhs.turn.angle);
        });
}

// a match is trivial if all turns can be associated with their best match in a valid way and the
// matches occur in order
bool canMatchTrivially(const Intersection &intersection, const LaneDataVector &lane_data)
{
    std::size_t road_index = 1, lane = 0;
    if (!lane_data.empty() && lane_data.front().tag == TurnLaneType::uturn)
    {
        // the very first is a u-turn to the right
        if (intersection[0].entry_allowed)
            lane = 1;
    }
    for (; road_index < intersection.size() && lane < lane_data.size(); ++road_index)
    {
        if (intersection[road_index].entry_allowed)
        {
            BOOST_ASSERT(lane_data[lane].from != INVALID_LANEID);
            if (!isValidMatch(lane_data[lane].tag, intersection[road_index].turn.instruction))
                return false;

            if (findBestMatch(lane_data[lane].tag, intersection) !=
                intersection.begin() + road_index)
                return false;

            ++lane;
        }
    }
    return lane == lane_data.size() ||
           (lane + 1 == lane_data.size() && lane_data.back().tag == TurnLaneType::uturn);
}

Intersection triviallyMatchLanesToTurns(Intersection intersection,
                                        const LaneDataVector &lane_data,
                                        const util::NodeBasedDynamicGraph &node_based_graph,
                                        const LaneDescriptionID lane_string_id,
                                        LaneDataIdMap &lane_data_to_id)
{
    std::size_t road_index = 1, lane = 0;

    const auto matchRoad = [&](ConnectedRoad &road, const TurnLaneData &data) {
        LaneTupelIdPair key{{LaneID(data.to - data.from + 1), data.from}, lane_string_id};

        auto lane_data_id = boost::numeric_cast<LaneDataID>(lane_data_to_id.size());
        const auto it = lane_data_to_id.find(key);

        if (it == lane_data_to_id.end())
            lane_data_to_id.insert({key, lane_data_id});
        else
            lane_data_id = it->second;

        // set lane id instead after the switch:
        road.turn.lane_data_id = lane_data_id;
    };

    if (!lane_data.empty() && lane_data.front().tag == TurnLaneType::uturn)
    {
        // the very first is a u-turn to the right
        if (intersection[0].entry_allowed)
        {
            std::size_t u_turn = 0;
            if (node_based_graph.GetEdgeData(intersection[0].turn.eid).reversed)
            {
                if (intersection.size() <= 1 || !intersection[1].entry_allowed ||
                    intersection[1].turn.instruction.direction_modifier !=
                        DirectionModifier::SharpRight)
                {
                    // cannot match u-turn in a valid way
                    return intersection;
                }
                u_turn = 1;
                road_index = 2;
            }
            intersection[u_turn].entry_allowed = true;
            intersection[u_turn].turn.instruction.type = TurnType::Turn;
            intersection[u_turn].turn.instruction.direction_modifier = DirectionModifier::UTurn;

            matchRoad(intersection[u_turn], lane_data.back());
            // continue with the first lane
            lane = 1;
        }
        else
            return intersection;
    }

    for (; road_index < intersection.size() && lane < lane_data.size(); ++road_index)
    {
        if (intersection[road_index].entry_allowed)
        {
            BOOST_ASSERT(lane_data[lane].from != INVALID_LANEID);
            BOOST_ASSERT(
                isValidMatch(lane_data[lane].tag, intersection[road_index].turn.instruction));
            BOOST_ASSERT(findBestMatch(lane_data[lane].tag, intersection) ==
                         intersection.begin() + road_index);

            if (TurnType::Suppressed == intersection[road_index].turn.instruction.type)
                intersection[road_index].turn.instruction.type = TurnType::UseLane;

            matchRoad(intersection[road_index], lane_data[lane]);
            ++lane;
        }
    }

    // handle reverse tag, if present
    if (lane + 1 == lane_data.size() && lane_data.back().tag == TurnLaneType::uturn)
    {
        std::size_t u_turn = 0;
        if (node_based_graph.GetEdgeData(intersection[0].turn.eid).reversed)
        {
            if (!intersection.back().entry_allowed ||
                intersection.back().turn.instruction.direction_modifier !=
                    DirectionModifier::SharpLeft)
            {
                // cannot match u-turn in a valid way
                return intersection;
            }
            u_turn = intersection.size() - 1;
        }
        intersection[u_turn].entry_allowed = true;
        intersection[u_turn].turn.instruction.type = TurnType::Turn;
        intersection[u_turn].turn.instruction.direction_modifier = DirectionModifier::UTurn;

        matchRoad(intersection[u_turn], lane_data.back());
    }
    return intersection;
}

} // namespace lane_matching
} // namespace guidance
} // namespace extractor
} // namespace osrm