//
// Created by Martin Gargalovic on 9. 9. 2021.
//

#include <opencv2/imgproc.hpp>
#include <iostream>
#include "arcSegment.h"
#include "meanShift.h"
#include "ellipseSet.h"
#include "utility.h"
#include "groupForming.h"
#include "lsd2OCV.h"


std::string imname(const std::string &name) {
    std::string out;
    for (int i = name.size() - 1; name[i] != '/' && i != 0; --i) {
        out += name[i];
    }

    std::reverse(out.begin(), out.end());
    return out;

}

std::vector<ellipseParameters>
arcSegment(std::string path, double scale, int polarity, double sigma,
           double inliersRatio, double Tr, double distanceTolerance,
           int searchSize, double Tcover, bool show, std::string out) {

    cv::Mat image = cv::imread(path, cv::IMREAD_COLOR);
    if (image.empty()) {
        std::cerr << "img with path: " << path << "doesn't exist"
                  << std::endl;
        return {};
    }
    cv::Mat src, src_gray, phase(image.rows, image.cols, 5,
                                 cv::Scalar::all(0)), modgrad(image.rows,
                                                              image.cols, 5,
                                                              cv::Scalar::all(
                                                                      0));

    src = image;
    if (sigma)
        GaussianBlur(image, src, cv::Size(3, 3), sigma, sigma,
                     cv::BORDER_DEFAULT);
    // Convert the image to grayscale

    cvtColor(src, src_gray, cv::COLOR_BGR2GRAY);

    std::vector<rectangle> rectangles;

    std::vector<std::vector<cv::Point2i >> regions;

    cv::Mat scaled;
    cv::resize(src_gray, scaled,
               cv::Size(src_gray.cols * scale, src_gray.rows * scale),0,0,cv::INTER_AREA);


    //extract arc-segments
    cv::Mat arcs = lsd2OCV(rectangles, scaled, 1.00, regions);


    auto arcSegmentGroups = groupForm(arcs, searchSize, rectangles);


    std::vector<std::vector<int>> groups;
    std::vector<double> coverages;


    for (auto[group, coverage]: arcSegmentGroups) {
        groups.emplace_back(group);
        coverages.emplace_back(coverage);
    }

    int ddepth = CV_32F;
    cv::Mat gradX, gradY, angles;

    cv::Sobel(scaled, gradX, ddepth, 1, 0);
    cv::Sobel(scaled, gradY, ddepth, 0, 1);
    cv::phase(gradX, gradY, angles, false);


    std::vector<ellipseParameters> elSet;


    std::vector<ellipseParameters> ellipses;
    if (!regions.empty() && !groups.empty()) {
        elSet = ellipseSet(regions, rectangles, angles, groups,
                           distanceTolerance, scale, polarity, coverages,
                           inliersRatio, Tr, 0);

        //cluster ellipses
        if (!elSet.empty()) {
            auto centers = clusterCenter(elSet); //cluster by center
            for (std::vector<ellipseParameters> clusteredAngle: centers) { // each center may have ellipses of different orientation
                auto anglesCentroid = clusterAngle(clusteredAngle);
                for (std::vector<ellipseParameters> angled: anglesCentroid) { // each ellipse orientation may have ellipses of different size
                    auto clusteredEllipses = clusterAxis(angled);
                    ellipses.insert(ellipses.end(), clusteredEllipses.begin(),
                                    clusteredEllipses.end());
                }
            }
        }
    }

    //    //display ellipses

    for (auto elipse: ellipses)
        cv::ellipse(image, elipse.center,
                    cv::Size(elipse.major,
                             elipse.minor),
                    elipse.theta * 180 / M_PI, 0, 360,
                    cv::Scalar(0, 150, 0), 3);



    std::string name = out + imname(path);
//    std::string cname = out + "c" + imname(path);
    cv::imwrite(name,image);
//    cv::imwrite(cname,arcs);
    if (show) {
        cv::namedWindow("extractedEllipses");
        cv::imshow("extractedEllipses",image);
        cv::waitKey();
    }

    return ellipses;
}