#include <maps/wikimap/mapspro/services/autocart/libs/detection/include/pp_pgns.h>
#include <maps/wikimap/mapspro/services/autocart/libs/detection/include/utils.h>

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

#include <contrib/libs/opencv/include/opencv2/imgproc.hpp>

namespace maps {
namespace wiki {
namespace autocart {

namespace {
// по трем точка, которые собираем в угол pt1-pt0-pt2 ищем третью result, такую чтобы угол
// pt1 - result - pt2 стал прямым
bool findOrtho(const cv::Point &pt0, const cv::Point &pt1, const cv::Point &pt2, cv::Point2d &result)
{
    const cv::Point2d temp = (cv::Point2d)pt1 + (cv::Point2d)pt2 - 2. * (cv::Point2d)pt0;
    const double a = temp.x * temp.x + temp.y * temp.y;
    const double b = -(temp.x * temp.x + temp.y * temp.y);
    const double c = pt0.dot(pt0) - pt0.x*(pt1.x + pt2.x) - pt0.y*(pt1.y + pt2.y) + pt1.dot(pt2);

    const double D = b*b - 4.*a*c;
    if (D < 0.)
        return false;
    const double t1 = (-b + sqrt(D)) / 2. / a;
    const double t2 = (-b - sqrt(D)) / 2. / a;
    result = (abs(t1) < abs(t2)) ? ((cv::Point2d)pt0 + t1 * temp) : ((cv::Point2d)pt0 + t2 * temp);
    return true;
}

bool removeNearStraightAngles(std::vector<cv::Point> &polygon, double nearStraightMaxDelta)
{
    REQUIRE(polygon.size() > 2, "Polygon should contain more than 2 vertices");

    bool changed = false;
    bool doit = true;
    while (doit && (4 <= polygon.size()))
    {
        doit = false;
        const double angle = calcAngle(polygon.back(), polygon[0], polygon[1]);
        if (abs(angle - CV_PI) < nearStraightMaxDelta)
        {
            polygon.erase(polygon.begin());
            doit = true;
            changed = true;
        }
        for (size_t i = 1; i < polygon.size(); i++)
        {
            const double angle = calcAngle(polygon[i - 1], polygon[i], polygon[(i + 1) % polygon.size()]);
            if (abs(angle - CV_PI) > nearStraightMaxDelta)
                continue;
            polygon.erase(polygon.begin() + i);
            i--;
            doit = true;
            changed = true;
            if (polygon.size() < 4)
                break;// остался треугольник мы его всё равно потом удалим
        }
    }
    return changed;
}

bool removeNearStraightAngles(std::vector< std::vector<cv::Point> > &polygons, double nearStraightMaxDelta)
{
    bool changed = false;
    for (int i = (int)polygons.size() - 1; i >= 0; i--)
    {
        changed |= removeNearStraightAngles(polygons[i], nearStraightMaxDelta);
        if (3 < polygons[i].size())
            continue;
        polygons.erase(polygons.begin() + i);
    }
    return changed;
}

bool removeSmallEdges(std::vector<cv::Point> &polygon, double smallEdgesMaxDist)
{
    if (polygon.size() < 4)
        return false;

    bool changed = false;
    bool doit = true;
    while (doit && (4 <= polygon.size()))
    {
        doit = false;
        for (size_t i = 0; i < polygon.size(); i++)
        {
            const double dist = cv::norm(polygon[i] - polygon[(i + 1) % polygon.size()]);
            if (dist > smallEdgesMaxDist)
                continue;

            polygon[i] = (polygon[i] + polygon[(i + 1) % polygon.size()]) / 2.0;
            polygon.erase(polygon.begin() + (i + 1) % polygon.size());
            i--;
            doit = true;
            changed = true;
            if (polygon.size() < 4)
                break;// остался треугольник мы его всё равно потом удалим
        }
    }
    return changed;
}

bool removeSmallEdges(std::vector< std::vector<cv::Point> > &polygons,
                      double smallEdgesMaxDist)
{
    bool changed = false;
    for (int i = (int)polygons.size() - 1; i >= 0; i--)
    {
        changed |= removeSmallEdges(polygons[i], smallEdgesMaxDist);
        if (3 < polygons[i].size())
            continue;
        polygons.erase(polygons.begin() + i);
    }
    return changed;
}

void removeThinRectangles(std::vector< std::vector<cv::Point> > &polygons,
                          double minFraction,
                          double minLength)
{
    for (int i = (int)polygons.size() - 1; i >= 0; i--)
    {
        if (4 != polygons[i].size())
            continue;

        const std::vector<cv::Point> &pgn = polygons[i];
        double minEdge = cv::norm(pgn[1] - pgn[0]);
        double maxEdge = minEdge;
        for (size_t j = 1; j < pgn.size(); j++)
        {
            double edge = cv::norm(pgn[(j + 1) % pgn.size()] - pgn[j]);
            if (minEdge > edge)
                minEdge = edge;
            if (maxEdge < edge)
                maxEdge = edge;
        }

        if (minEdge / maxEdge > minFraction && minEdge > minLength)
            continue;
        polygons.erase(polygons.begin() + i);
    }
}

bool removeAcuteAngle(std::vector<cv::Point> &polygon,
                      double removeAcuteAngleMax)
{
    REQUIRE(removeAcuteAngleMax < CV_PI / 2., "removeAcuteAngleMax should be less than pi/2");

    bool changed = false;
    bool doit = true;
    while (doit && (4 <= polygon.size()))
    {
        doit = false;

        size_t pgnVertsCnt = polygon.size();

        size_t minAngleVertIdx = 0;
        double minAngle = calcAngle(polygon[pgnVertsCnt - 1], polygon[0], polygon[1]);
        for (size_t i = 1; i < pgnVertsCnt; i++)
        {
            const double angle = calcAngle(polygon[(i + pgnVertsCnt - 1) % pgnVertsCnt], polygon[i], polygon[(i + 1) % pgnVertsCnt]);
            if (angle < minAngle)
            {
                minAngle = angle;
                minAngleVertIdx = i;
            }
        }

        if (minAngle > removeAcuteAngleMax)
            break;
        cv::Point prevPt = polygon[(minAngleVertIdx + pgnVertsCnt - 1) % pgnVertsCnt];
        cv::Point curPt = polygon[minAngleVertIdx];
        cv::Point nextPt = polygon[(minAngleVertIdx + 1) % pgnVertsCnt];
        const double prevEdge = cv::norm(prevPt - curPt);
        const double nextEdge = cv::norm(nextPt - curPt);
        cv::Point result;
        if (prevEdge < nextEdge)
        {
            if (!projectPointOnSegment(prevPt, curPt, nextPt, result))
                break;
        }
        else
        {
            if (!projectPointOnSegment(nextPt, curPt, prevPt, result))
                break;
        }
        if (prevPt == result || nextPt == result)
            polygon.erase(polygon.begin() + minAngleVertIdx);
        else
            polygon[minAngleVertIdx] = result;
        doit = true;
        changed = true;
    }
    return changed;
}

bool removeAcuteAngle(std::vector< std::vector<cv::Point> > &polygons,
                      double removeAcuteAngleMax)
{
    bool changed = false;
    for (int i = (int)polygons.size() - 1; i >= 0; i--)
    {
        changed |= removeAcuteAngle(polygons[i], removeAcuteAngleMax);
        if (3 < polygons[i].size())
            continue;
        polygons.erase(polygons.begin() + i);
    }
    return changed;
}

void rectifyQuadToRect(std::vector<cv::Point> &polygon,
                       bool rectifyQuadToRectByEdge)
{
    constexpr int pgnVertsCnt = 4;

    REQUIRE(polygon.size() == pgnVertsCnt, "Polygon should contain 4 vertices");

    double edgesLen[4];
    int baseEdgeIdx = 0;
    if (rectifyQuadToRectByEdge)
    {
        double maxEdgeLen = edgesLen[0] = cv::norm(polygon[1] - polygon[0]);
        for (size_t i = 1; i < polygon.size(); i++)
        {
            edgesLen[i] = cv::norm(polygon[(i + 1) % pgnVertsCnt] - polygon[i]);
            if (edgesLen[i] > maxEdgeLen)
            {
                maxEdgeLen = edgesLen[i];
                baseEdgeIdx = (int)i;
            }
        }
    }
    else
    {
        double minDelta = abs(calcAngle(polygon.back(), polygon[0], polygon[1]) - CV_PI / 2.);
        int minVertIdx = 0;
        edgesLen[0] = cv::norm(polygon[1] - polygon[0]);
        for (size_t i = 1; i < polygon.size(); i++)
        {
            edgesLen[i] = cv::norm(polygon[(i + 1) % pgnVertsCnt] - polygon[i]);
            double temp = abs(calcAngle(polygon[i - 1], polygon[i], polygon[(i + 1) % pgnVertsCnt]) - CV_PI / 2.);
            if (temp < minDelta)
            {
                minDelta = temp;
                minVertIdx = (int)i;
            }
        }

        if (edgesLen[minVertIdx] < edgesLen[(minVertIdx + pgnVertsCnt - 1) % pgnVertsCnt])
            baseEdgeIdx = (minVertIdx + pgnVertsCnt - 1) % pgnVertsCnt;
        else
            baseEdgeIdx = minVertIdx;
    }

    const cv::Point baseVert = polygon[baseEdgeIdx];
    const double maxNewEdgeLen = (edgesLen[baseEdgeIdx] + edgesLen[(baseEdgeIdx + 2) % pgnVertsCnt]) / 2.0;
    const double minNewEdgeLen = (edgesLen[(baseEdgeIdx + 1) % pgnVertsCnt] + edgesLen[(baseEdgeIdx + 3) % pgnVertsCnt]) / 2.0;
    cv::Point temp = polygon[(baseEdgeIdx + 1) % pgnVertsCnt] - polygon[baseEdgeIdx];
    const double baseAngle = atan2(temp.y, temp.x);

    const cv::Point a = polygon[(baseEdgeIdx + 1) % pgnVertsCnt] - polygon[baseEdgeIdx];
    const cv::Point b = polygon[(baseEdgeIdx + pgnVertsCnt - 1) % pgnVertsCnt] - polygon[baseEdgeIdx];
    int orient = (a.cross(b) > 0) ? 1 : -1;

    makeRectangle(baseVert, baseAngle, maxNewEdgeLen, minNewEdgeLen, orient, polygon);
}

void rectifyAnglesToOrtho(std::vector<cv::Point> &polygon, double rectifyAngleToOrthoMaxDelta)
{
    const int pgnVertsCnt = (int)polygon.size();

    double delta = abs(calcAngle(polygon.back(), polygon[0], polygon[1]) - CV_PI / 2.);
    if (delta < rectifyAngleToOrthoMaxDelta)
    {
        cv::Point2d temp;
        if (findOrtho(polygon[0], polygon.back(), polygon[1], temp))
            polygon[0] = temp;
    }
    for (size_t i = 1; i < polygon.size(); i++)
    {
        double temp = abs(calcAngle(polygon[i - 1], polygon[i], polygon[(i + 1) % pgnVertsCnt]) - CV_PI / 2.);
        if (temp < rectifyAngleToOrthoMaxDelta)
        {
            cv::Point2d temp;
            if (findOrtho(polygon[i], polygon[i - 1], polygon[(i + 1) % pgnVertsCnt], temp))
                polygon[i] = temp;
        }
    }
}

void calcRLPattern(const std::vector<cv::Point> &pgn, std::vector<int> &pattern)
{
    const size_t pgnVertsCnt = pgn.size();
    pattern.resize(pgnVertsCnt);
    for (size_t i = 0; i < pgnVertsCnt; i++)
    {
        const cv::Point p1 = pgn[(i + pgnVertsCnt - 1) % pgnVertsCnt] - pgn[i];
        const cv::Point p2 = pgn[(i + 1) % pgnVertsCnt] - pgn[i];
        if (0 < p2.cross(p1))
            pattern[i] = 1;
        else
            pattern[i] = 0;
    }
}

struct LVertex
{
    LVertex()
        : idx(-1)
        , beforeRCnt(0)
        , afterRCnt(0)
    {
    }
    size_t idx;
    size_t beforeRCnt;
    size_t afterRCnt;
};

struct DivLine
{
    size_t vertexIdx;
    bool forward;
    cv::Point2d endLine;
    double lengthDL;
    double widthBR; // width branch roof
    double widthMR; // width main roof

