#include <graph_pgn_extract.hpp>

#include <graph.hpp>
#include "utils.hpp"

#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/highgui/highgui.hpp"

#include <iostream>

namespace maps {
namespace wiki {
namespace autocart {

//#define ENABLE_GRAPH_DRAW

namespace {

#ifdef ENABLE_GRAPH_DRAW
static cv::Scalar getColor(float dist)
{
    if (dist < 0.f)
        return cv::Scalar(128, 0, 0);
    else if (dist < 0.25f)
        return cv::Scalar(0, 255, 0) * dist / 0.25f + cv::Scalar(128, 0, 0) * (0.25f - dist) / 0.25f;
    else if (dist < 0.5f)
    {
        dist -= 0.25f;
        return cv::Scalar(0, 255, 255) * dist / 0.25f + cv::Scalar(0, 255, 0) * (0.25f - dist) / 0.25f;
    }
    else if (dist < 0.75f)
    {
        dist -= 0.5f;
        return cv::Scalar(0, 128, 255) * dist / 0.25f + cv::Scalar(0, 255, 255) * (0.25f - dist) / 0.25f;
    }
    else if (dist < 1.f)
    {
        dist -= 0.75f;
        return cv::Scalar(0, 0, 255) * dist / 0.25f + cv::Scalar(0, 128, 255) * (0.25f - dist) / 0.25f;
    }
    return cv::Scalar(0, 0, 255);
}
#endif


/**
    Extract local maximum from img.
*/
void extractLocalMaximum(const cv::Mat &img, cv::Mat &lm_img, double vertsMinThreshold)
{
    cv::Mat img_th;
    if (1. / 255. < vertsMinThreshold)
        cv::threshold(img, img_th, vertsMinThreshold * 255, 255, cv::THRESH_TOZERO);
    else
        img_th = img;

    cv::Mat mask = cv::Mat::ones(img_th.size(), CV_8UC1) * 255;
    lm_img = cv::Mat::zeros(img_th.size(), CV_8UC1);

    double maxVal; cv::Point maxLoc;
    for (;;)
    {
        cv::minMaxLoc(img_th, NULL, &maxVal, NULL, &maxLoc, mask);
        if (maxVal == 0)
            break;

        cv::Point2d pt_sum(0.0, 0.0);
        double val_sum = 0.;
        std::vector<cv::Point> pts; pts.emplace_back(maxLoc);
        mask.at<uchar>(maxLoc) = 0;
        pt_sum.x = (double)maxLoc.x * (double)maxVal / 255.;
        pt_sum.y = (double)maxLoc.y * (double)maxVal / 255.;
        val_sum += (double)maxVal / 255.;
        while (!pts.empty())
        {
            cv::Point pt = pts.back(); pts.pop_back();
            //left
            {
                uchar val = img_th.at<uchar>(pt);
                for (int i = pt.x - 1; i >= 0; i--)
                {
                    if (0 == mask.at<uchar>(pt.y, i))
                        break;
                    if (val < img_th.at<uchar>(pt.y, i))
                        break;
                    val = img_th.at<uchar>(pt.y, i);
                    mask.at<uchar>(pt.y, i) = 0;
                    pts.emplace_back(cv::Point(i, pt.y));

                    const double w = (double)val / 255.;
                    pt_sum.x += (double)   i * w;
                    pt_sum.y += (double)pt.y * w;
                    val_sum += w;
                }
            }
            //right
            {
                uchar val = img_th.at<uchar>(pt);
                for (int i = pt.x + 1; i < img_th.cols; i++)
                {
                    if (0 == mask.at<uchar>(pt.y, i))
                        break;
                    if (val < img_th.at<uchar>(pt.y, i))
                        break;
                    val = img_th.at<uchar>(pt.y, i);
                    mask.at<uchar>(pt.y, i) = 0;
                    pts.emplace_back(cv::Point(i, pt.y));

                    const double w = (double)val / 255.;
                    pt_sum.x += (double)   i * w;
                    pt_sum.y += (double)pt.y * w;
                    val_sum += w;
                }
            }
            //up
            {
                uchar val = img_th.at<uchar>(pt);
                for (int i = pt.y - 1; i > 0; i--)
                {
                    if (0 == mask.at<uchar>(i, pt.x))
                        break;
                    if (val < img_th.at<uchar>(i, pt.x))
                        break;
                    val = img_th.at<uchar>(i, pt.x);
                    mask.at<uchar>(i, pt.x) = 0;
                    pts.emplace_back(cv::Point(pt.x, i));

                    const double w = (double)val / 255.;
                    pt_sum.x += (double)pt.x * w;
                    pt_sum.y += (double)   i * w;
                    val_sum += w;
                }
            }
            //down
            {
                uchar val = img_th.at<uchar>(pt);
                for (int i = pt.y + 1; i < img_th.rows; i++)
                {
                    if (0 == mask.at<uchar>(i, pt.x))
                        break;
                    if (val < img_th.at<uchar>(i, pt.x))
                        break;
                    val = img_th.at<uchar>(i, pt.x);
                    mask.at<uchar>(i, pt.x) = 0;
                    pts.emplace_back(cv::Point(pt.x, i));

                    const double w = (double)val / 255.;
                    pt_sum.x += (double)pt.x * w;
                    pt_sum.y += (double)   i * w;
                    val_sum += w;
                }
            }
        }
        if (val_sum > DBL_EPSILON)
            lm_img.at<uchar>(pt_sum / val_sum) = 255;
    }
}
void extractCoords(const cv::Mat &lm_img, std::vector<cv::Point> &coords)
{
    const int rows = lm_img.rows;
    const int cols = lm_img.cols;
    for (int row = 0; row < rows; row++)
    {
        const uchar *ptr = lm_img.ptr<uchar>(row);
        for (int col = 0; col < cols; col++)
        {
            if (ptr[col])
                coords.emplace_back(cv::Point(col, row));
        }
    }
}
void extractVerticesCoords(const cv::Mat &vertData, std::vector<cv::Point> &vertices, double vertsMinThreshold)
{
    cv::Mat lm_img;
    extractLocalMaximum(vertData, lm_img, vertsMinThreshold);
    extractCoords(lm_img, vertices);
}
void nonMaximumSuppresion(const cv::Mat &vertData, std::vector<cv::Point> &vertices, int cellSz)
{
    const int cellSzSq = cellSz * cellSz;
    for (int i = 0; i < (int)vertices.size() - 1; i++)
    {
        const cv::Point &pt1 = vertices[i];
        const uchar val1 = vertData.at<uchar>(pt1);
        bool del = false;
        for (int j = i + 1; j < (int)vertices.size(); j++)
        {
            const cv::Point &pt2 = vertices[j];
            const uchar val2 = vertData.at<uchar>(pt2);
            const cv::Point diff = pt2 - pt1;
            if (cellSzSq < diff.dot(diff))
                continue;

            if (val1 < val2)
            {
                del = true;
                break;
            }
            vertices.erase(vertices.begin() + j);
            j--;
        }
        if (del)
        {
            vertices.erase(vertices.begin() + i);
            i--;
        }
    }
}

double lineWeightSimple(const cv::Mat &edgeData, const cv::Point &pt1, const cv::Point &pt2)
{
    cv::LineIterator it(edgeData, pt1, pt2, 8);
    double edgesWeight = 0.;
    for (int k = 0; k < it.count; k++, ++it)
    {
        edgesWeight += (double)(**it) / 255.;
    }
    return (edgesWeight / it.count);
}
double lineWeightComplex(const cv::Mat &vertData, const cv::Mat &edgeData, const cv::Point &pt1, const cv::Point &pt2, int vertRad)
{
    const int maxVal1 = vertData.at<uchar>(pt1);
    const int maxVal2 = vertData.at<uchar>(pt2);
    const int maxValMean = (maxVal1 + maxVal2) / 2;

    double edgesWeight = 0.;
    size_t pxCnt = 0;
    cv::Point pt1c, pt2c;
    for (int y = -vertRad; y <= vertRad; y++)
    {
        pt1c.y = pt1.y + y;
        pt2c.y = pt2.y + y;
        if (pt1c.y < 0 || pt2c.y < 0 || pt1c.y >= vertData.rows || pt2c.y >= vertData.rows)
            continue;
        for (int x = -vertRad; x <= vertRad; x++)
        {
            pt1c.x = pt1.x + x;
            pt2c.x = pt2.x + x;
            if (pt1c.x < 0 || pt2c.x < 0 || pt1c.x >= vertData.cols || pt2c.x >= vertData.cols)
                continue;
            const int vertVal = vertData.at<uchar>(pt1c) + vertData.at<uchar>(pt2c);
            if (vertVal * 10 < maxValMean * 7)
                continue;//TODO PARAM
            cv::LineIterator it(edgeData, pt1c, pt2c, 8);
            for (int k = 0; k < it.count; k++, ++it)
            {
                edgesWeight += (double)(**it) / 255.;
            }
            pxCnt += it.count;
        }
    }
    return (edgesWeight / pxCnt);
}
double lineWeight(const cv::Mat &vertData, const cv::Mat &edgeData, const cv::Point &pt1, const cv::Point &pt2, int vertRad)
{
    //double simple = lineWeightSimple(edgeData, pt1, pt2);
    //double complex = lineWeightComplex(vertData, edgeData, pt1, pt2, vertRad);
    if (0 == vertRad)
        return lineWeightSimple(edgeData, pt1, pt2);
    return lineWeightComplex(vertData, edgeData, pt1, pt2, vertRad);
}

class CoordGraph
    : public GraphImpl<cv::Point, double>
{
private:
    struct WeightedEdgesCycle
    {
        EdgesPath m_cycle;
        double m_weight;
    };
    struct WeightedEdgesCycleComparer {
        bool operator() (const WeightedEdgesCycle& lhs, const WeightedEdgesCycle& rhs) const
        {
            return lhs.m_weight > rhs.m_weight;
        }
    };
    using WeightedEdgesCyclesSet = std::set < WeightedEdgesCycle, WeightedEdgesCycleComparer >;
public:
    CoordGraph() {}
    ~CoordGraph() {}

    /*
        vertices - propability of the pixel into vertex of building
        edges    - propability of the pixel into edge of building
    */
    bool importData(const cv::Mat &vertData, const cv::Mat &edgeData, const ExtractPolygonsParams &params)
    {
        if (vertData.size() != edgeData.size())
            return false;
        clear();

        importVertices(vertData, params.m_vertsParams);
#ifdef ENABLE_GRAPH_DRAW
        if (false)
        {
            const double scale = 2.0;
            cv::Mat img_show; cv::cvtColor(vertData, img_show, cv::COLOR_GRAY2BGR);
            cv::resize(img_show, img_show, cv::Size(), scale, scale);
            for (auto itv1 = beginVertices(); itv1 != endVertices(); ++itv1)
            {
                const cv::Point &pt1 = (itv1->second.m_data * scale);
                img_show.at<cv::Vec3b>(pt1) = cv::Vec3b(0, 0, 255);
            }
            cv::imshow("vert", img_show);
            cv::waitKey();
        }
        if (false)
        {
            const double scale = 2.0;

            cv::Mat lm_img;
            extractLocalMaximum(vertData, lm_img, params.m_vertsParams.m_minThreshold);
            cv::resize(lm_img, lm_img, cv::Size(), scale, scale, cv::INTER_NEAREST);

            cv::Mat img_show; cv::cvtColor(vertData, img_show, cv::COLOR_GRAY2BGR);
            cv::resize(img_show, img_show, cv::Size(), scale, scale);
            img_show.setTo(cv::Scalar(0., 0., 255.), lm_img);

            cv::imshow("vert", img_show);
            cv::waitKey();
        }
#endif
        importEdges(vertData, edgeData, params.m_edgesParams);

        return true;
    }
    /**
        remove vertices
        maxAngle - in degree
    */
    void removeKeenVertices(double maxAngle)
    {
        bool doit = true;
        while (doit)
        {
            doit = false;
            for (auto it = beginVertices(); it != endVertices(); it++)
            {
                if (it->second.m_edges.size() <= 1)
                {
                    eraseVertex(it->first);
                    doit = true;
                    break;
                }
                if (it->second.m_edges.size() == 2)
                {
                    auto ite = it->second.m_edges.begin();
                    const int e1 = (*ite);
                    ite++;
                    const int e2 = (*ite);
                    auto ite1 = findEdge(e1)->second;
                    auto ite2 = findEdge(e2)->second;

                    const int v1 = ite1.getOverVertex(it->first);
                    const int v2 = ite2.getOverVertex(it->first);
                    const double angle = calcAngle(findVertex(v1)->second.m_data, findVertex(it->first)->second.m_data, findVertex(v2)->second.m_data);
                    if (angle < maxAngle / 180. * CV_PI)
                    {
                        eraseVertex(it->first);
                        doit = true;
                        break;
                    }
                }
            }
        }
    }
    void removeThinPolygon(const ThinPolygonsParams &params)
    {
        //remove thin and long triangles (one vertex very near edges) by erasing longest edge
        const double maxDistSq = params.m_maxVertEdgeDist * params.m_maxVertEdgeDist;
        for (auto ite = beginEdges(); ite != endEdges();)
        {
            const auto itv1 = findVertex(ite->second.m_vertex1);
            const auto itv2 = findVertex(ite->second.m_vertex2);

            const cv::Point &coordv1 = itv1->second.m_data;
            const cv::Point &coordv2 = itv2->second.m_data;

            bool del = false;
            for (auto itv = beginVertices(); itv != endVertices(); itv++)
            {
                if (itv->first == itv1->first || itv->first == itv2->first)
                    continue;
                const cv::Point &coordv = itv->second.m_data;

                const cv::Point vv1 = coordv - coordv1;
                const cv::Point v2v1 = coordv2 - coordv1;
                const double t = vv1.ddot(v2v1) / v2v1.ddot(v2v1);
                //if (t < 0. || t > 1.0)
                //    continue;
                if (t < params.m_leftRange || t > params.m_rightRange)
                    continue;

                const cv::Point2d diff = coordv1 + t * v2v1 - coordv;
                if (diff.ddot(diff) < maxDistSq)
                {
                    // пробовал при отсутствии общего ребра его добавлять получилось плохо
                    // потому что вариантов два
                    //   1. добавляем только если ребро имеет вес выше порогового,
                    //      но оно тогда и так бы уже было добавлено.
                    //   2. добавляем в любом случае, даже при недостаточном весе -
                    //      мы т.о. оказываем воздействие на взвешивание циклов,
                    //      потому что получаем ребра, которые слабые с т.з. веса
                    //      но могут образовать "хороший" угол и не понятно, почему
                    //      мы делаем это для ребер из тонких треугольников?
                    // поэтому пока решение удаляем ребро, только если не нарушается связность
                    //del = hasCommonEdge(itv1->second, itv->second) && hasCommonEdge(itv2->second, itv->second);
                    // может случиться, что itv1 и itv не связаны ребром, но связаны через промежуточную вершину
                    // поэтому вместо hasCommonEdge пользуем isVertsConnected
                    del = isVertsConnected(itv1->first, itv->first, ite->first) &&
                          isVertsConnected(itv2->first, itv->first, ite->first);
                    break;
                }
            }
            if (del)
                ite = eraseEdge(ite);
            else
                ite++;
        }
    }
    void removeIntersectedEdges()
    {// filter by intersection
        auto ite1 = beginEdges();
        while (ite1 != endEdges())
        {
            const cv::Point &e1v1 = findVertex(ite1->second.m_vertex1)->second.m_data;
            const cv::Point &e1v2 = findVertex(ite1->second.m_vertex2)->second.m_data;

            bool del1 = false;
            auto ite2 = ite1;
            ite2++;
            while (ite2 != endEdges())
            {
                bool del2 = false;
                if (ite1->second.m_vertex1 != ite2->second.m_vertex1 &&
                    ite1->second.m_vertex1 != ite2->second.m_vertex2 &&
                    ite1->second.m_vertex2 != ite2->second.m_vertex1 &&
                    ite1->second.m_vertex2 != ite2->second.m_vertex2)
                {
                    const cv::Point &e2v1 = findVertex(ite2->second.m_vertex1)->second.m_data;
                    const cv::Point &e2v2 = findVertex(ite2->second.m_vertex2)->second.m_data;
                    if (isSegmentIntersect(e1v1, e1v2, e2v1, e2v2))
                    {
                        if (ite1->second.m_data < ite2->second.m_data)
                        {
                            del1 = true;
                            break;
                        }
                        del2 = true;
                    }
                }
                if (del2)
                    ite2 = eraseEdge(ite2);
                else
                    ++ite2;
            }
            if (del1)
                ite1 = eraseEdge(ite1);
            else
                ++ite1;
        }
    }
    //
    void extractPolygons(const cv::Mat &segm_data, std::vector< std::vector<cv::Point> > &polygons, const ExtractPolygonsParams &params)
    {
        std::vector<WeightedEdgesCyclesSet> cyclesSet;
        extractCycles(segm_data, cyclesSet, params);

        std::vector<EdgesPath> best;
        for (const auto &cycle : cyclesSet)
        {
            getBestEdgesCycles(cycle, segm_data.size(), best, params.m_bestCycleParams);
        }

        polygons.resize(best.size());
        for (size_t i = 0; i < best.size(); i++)
        {
            convertEdgeCycleToCoords(best[i], polygons[i]);
        }
    }
#ifdef ENABLE_GRAPH_DRAW
public:
    void draw(cv::Mat &img_show, bool weighedEdgeColor, double scale = 1.0)
    {
        drawEdges(img_show, weighedEdgeColor, scale);
        drawVertices(img_show, scale);
    }
private:
    void drawVertices(cv::Mat &img_show, double scale)
    {
        for (auto it = beginVertices(); it != endVertices(); ++it)
        {
            cv::circle(img_show, it->second.m_data * scale, 1, cv::Scalar(0, 0, 255), 2);
        }
    }
    void drawEdges(cv::Mat &img_show, bool weighedColor, double scale)
    {
#if 0
        double minw = DBL_MAX;
        double maxw = -DBL_MAX;
        if (weighedColor)
        {
            for (auto it = beginEdges(); it != endEdges(); ++it)
            {
                if (it->second.m_data < minw)
                    minw = it->second.m_data;
                if (it->second.m_data > maxw)
                    maxw = it->second.m_data;
            }
        }
        const double rangew = maxw - minw;
        std::cout << "[" << minw << ", " << maxw << "] " << rangew << std::endl;
#else
        const double minw = 0.;
        const double maxw = 1.0;
        const double rangew = maxw - minw;
#endif
        for (auto it = beginEdges(); it != endEdges(); ++it)
        {
            auto v1 = findVertex(it->second.m_vertex1);
            if (v1 == endVertices())
                continue;
            auto v2 = findVertex(it->second.m_vertex2);
            if (v2 == endVertices())
                continue;

            cv::Scalar color(255., 0., 0.);
            if (weighedColor)
                color = getColor((float)((it->second.m_data - minw) / rangew));
            cv::line(img_show, v1->second.m_data * scale , v2->second.m_data * scale, color);
        }
    }
#endif
private:
    void importVertices(const cv::Mat &vertData, const ExtractVerticesParams &params)
    {
        std::vector<cv::Point> vertices;
        extractVerticesCoords(vertData, vertices, params.m_minThreshold);
        nonMaximumSuppresion(vertData, vertices, params.m_nmsCellSz);
        for (size_t i = 0; i < vertices.size(); i++)
        {
            addVertex(vertices[i]);
        }
    }
    void importEdges(const cv::Mat &vertData, const cv::Mat &edgeData, const ExtractEdgesParams &params)
    {
        for (auto itv1 = beginVertices(); itv1 != endVertices(); ++itv1)
        {
            const cv::Point &pt1 = itv1->second.m_data;
            auto itv2 = itv1; ++itv2;
            for (; itv2 != endVertices(); ++itv2)
            {
                const cv::Point &pt2 = itv2->second.m_data;
                const double length = cv::norm(pt2 - pt1);
                if (length > params.m_maxLength ||
                    length < params.m_minLength)
                    continue;

                const double weight = lineWeight(vertData, edgeData, pt1, pt2, 3);
                if (weight > params.m_minWeightThreshold)
                    addEdge(itv1->first, itv2->first, weight);
            }
        }
    }
private:
    double edgesAngle(const Edge &edge1, const Edge &edge2) const
    {
        int center_id = -1;
        int vertex_id1 = -1;
        int vertex_id2 = -1;
        if (edge1.m_vertex1 == edge2.m_vertex1)
        {
            center_id = edge1.m_vertex1;
            vertex_id1 = edge1.m_vertex2;
            vertex_id2 = edge2.m_vertex2;
        }
        else if (edge1.m_vertex1 == edge2.m_vertex2)
        {
            center_id = edge1.m_vertex1;
            vertex_id1 = edge1.m_vertex2;
            vertex_id2 = edge2.m_vertex1;
        }
        else if (edge1.m_vertex2 == edge2.m_vertex1)
        {
            center_id = edge1.m_vertex2;
            vertex_id1 = edge1.m_vertex1;
            vertex_id2 = edge2.m_vertex2;
        }
        else if (edge1.m_vertex2 == edge2.m_vertex2)
        {
            center_id = edge1.m_vertex2;
            vertex_id1 = edge1.m_vertex1;
            vertex_id2 = edge2.m_vertex1;
        }
        else
            throw std::runtime_error("edges don't connected");

        return calcAngle(findVertex(vertex_id1)->second.m_data, findVertex(center_id)->second.m_data, findVertex(vertex_id2)->second.m_data);
    }
    void convertEdgeCycleToCoords(const EdgesPath &cycle, std::vector<cv::Point> &coords) const
    {
        coords.resize(cycle.size() + 1);
        auto zero_edge = findEdge(cycle[0]);
        int last_vert = findEdge(cycle[1])->second.getOverVertex(zero_edge->second.m_vertex1);
        if (last_vert != -1)
        {
            coords[0] = findVertex(zero_edge->second.m_vertex2)->second.m_data;
            last_vert = zero_edge->second.m_vertex1;
        }
        else
        {
            coords[0] = findVertex(zero_edge->second.m_vertex1)->second.m_data;
            last_vert = zero_edge->second.m_vertex2;
        }
        for (size_t j = 1; j < cycle.size(); ++j)
        {
            coords[j] = findVertex(last_vert)->second.m_data;
            last_vert = findEdge(cycle[j])->second.getOverVertex(last_vert);
            CV_Assert(-1 != last_vert);
        }
        coords[cycle.size()] = coords[0];
    }
    void sumEdgesCycle(const EdgesPath &cycle1, const EdgesPath &cycle2, EdgesPath &sum_cycle) const
    {
        std::set<int> result;
        for (int i : cycle1)
            result.insert(i);
        for (int i : cycle2)
        {
            auto it = result.find(i);
            if (it == result.end())
                result.insert(i);
            else
                result.erase(it);
        }
        if (result.size() == cycle1.size() + cycle2.size())
            return; //no common elements

        auto it = result.begin();
        int last_edge_id = *it;
        int last_vertex_id = -1;
        {
            sum_cycle.push_back(last_edge_id);
            result.erase(it);
            const auto last_edge = findEdge(last_edge_id);
            last_vertex_id = last_edge->second.m_vertex1;
        }
        while (!result.empty())
        {
            const auto vert = findVertex(last_vertex_id);
            bool found = false;
            for (int edge : vert->second.m_edges)
            {
                it = result.find(edge);
                if (it != result.end())
                {
                    last_edge_id = *it;
                    sum_cycle.push_back(last_edge_id);
                    result.erase(it);
                    const auto last_edge = findEdge(last_edge_id);
                    last_vertex_id = last_edge->second.getOverVertex(last_vertex_id);
                    found = true;
                    break;
                }
            }
            if (!found)
            {
                sum_cycle.clear();
                return;
            }
        }
    }
    void rasterizeEdgeCycle(const EdgesPath &cycle, cv::Rect &roi, cv::Mat &cycle_mask) const
    {
        std::vector <std::vector<cv::Point>> coords(1);
        convertEdgeCycleToCoords(cycle, coords[0]);
        int minx = INT_MAX;
        int maxx = INT_MIN;
        int miny = INT_MAX;
        int maxy = INT_MIN;
        for (const auto &pt : coords[0])
        {
            minx = MIN(pt.x, minx);
            maxx = MAX(pt.x, maxx);
            miny = MIN(pt.y, miny);
            maxy = MAX(pt.y, maxy);
        }
        roi = cv::Rect(minx, miny, maxx - minx + 1, maxy - miny + 1);
        cycle_mask = cv::Mat::zeros(roi.size(), CV_8UC1);
        for (size_t i = 0; i < coords[0].size(); i++)
        {
            coords[0][i].x -= minx;
            coords[0][i].y -= miny;
        }
        cv::fillPoly(cycle_mask, coords, cv::Scalar(1., 1., 1.));
    }
    // есть ли вершины в которые заходит больше двух ребер
    bool hasVertDegreeGreatTwo(const EdgesPath &cycle) const
    {
        std::map<int, int> vertsDegree;
        for (int i : cycle)
        {
            const auto edge = findEdge(i);
            auto it1 = vertsDegree.find(edge->second.m_vertex1);
            if (it1 != vertsDegree.end())
            {
                it1->second++;
                if (it1->second > 2)
                    return true;
            }
            else
                vertsDegree[edge->second.m_vertex1] = 1;
            auto it2 = vertsDegree.find(edge->second.m_vertex2);
            if (it2 != vertsDegree.end())
            {
                it2->second++;
                if (it2->second > 2)
                    return true;
            }
            else
                vertsDegree[edge->second.m_vertex2] = 1;
        }
        return false;
    }

    double weightCycle(const EdgesPath &cycle, const WeightCycleParams &params) const
    {
        const double smallAngleMaxRad = params.m_smallAngleMax / 180. * CV_PI;

        if (cycle.size() < 4)
            return 0.;
        double sum_delta = 0.;
        auto zero_edge = findEdge(cycle[0]);
        auto last_edge = zero_edge;
        double weight = last_edge->second.m_data;
        for (size_t i = 1; i < cycle.size(); i++)
        {
            auto cur_edge = findEdge(cycle[i]);
            const double angle = edgesAngle(last_edge->second, cur_edge->second);
            if (angle < smallAngleMaxRad)
                sum_delta += 1.0 / (params.m_smallAnglePenaltyEps + angle);
            else
                sum_delta += deltaAngle(angle);
            weight += cur_edge->second.m_data;
            last_edge = cur_edge;
        }
        const double angle = edgesAngle(last_edge->second, zero_edge->second);
        if (angle < smallAngleMaxRad)
            sum_delta += 1.0 / (params.m_smallAnglePenaltyEps + angle);
        sum_delta += deltaAngle(angle);

        weight /= (cycle.size() + params.m_angleDeltaCoefPenalty * sum_delta);
        weight -= (cycle.size() > 4) ? params.m_nonQuadPenalty * (cycle.size() - 4) : 0.0;
        return weight;
    }
    double weightCycle(const EdgesPath &cycle, const cv::Mat &segm, const WeightCycleParams &params) const
    {
        if (cycle.size() < 4)
            return 0.;
        //TODO OPT
        const double weight = weightCycle(cycle, params);

        cv::Rect roi;
        cv::Mat cycle_mask;
        rasterizeEdgeCycle(cycle, roi, cycle_mask);
        const int cycle_area = cv::countNonZero(cycle_mask);
        if (cycle_area < params.m_minArea)
            return 0.0;

        if (segm.empty())
            return weight;
        cycle_mask = cycle_mask.mul(segm(roi));
        const int valid_area = cv::countNonZero(cycle_mask);
        return weight + params.m_segmAreaCoeff * ((double)valid_area / (double)cycle_area);
    }
    void extractCycles(const cv::Mat &segm, const EdgesPath &baseCycle, const std::set<size_t> &used, const std::vector<EdgesPath> &cycles, const WeightCycleParams &params, int restLvl, WeightedEdgesCyclesSet &cyclesSet) const
    {
        restLvl--;
        for (size_t i = 0; i < cycles.size(); i++)
        {
            if (used.find(i) != used.end())
                continue;
            const EdgesPath &cycle = cycles[i];
            WeightedEdgesCycle sum_cycle;
            sumEdgesCycle(baseCycle, cycle, sum_cycle.m_cycle);
            if (sum_cycle.m_cycle.empty())
                continue;
            if (!hasVertDegreeGreatTwo(sum_cycle.m_cycle))
            {
                sum_cycle.m_weight = weightCycle(sum_cycle.m_cycle, segm, params);
                cyclesSet.insert(sum_cycle);
            }
            if (0 < restLvl)
            {
                std::set<size_t> usedNew = used;
                usedNew.insert(i);
                extractCycles(segm, sum_cycle.m_cycle, usedNew, cycles, params, restLvl, cyclesSet);
            }
        }
    }
    void extractCycles(const cv::Mat &segm, std::vector<WeightedEdgesCyclesSet> &arCyclesSet, const ExtractPolygonsParams &params) const
    {
        CV_Assert(params.m_extractLevelsCount > 2);
        std::vector< std::vector<EdgesPath> > arcycles;
        extractFundamentalCyclesEdges(arcycles);
        arCyclesSet.resize(arcycles.size());

        for (size_t c = 0; c < arcycles.size(); c++)
        {
            const std::vector<EdgesPath> &cycles = arcycles[c];
            if (0 == cycles.size())
                continue;
            std::cout << "tree number: " << c << " cycles count: " << cycles.size() << std::endl;
            WeightedEdgesCyclesSet &cyclesSet = arCyclesSet[c];

            for (size_t i = 0; i < cycles.size(); i++)
            {
                WeightedEdgesCycle cycle;
                cycle.m_cycle = cycles[i];
                cycle.m_weight = weightCycle(cycle.m_cycle, segm, params.m_weightCycleParams);
                cyclesSet.emplace(cycle);
            }
            for (size_t i0 = 0; i0 < cycles.size() - 1; i0++)
            {
                const EdgesPath &cycle1 = cycles[i0];
                for (size_t i1 = i0 + 1; i1 < cycles.size(); i1++)
                {
                    const EdgesPath &cycle2 = cycles[i1];
                    WeightedEdgesCycle sum_cycle;
                    sumEdgesCycle(cycle1, cycle2, sum_cycle.m_cycle);
                    if (sum_cycle.m_cycle.empty())
                        continue;
                    if (!hasVertDegreeGreatTwo(sum_cycle.m_cycle))
                    {
                        sum_cycle.m_weight = weightCycle(sum_cycle.m_cycle, segm, params.m_weightCycleParams);
                        cyclesSet.insert(sum_cycle);
                    }

                    std::set<size_t> used({i0, i1});
                    extractCycles(segm, sum_cycle.m_cycle, used, cycles, params.m_weightCycleParams, params.m_extractLevelsCount - 2, cyclesSet);
                }
            }
        }
    }
    void getBestEdgesCycles(const WeightedEdgesCyclesSet &cyclesSet, const cv::Size &img_sz, std::vector<EdgesPath> &best, const BestCycleParams &params) const
    {
        size_t graph_edges_cnt = edgesSize();
        cv::Mat img = cv::Mat::zeros(img_sz, CV_8UC1);
        cv::Rect roi;
        cv::Mat cycle_mask(img_sz, CV_8UC1); // allocate buffer
        cv::Mat temp(img_sz, CV_8UC1); // allocate buffer
        std::set<int> edges;
        for (const auto &cycle : cyclesSet)
        {
            if (cycle.m_weight < params.m_minCycleWeight)
                break;

            //std::cout << cycle.m_weight << std::endl;
            rasterizeEdgeCycle(cycle.m_cycle, roi, cycle_mask);

            if (false)
            {
                cv::Mat temptemp = cv::Mat::zeros(img_sz, CV_8UC1);
                cycle_mask.copyTo(temptemp(roi));
                cv::imshow("temptemp", temptemp * 255);
                cv::waitKey();
            }

            const int cycle_area = cv::countNonZero(cycle_mask);
            temp = cycle_mask.mul(img(roi));
            const int intersect_area = cv::countNonZero(temp);
            if ((double)intersect_area > params.m_minRelativeIntersectionArea * (double)cycle_area ||
                (double)intersect_area > params.m_minIntersectionArea)
                continue;

            int newEdges = 0;
            for (const auto &edge : cycle.m_cycle)
            {
                if (edges.find(edge) == edges.end())
                    newEdges++;
            }
            if ((double)newEdges < (double)cycle.m_cycle.size() * params.m_newEdgesPercent)
                continue;

            img(roi).setTo(1, cycle_mask);
            if (false)
            {
                cv::imshow("a", img * 255);
                std::cout << cycle_area << std::endl;
                cv::waitKey();
            }

            for (const auto &edge : cycle.m_cycle)
            {
                edges.insert(edge);
            }
            best.push_back(cycle.m_cycle);
            if (edges.size() == graph_edges_cnt)
                break;
        }
    }
};
}

bool extractPolygons(const cv::Mat &vertData, const cv::Mat &edgeData, const cv::Mat &segm_data, const ExtractPolygonsParams &params, std::vector<std::vector<cv::Point> > &polygons)
{
    CoordGraph graph;
    if (!graph.importData(vertData, edgeData, params))
        return false;

    if (0. < params.m_keenVerticesMaxAngle)
        graph.removeKeenVertices(params.m_keenVerticesMaxAngle);
    else
        graph.eraseLeaves();
    if (0. < params.m_thinPolygonsParams.m_maxVertEdgeDist)
        graph.removeThinPolygon(params.m_thinPolygonsParams);
    if (params.m_removeIntersectEdges)
        graph.removeIntersectedEdges();
#ifdef ENABLE_GRAPH_DRAW
    constexpr double scale = 2.0;
    cv::Mat img_show; cv::cvtColor(edgeData, img_show, cv::COLOR_GRAY2BGR);
    cv::resize(img_show, img_show, cv::Size(), scale, scale);
    graph.draw(img_show, true, scale);
    cv::imshow("graph", img_show);
    cv::waitKey();
#endif

    graph.extractPolygons(segm_data, polygons, params);
    return true;
}
} //namespace autocart
} //namespace wiki
} //namespace maps
