//
// Created by martin on 18. 2. 2021.
//

#include "ArcExtraction.h"

using std::sqrt;
using std::pow;


posArc mkArc(int index, std::vector<cv::Point2i> &arc) {
    posArc a;
    a.index = index;
    a.arc = arc;
    return a;
}

ArcVector::ArcVector(const cv::Point2i& A, const cv::Point2i& B) {
    x = B.x - A.x;
    y = B.y - A.y;
}


double ArcVector::scalProd(const ArcVector &other) const {
    return (x * other.y) - (y * other.x);
}

double ArcVector::dotProd(const ArcVector &other) const {
    return x * other.x + y * other.y;
}

double ArcVector::size() const{
    return sqrt(pow(x, 2) + pow(y, 2));
}


int sgn(double val) {
    return (0 < val) - (val < 0);
}


std::vector<double> eta(ArcVector k, ArcVector k1, int s) {
    std::vector<double> v;
    // dot product of 2 vectors
    v.push_back(k.x* k1.x + k.y *k1.y);
    //cross product of 2 vectors

    v.push_back(s*k.scalProd(k1));
    //dividing the vector by |Vk|^2
    v[0] /= pow(k.size(), 2);
    v[1] /= pow(k.size(), 2);
    return v;
}


double cot(double i) {
    return cos(i) / sin(i);
}

double csc(double i) {
    return 1 / std::sin(i);
}


std::vector<double> eq3(std::vector<double> e, double O_arc) {
    std::vector<double> vals;
    vals.push_back(e[0] - cot(O_arc)*e[1]);
    vals.push_back(csc(O_arc)*e[1]);
    return vals;
}

void res(std::vector<cv::Point2i> &E, std::vector<cv::Point2i> &DP, int k) {
    E.push_back(DP[k - 1]);
    E.push_back(DP[k]);
    E.push_back(DP[k + 1]);
}

/*
 * @param DP is DP contour
 * @param O_arc is a threshold value
 * @param L_arc is a threshold value
 * doesn't say what the value of these thresholds is supposed to be.
 */
// algorithm1
std::vector<std::vector<cv::Point2i>> ArcExtract(std::vector<std::vector<cv::Point2i>> &DP, double O_arc, double L_arc) {
    std::vector<std::vector<cv::Point2i>> R;
    if (L_arc < 1 / L_arc) {
        L_arc = 1 / L_arc;
    }

    for (auto contour : DP)
    {
        for (int k = 1; k < contour.size() - 1; k++) { //may change to while cycle
            std::vector<cv::Point2i> E = {};
            ArcVector V_k(contour[k - 1], contour[k]);
            ArcVector V_k1(contour[k], contour[k + 1]);
            int s = sgn(V_k.scalProd(V_k1));
            std::vector<double> vectEta = eta(V_k, V_k1, s);
            double t = eq3(vectEta, O_arc)[0];
            double p = eq3(vectEta, O_arc)[1];
            double u = sqrt(pow(t, 2) + pow(p, 2));
            if (p <= 0 || t <= 0 || !((1 / L_arc) < u && u < L_arc)) {
                continue;
            }
            res(E, contour, k);
            k++;
            for (int j = k; j < contour.size() - 1; j++) { //may change to while cycle
                //            std::vector<cv::Point2i> E;

                ArcVector V_j(contour[j - 1], contour[j]);
                ArcVector V_j1(contour[j], contour[j + 1]);
                s = sgn(V_j.scalProd(V_j1));
                // algorithm sais get t,p with V_k,V_k1 but it doesn't make sense to calc it a second time so i use V_j
                t = eq3(eta(V_j, V_j1, s), O_arc)[0];
                p = eq3(eta(V_j, V_j1, s), O_arc)[1];
                u = sqrt(pow(t, 2) + pow(p, 2));
                //(p > 0 && t > 0 &&  (1/ L_arc < L_arc? 1 / L_arc <  u < L_arc : L_arc <  u < 1/L_arc))

                //(p <= 0 || t <= 0 || !(1/ L_arc < L_arc? 1 / L_arc <  u < L_arc : L_arc <  u < 1/L_arc))
                if (p > 0 && t > 0 && (1 / L_arc) < u && u < L_arc) {
                    E.push_back(contour[j + 1]);
                    k = j;
                    continue;
                } else {
                    break;
                }
            }
            if (E.size() > 3) {
                R.push_back(E);
            }
        }
    }
    return R;
}






