#include <maps/libs/cmdline/include/cmdline.h>
#include <maps/libs/common/include/exception.h>
#include <maps/libs/log8/include/log8.h>

#include <maps/libs/geolib/include/polygon.h>
#include <maps/libs/geolib/include/spatial_relation.h>
#include <maps/libs/geolib/include/intersection.h>
#include <maps/libs/geolib/include/vector.h>
#include <maps/libs/geolib/include/distance.h>
#include <maps/libs/geolib/include/segment.h>
#include <maps/libs/geolib/include/bounding_box.h>
#include <maps/libs/geolib/include/static_geometry_searcher.h>
#include <maps/libs/geolib/include/conversion_geos.h>

#include <algorithm>
#include <fstream>
#include <string>
#include <utility>
#include <list>
#include <sstream>
#include <vector>
#include <iterator>
#include <queue>

#include <opencv2/opencv.hpp>

#include <boost/optional.hpp>

namespace geolib3 = maps::geolib3;

namespace {

struct CompareItem {
    // путь к текстовому файлу с истинными полилиниями дорог
    std::string gt;
    // путь к текстовому файлу с результатами детектирования
    std::string test;
    // путь до изображения, на которым проводилась детекция
    std::string image;
};

/**
 * @brief Загружает список файлов для проверки качества
 * @param inpListPath путь к файлу со списком сравниваемых файлов.
 *      Каждая строчка должна содержать: путь до файла с истинным графом,
 *      путь до результатов детектировния, путь до изображения (опционально)
 * @return список файлов для сравнения
 */
std::list<CompareItem> loadCompareItems(const std::string& inpListPath)
{
    std::list<CompareItem> listFiles;

    std::ifstream ifs(inpListPath);
    if (!ifs.is_open())
        return {};

    while (!ifs.eof()) {
        std::string line; std::getline(ifs, line);
        if (line.empty())
            continue;
        std::stringstream ss(line);
        CompareItem item;
        ss >> item.gt >> item.test >> item.image;
        listFiles.push_back(item);
    }

    return listFiles;
}

/**
 * @brief Вычисляет суммарную длину всех отрезков в наборе
 * @param segments набор отрезков
 * @return суммарную длину отрезков
 */
double length(const std::vector<geolib3::Segment2>& segments) {
    double length = 0.;
    for (const auto& segment : segments) {
        length += geolib3::length(segment);
    }
    return length;
}

struct CompareResult {
    // найденные дороги в истинном графе
    std::vector<geolib3::Segment2> found;
    // пропущеные дороги в истинном графе
    std::vector<geolib3::Segment2> notfound;
    // верно задетектированные дороги в тестовом графе
    std::vector<geolib3::Segment2> correct;
    // ложно задатектированные дороги в тестовом графе
    std::vector<geolib3::Segment2> incorrect;
    // вычисляет отношение длины верно задетектированных дорог к общей длине
    // дорог в тестовом графе
    double getCorrectness() {
        double correctLength = length(correct);
        double incorrectLength = length(incorrect);
        return correctLength / (correctLength + incorrectLength);
    }
    // вычисляет отношение длины найденных дорог к общей длине
    // дорог в истинном графе
    double getCompleteness() {
        double foundLength = length(found);
        double notfoundLength = length(notfound);
        return foundLength / (foundLength + notfoundLength);
    }
};

/**
 * @brief The BufferedRoadGraph class
 */
class BufferedRoadGraph {
public:
    BufferedRoadGraph(const std::string& path, double bufferWidth)
        : bufferWidth_(bufferWidth) {
        readSegments(path);
        createBufferedSegments();
        buildSearcher();
    }

    size_t segmentsNumber() const {
        return segments_.size();
    }

    geolib3::Segment2 segmentAt(size_t index) const {
        REQUIRE(index < segments_.size(), "Index out of range");
        return segments_[index];
    }

