#include "operations.h"
#include <stdio.h>
#include <iostream>
#include <stdio.h>
#include <stdlib.h>

int filterDisparity(cv::Mat disparity, cv::Mat* disparityMap, int N) {
    cv::Mat adisp, bdisp, disparityCopy;
    cv::Mat cdisp(disparity.rows, disparity.cols, CV_8UC1);
    cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3), cv::Point(1, 1));
    char q;

    if (N % 2 == 1) {
        printf("N tiene que ser par\n");
        return -1;
    }

    //cv::GaussianBlur(disparity, disparityCopy, cv::Size(3,3), 1, 1, cv::BORDER_DEFAULT);

    cv::imshow("Disparity", disparity);
    //cv::imshow("Gaussian disparity", disparityCopy);
    //cv::waitKey(0);

    for (int u = 0; u < cdisp.cols; u++)
        for (int v = 0; v < cdisp.rows; v++)
            cdisp.at<uchar>(v, u) = 0;


    for (int i = (255 - N / 2); i > (N / 2); i--) {
        cv::threshold(disparity, adisp, (i + N / 2), 255, cv::THRESH_TOZERO_INV);
        cv::threshold(adisp, bdisp, (i - N / 2), 255, cv::THRESH_TOZERO);
        cv::dilate(bdisp, adisp, element);
        cv::dilate(adisp, bdisp, element);
        cv::erode(bdisp, adisp, element);
        cv::erode(adisp, bdisp, element);

        for (int u = 0; u < bdisp.cols; u++)
            for (int v = 0; v < bdisp.rows; v++)
                if (bdisp.at<uchar>(v, u) != 0)
                    cdisp.at<uchar>(v, u) = bdisp.at<uchar>(v, u);
    }

    cv::imshow("pimienta", cdisp);
    *(disparityMap) = cdisp;
    return 1;
}

void elevationMap(cv::Mat scr, cv::Mat* dst, int u_div, int v_div, float z_min, float z_max, float bean_size) {

    int px_width = scr.cols / u_div;
    int px_height = scr.rows / v_div;
    int nBins = (z_max - z_min) / bean_size;

    cv::Mat subImg(px_height, px_width, CV_32FC1);
    cv::Mat subImgDisp(px_height, px_width, CV_8UC1);
    cv::Mat elevationMode(v_div, u_div, CV_32FC1);
    cv::Mat dispImg8U;

    double min, max;
    cv::minMaxLoc(scr, &min, &max, NULL, NULL);
    scr.convertTo(dispImg8U, CV_8UC1, 255 / (max - min));
    //cv::imshow("Disparidad en 8UC1", dispImg8U);

    int channels[] = {0};
    int hisSize[] = {nBins};
    float zRanges[] = {z_min, z_max};
    const float* ranges[] = {zRanges};
    cv::MatND hist;
    double minZ, maxZ;



    for (int u = 0; u < u_div; u++)
        for (int v = 0; v < v_div; v++) {
            scr(cv::Rect(u*px_width, v*px_height, px_width, px_height)).copyTo(subImg); //COPIA DEL AREA (ROI)
            dispImg8U(cv::Rect(u*px_width, v*px_height, px_width, px_height)).copyTo(subImgDisp);

            cv::calcHist(&subImg, 1, channels, subImgDisp, hist, 1, hisSize, ranges, true, false);

            cv::minMaxLoc(hist, NULL, &maxZ, NULL, NULL);
            elevationMode.at<float>(v, u) = maxZ;
        }

    cv::minMaxLoc(elevationMode, &minZ, &maxZ, NULL, NULL);
    //*(dst) = elevationMode; 
    cv::Mat elevationMode8U, elevationModeVis;
    elevationMode.convertTo(elevationMode8U, CV_8UC1, 255 / (maxZ - minZ));
    //            cv::imshow("Elevation Map", elevationMode8U);
    cv::resize(elevationMode8U, elevationModeVis, cv::Size(scr.cols, scr.rows));
    *(dst) = elevationModeVis;
    //            cv::imshow("Elevation ReMap", elevationModeVis);
}

int histogram(cv::Mat scr, float z_min, float z_max, float bean_size, float* z_mode, cv::Mat mask, cv::Mat* histogram) {

    int nBins = (z_max - z_min) / bean_size;
    int channels[] = {0};
    int hisSize[] = {nBins};
    float zRanges[] = {z_min, z_max};
    const float* ranges[] = {zRanges};
    cv::MatND hist;
    double NumZ;
    cv::Point maxZValue;

    cv::calcHist(&scr, 1, channels, mask, hist, 1, hisSize, ranges, true, false);
    cv::minMaxLoc(hist, NULL, &NumZ, NULL, &maxZValue);

    *z_mode = (float) z_min + (maxZValue.y + 1) * bean_size;
    *histogram = hist;

    return NumZ;
}

