//
// Created by Martin Gargalovič on 9. 11. 2021.
//

#include <opencv2/imgcodecs.hpp>

#include "lsd2OCV.h"
#include "lsd.hpp"
#include "utility.h"



double *ParametrizedLSD(int *n_out,
                        double *img, int X, int Y, double scale,
                        int **reg_img, int *reg_x, int *reg_y, std::vector<std::vector<cv::Point2i>> & lines ) {
    /* LSD parameters */
    double sigma_scale = 0.6; /* Sigma for Gaussian filter is computed as
                                sigma = sigma_scale/scale.                    */
    double quant = 2.0;       /* Bound to the quantization error on the
                                gradient norm.                                */
    double ang_th = 22.5;     /* Gradient angle tolerance in degrees.           */
    double log_eps = 0.0;     /* Detection threshold: -log10(NFA) > log_eps     */
    double density_th = 0.7;  /* Minimal density of region points in rectangle. */
    int n_bins = 1024;        /* Number of bins in pseudo-ordering of gradient
                               modulus.                                       */


    return LineSegmentDetection(n_out, img, X, Y, scale, sigma_scale, quant,
                                ang_th, log_eps, density_th, n_bins,
                                reg_img, reg_x, reg_y,lines);
}


cv::Mat lsd2OCV(std::vector<rectangle> & rectangles,cv::Mat & grey,double scale,std::vector<std::vector<cv::Point2i>> & regions) {
    int n;
    int *reg;
    int reg_x,reg_y;
    int width,height;

    //converting uchar grey scale img to double for use with lsd
    cv::Mat greyDouble;
    grey.convertTo(greyDouble, CV_64F);

    double * data = greyDouble.ptr<double>(0);

    width = grey.cols;
    height = grey.rows;
//    double* out= PLSD(&n, data, width, height, &reg, &reg_x, &reg_y, regions, scale);
    double* out= ParametrizedLSD(&n, data, width, height, scale, &reg, &reg_x,
                                 &reg_y, regions);

    for (int i = 0; i < n;i++) {
        rectangle rect;
        rect.x1 = out[i*8+0];
        rect.y1 = out[i*8+1];
        rect.x2 = out[i*8+2];
        rect.y2 = out[i*8+3];
        rect.dx = out[i*8+4];
        rect.dy = out[i*8+5];
        rect.theta = atan2(rect.dy,rect.dx);
        rect.width = out[i*8+6];
        rect.polarity = out[i*8+7];
        rect.x = (rect.x1+rect.x2)/2;
        rect.y = (rect.y1+rect.y2)/2;
        rectangles.emplace_back(rect);
    }

    cv::Mat workImage(reg_y,reg_x,5,cv::Scalar::all(0));
    for(int y = 0; y < reg_y; y++) {
        for (int x = 0; x < reg_x; x++) {
            workImage.at<float>(cv::Point2i(x,y)) =reg[x+y*reg_x];
        }
    }
    free((int*)reg);
    return workImage;
}