//
// Created by martin on 17. 7. 2021.
//
#include "arcMatrix.h"
#include <iostream>
#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>
#include <eigen3/Eigen/Core>
#include <opencv2/core/types.hpp>
#include <map>


#include "DP.h"
#include "ArcExtraction.h"
#include "AAMBuild.h"
#include "GenerateCombinations.h"
#include "eliFit.h"
#include "ellipseValidation.h"
#include "meanShift.h"

/**
 * gets the path of the image
 * @param path is the path to the image
 * @return name of the file
 */
std::string imname(const std::string &path) {
    std::string out;
    for (int i = path.size() - 1; path[i] != '/' && i != 0; --i) {
        out += path[i];
    }

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

}


/**
 * function takes an image in grey scale, calcs it's histogram and chooses thresholdolds for cany depending on cummulative histogram
 * @param grey is an image in grey scale
 * edges extracted by Canny's edge detector
 */

cv::Mat cummulative_canny(cv::Mat &grey) {
    // getting the histogram
    int histSize = 256;
    float range[] = {0, 256}; //the upper boundary is exclusive
    const float *histRange = {range};
    cv::Mat dx, dy, out;

    cv::Sobel(grey, dx, CV_32F, 1, 0);
    cv::Sobel(grey, dy, CV_32F, 0, 1);

    cv::magnitude(dx, dy, out);

    cv::Mat hist;
    cv::calcHist(&out, 1, 0, cv::Mat(), hist, 1, &histSize, &histRange);
    // calcing the upper thresholdold for canny
    double upper = grey.rows * grey.cols * 0.9;
    int sum = 0;
    int i = 0;
    for (; i < 256; i++) {
        sum += hist.at<float>(i);
        if (sum >= upper) {
            break;
        }
    }

    // lower thresholdold for canny
    double lower = i * 0.3;
    cv::Mat edges;
    cv::Canny(grey, edges, lower, i);
    return edges;
}


std::vector<ellipseParameters>
arcMatrix(const std::string &path, double phi_arc,
          double lambda_arc, double tresh, double sigma,
          double scale,bool show, const std::string &outputPath) {

    cv::Mat img, scaled;
    img = cv::imread(path);
//    check if img was loaded properly
    if (img.empty()) {
        std::cerr << "image with path: " << path << " was not loaded properly";
        return {};
    }


    scaled = img;

    if (abs(scale - 1.00) >= 10e-6)
        cv::resize(img, scaled, cv::Size(img.cols * scale, img.rows * scale), 0,
                   0, cv::INTER_AREA);


    cv::Mat blurredIM;
    cv::GaussianBlur(scaled, blurredIM, cv::Size(5, 5), sigma, sigma);

    cv::Mat grey;
    cv::cvtColor(blurredIM, grey, cv::COLOR_BGR2GRAY);

    cv::Mat edges = cummulative_canny(grey);

    std::vector<std::vector<cv::Point2i>> contours;
    cv::Mat hierarchy;

    cv::findContours(edges, contours, hierarchy, cv::RETR_EXTERNAL,
                     cv::CHAIN_APPROX_NONE);


    //get DP contour
    std::vector<std::vector<cv::Point2i>> DP = getDP(contours);

    auto arcs = ArcExtract(DP, phi_arc, lambda_arc);

    if (arcs.empty()) {
        std::cerr << "no arcs found" << std::endl;
        return {};
    }

    //build Arc Adjacency matrix
    std::vector<posArc> arc_index;
    std::vector<std::vector<int>> ArcAdjacencyMatrix = AAM_Build(arcs,
                                                                 arc_index,
                                                                 lambda_arc);

    //generate all ellipse combinations
    using arc = std::vector<cv::Point2i>;
    auto combinations = BCV(arcs, ArcAdjacencyMatrix);

    // calculate individual arc matrices
    std::map<int, Eigen::Matrix<double, 6, 6>> matrices;
    for (int i = 0; i < arc_index.size(); i++) {
        Eigen::Matrix<double, 6, 6> scatterMatrix = cumulativeMatrix(
                arc_index[i].arc);
        matrices.emplace(i, scatterMatrix);
    }

    // fit and verify ellipses
    std::vector<ellipseParameters> candidates;
    for (auto ark: combinations) {
        Eigen::Matrix<double, 6, 1> sol;
        sol = fit(ark, matrices);
        if (sol.isZero())
            continue;
        auto params = ellipse_equation(sol);
        double src = score(sol, params, edges, phi_arc);
        if (src > tresh)
            candidates.emplace_back(params);
    }

    //cluster ellipses
    std::vector<ellipseParameters> ellipses = clusterAll(candidates);

    for (auto elipse: ellipses)
        cv::ellipse(img, elipse.center / scale,
                    cv::Size(elipse.major / scale,
                             elipse.minor / scale),
                    elipse.theta * 180 / M_PI, 0, 360,
                    cv::Scalar(0, 255, 0), 2);


    //write the output
    std::string name = outputPath + imname(path);
    cv::imwrite(name, img);
    if (show) {
        cv::namedWindow("extractedEllipses");
        cv::imshow("extractedEllipses",img);
        cv::waitKey();
    }

    return ellipses;
}