int gaussianFit(float z_min, cv::Mat hist, float bean_size, float *peak, float *mean, float *variance) {
    float s_xxxx = 0, s_xxx = 0, s_xx = 0, s_x = 0, s_xxy = 0, s_xy = 0, s_y = 0, s_1 = 0;
    float a, b, c;

    // Valores iguales a 0 error en log()
    for (int i = 0; i < hist.rows; i++)
        if (hist.at<float>(i, 0) == 0)
            hist.at<float>(i, 0) = 0.001;

    for (int i = 0; i < hist.rows; i++) {
        float a, b, c, d, e;
        hist.at<float>(i, 0) = log(hist.at<float>(i, 0));

        a = z_min + (i * bean_size);
        b = a * a;
        c = b * a;
        d = c * a;

        s_x = s_x + a;
        s_xx = s_xx + b;
        s_xxx = s_xxx + c;
        s_xxxx = s_xxxx + d;

        e = hist.at<float>(i, 0);

        s_y = s_y + e;
        s_xy = s_xy + (e * a);
        s_xxy = s_xxy + (e * b);
    }

    s_1 = hist.rows;

    cv::Mat matrix(3, 4, CV_32FC1);

    matrix.at<float>(0, 0) = s_xxxx;
    matrix.at<float>(0, 1) = s_xxx;
    matrix.at<float>(0, 2) = s_xx;
    matrix.at<float>(0, 3) = s_xxy;
    matrix.at<float>(1, 0) = s_xxx;
    matrix.at<float>(1, 1) = s_xx;
    matrix.at<float>(1, 2) = s_x;
    matrix.at<float>(1, 3) = s_xy;
    matrix.at<float>(2, 0) = s_xx;
    matrix.at<float>(2, 1) = s_x;
    matrix.at<float>(2, 2) = s_1;
    matrix.at<float>(2, 3) = s_y;

    float factor1 = matrix.at<float>(2, 0);
    float factor2 = matrix.at<float>(1, 0);

    //Diagonalizar matriz
    for (int i = 0; i < matrix.cols; i++) {
        matrix.at<float>(2, i) = matrix.at<float>(2, i) - (matrix.at<float>(0, i) * factor1 / matrix.at<float>(0, 0));
        matrix.at<float>(1, i) = matrix.at<float>(1, i) - (matrix.at<float>(0, i) * factor2 / matrix.at<float>(0, 0));
    }

    float factor3 = matrix.at<float>(2, 1);

    for (int i = 0; i < matrix.cols; i++)
        matrix.at<float>(2, i) = matrix.at<float>(2, i) - (matrix.at<float>(1, i) * factor3 / matrix.at<float>(1, 1));

    if (matrix.at<float>(2, 3) * matrix.at<float>(2, 3) * matrix.at<float>(2, 3) != 0.0) {
        c = matrix.at<float>(2, 3) / matrix.at<float>(2, 2);
        b = (matrix.at<float>(1, 3) - c * matrix.at<float>(1, 2)) / matrix.at<float>(1, 1);
        a = (matrix.at<float>(0, 3) - c * matrix.at<float>(0, 2) - b * matrix.at<float>(0, 1)) / matrix.at<float>(0, 0);

        printf("a: %f\nb: %f\nc: %f\n", a, b, c);

        *peak = exp(c - (b * b) / (4 * a));
        *mean = (-b) / (2 * a);
        *variance = sqrt((-1) / a);
        return 0;
    }
    else {
        *peak = 0;
        *mean = 0;
        *variance = 0;
        return -1;
    }
}

int drawHistAndGaussian(cv::Mat hist, float z_min, float bean_size, float peak, float mean, float variance) {
    double min, max;
    cv::minMaxLoc(hist, &min, &max, NULL, NULL);

    cv::Mat img = cv::Mat::zeros(max, hist.rows, CV_8UC3);

    for (int i = 0; i < img.cols; i++)
        img.at<unsigned char>(((unsigned char) max - hist.at<float>(i, 0)), i, 2) = 255;

    for (float i = z_min; i < z_min + bean_size * hist.rows; i = i + bean_size);

    cv::Mat img_aux;

    cv::resize(img, img_aux, cv::Size(0, 0), 20, 20, cv::INTER_LINEAR);

    cv::imshow("HISTOGRAMA", img_aux);
}