    geolib3::Polygon2 segmentBufferAt(size_t index) const {
        REQUIRE(index < bufferedSegments_.size(), "Index out of range");
        return bufferedSegments_[index];
    }
    /**
     * @brief Находит набор окаймляющих прямоугольников, с которыми пересекается
     *      отрезок.
     * @param segment отрезок
     * @return набор прямоугольников
     */
    std::vector<geolib3::Polygon2>
    intersect(const geolib3::Segment2& segment) const {
        // Так как у горизонтальных и вертикальных отрезков нельзя найти
        // окаймляющий прямоугольник, отрезок преобразуется в полигон
        constexpr double PAD = 1.;
        boost::optional<geolib3::Polygon2>
                paddedSegment = getBufferedSegment(segment, PAD);
        if (!paddedSegment) {
            return {};
        }

        std::vector<geolib3::Polygon2> result;
        auto searchResult = searcher_.find(paddedSegment->boundingBox());
        for (auto it = searchResult.first; it != searchResult.second; ++it) {
            const geolib3::Polygon2 polygon = bufferedSegments_[it->value()];
            if (geolib3::spatialRelation(polygon, segment,
                                         geolib3::SpatialRelation::Intersects)) {
                result.push_back(polygon);
            }
        }

        return result;
    }

private:
    void readSegments(const std::string& path) {
        std::ifstream ifs(path);
        REQUIRE(ifs.is_open(), "Failed to open file: " << path);

        while(!ifs.eof()) {
            std::string line; std::getline(ifs, line);
            if (line.empty()) {
                continue;
            }
            std::stringstream ss(line);

            std::string className;
            ss >> className;
            if ("road" == className) {
                size_t ptsCnt;
                ss >> ptsCnt;
                std::vector<geolib3::Point2> pts;
                pts.reserve(ptsCnt);

                double x, y;
                ss >> x >> y;
                geolib3::Point2 startPt(x, y);
                for (size_t i = 0; i < ptsCnt - 1; ++i) {
                    ss >> x >> y;
                    geolib3::Point2 endPt(x, y);
                    if (startPt != endPt) {
                        segments_.emplace_back(startPt, endPt);
                        startPt = endPt;
                    }
                }
            }
        }
    }

    void createBufferedSegments() {
        for (const auto& segment : segments_) {
            auto bufferedSegment = getBufferedSegment(segment, bufferWidth_);
            if (bufferedSegment) {
                bufferedSegments_.push_back(*bufferedSegment);
            }
        }
    }

    void buildSearcher() {
        for (size_t i = 0; i < bufferedSegments_.size(); i++) {
            searcher_.insert(&bufferedSegments_[i], i);
        }
        searcher_.build();
    }
    /**
     * @brief Строит полосу ширины width вокруг отрезка.
     * @param segment отрезок
     * @param width   ширина полосы
     * @return полигоны описывающий полосу вокруг отрезка. Если отрезок
     *      выраждается в точку, то возвращается boost::none.
     */
    boost::optional<geolib3::Polygon2>
    getBufferedSegment(const geolib3::Segment2& segment, double width) const {
        REQUIRE(width > 0, "Buffer width should be positive");
        geolib3::Vector2 direct = segment.vector();
        if (0 == geolib3::length(direct)) {
            return boost::none;
        } else {
            direct.normalize();
        }
        geolib3::Vector2 normal = geolib3::rotateBy90(direct,
                                                      geolib3::Orientation::Clockwise);
        normal.normalize();

        std::vector<geolib3::Point2> bufferPts(4);
        bufferPts[0] = (segment.start() + width * (normal - direct) / 2.);
        bufferPts[1] = (segment.start() - width * (normal + direct) / 2.);
        bufferPts[2] = (segment.end() - width * (normal - direct) / 2.);
        bufferPts[3] = (segment.end() + width * (normal + direct) / 2.);

        return geolib3::Polygon2(bufferPts);
    }

