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

#include "KDTree.h"


void KDBuild(std::vector<posArc> &arcs,KDNode* node, int depth, int left, int right) {

    // sort specified part of the array depending on depth (working axis)
    if (depth%2) {
        std::sort(arcs.begin()+left,arcs.begin()+right,[](const posArc& left,const posArc& right) {
            return left.arc[0].y < right.arc[0] .y;
        });
    }
    else {
        std::sort(arcs.begin() + left, arcs.begin() + right, [](posArc &left, posArc &right) {
            return left.arc[0].x < right.arc[0].x;
        });
    }
    // set index
    int mid = left+(right-left)/2;
    node->setIndex(arcs[mid]);
    node->right = nullptr;
    node->left = nullptr;

    //if there is still node to the left to work on
    if (left <= mid-1) {
        node->left = std::make_unique<KDNode>();
        KDBuild(arcs,node->left.get(),depth+1,left,mid);
    }

    //if there is still a node to the right to work on
    if (mid+1 < right) {
        node->right = std::make_unique<KDNode>();
        KDBuild(arcs,node->right.get(),depth+1,mid+1,right);
    }

}


// dist = D(i,i)
void findNear(KDNode *node, posArc arc, int depth, double dist,const std::vector<std::vector<cv::Point2i>> &arcs , std::vector<int>& arcIndexes) {
    if (node == nullptr) {
        return;
    }

    if (D(arc.arc,arcs[node->getIndex()]) <= dist)  {
        arcIndexes.emplace_back(node->getIndex());
    }



    if (depth%2) {
        // compare y values
        if (arc.arc[0].y > arcs[node->getIndex()][0].y) {
            findNear(node->right.get(),arc,depth+1,dist,arcs,arcIndexes);
            if (abs(arc.arc[0].y - arcs[node->getIndex()][0].y) < dist) {
                findNear(node->left.get(),arc,depth+1,dist,arcs,arcIndexes);
            }
        }
        else {
            findNear(node->left.get(),arc,depth+1,dist,arcs,arcIndexes);
            if (abs(arc.arc[0].y - arcs[node->getIndex()][0].y) < dist) {
                findNear(node->right.get(),arc,depth+1,dist,arcs,arcIndexes);
            }
        }
    }
    else{
        //compare x values
        if (arc.arc[0].x > arcs[node->getIndex()][0].x) {
            findNear(node->right.get(),arc,depth+1,dist,arcs,arcIndexes);
            if (abs(arc.arc[0].x - arcs[node->getIndex()][0].x) < dist) {
                findNear(node->left.get(),arc,depth+1,dist,arcs,arcIndexes);
            }
        }
        else {
            findNear(node->left.get(),arc,depth+1,dist,arcs,arcIndexes);
            if (abs(arc.arc[0].x - arcs[node->getIndex()][0].x) < dist) {
                findNear(node->right.get(),arc,depth+1,dist,arcs,arcIndexes);
            }
        }
    }

}