#include "utility.hpp"

#include <filesystem>
#include <fstream>
#include <nlohmann/json.hpp>

using namespace cv;
using namespace std;
namespace fs = std::filesystem;

using json = nlohmann::json;

void utility::detect_shape(Ptr<GeneralizedHoughGuil> alg, int pos_thresh, vector<Vec4f>& positive_positions,
                           Mat& positive_votes, vector<Vec4f>& negative_positions, Mat& negative_votes,
                           Mat processing_area) {
    alg->setPosThresh(pos_thresh);

    int num_prev_matches = 0;
    int threshold_increment = 0;
    int max_match_score = 0;

    // Process shapes with positive angles
    alg->setMinAngle(0);
    alg->setMaxAngle(3);
    while (true) {
        alg->detect(processing_area, positive_positions, positive_votes);
        int current_matches = positive_positions.size();
        if (current_matches == 1 || (current_matches == 0 && num_prev_matches == 0)) {
            // We detected the most interesting shape
            // Impossible to find with these parameters
            break;
        } else if (current_matches == 0 && num_prev_matches > 0) {
            // It is not possible to detect only one shape with the current
            // parameters
            alg->setPosThresh(pos_thresh + threshold_increment - 1);  // Decrease position value
            alg->detect(processing_area, positive_positions,
                        positive_votes);  // Detect all available shapes
            break;
        }
        num_prev_matches = current_matches;
        // Find maximum vote
        for (int j = 0; j < positive_votes.cols / 3; j++) {
            if (positive_votes.at<int>(3 * j) > max_match_score) max_match_score = positive_votes.at<int>(3 * j);
        }

        if (current_matches > 10) {
            threshold_increment += 5;  // To speed up computation when there are too many matches
        } else if (max_match_score - (pos_thresh + threshold_increment) > 100) {
            threshold_increment += 100;  // To speed up computation when there are few super high
                                         // matches
        } else {
            threshold_increment++;
        }
        alg->setPosThresh(pos_thresh + threshold_increment);
    }

    // Reset incremental position value
    threshold_increment = 0;
    num_prev_matches = 0;
    max_match_score = 0;
    // Process shapes with negative angles
    alg->setMinAngle(357);
    alg->setMaxAngle(360);
    while (true) {
        alg->detect(processing_area, negative_positions, negative_votes);
        int current_matches = negative_positions.size();
        if (current_matches == 1 || (current_matches == 0 && num_prev_matches == 0)) {
            // We detected the most interesting shape
            // Impossible to found with these parameters
            break;
        } else if (current_matches == 0 && num_prev_matches > 0) {
            // It is not possible to detect only one shape with the current
            // parameters
            alg->setPosThresh(pos_thresh + threshold_increment - 1);  // Decrease position value
            alg->detect(processing_area, negative_positions,
                        negative_votes);  // Detect all available shapes
            break;
        }
        num_prev_matches = current_matches;

        // Find maximum vote
        for (int j = 0; j < positive_votes.cols / 3; j++) {
            if (positive_votes.at<int>(3 * j) > max_match_score) max_match_score = positive_votes.at<int>(3 * j);
        }

        if (current_matches > 10) {
            threshold_increment += 5;  // To speed up computation when there are too many matches
        } else if (max_match_score - (pos_thresh + threshold_increment) > 100) {
            threshold_increment += 100;  // To speed up computation when there are few super high
                                         // matches
        } else {
            threshold_increment++;
        }
        alg->setPosThresh(pos_thresh + threshold_increment);
    }
}

RotatedRect utility::drawShapes(Mat frame, const Vec4f& positions, Scalar color, int width, int height, int offsetX,
                                int offsetY, float processingScale) {
    RotatedRect rr;
    Point2f rrpts[4];

    Point2f pos(positions[0] + offsetX, positions[1] + offsetY);
    float scale = positions[2];
    float angle = positions[3];

    rr.center = pos * processingScale;
    rr.size = Size2f(width * scale * processingScale, height * scale * processingScale);
    rr.angle = angle;

    rr.points(rrpts);

    line(frame, rrpts[0], rrpts[1], color, 2);
    line(frame, rrpts[1], rrpts[2], color, 2);
    line(frame, rrpts[2], rrpts[3], color, 2);
    line(frame, rrpts[3], rrpts[0], color, 2);

    return rr;
}

Threshold::Threshold(float percentual, int angle, int scale, int pos) {
    if (percentual < 0 || percentual > 100) throw std::invalid_argument("Percentual must be between 0 and 100");

    this->percentual = percentual;
    this->angle = angle;
    this->scale = scale;
    this->pos = pos;
}

SceneObject::SceneObject(int minDist, Threshold threshold) {
    this->minDist = minDist;
    this->threshold = threshold;
}

SceneObject SceneObject::from_file(fs::path path, ROI obj) {
    ifstream iConfig(path);
    json j;
    iConfig >> j;

    if (obj == ROI::TAPE) {
        return SceneObject(j["MinDist"],
                           Threshold(j["TapeThresholdPercentual"], j["AngleThresh"], j["ScaleThresh"], j["PosThresh"]));
    } else {
        return SceneObject(j["MinDistCapstan"], Threshold(j["CapstanThresholdPercentual"], j["AngleThreshCapstan"],
                                                          j["ScaleThreshCapstan"], j["PosThreshCapstan"]));
    }
}