    std::vector<geolib3::Segment2> segments_;
    std::vector<geolib3::Polygon2> bufferedSegments_;
    geolib3::StaticGeometrySearcher<geolib3::Polygon2, size_t> searcher_;
    double bufferWidth_;
};

/**
 * @brief Проверяет вложенность отрезка в полигон.
 *      Полигон содержит в себе отрезок, если оба конца
 *      отрезка лежат внутри полигона.
 * @param segment отрезок, для которого выполняется проверка
 * @param polygon полигон для проверки
 * @return истинность пренадлежности отрезка полигону.
 */
bool isSegmentInPolygon(const geolib3::Segment2& segment,
                        const geolib3::Polygon2& polygon) {
    if(!geolib3::spatialRelation(polygon, segment.start(),
                                 geolib3::SpatialRelation::Intersects)) {
        return false;
    }
    if(!geolib3::spatialRelation(polygon, segment.end(),
                                 geolib3::SpatialRelation::Intersects)) {
        return false;
    }

    return true;
}

/**
 * @brief Находит покрытые и непокрытые полигоном части отрезка.
 *      Определяются все точки пересечения отрезка и полигона. Из этих точек
 *      формируются части отрезка, для каждой из которых проверяется
 *      принадлежность полигону.
 * @param segment         отрезок, для которого выполняется проверка
 * @param polygon         полигон для проверки
 * @param covered    покрытые части отрезка
 * @param uncovered  непокрытые части отрезка
 */
void coverByPolygon(const geolib3::Segment2& segment,
                    const geolib3::Polygon2& polygon,
                    std::vector<geolib3::Segment2>* covered,
                    std::vector<geolib3::Segment2>* uncovered) {
    std::vector<geolib3::Point2> pts;
    for (size_t i = 0; i < polygon.segmentsNumber(); ++i) {
        std::vector<geolib3::Point2> tmpPts;
        tmpPts = geolib3::intersection(segment, polygon.segmentAt(i));
        pts.insert(pts.end(), tmpPts.begin(), tmpPts.end());
    }

    pts.push_back(segment.start());
    pts.push_back(segment.end());
    std::sort(pts.begin(), pts.end(),
              [&](const auto& lhs, const auto& rhs) {
        return geolib3::distance(lhs, segment.start()) < geolib3::distance(rhs, segment.start());
    });
    pts.erase(std::unique(pts.begin(), pts.end()), pts.end());

    std::vector<geolib3::Segment2> parts;
    for (size_t i = 0; i < pts.size() - 1; i++) {
        parts.emplace_back(pts[i], pts[i + 1]);
    }
    for (const auto& part : parts) {
        if(!isSegmentInPolygon(part, polygon)) {
            uncovered->push_back(part);
        } else {
            covered->push_back(part);
        }
    }
}

/**
 * @brief Находит покрытые и непокрытые набором полигонов части отрезка.
 *      Последовательно исключает части отрезка, покрываемые полигонами.
 *      Части отрезка, которые не покрываются ни одним из полигонов,
 *      считаются непокрытыми
 * @param segment         отрезок, для которого выполняется проверка
 * @param polygons        набор полигонов для проверки
 * @param covered    покрытые части отрезка
 * @param uncovered  непокрытые части отрезка
 */
void coverSegment(const geolib3::Segment2& segment,
                  const std::vector<geolib3::Polygon2>& polygons,
                  std::vector<geolib3::Segment2>* covered,
                  std::vector<geolib3::Segment2>* uncovered) {
    *uncovered = {segment};
    for (const auto& polygon : polygons) {
        std::vector<geolib3::Segment2> newUncovered;
        for (const auto& part : *uncovered) {
            std::vector<geolib3::Segment2> tmpCovered;
            std::vector<geolib3::Segment2> tmpUncovered;
            coverByPolygon(part, polygon, &tmpCovered, &tmpUncovered);
            covered->insert(covered->end(), tmpCovered.begin(), tmpCovered.end());
            newUncovered.insert(newUncovered.end(), tmpUncovered.begin(), tmpUncovered.end());
        }
        *uncovered = newUncovered;
    }
}

CompareResult compareGraph(const BufferedRoadGraph& gtGraph,
                           const BufferedRoadGraph& testGraph) {
    CompareResult result;
    for (size_t i = 0; i < testGraph.segmentsNumber(); ++i) {
        geolib3::Segment2 segment = testGraph.segmentAt(i);
        std::vector<geolib3::Polygon2> bufferedRoads = gtGraph.intersect(segment);
        std::vector<geolib3::Segment2> covered;
        std::vector<geolib3::Segment2> uncovered;
        coverSegment(segment, bufferedRoads, &covered, &uncovered);
        result.correct.insert(result.correct.end(),
                              covered.begin(), covered.end());
        result.incorrect.insert(result.incorrect.end(),
                                uncovered.begin(), uncovered.end());
    }

    for (size_t i = 0; i < gtGraph.segmentsNumber(); ++i) {
        geolib3::Segment2 segment = gtGraph.segmentAt(i);
        std::vector<geolib3::Polygon2> bufferedRoads = testGraph.intersect(segment);
        std::vector<geolib3::Segment2> covered;
        std::vector<geolib3::Segment2> uncovered;
        coverSegment(segment, bufferedRoads, &covered, &uncovered);
        result.found.insert(result.found.end(),
                            covered.begin(), covered.end());
        result.notfound.insert(result.notfound.end(),
                               uncovered.begin(), uncovered.end());
    }

    return result;
}

void drawSegment(cv::Mat& canvas,
                 const geolib3::Segment2& segment,
                 const cv::Scalar& color,
                 const size_t thickness = 7) {
    cv::Point2f start(segment.start().x(), segment.start().y());
    cv::Point2f end(segment.end().x(), segment.end().y());
    cv::line(canvas, start, end, color, thickness);
}

void drawPolygon(cv::Mat& canvas,
                 const geolib3::Polygon2& polygon,
                 const cv::Scalar& color,
                 const size_t thickness = 1) {
    for (size_t i = 0; i < polygon.segmentsNumber(); ++i) {
        drawSegment(canvas, polygon.segmentAt(i), color, thickness);
    }
}

class Color {
public:
    // Цвет верно задетектированных дорог в тестовом графе (зеленый)
    static cv::Scalar Correct() {
        return cv::Scalar(0, 255, 0);
    }
    // Цвет ложно задатектированных дорог в тестовом графе (красный)
    static cv::Scalar Incorrect() {
        return cv::Scalar(0, 0, 255);
    }
    // Цвет найденных дорог в истинном графе (желтый)
    static cv::Scalar Founded() {
        return cv::Scalar(0, 255, 255);
    }
    // Цвет пропущеных дорог в истинном графе (синий)
    static cv::Scalar NotFounded() {
        return cv::Scalar(255, 0, 0);
    }
    // Цвет окаймляющего прямоугольника (фиолетовый)
    static cv::Scalar Buffer() {
        return cv::Scalar(255, 0, 255);
    }
};

void dumpCompareTestImage(const std::string& name,
                          const cv::Mat& image,
                          const CompareResult& result,
                          const BufferedRoadGraph& gt) {
    cv::Mat canvas;
    image.copyTo(canvas);
    for (const auto& segment : result.correct) {
        drawSegment(canvas, segment, Color::Correct());
    }
    for (const auto& segment : result.incorrect) {
        drawSegment(canvas, segment, Color::Incorrect());
    }
    for (size_t i = 0; i < gt.segmentsNumber(); ++i) {
        drawPolygon(canvas, gt.segmentBufferAt(i), Color::Buffer());
    }

    cv::imwrite(name, canvas);
}


void dumpCompareGTImage(const std::string& name,
                        const cv::Mat& image,
                        const CompareResult& result,
                        const BufferedRoadGraph& testGraph) {
    cv::Mat canvas;
    image.copyTo(canvas);
    for (const auto& segment : result.found) {
        drawSegment(canvas, segment, Color::Founded());
    }
    for (const auto& segment : result.notfound) {
        drawSegment(canvas, segment, Color::NotFounded());
    }
    for (size_t i = 0; i < testGraph.segmentsNumber(); ++i) {
        drawPolygon(canvas, testGraph.segmentBufferAt(i), Color::Buffer());
    }

    cv::imwrite(name, canvas);
}

} //namespace