    int cutOffVertsCnt;
};

static void findDivLines(const std::vector<cv::Point> &pgn, std::vector<DivLine> &DLs)
{
    constexpr double epsilon = 1.e-5;
    const size_t pgnVertsCnt = pgn.size();

    std::vector<int> pattern;
    calcRLPattern(pgn, pattern);

    std::vector<LVertex> lvertices;
    int rcnt = 0;
    for (size_t i = 0; i < pattern.size(); i++)
    {
        if (0 == pattern[i])
        {
            if (0 != lvertices.size())
                lvertices.back().afterRCnt = rcnt;
            LVertex vert;
            vert.idx = i;
            vert.beforeRCnt = rcnt;
            vert.afterRCnt = 0;
            lvertices.emplace_back(vert);
            rcnt = 0;
        }
        else
            rcnt++;
    }
    if (0 != lvertices.size())
    {
        lvertices.front().beforeRCnt += rcnt;
        lvertices.back().afterRCnt = lvertices.front().beforeRCnt;
    }

    //std::vector<DivLine> DLs;
    for (size_t i = 0; i < lvertices.size(); i++)
    {
        if (2 <= lvertices[i].afterRCnt)
        {
            const cv::Point2d segm0s = pgn[(lvertices[i].idx + pgnVertsCnt - 1) % pgnVertsCnt];
            const cv::Point2d segm0e = pgn[lvertices[i].idx];

            const cv::Point2d segm1s = pgn[(lvertices[i].idx + 2) % pgnVertsCnt];
            const cv::Point2d segm1e = pgn[(lvertices[i].idx + 3) % pgnVertsCnt];

            const cv::Point2d segm0 = segm0e - segm0s;
            const cv::Point2d segm1 = segm1e - segm1s;

            const double cross = segm0.cross(segm1);
            if (abs(cross) > epsilon)
            {
                const double t = (segm1s - segm0s).cross(segm0) / cross;
                if (0. < t && t <= 1.0)
                {
                    DivLine dl;
                    dl.vertexIdx = lvertices[i].idx;
                    dl.forward = true;
                    dl.endLine = segm1s + segm1 * t;
                    dl.lengthDL = cv::norm(dl.endLine - segm0e);
                    dl.widthBR = segmentsDistance(
                        pgn[lvertices[i].idx],
                        dl.endLine,
                        pgn[(lvertices[i].idx + 1) % pgnVertsCnt],
                        pgn[(lvertices[i].idx + 2) % pgnVertsCnt]);

                    const bool endVertex =
                            (0 == pattern[(lvertices[i].idx + 3) % pgnVertsCnt]) &&
                            (cv::norm(segm1e - dl.endLine) < 1.);
                    size_t s = (lvertices[i].idx + 3 + (endVertex ? 1 : 0));
                    size_t e = lvertices[i].idx + pgnVertsCnt - 1;
                    dl.widthMR = -DBL_MAX;
                    for (size_t j = s; j < e; j++)
                    {
                        const double dist = segmentsDistance(
                            pgn[lvertices[i].idx],
                            dl.endLine,
                            pgn[j % pgnVertsCnt],
                            pgn[(j + 1) % pgnVertsCnt]);
                        dl.widthMR = MAX(dist, dl.widthMR);
                    }
                    dl.cutOffVertsCnt = endVertex ? 4 : 3;
                    if (dl.widthBR < dl.widthMR)
                        DLs.emplace_back(dl);
                }
            }
        }
        if (2 <= lvertices[i].beforeRCnt)
        {
            const cv::Point2d segm0s = pgn[(lvertices[i].idx + 1) % pgnVertsCnt];
            const cv::Point2d segm0e = pgn[lvertices[i].idx];

            const cv::Point2d segm1s = pgn[(lvertices[i].idx + pgnVertsCnt - 2) % pgnVertsCnt];
            const cv::Point2d segm1e = pgn[(lvertices[i].idx + pgnVertsCnt - 3) % pgnVertsCnt];

            const cv::Point2d segm0 = segm0e - segm0s;
            const cv::Point2d segm1 = segm1e - segm1s;

            const double cross = segm0.cross(segm1);
            if (abs(cross) > epsilon)
            {
                const double t = (segm1s - segm0s).cross(segm0) / cross;
                if (0. < t && t <= 1.0)
                {
                    DivLine dl;
                    dl.vertexIdx = lvertices[i].idx;
                    dl.forward = false;
                    dl.endLine = segm1s + segm1 * t;
                    dl.lengthDL = cv::norm(dl.endLine - segm0e);
                    dl.widthBR = segmentsDistance(
                        pgn[lvertices[i].idx],
                        dl.endLine,
                        pgn[(lvertices[i].idx + pgnVertsCnt - 1) % pgnVertsCnt],
                        pgn[(lvertices[i].idx + pgnVertsCnt - 2) % pgnVertsCnt]);
                    const bool endVertex =
                        (0 == pattern[(lvertices[i].idx + pgnVertsCnt - 3) % pgnVertsCnt]) &&
                        (cv::norm(segm1e - dl.endLine) < 1.);
                    size_t s = lvertices[i].idx + 1;
                    size_t e = lvertices[i].idx + pgnVertsCnt - 3 - (endVertex ? 1 : 0);
                    dl.widthMR = -DBL_MAX;
                    for (size_t j = s; j < e; j++)
                    {
                        const double dist = segmentsDistance(
                            pgn[lvertices[i].idx],
                            dl.endLine,
                            pgn[j % pgnVertsCnt],
                            pgn[(j + 1) % pgnVertsCnt]);
                        dl.widthMR = MAX(dist, dl.widthMR);
                    }

                    dl.cutOffVertsCnt = (cv::norm(segm1e - dl.endLine) < 1.) ? 4 : 3;
                    if (dl.widthBR < dl.widthMR)
                        DLs.emplace_back(dl);
                }
            }
        }
    }
}

struct Branch
{
    std::vector<cv::Point> quad;
    int mainPartEdgeIdx;
    double mainPartEdgeT0;
    double mainPartEdgeT1;
    DivLine dl;
};

/*
    branchPart - ребро смежное к mainPart будет [ branchPart.back() --- branchPart.front() ]
*/
bool cutOffBranch(const std::vector<cv::Point> &pgn, std::vector<cv::Point> &mainPart, Branch &branch)
{
    REQUIRE(mainPart.empty(), "not empty vector parameter");
    const size_t pgnVertsCnt = pgn.size();

    std::vector<DivLine> DLs;
    findDivLines(pgn, DLs);
    std::sort(DLs.begin(), DLs.end(),
        [](const DivLine &a, const DivLine &b)
    {
        if (a.cutOffVertsCnt > b.cutOffVertsCnt)
            return true;
        else if (a.cutOffVertsCnt < b.cutOffVertsCnt)
            return false;
        return a.lengthDL < b.lengthDL;
    });
    if (DLs.empty())
        return false;
    const DivLine &dl = DLs.front();
    if (dl.forward)
    {
        for (size_t i = dl.vertexIdx; i < dl.vertexIdx + 3; i++)
        {
            branch.quad.push_back(pgn[i % pgnVertsCnt]);
        }
        branch.quad.push_back((cv::Point)dl.endLine);
        /////////////////////////////////////////

        for (size_t i = dl.vertexIdx + dl.cutOffVertsCnt; i <= dl.vertexIdx + pgnVertsCnt - 1; i++)
        {
            mainPart.push_back(pgn[i % pgnVertsCnt]);
        }
        branch.mainPartEdgeIdx = (int)mainPart.size() - 1;
        if (3 == dl.cutOffVertsCnt)
            mainPart.push_back((cv::Point)dl.endLine);
        else if (mainPart.size() < 4)
            mainPart.push_back((cv::Point)dl.endLine);
    }
    else
    {
        branch.quad.push_back((cv::Point)dl.endLine);
        for (size_t i = dl.vertexIdx + pgnVertsCnt - 2; i <= dl.vertexIdx + pgnVertsCnt; i++)
        {
            branch.quad.push_back(pgn[i % pgnVertsCnt]);
        }
        /////////////////////////////////////////

        for (size_t i = dl.vertexIdx + 1; i <= dl.vertexIdx + pgnVertsCnt - dl.cutOffVertsCnt; i++)
        {
            mainPart.push_back(pgn[i % pgnVertsCnt]);
        }
        if (3 == dl.cutOffVertsCnt)
            mainPart.push_back((cv::Point)dl.endLine);
        else if (mainPart.size() < 4)
            mainPart.push_back((cv::Point)dl.endLine);
        branch.mainPartEdgeIdx = (int)mainPart.size() - 1;
    }
    const double mainEdgeLen = cv::norm(mainPart[(branch.mainPartEdgeIdx + 1) % mainPart.size()] - mainPart[branch.mainPartEdgeIdx]);
    branch.mainPartEdgeT0 = cv::norm(branch.quad[0] - mainPart[branch.mainPartEdgeIdx]) / mainEdgeLen;
    branch.mainPartEdgeT1 = cv::norm(branch.quad[3] - mainPart[branch.mainPartEdgeIdx]) / mainEdgeLen;
    branch.dl = dl;
    return true;
}

void fixQuadToRect(std::vector<cv::Point> &pgn, double mainPgnAngle)
{
    const size_t pgnVertsCnt = pgn.size();

    int mainEdgeIdx = -1;
    double minDelta = DBL_MAX;
    for (int i = 0; i < (int)pgnVertsCnt; i++)
    {
        const cv::Point edge = pgn[(i + 1) % pgnVertsCnt] - pgn[i];
        double angle = atan2(edge.y, edge.x);
        if (angle < 0.)
            angle += CV_PI;
        double delta = abs(mainPgnAngle - angle);
        if (delta < minDelta)
        {
            minDelta = delta;
            mainEdgeIdx = i;
        }
    }
    const cv::Point mainEdge = pgn[(mainEdgeIdx + 1) % pgnVertsCnt] - pgn[mainEdgeIdx];
    double angle = atan2(mainEdge.y, mainEdge.x);
    if (angle < 0.)
        mainPgnAngle -= CV_PI;

    double edgeLen0 =
        (cv::norm(pgn[(mainEdgeIdx + 1) % pgnVertsCnt] - pgn[mainEdgeIdx]) +
            cv::norm(pgn[(mainEdgeIdx + 3) % pgnVertsCnt] - pgn[(mainEdgeIdx + 2) % pgnVertsCnt])) / 2.0;
    double edgeLen1 =
        (cv::norm(pgn[(mainEdgeIdx + 2) % pgnVertsCnt] - pgn[(mainEdgeIdx + 1) % pgnVertsCnt]) +
            cv::norm(pgn[mainEdgeIdx] - pgn[(mainEdgeIdx + pgnVertsCnt - 1) % pgnVertsCnt])) / 2.0;

    cv::Point base = pgn[mainEdgeIdx];
    makeRectangle(base, mainPgnAngle, edgeLen0, edgeLen1, 1, pgn);
    std::rotate(pgn.begin(), pgn.begin() + (pgnVertsCnt - mainEdgeIdx) % pgnVertsCnt, pgn.end());
}

double polygonAngle(const std::vector<cv::Point> &pgn)
{
    constexpr double epsilon = 1.e-5;

    const size_t pgnVertsCnt = pgn.size();
    std::vector< std::pair<double, double> > angleLength;

    double angleMax = 0.;
    double lengthMax = 0.;
    for (size_t i = 0; i < pgnVertsCnt; i++)
    {
        const cv::Point edge = pgn[(i + 1) % pgnVertsCnt] - pgn[i];

        double angle = atan2(edge.y, edge.x); // -pi <= angle <= pi
        if (angle < 0.)
            angle += CV_PI;
        //// 0 <= angle <= pi
        double length = cv::norm(edge);

        int j = 0;
        for (; j < (int)angleLength.size(); j++)
        {
            if (abs(angleLength[j].first - angle) < epsilon)
            {
                angleLength[j].first = (angleLength[j].first + angle) / 2.0;
                angleLength[j].second = angleLength[j].second + length;
                if (lengthMax < angleLength[j].second)
                {
                    lengthMax = angleLength[j].second;
                    angleMax = angleLength[j].first;
                }
                break;
            }
        }
        if ((int)angleLength.size() == j)
        {
            if (lengthMax < length)
            {
                lengthMax = length;
                angleMax = angle;
            }
            angleLength.emplace_back(angle, length);
            continue;
        }
    }
    return angleMax;
}

void appendBranch(std::vector<cv::Point> &mainPart, const Branch &branch)
{
    constexpr double epsilon = 1.e-5;

    cv::Point base;
    cv::Point edge;
    int orient = isClockwise(branch.quad) ? -1 : 1;
    if (abs(branch.mainPartEdgeT0) < epsilon)
    {
        base = mainPart[branch.mainPartEdgeIdx];
        edge = mainPart[(branch.mainPartEdgeIdx + 1) % mainPart.size()] - mainPart[branch.mainPartEdgeIdx];
    }
    else if (abs(branch.mainPartEdgeT1 - 1.) < epsilon)
    {
        base = mainPart[(branch.mainPartEdgeIdx + 1) % mainPart.size()];
        edge = mainPart[branch.mainPartEdgeIdx] - mainPart[(branch.mainPartEdgeIdx + 1) % mainPart.size()];
        orient = -orient;
    }
    else
    {
        edge = mainPart[(branch.mainPartEdgeIdx + 1) % mainPart.size()] - mainPart[branch.mainPartEdgeIdx];
        base = mainPart[branch.mainPartEdgeIdx] + branch.mainPartEdgeT0 * edge;
    }
    double angle = atan2(edge.y, edge.x);

    double edgeLen0 = (cv::norm(branch.quad[3] - branch.quad[0]) + cv::norm(branch.quad[2] - branch.quad[1])) / 2.0;
    double edgeLen1 = (cv::norm(branch.quad[1] - branch.quad[0]) + cv::norm(branch.quad[3] - branch.quad[2])) / 2.0;

    std::vector<cv::Point> quadNew;
    makeRectangle(base, angle, edgeLen0, edgeLen1, orient, quadNew);

    int shift = 0;
    if (abs(branch.mainPartEdgeT0) < epsilon)
    {
        mainPart[branch.mainPartEdgeIdx] = quadNew[3];
        mainPart.insert(mainPart.begin() + branch.mainPartEdgeIdx + 1, quadNew[2]);
        mainPart.insert(mainPart.begin() + branch.mainPartEdgeIdx + 2, quadNew[1]);
        REQUIRE(!branch.dl.forward, "DivLine should has backward type");
        shift = branch.mainPartEdgeIdx + 2 - (int)branch.dl.vertexIdx;
    }
    else if (abs(branch.mainPartEdgeT1 - 1.) < epsilon)
    {
        mainPart[branch.mainPartEdgeIdx + 1] = quadNew[3];
        mainPart.insert(mainPart.begin() + branch.mainPartEdgeIdx + 1, quadNew[1]);
        mainPart.insert(mainPart.begin() + branch.mainPartEdgeIdx + 2, quadNew[2]);
        REQUIRE(branch.dl.forward, "DivLine should has forward type");
        shift = branch.mainPartEdgeIdx + 1 - (int)branch.dl.vertexIdx;
    }
    else
    {
        mainPart.insert(mainPart.begin() + branch.mainPartEdgeIdx + 1, quadNew[0]);
        mainPart.insert(mainPart.begin() + branch.mainPartEdgeIdx + 2, quadNew[3]);
        mainPart.insert(mainPart.begin() + branch.mainPartEdgeIdx + 3, quadNew[2]);
        mainPart.insert(mainPart.begin() + branch.mainPartEdgeIdx + 4, quadNew[1]);
        if (branch.dl.forward)
            shift = branch.mainPartEdgeIdx + 1 - (int)branch.dl.vertexIdx;
        else
            shift = branch.mainPartEdgeIdx + 4 - (int)branch.dl.vertexIdx;
    }
    while (shift < 0)
        shift += (int)mainPart.size();
    shift %= mainPart.size();
    std::rotate(mainPart.begin(), mainPart.begin() + shift, mainPart.end());
}

void rectifyPolyToOrtho(std::vector<cv::Point> &polygon, double rectifyAngleToOrthoMaxDelta)
{
    if (!isClockwise(polygon))
        std::reverse(polygon.begin(), polygon.end());

    double pgnAngle = polygonAngle(polygon);
    std::vector< Branch> branchParts;
    for (;;)
    {
        REQUIRE(polygon.size() > 4, "Polygon should contain more than 4 vertices");
        std::vector<cv::Point> mainPart;
        Branch branch;
        if (!cutOffBranch(polygon, mainPart, branch))
            return;
        branchParts.emplace_back(branch);
        if (mainPart.size() <= 5)
        {
            branch.quad = mainPart;
            branch.mainPartEdgeIdx = -1;
            branch.mainPartEdgeT0 = 0.0;
            branch.mainPartEdgeT1 = 0.0;
            branchParts.emplace_back(branch);
            break;
        }
        polygon = mainPart;
    }

    polygon = branchParts.back().quad;
    if (polygon.size() == 4)
        fixQuadToRect(polygon, pgnAngle);
    else
        rectifyAnglesToOrtho(polygon, rectifyAngleToOrthoMaxDelta);
    fixQuadToRect(polygon, pgnAngle);
    for (int i = (int)branchParts.size() - 2; i >= 0; i--)
    {
        appendBranch(polygon, branchParts[i]);
    }
}

void rectifyOrtho(std::vector< std::vector<cv::Point> > &polygons, const PPPolygonsParams &params)
{
    for (int i = (int)polygons.size() - 1; i >= 0; i--)
    {
        if (polygons[i].size() < 4)
            polygons.erase(polygons.begin() + i);
        else if (4 == polygons[i].size())
            rectifyQuadToRect(polygons[i], params.rectifyQuadToRectByEdge);
        else if (5 == polygons[i].size())
            rectifyAnglesToOrtho(polygons[i], params.rectifyAnglesToOrthoMaxDelta);
        else
            rectifyPolyToOrtho(polygons[i], params.rectifyAnglesToOrthoMaxDelta);
    }
}
}

void postprocessPolygons(const PPPolygonsParams &params, std::vector<std::vector<cv::Point> > &polygons)
{
    bool doit = true;
    while (doit)
    {
        doit = removeNearStraightAngles(polygons, params.nearStraightMaxDelta);
        doit |= removeSmallEdges(polygons, params.smallEdgesMaxDist);
        doit |= removeAcuteAngle(polygons, params.removeAcuteAngleMax);
    }
    rectifyOrtho(polygons, params);
    removeThinRectangles(polygons, params.thinRectMinFraction, params.thinRectMinLength);
}

} //namespace autocart
} //namespace wiki
} //namespace maps
