//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Copyright (c) 2015, University of Osnabrueck                                                                         //
//   All rights reserved.                                                                                               //
//                                                                                                                      //
//   Redistribution and use in source and binary forms, with or without modification, are permitted provided that the   //
//   following conditions are met:                                                                                      //
//                                                                                                                      //
//   1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following//
//       disclaimer.                                                                                                    //
//                                                                                                                      //
//   2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the       //
//       following disclaimer in the documentation and/or other materials provided with the distribution.               //
//                                                                                                                      //
//   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, //
//   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE  //
//   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, //
//   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR    //
//   SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,  //
//   WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE   //
//   USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                           //
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Class for RaLaNS calculation with Lists

#include "ralans-list.h"

RaLaNSList::RaLaNSList(){
	this->constructed = false;

}

RaLaNSList::RaLaNSList(const std::vector< std::vector<double> > &points, RaLaNSAlone *ralansAlone){

	DEBUG_MSG("set Points");
	this->pa = this->PointsToANNPointArray(points);

	DEBUG_MSG("init tree");
	this->tree = new ANNkd_tree(pa, points.size(), LENPOINT);

	DEBUG_MSG("set ralans pointer");
	this->ralansAlone = ralansAlone;

	this->constructed = true;
}

RaLaNSList::~RaLaNSList(){

	if(constructed){

		for (int i = 0; i < this->tree->nPoints(); i++) {

			delete [] this->pa[i];	
		}
		delete [] this->pa;
		delete this->tree;
		annClose();
	}
}

ANNpoint RaLaNSList::PointVecToANNPoint(const std::vector<double> &point){

	ANNcoord *annCoord = new ANNcoord[LENPOINT];
	annCoord[0] = point[0];
	annCoord[1] = point[1];
	annCoord[2] = point[2];

	return annCoord;
}

ANNpointArray RaLaNSList::PointsToANNPointArray(const std::vector< std::vector<double> > &points){

	ANNcoord **annCoord = new ANNcoord*[points.size()];
	for (unsigned long i = 0; i < points.size(); i++) {
		annCoord[i] = this->PointVecToANNPoint(points[i]);
	}

	return annCoord;
}

void RaLaNSList::calcNearestPos(PointVec point, int num, ANNidxArray &nnAry, ANNdistArray &dists){

	double arr[LENPOINT];                                                                                           
	std::copy(point.begin(), point.end(), arr); 
	ANNpoint p = &arr[0];
	DEBUG_MSG("annkSearch");
	this->tree->annkSearch(p, num, nnAry, dists, 0.0);
}

int RaLaNSList::calcNearestPos(PointVec point){

	ANNidxArray nnAry = new ANNidx[1];
	ANNdistArray dists = new ANNdist[1];
	this->calcNearestPos(point, 1, nnAry, dists);
	DEBUG_MSG("set index");
	int index = nnAry[0];
	DEBUG_MSG("delete nnAry");
	delete [] nnAry;
	delete [] dists;
	return index;
}

//TODO decide the way of calculation
PosAWeightVec RaLaNSList::getInterpolPoints(PointVec point){


	int interpolPointsNum = this->ralansAlone->GetListInterpolationNum();

	ANNidxArray nnAry = new ANNidx[interpolPointsNum];
	ANNdistArray dists = new ANNdist[interpolPointsNum];
	this->calcNearestPos(point, interpolPointsNum, nnAry, dists);

	PosAWeightVec vec = this->makeInterpolPointsVec(nnAry, dists, interpolPointsNum);
	delete [] nnAry;
	delete [] dists;
	return vec;
}

PosAWeightVec RaLaNSList::makeInterpolPointsVec(ANNidxArray nnAry, ANNdistArray dists, int num){

	//check if 1 interpolation point(point in nnAry) matches perfectly(dists = 0)
	//if that is the case the point will be the only one for the interpolation.
	int ifExactIndex = -1;
	double div = 0;
	for (int i = 0; i < num; i++) {

		if(dists[i] == 0){

			ifExactIndex = nnAry[i];
		} else {

			div += dists[0] / dists[i];
		}
	}

	if(ifExactIndex != -1 ){

		//make interpolation point, if 1 points distance was 0.
		PosAWeightVec vec(1);
		vec[0] = PosAWeight(ifExactIndex, 1);
		return vec;
	} else {

		//make all interpolation with a weight corresponding to their distance
		
		//the basisweight to have all weights adding up to 1
		double basisWeight = 1 / div;

		PosAWeightVec vec(num);

		for (int i = 0; i < num; i++) {
			double weight = basisWeight * dists[0] / dists[i];
			vec[i] = PosAWeight(nnAry[i], weight);
		}

		return vec;
	}
}