int main(int argc, char** argv)
try {
    maps::cmdline::Parser parser;
    auto inpListParam = parser.string("input_list")\
            .required()\
            .help("path to the text file, every line consists of:\n"
                  "\t<path to ground truth file> <path to file for compare> <path to image> (optional)");

    auto bufferWidthParam = parser.num("buffer_width")\
            .defaultValue(3)\
            .help("width of lane around road");

    auto compareImgPrefixParam = parser.string("compare_img")\
            .defaultValue("compare")
            .help("prefix for result compare images");

    parser.parse(argc, argv);

    REQUIRE(bufferWidthParam > 0, "Buffer width must be great than 0");

    std::list<CompareItem> compareItems = loadCompareItems(inpListParam);
    double foundedLength = 0.;
    double notfoundedLength = 0.;
    double correctLength = 0.;
    double incorrectLength = 0.;

    for (const auto& item : compareItems) {
        BufferedRoadGraph gtGraph(item.gt, bufferWidthParam);
        BufferedRoadGraph testGraph(item.test, bufferWidthParam);

        CompareResult result = compareGraph(gtGraph, testGraph);

        INFO() << "Quality: " << item.gt;
        INFO() << "  Correctness: " << result.getCorrectness();
        INFO() << "  Completeness: " << result.getCompleteness();

        foundedLength += length(result.found);
        notfoundedLength += length(result.notfound);
        correctLength += length(result.correct);
        incorrectLength += length(result.incorrect);

        if (!item.image.empty()) {
            cv::Mat img = cv::imread(item.image);
            if (!img.empty()) {
                dumpCompareTestImage(compareImgPrefixParam + "_test_" +item.image,
                                     img, result, gtGraph);
                dumpCompareGTImage(compareImgPrefixParam + "_gt_" +item.image,
                                   img, result, testGraph);
            }
        }
    }

    double totalCorrectness = correctLength / (correctLength + incorrectLength);
    double totalCompleteness = foundedLength / (foundedLength + notfoundedLength);
    INFO() << "Quality: total";
    INFO() << "  Correctness: " << totalCorrectness;
    INFO() << "  Completeness: " << totalCompleteness;

    return EXIT_SUCCESS;
}
catch (const maps::Exception& e) {
    INFO() << e;
    return EXIT_FAILURE;
}
catch (const std::exception& e) {
    INFO() << e.what();
    return EXIT_FAILURE;
}
catch (...) {
INFO() << "Caught unknown exception";
return EXIT_FAILURE;
}
