//
// Created by martin on 16. 2. 2021.
//
//#include <stdio.h>
//#include <set>
#include "DP.h"

// calcs the distance of point x from a line a->b
double dist(cv::Point2i &a, cv::Point2i &b, cv::Point2i &x) {
    // equation for distance of a point from a line when the line is defined by 2 points (a,b)
    return std::abs((b.x - a.x) * (a.y - x.y) - (a.x - x.x) * (b.y - a.y)) /
           std::sqrt(std::pow(b.x - a.x, 2) + std::pow(b.y - a.y, 2));
}



// calculates DP contour
void L_DPR(std::vector<cv::Point2i> &edges, int X, int Y, std::vector<cv::Point2i> &DP) {

    //initialize required variables
    double s, m, fi, fiMax, D_tol;

    // finding D_tol
    s = std::sqrt(std::pow((edges[Y].x - edges[X].x), 2) + std::pow((edges[Y].y - edges[X].y), 2));
    // Problem to be solved (edges[Y].first-edges[X].first) can aquire value of 0 -> division by 0

    if ((edges[Y].x - edges[X].x) == 0) {
        // lim arctan( (y2-y1) / (x2 - x1) = M_PI/2
        fi = M_PI / 2;
    } else {
        m = (static_cast<double>(edges[Y].y) - edges[X].y) / (edges[Y].x - edges[X].x);
        // fi is slope of line -> XY
        fi = atan(m);
    }
    // calc fi_max derived
    fiMax = maxFi(fi, s);
    // equation for D_tol
    D_tol = s * fiMax;

    // finding max distance of a point and the point
    double D_max = -1, distance;
    int P_max = -1;

    for (int i = X + 1; i < Y; i++) {
        distance = dist(edges[X], edges[Y], edges[i]);
        if (distance > D_max) {
            D_max = distance;
            P_max = i;
        }
    }


    // recursive call and adding to the DP
    if (D_max > D_tol) {
        L_DPR(edges, X, P_max, DP);

        //add the furthest point to the DP
        DP.push_back(edges[P_max]);

        L_DPR(edges, P_max, Y, DP);
    }
}

/*
 * calcs one of fi_max derivations
 */
double mFi(double A, double B, double s) {
    // A * (std::abs( std::pow(s,2) - s*b + std::pow(B,2)) / std::pow(s,3);
    return A * std::abs(std::pow(s, 2) - s * B + std::pow(B, 2)) / std::pow(s, 3);
}

/*
 * calcs derived fi max for D_tol
 */
double maxFi(double fi, double s) {
    double A1, A2, B1, B2, B3, B4;
    // A = abs(sin(fi) +- cos(fi)
    A1 = abs(sin(fi) + cos(fi));
    A2 = abs(sin(fi) - cos(fi));

    // B = +-cos(fi) +- sin(fi)
    B1 = cos(fi) + sin(fi);
    B2 = cos(fi) - sin(fi);
    B3 = -cos(fi) + sin(fi);
    B4 = -cos(fi) - sin(fi);
    // fi_max derived =  max (A( |s^2 -s*B + B^2| )/ s^3)
    std::vector<double> vals{mFi(A1, B1, s), mFi(A1, B2, s), mFi(A1, B3, s), mFi(A1, B4, s), mFi(A2, B1, s),
                             mFi(A2, B2, s), mFi(A2, B3, s), mFi(A2, B4, s)};
    return *max_element(vals.begin(), vals.end());

}

std::vector<std::vector<cv::Point2i>> getDP(std::vector<std::vector<cv::Point2i>> &contours) {
    std::vector<std::vector<cv::Point2i>> DP;
    for (std::vector<cv::Point2i> contour: contours) {
        std::vector<cv::Point2i> a_contour;
        a_contour.push_back(contour[0]);
        L_DPR(contour, 0, contour.size() - 1, a_contour);
        a_contour.push_back(contour[contour.size() - 1]);
        DP.emplace_back(a_contour);
    }
    return DP;
}
