//
// Created by Martin Gargalovič on 10. 8. 2021.
//

#include <iostream>
#include <opencv2/imgproc.hpp>

#include "groupForming.h"


constexpr double alpha = M_PI / 8;
constexpr double rotateAngle = M_PI / 8;

int maxVal(const std::vector<int> &vec) {
    int pos = 0;
    for (int i = 0; i < vec.size(); i++) {
        if (vec[pos] < vec[i]) pos = i;
    }
    return pos;
}

std::vector<int>
supportGroup(size_t start, cv::Mat &marked, std::vector<bool> &used,
             std::vector<rectangle> &rectangles, int search_size,
             bool headRegion) {
    cv::Point2i current;
    std::vector<int> Group;

    //do while area[maxRegion] != 0; should be better
    int areaCount = 1;
    size_t currentRegion = start;
    while (areaCount != 0) {
        Group.emplace_back(currentRegion);
        std::vector<int> area(rectangles.size(), 0);
        used[currentRegion] = true;

        double rotate = (-1 * headRegion + 1 * !headRegion) * rectangles[currentRegion].polarity * rotateAngle;

        double dx1 = (-1 * headRegion + 1 * !headRegion) * rectangles[currentRegion].dx;
        double dy1 = (-1 * headRegion + 1 * !headRegion) * rectangles[currentRegion].dy;

        double dy = dx1 * sin(rotate) + dy1 * cos(rotate);
        double dx = dx1 * cos(rotate) - dy1 * sin(rotate);


        current = headRegion ? cv::Point2i(rectangles[currentRegion].x1, rectangles[currentRegion].y1) : cv::Point2i(
                rectangles[currentRegion].x2, rectangles[currentRegion].y2);
        //search the area around head/tail
        for (int i = 0; i <= search_size; i++) {
            for (int j = 0; j <= search_size; j++) {

                int x = current.x + i * dx + j * dx;
                int y = current.y + j * dy + j * dy;

                if (x < 0 || x >= marked.cols || y < 0 || y >= marked.rows) continue;
                if (int val = (int) marked.at<float>(cv::Point2i(x, y));val > 0 && !used[val - 1] &&
                                                                        abs(rectangles[currentRegion].theta -
                                                                            rectangles[val - 1].theta) < 2 * alpha) {
                    area[val - 1]++;
                }

            }
        }

        //choose next arcSegment
        currentRegion = maxVal(area);
        areaCount = area[currentRegion];
    }

    Group.erase(Group.begin());
    return Group;
}


std::vector<std::pair<std::vector<int>, double>>
groupForm(cv::Mat &marked, int searchSize,
          std::vector<rectangle> &rectangles) {
    std::vector<bool> usedArc(rectangles.size(), false);
    std::vector<std::pair<std::vector<int>, double>> groups;


    for (size_t i = 0; i < rectangles.size(); i++) {
        if (usedArc[i]) continue;

        std::vector<int> group;
        std::vector<int> headGroup = supportGroup(i, marked, usedArc,
                                                  rectangles, searchSize, true);
        std::vector<int> tailGroup = supportGroup(i, marked, usedArc,
                                                  rectangles, searchSize,
                                                  false);

        group.insert(group.begin(), tailGroup.rbegin(), tailGroup.rend());
        group.emplace_back(i);
        group.insert(group.end(), headGroup.begin(), headGroup.end());

        double angleT = abs(rectangles[group[0]].theta - rectangles[group[group.size() - 1]].theta);
        if (angleT == 0) continue;
        groups.emplace_back(make_pair(group, angleT));
    }

    return groups;
}

