#include "FM0PC.h"

namespace FM
{

double FM0PC::computeDistance(int idx, int idx1)
{
    npoint3 pt0, pt1;
    double distance;

    PC->getNode(idx,pt0);
    PC->getNode(idx1,pt1);

    npoint3 vec(pt0-pt1);

    distance = vec.normalize();

    if(curvCorrection) distance = distanceCurvatureCorrection(idx,vec,distance);

    return distance + data->getDistance(idx1);
}

resultComputation FM0PC::computeFM(int idx, int idx1)
{
    resultComputation results;

    results.distance = computeDistance(idx,idx1);

    return results;
}

double FM0PC::interpolate(npoint3 &pt)
{
    double distance, tempDist;
    bool first = true;
    
    std::vector<std::pair<int,double>> closest;
    int nbrNeig = PC->getNbrNeighbour();
    PC->findKClosest(pt,closest,nbrNeig*2,nbrNeig,0.);

    distance = data->getDistance(closest[0].first) + closest[0].second;

    for(const std::pair<int,double>& pt : closest){
        tempDist = pt.second + data->getDistance(pt.first);
        if(tempDist < distance || first){
            distance = tempDist;
            first = false;
        } 
    }

    return distance;
}

}