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

#include "AAMBuild.h"
#include <iostream>

double D(std::vector<cv::Point2i> A, std::vector<cv::Point2i> B) {
    return ArcVector(B[0], A[A.size() - 1]).size();
}

double T(std::vector<std::vector<cv::Point2i>> &arcs, int i, int k) {
    double I = ArcVector(arcs[i][arcs[i].size() - 1], arcs[i][arcs[i].size() - 2]).size();
    double K = ArcVector(arcs[k][1], arcs[k][0]).size();
    return I < K ? I : K;
}



std::vector<std::vector<int>> AAM_Build(std::vector<std::vector<cv::Point2i>> arcs, std::vector<posArc>& arc_index,const double& lambda_arc) {
//    //build kd-tree

    for (int i = 0; i < arcs.size(); i++) {
        arc_index.push_back(mkArc(i, arcs[i]));
    }

    std::vector<std::vector<int>> AAM(arcs.size(),std::vector<int>(arcs.size(), 0));
    // make KDTree
    KDTree tree;
    tree.setRoot(std::make_unique<KDNode>());
    KDBuild(arc_index, tree.getRoot(),0,0,arc_index.size());

    //sort Arcs by index
    std::sort(arc_index.begin(), arc_index.end(), [](const posArc &lhs, const posArc &rhs) {
        return lhs.index < rhs.index;
    });
    // set up the matrix

    for (int i = 0; i < arcs.size(); i++) {


        //search kd tree for closest arcs where D(i,k) < D(i,i)
        std::vector<int> indexes;
        findNear(tree.getRoot(), arc_index[i], 0, D(arc_index[i].arc, arc_index[i].arc),arcs, indexes);
        for (int k :indexes) {
            if (k == i) {
                AAM[i][i] = 1;
                continue;;
            }

            //// implementation of cases 1-4 from the algorithm 2 from the paper

            if (D(arcs[i], arcs[k]) >= T(arcs, i, k) && D(arcs[k], arcs[i]) >= T(arcs, k, i)) {
                //case1
                //reg(i,k,false) false -> checks point arcs[k][0] true -> checks arcs[k][end]


                if (RegionConstraint(arcs[i], arcs[k], false) &&
                    RegionConstraint(arcs[k], arcs[i], true) &&
                    RegionConstraint(arcs[i], arcs[k], true) &&
                    RegionConstraint(arcs[k], arcs[i], false)) {
                    AAM[i][k] = 1;
                } else {
                    AAM[i][k] = AAM[k][i] = -1;
                }
            }
            if (D(arcs[i], arcs[k]) >= T(arcs, i, k) && D(arcs[k], arcs[i]) < T(arcs, k, i)) {
                //case 2

                // check whether arc_k[0] satisfies arc_i and arc_i[end] satisfies arc_k
                if (RegionConstraint(arcs[i], arcs[k], false) &&
                    RegionConstraint(arcs[k], arcs[i], true)) {
                    AAM[i][k] = 1;
                } else {
                    AAM[i][k] = AAM[k][i] = -1;
                }
            }
            if (D(arcs[i], arcs[k]) < T(arcs, i, k) && D(arcs[k], arcs[i]) >= T(arcs, k, i)) {
                //case 3


                if (CurveCons(arcs[i], arcs[k], lambda_arc) && CurveCons(arcs[k], arcs[i], lambda_arc) &&
                        RegionConstraint(arcs[i], arcs[k], true) &&
                    RegionConstraint(arcs[k], arcs[i], false)) {
                    AAM[i][k] = 1;
                }
                else {
                    AAM[i][k] = AAM[k][i] = -1;
                }
            }
            if (D(arcs[i], arcs[k]) < T(arcs, i, k) && D(arcs[k], arcs[i]) < T(arcs, k, i)) {
                //case 4
                if (CurveCons(arcs[i], arcs[k], lambda_arc) &&
                        RegionConstraint(arcs[i], arcs[k], true) &&
                    RegionConstraint(arcs[k], arcs[i], false)) {
                    AAM[i][k] = 1;
                }
                else{
                    AAM[i][k] = AAM[k][i] = -1;
                }
            }
        }
    }
    return AAM;
}

// determines whether the point M is to the left or right of a line  A->B
int side(cv::Point2i A, cv::Point2i B, cv::Point2i M) {
    return sgn((B.x - A.x) * (M.y - A.y) - (B.y - A.y) * (M.x - A.x));
}
//position = sign((Bx - Ax) * (Y - Ay) - (By - Ay) * (X - Ax))

// calcs whether the arc2 is within the search region region
bool RegionConstraint(std::vector<cv::Point2i> &arc1, std::vector<cv::Point2i> &arc2, bool s) {
    cv::Point2i middle;
    for (cv::Point2i point : arc1) {
        middle += point;
    }
    middle.x /= arc1.size();
    middle.y /= arc1.size();
    int size = arc1.size();
    // side of the point to line arc1[1] and [0]
    int c1 = side(arc1[1], arc1[0], arc2[(arc2.size() - 1) * s]);
    int mid1 = side(arc1[1], arc1[0], middle);
    //side of the point to line arc1[-2] and [-1]
    int c2 = side(arc1[size - 2], arc1[size - 1], arc2[(arc2.size() - 1) * s]);
    int mid2 = side(arc1[size - 2], arc1[size - 1], middle);
    //side of the point to line arc1[0] and [-1]
    int c3 = side(arc1[0], arc1[size - 1], arc2[(arc2.size() - 1) * s]);
    int mid3 = side(arc1[0], arc1[size - 1], middle);
    // if the next logical equation is true then the arc2 is in the search region of arc1
    return c1 == mid1 && c2 == mid2 && c3 != mid3;
}

// calcs whether the arcs satisfy the curvature constraint
bool CurveCons(std::vector<cv::Point2i> &arc1, std::vector<cv::Point2i> &arc2, double L_arc) {
    int size = arc1.size();
    cv::Point2i mid = cv::Point((arc1[size - 1].x + arc2[0].x) / 2, (arc1[size - 1].y + arc2[0].y) / 2);
    ArcVector vk = ArcVector(arc1[size - 2], mid);
    ArcVector vk1 = ArcVector(mid, arc2[1]);
    double t = eq3(eta(vk, vk1, -1), L_arc)[0];
    double p = eq3(eta(vk, vk1, -1), L_arc)[1];
    double u = sqrt(pow(t, 2) + pow(p, 2));
    if (t > 0 && t > 0 && (1 / L_arc < L_arc ? 1 / L_arc < u < L_arc : L_arc < u < 1 / L_arc)) {
        return true;
    }
    return false;
}