#include "generate_edges.h"

#include "track.h"
#include "turn.h"
#include "link.h"

#include <maps/libs/geolib/include/vector.h>
#include <maps/libs/geolib/include/point.h>
#include <maps/libs/geolib/include/distance.h>

#include <vector>

namespace maps {
namespace wiki {
namespace signals_graph {
namespace {

const double PROJECTION_EPSILON = 0.001;

const double MINIMAL_LINK_LENGTH_RATIO = 0.1;

struct Connection {
    int crossFrom;
    int crossTo;
};

bool operator<(const Connection& lhs, const Connection& rhs) {
    return lhs.crossFrom < rhs.crossFrom ||
        (lhs.crossFrom == rhs.crossFrom && lhs.crossTo < rhs.crossTo);
}

using LinkMap = std::map<Connection, std::vector<Link>>;

LinkMap createLinkMap(const std::vector<Link>& links) {
    LinkMap result;

    for (auto link : links) {
        result[{link.crossFrom, link.crossTo}].push_back(link);
    }

    return result;
}

struct PointPos {
    geolib3::Vector2 point;
    double pos;
    size_t linkIndex;
};

bool operator<(const PointPos& lhs, const PointPos& rhs) {
    return lhs.pos < rhs.pos;
}

geolib3::Vector2 weightedPoint(PointPos a, PointPos b, double pc) {
    if (b.pos - a.pos < PROJECTION_EPSILON) {
        return (a.point + b.point) * 0.5;
    }

    return a.point + (b.point - a.point) / (b.pos - a.pos) * (pc - a.pos);
}

std::vector<geolib3::Vector2> weightedCurrent(
    const std::vector<size_t>& state,
    const std::vector<std::vector<PointPos>>& stream,
    const PointPos& pointPos
) {
    std::vector<geolib3::Vector2> result;
    result.reserve(state.size());

    for (size_t index = 0; index < pointPos.linkIndex; ++index) {
        result.push_back(weightedPoint(
            stream[index][state[index]],
            stream[index][state[index]+1],
            pointPos.pos
        ));
    }

    result.push_back(pointPos.point);

    for (size_t index = pointPos.linkIndex + 1; index < state.size(); ++index) {
        result.push_back(weightedPoint(
            stream[index][state[index]],
            stream[index][state[index]+1],
            pointPos.pos
        ));
    }

    return result;
}

geolib3::Point2 meanPos(const std::vector<geolib3::Vector2>& weightedCurrent) {
    geolib3::Vector2 pointSum(0.0, 0.0);

    for (const geolib3::Vector2 vec : weightedCurrent) {
        pointSum += vec;
    }

    pointSum /= weightedCurrent.size();

    return { pointSum.x(), pointSum.y() };
}

Edge createEdge(
    const std::vector<Track>& tracks,
    const std::vector<Link>& links,
    const std::vector<Turn>& crosses,
    Connection connection
) {
    auto fromPoint = crosses[connection.crossFrom].mercPoint;
    auto toPoint = crosses[connection.crossTo].mercPoint;

    maps::geolib3::Vector2 vec = toPoint - fromPoint;
    double vecLength = geolib3::length(vec);
    vec /= vecLength;

    std::vector<Link> usedLinks;

    for (auto link : links) {
        const std::vector<Signal>& trackPoints = tracks[link.trackId].points();

        double linkLength = geolib3::distance(
            trackPoints[link.posFrom].mercPoint(),
            trackPoints[link.posTo].mercPoint()
        );

        if (linkLength >= MINIMAL_LINK_LENGTH_RATIO * vecLength) {
            usedLinks.push_back(link);
        }
    }

    if (usedLinks.empty()) {
        return Edge({}, 0, connection.crossFrom, connection.crossTo);
    }

    std::vector<geolib3::Vector2> fromBatch;
    std::vector<geolib3::Vector2> toBatch;
    for (auto link : usedLinks) {
        const std::vector<Signal>& trackPoints = tracks[link.trackId].points();

        fromBatch.push_back(trackPoints[link.posFrom].mercPoint() - geolib3::Point2{0, 0});
        toBatch.push_back(trackPoints[link.posTo].mercPoint() - geolib3::Point2{0, 0});
    }

    fromPoint = meanPos(fromBatch);
    toPoint = meanPos(toBatch);
    vec = toPoint - fromPoint;
    vecLength = geolib3::length(vec);
    vec /= vecLength;

    geolib3::Vector2 init{fromPoint.x(), fromPoint.y()};

    std::vector<PointPos> projection;
    std::vector<std::vector<PointPos>> stream(usedLinks.size());

    for (size_t linkIndex = 0; linkIndex < usedLinks.size(); ++linkIndex) {
        const Link& link = usedLinks[linkIndex];

        const std::vector<Signal>& trackPoints = tracks[link.trackId].points();

        stream[linkIndex].push_back({
            geolib3::Vector2{fromPoint.x(), fromPoint.y()},
            0.0,
            linkIndex
        });

        for (size_t pos = link.posFrom; pos <= link.posTo; ++pos) {
            Signal curSignal = trackPoints[pos];
            geolib3::Point2 cur = curSignal.mercPoint();
            geolib3::Vector2 curVec(cur.x(), cur.y());

            const maps::geolib3::Vector2& diff = curVec - init;
            double product = innerProduct(vec, diff);

            if (product < 0 || product > vecLength) {
                continue;
            }

            PointPos pointPos{curVec, product, linkIndex};

            projection.push_back(pointPos);
            stream[linkIndex].push_back(pointPos);
        }

        stream[linkIndex].push_back({
            geolib3::Vector2{toPoint.x(), toPoint.y()},
            vecLength,
            linkIndex
        });
    }

    std::sort(projection.begin(), projection.end());

    std::vector<size_t> state(usedLinks.size(), 0);
    std::vector<geolib3::Point2> edge;

    // Don't push_back fromPoint and toPoint
    for (const PointPos& pointPos : projection) {
        auto current = weightedCurrent(state, stream, pointPos);
        edge.push_back(meanPos(current));
        state[pointPos.linkIndex]++;
    }

    return Edge(edge, usedLinks.size(), connection.crossFrom, connection.crossTo);
}

} // namespace

std::vector<Edge> generateEdges(
    const std::vector<Track>& tracks,
    const std::vector<Link>& links,
    const std::vector<Turn>& crosses
) {
    std::vector<Edge> result;

    LinkMap linkMap = createLinkMap(links);
    for (const auto& connectionLinkMap : linkMap) {
        result.emplace_back(createEdge(
            tracks,
            connectionLinkMap.second,
            crosses,
            connectionLinkMap.first
        ));
    }

    return result;
}

} // namespace maps
} // namespace wiki
} // namespace signals_graph
