//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// 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.                                           //
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
#include "ralans-alone.h"
#include <cstdlib>
#include <cmath>
#include <iostream>
#include <vector>
#include <fstream>
#include <sstream>
#include <iterator>
#include <string>
#include <math.h>
//include boost for checking file extension
//#include <boost/filesystem.hpp>
//for using pairs
#include <utility>
//for handling zip files
//#include <zip.h>
//for using time stamps
#include <sys/time.h>


RaLaNSAlone::RaLaNSAlone ()
{
	this->initValues();
}

RaLaNSAlone::RaLaNSAlone (std::string filename)
{
	this->construct(filename, THRESHOLD, NOSIGNAL, INTERPOLATE, BADNUM, LISTINTERPOLATIONNUM);
}

RaLaNSAlone::RaLaNSAlone (std::string filename, double threshold, double noSignal, bool interpolate, int badNum, 
		int listInterpolationNum) {
	this->construct(filename, threshold, noSignal, interpolate, badNum, listInterpolationNum);
}

RaLaNSAlone::~RaLaNSAlone ()
{

}

bool RaLaNSAlone::construct(std::string filename, double threshold, double noSignal, bool interpolate, int badNum, 
		int listInterpolationNum) {

	this->initValues();
	this->SetThreshold(threshold);
	this->SetNoSignal(noSignal);
	this->SetInterpolate(interpolate);
	this->SetBadNum(badNum);
	this->SetListInterpolationNum(listInterpolationNum);
	return this->SetFile(filename);
}

void RaLaNSAlone::debugParams(){
	DEBUG_MSG("filename= " << filename);
	DEBUG_MSG("badNum= " << badNum);
	DEBUG_MSG("noSignal= " << noSignal);
	DEBUG_MSG("getthreshold= " << GetThreshold());
	DEBUG_MSG("getInterpolate= " << GetInterpolate());
	DEBUG_MSG("GetListInterpolationNum= " << GetListInterpolationNum());
}

void RaLaNSAlone::initValues()
{
	this->len.reserve(2 * LENPOINT);
	this->step.reserve(2 * LENPOINT);
	this->listCalc.reserve(2);
}
bool RaLaNSAlone::GetInterpolate() const{
	return this->interpolate;
}

void RaLaNSAlone::SetInterpolate(bool interpolate){
	this->interpolate = interpolate;
}

double RaLaNSAlone::GetNoSignal() const{
	return this->noSignal;
}

void RaLaNSAlone::SetNoSignal(double noSignal){
	this->noSignal = noSignal;
}

double RaLaNSAlone::GetThreshold() const{
	return sqrt(threshold);
}

void RaLaNSAlone::SetThreshold(double threshold){
	if(threshold <= 0){
		this->threshold = 0;
	} else {
		this->threshold = std::pow(threshold, 2);
	}
}

int RaLaNSAlone::GetListInterpolationNum() const{
	return this->listInterpolationNum;
}

void RaLaNSAlone::SetListInterpolationNum(int listInterpolationNum){
	if(listInterpolationNum < 1){

		this->listInterpolationNum = 1;
	} else {
		this->listInterpolationNum = listInterpolationNum;	
	}
}
void RaLaNSAlone::SetBadNum(int badNum){
	this->badNum = badNum;
}

int RaLaNSAlone::GetBadNum() const{
	return this->badNum;
}

long int GetTime(){

	struct timeval tp;
	gettimeofday(&tp, NULL);
	long int ms = tp.tv_sec * 1000 + tp.tv_usec / 1000;

	return ms;
}

/**
 * \brief sums up two vectors
 *
 * calculates the sum of the 2 given vectors, which must be of size 3.
 * 
 * \param vec1 the 1st summand 
 * 
 * \param vec2 the 2nd summand 
 * 
 * \return the sum of the 2 vectors
 */
std::vector<double> sum (const std::vector<double> vec1, const std::vector<double> vec2) {

	std::vector<double> result(LENPOINT);	

	for(std::vector<double>::size_type i = 0; i < vec1.size(); i++){
		result[i] = vec1[i] + vec2[i];
	}

	return result;
}


/**
 * \brief calculates the length of the vector
 *
 * calculates the euclidean norm of the given vector and returns it as a double
 * 
 * \param vec the vector, whose length will be calculated
 * 
 * \return the length of the vector
 */
double vecLen(const std::vector<double> vec){
	return sqrt(vec[XAXIS] * vec[XAXIS]+ vec[YAXIS] * vec[YAXIS] + vec[ZAXIS] * vec[ZAXIS]);
}

template <class T>
std::string to_string(T const &value) {
	std::ostringstream os;
	os << value;
	return os.str();
}

std::string RaLaNSAlone::printInterpolPoints(
		const std::vector< std::pair<int, double> > &interpolPoints, 
		const std::vector< std::vector<double> > &points) const{

	DEBUG_MSG("Start printing");
	double totWeight = 0;
	double totRealWeight = 0;
	double totNoSignalWeight = 0;
	std::string res = "";
	for(int i = 0, end = interpolPoints.size(); i != end; i++){

		int pos = interpolPoints[i].first;
		res += "    " + to_string(i) + " pos=" + to_string(pos) + " weight=" 
			+ to_string(interpolPoints[i].second) + "\n";
		DEBUG_MSG("pos: " << pos << " vs. size: " << points.size());
		if(pos >= 0){
			res+="TEST: Vec: pos: " + to_string(pos) 
				+ ", x:" + to_string(points[pos][XAXIS])
				+ ", y:" + to_string(points[pos][YAXIS])
				+ ", z:" + to_string(points[pos][ZAXIS]) + "\n";
			totRealWeight += interpolPoints[i].second;
		} else {

			totNoSignalWeight += interpolPoints[i].second;
		}
		totWeight += interpolPoints[i].second;
	}

	res +="total real Weight: " + to_string(totRealWeight) + "\n";
	res +="total no signal Weight: " + to_string(totNoSignalWeight) + "\n";
	res +="real Weight + no signal Weight: " + to_string(totRealWeight + totNoSignalWeight) + "\n";
	res +="total Weight: " + to_string(totWeight) + "\n";

	return res;
}

/**
 * \brief changes length of vector
 *
 * calculates a vector, which has the same direction as the given vector, but has a new given length.
 * 
 * \param vec the vector, whose length will be changed
 * 
 * \param newLength the new length of the given vector
 * 
 * \return the new vector with a different length
 */
std::vector<double> cVecLen(const std::vector<double> vec, double newLength) {

	std::vector<double> result(LENPOINT);	

	double vecLength = vecLen(vec);

	for(int i = 0; i < LENPOINT; i++){
		result[i] = (vec[i] / vecLength ) * newLength;
	}

	return result;
}

/**
 * \brief subtracts 2 vectors
 *
 * subtracts the second from the first vector. both vectors must be of size 3.
 * 
 * \param vec1 the minuend
 * 
 * \param vec1 the subtrahend
 * 
 * \return the subtraction of the 2 vectors
 */
std::vector<double> substract (const std::vector<double> vec1, const std::vector<double> vec2) {

	std::vector<double> result(LENPOINT);	

	for(std::vector<double>::size_type i = 0, size = vec1.size(); i < size; i++){
		result[i] = vec1[i] - vec2[i];
	}

	return result;
}

void RaLaNSAlone::setPoint (std::vector<double> pointVec, 
		std::vector< std::vector<double> > &header, int TransOrRec) {

	if(pointVec.size() == LENPOINTHEAD){

		len[XAXIS + TransOrRec * LENPOINT] = 1;
		len[YAXIS + TransOrRec * LENPOINT] = 1;
		len[ZAXIS + TransOrRec * LENPOINT] = 1;

		header.push_back(pointVec);
	} else{
		std::cerr << "The number of arguments for the point header is not 3: " 
			<< pointVec.size() << "\n";
	}
}

void RaLaNSAlone::setLine (std::vector<double> lineVec, 
		std::vector< std::vector<double> > &header, int TransOrRec) {

	if(lineVec.size() == LENLINEHEAD){

		/*
		 * set the number of elements in x and y dimension, 
		 * they are 1, because a line Vector is written in 1 line in the file
		 */
		len[XAXIS + TransOrRec * LENPOINT] = 1;
		len[YAXIS + TransOrRec * LENPOINT] = 1;
		len[ZAXIS + TransOrRec * LENPOINT] = 1;

		/*
		 * read the beginning vector for the line
		 */
		std::vector<double>::const_iterator first = lineVec.begin();
		std::vector<double>::const_iterator last = lineVec.begin() + LENPOINT;
		std::vector<double> beginLineVec(first, last);

		/*
		 * read the ending vector for the line
		 */
		first = lineVec.begin()+ LENPOINT;
		last = lineVec.begin()+ LENPOINT + LENPOINT;
		std::vector<double> endLineVec(first, last);

		/*
		 * read the stepSize for the changing of each point for the line,
		 * calculate the direction vector from the beginning of the line to the end
		 * calculate the length of that vector to compare it in the loop 
		 * set the length of the direction vector to stepSize
		 */
		double stepSize = lineVec[6];
		std::vector<double> vecFromBeginToEnd = substract(endLineVec, beginLineVec);
		double lenBeginToEnd = vecLen(vecFromBeginToEnd);
		std::vector<double> stepVec = cVecLen(vecFromBeginToEnd, stepSize);

		std::vector<double> newInterPointVec(beginLineVec);

		/*
		 * if the length of 
		 * (the current Vector - the beginning Vector) > the total length from begin to end,
		 * then all the points are set.
		 */
		while(vecLen(substract(newInterPointVec, beginLineVec)) < lenBeginToEnd){

			header.push_back(newInterPointVec);
			newInterPointVec = sum(newInterPointVec, stepVec);
		}
	} else{

		std::cerr << "The number of arguments for the line header is not LENLINEHEAD: " 
			<< lineVec.size() << "\n";
	}
}

void RaLaNSAlone::setAreaAndCubic(std::vector<double> beginVec, 
		std::vector< std::vector<double> > &header, int TransOrRec){

	header.reserve(len[ZAXIS + TransOrRec * LENPOINT] * len[XAXIS + TransOrRec * LENPOINT] 
			* len[YAXIS + TransOrRec * LENPOINT]);

	std::vector<double> newInterPointVec(beginVec);
	for(int k = 0; k < len[ZAXIS + TransOrRec * LENPOINT]; k++){
		for(int i = 0; i < len[YAXIS + TransOrRec * LENPOINT]; i++){
			for(int j = 0; j < len[XAXIS + TransOrRec * LENPOINT]; j++){
				header.push_back(newInterPointVec);
				newInterPointVec[XAXIS] += step[XAXIS + TransOrRec * LENPOINT];
			}
			newInterPointVec[YAXIS] += step[YAXIS + TransOrRec * LENPOINT];
			newInterPointVec[XAXIS] = beginVec[XAXIS];
		}
		newInterPointVec[ZAXIS] += step[ZAXIS + TransOrRec * LENPOINT];
		newInterPointVec[YAXIS] = beginVec[YAXIS];
	}
}

void RaLaNSAlone::setArea (std::vector<double> areaVec, 
		std::vector< std::vector<double> > &header, int TransOrRec) {

	if(areaVec.size() == LENAREAHEAD){

		/*
		 * read the beginning vector for the area
		 */
		std::vector<double> beginAreaVec(3);
		beginAreaVec[XAXIS] = areaVec[0];
		beginAreaVec[YAXIS] = areaVec[1];	
		beginAreaVec[ZAXIS] = areaVec[4];	

		/*
		 * read the ending vector for the area
		 */
		std::vector<double> endAreaVec(3);
		endAreaVec[XAXIS] = areaVec[2];
		endAreaVec[YAXIS] = areaVec[3];	
		endAreaVec[ZAXIS] = areaVec[4];	

		/*
		 * read the step size for the area
		 */
		step[XAXIS + TransOrRec * LENPOINT] = areaVec[5];			
		step[YAXIS + TransOrRec * LENPOINT] = areaVec[6];			
		step[ZAXIS + TransOrRec * LENPOINT] = 0;			

		/*
		 * calculate the number of elements in the x, y and z dimension
		 */
		len[XAXIS + TransOrRec * LENPOINT] = ( (int) ((endAreaVec[XAXIS] - beginAreaVec[XAXIS]) 
					/ step[XAXIS + TransOrRec * LENPOINT])) + 1; 
		len[YAXIS + TransOrRec * LENPOINT] = ((int) ((endAreaVec[YAXIS] - beginAreaVec[YAXIS]) 
					/ step[YAXIS + TransOrRec * LENPOINT])) + 1;
		len[ZAXIS + TransOrRec * LENPOINT] = 1;


		/*
		 * fill the header with all points of the area,
		 */
		setAreaAndCubic(beginAreaVec, header, TransOrRec);
	}

	else{
		std::cerr << "The number of arguments for the area header is not LENAREAHEAD: " 
			<< areaVec.size() << "\n";
	}	

}

void RaLaNSAlone::setList (std::vector<double> listVec, 
		std::vector< std::vector<double> > &header, int TransOrRec) {

	if((listVec.size() - 1) % 3 == 0){

		std::vector<double> pointVec(3);	

		/*
		 * fill header with all points of the list in the order of the list
		 */
		for(std::vector<double>::size_type i = 1; i < listVec.size(); i += 3){

			pointVec[XAXIS] = listVec[i + XAXIS];
			pointVec[YAXIS] = listVec[i + YAXIS];
			pointVec[ZAXIS] = listVec[i + ZAXIS];

			header.push_back(pointVec);
		}	

		len[XAXIS + TransOrRec * LENPOINT] = 1;
		len[YAXIS + TransOrRec * LENPOINT] = 1;
		len[ZAXIS + TransOrRec * LENPOINT] = 1;

		RaLaNSList *raLaNSList = new RaLaNSList(header, this);
		DEBUG_MSG("use pointer begin");
		this->listCalc[TransOrRec] = *raLaNSList;
		DEBUG_MSG("use pointer end");

	} else{
		std::cerr << "The number of arguments for the list header is not dividable by 3, " 
			<< "(listVec.size() - 1) % 3 == 0 : " 
			<< listVec.size() << "\n";
	}
}

void RaLaNSAlone::setCubic (std::vector<double> cubicVec, 
		std::vector< std::vector<double> > &header, int TransOrRec) {

	if(cubicVec.size() == LENCUBICHEAD){

		std::vector<double> beginCubicVec(3);
		beginCubicVec[XAXIS] = cubicVec[0];
		beginCubicVec[YAXIS] = cubicVec[1];	
		beginCubicVec[ZAXIS] = cubicVec[2];	

		std::vector<double> endCubicVec(3);
		endCubicVec[XAXIS] = cubicVec[3];
		endCubicVec[YAXIS] = cubicVec[4];	
		endCubicVec[ZAXIS] = cubicVec[5];	

		step[XAXIS + TransOrRec * LENPOINT] = cubicVec[6];
		step[YAXIS + TransOrRec * LENPOINT] = cubicVec[7];
		step[ZAXIS + TransOrRec * LENPOINT] = cubicVec[8];

		len[XAXIS + TransOrRec * LENPOINT] = ((int) ((endCubicVec[XAXIS] - beginCubicVec[XAXIS]) 
					/ step[XAXIS + TransOrRec * LENPOINT])) + 1;
		len[YAXIS + TransOrRec * LENPOINT] = ((int) ((endCubicVec[YAXIS] - beginCubicVec[YAXIS]) 
					/ step[YAXIS + TransOrRec * LENPOINT])) + 1;
		len[ZAXIS + TransOrRec * LENPOINT] = ((int) ((endCubicVec[ZAXIS] - beginCubicVec[ZAXIS]) 
					/ step[ZAXIS + TransOrRec * LENPOINT])) + 1;

		/*
		 * fill the header with all points of the cubic structure,
		 * put all the values behind each other in the order all x for every y for every z,
		 * because that order is used for the signals between the transmitters and receivers
		 */
		setAreaAndCubic(beginCubicVec, header, TransOrRec);


	} else{
		std::cerr << "The number of arguments for the cubic header is not LENCUBICHEAD: " 
			<< cubicVec.size() << "\n";
	}

}

void RaLaNSAlone::setHeader (std::string headerStringWithSpaces, 
		std::vector< std::vector<double> > &header, int &type, int TransOrRec) {

	std::istringstream buffer(headerStringWithSpaces);

	std::vector<double> headerVec;
	double d;
	std::string tmp;

	while (getline( buffer, tmp, ' ' )) {
		d = atof(tmp.c_str());
		headerVec.push_back(d);
	}

	type = (int) headerVec[0];
	headerVec.erase(headerVec.begin());

	if(type == POINT){

		DEBUG_MSG("SETDATA: set Point");
		setPoint(headerVec, header, TransOrRec);							
	} else if(type == LINE){

		DEBUG_MSG("SETDATA: set Line");
		setLine(headerVec, header, TransOrRec);					
	} else if(type == AREA){

		DEBUG_MSG("SETDATA: set Area");
		setArea(headerVec, header, TransOrRec);			
	} else if(type == CUBIC){

		DEBUG_MSG("SETDATA: set Cubic");
		setCubic(headerVec, header, TransOrRec);				
	} else if(type == LIST){

		DEBUG_MSG("SETDATA: set List");
		setList(headerVec, header, TransOrRec);				
	} else{

		std::cerr << "The value of the type in one of the headers is not valid: " << type << "\n";
	}

}

std::string RaLaNSAlone::printPoints(const std::vector<std::vector<double> > &points, 
		std::string transOrRec) const{

	std::string string = "";
	for(std::vector<double>::size_type i = 0; i < points.size(); i++){
		string += "CONTENT: " + transOrRec + " : "+ to_string(points[i][XAXIS]) +", "
			+ to_string(points[i][YAXIS]) +", "+ to_string(points[i][ZAXIS]) + "\n";
	}

	return string;
}

bool RaLaNSAlone::GenMap(){

	this->debugParams();

	START;

	std::string filenameTxt;
	//gets the last 4 chars to check whether match .zip or .txt
	std::string filenameExtension(this->filename, filename.size() - 4, 4);
	std::cout << "parsing map" << std::endl;
	if(filenameExtension == ".zip"){


		//TODO maybe improve the unzipping
		//Extract the zip file to a directory with the same name using the unzip command
		std::string filenameDir(this->filename.begin(), this->filename.begin() + (this->filename.size() - 4));
		std::string command = "unzip -u " + this->filename + " -d " + filenameDir;
		system(command.c_str());
		std::cout << "finished unzipping" << std::endl;

		//build the path to the txt file in the extracted zip directory
		filenameTxt = filenameDir + "/" + "result" + ".txt";


	} else if (filenameExtension == ".txt"){

		filenameTxt = this->filename;
	} else {

		std::cerr << "Unable to read the given file: " << this->filename 
			<< " The file has the wrong file type. "
			"The file has to be a zip or txt file" << "\n";
		return false;
	}

	//open file
	std::ifstream infile;
	infile.open (filenameTxt.c_str());
	if (!infile.good()) {
		std::cerr << "Unable to read the txt file with the data: " << filenameTxt << "\n";
		infile.close();
		return false;
	} else{	

		std::string headerLine;

		std::getline(infile, headerLine);
		DEBUG_MSG("Set Transmitters");
		setHeader(headerLine, transmitters, typeTrans, TRANS);
		DEBUG_MSG("SETDATA: Number of setted Transmitters: "<< transmitters.size());

		std::getline(infile, headerLine);
		DEBUG_MSG("Set Receivers");
		setHeader(headerLine, receivers, typeRec, REC);
		DEBUG_MSG("SETDATA: Number of setted Receivers: "<< receivers.size());

		if(typeRec == LINE || typeRec == LIST){
			std::getline(infile, headerLine);
		}

		std::string temp, tmp;

		int lineCounter = 1;
		unsigned long signalCounter = 0;
		std::vector<double> numbers;

		this->signals.reserve(transmitters.size());

		while(std::getline(infile, temp)) {

			/*
			 * read the next line and store it in the line vector
			 */
			std::istringstream buffer(temp);
			std::vector<double> line;
			line.reserve(receivers.size());
			double d;
			while (getline( buffer, tmp, ' ' )) {
				d = atof(tmp.c_str());
				line.push_back(d);
				signalCounter++;
			}

			/*
			 * concatenate the vectors, new values after old values
			 */
			numbers.insert(numbers.end(), line.begin(), line.end());

			/*
			 * yLen * zLen stands for the number of receivers each tranmitters has,
			 * so if that case happens all receiver signals 
			 * for that tranmitter are already in the numbers vector
			 * so then all that signals will be added to the signals vector 
			 * and the process goes again until the 
			 * lineCounter has has the value of yLen * zLen, 
			 * so that all signals are read for that tranmitter
			 */
			if (lineCounter == len[YAXIS + REC * LENPOINT] * len[ZAXIS + REC * LENPOINT] ) {

				DEBUG_MSG("SETDATA: currently parsed " 
						<< signals.size() << " transmitter signal groups");

				signals.push_back(numbers);
				numbers.clear();
				lineCounter = 1;
			} else {
				lineCounter++;
			}

		}
		infile.close();

		DEBUG_MSG("SETDATA: finished parsing " << signals.size() << " transmitter signal groups");
		DEBUG_MSG("SETDATA: finished parsing " << signalCounter << " signals");
		DEBUG_MSG("SETDATA: number of signals should be for area and cubic: " 
				<< signals.size() * signals[0].size());

		if(lineCounter != 1){

			ERROR("Fault in reading of the setted file. File is probably wrong!");
			return false;
		}

		if(signalCounter != transmitters.size() * receivers.size()){

			ERROR("Wrong number of signals in the setted file: " 
					<< "Should be: " << transmitters.size() * receivers.size() 
					<< ", but is: " << signalCounter);
			return false;
		}

		if(signals.size() != transmitters.size()){

			ERROR("Wrong number of transmitter signal groups in the setted file: " 
					<< "Should be: " << transmitters.size()
					<< ", but is: " << signals.size());
			return false;
		}
	}

	DIFF("SETDATA");
	DEBUG_MSG(printPoints(transmitters, "Trans"));
	DEBUG_MSG(printPoints(receivers, "Rec"));
	return true;
}

bool RaLaNSAlone::SetFile(std::string filename) {
	this->filename = filename;
	return this->GenMap();
}

std::string RaLaNSAlone::GetFile() const{
	return this->filename;
}

std::string RaLaNSAlone::genDistError(const std::vector<double> &point, int TransOrRec, 
		int pos, double dst, const std::vector<std::vector<double> > &points) const{

	std::string transOrRec;
	if(TransOrRec == TRANS){
		transOrRec = "Transmitter";
	} else {
		transOrRec = "Receiver";
	}

	std::string interpol = "";
	if(this->GetInterpolate()){

		interpol = "nearest interpolation ";
	}

	return transOrRec + " far away from " + interpol + "match!"  
		+ to_string(point[XAXIS]) + " " + to_string(point[YAXIS]) + " "
		+ to_string(point[ZAXIS]) +  "|" 
		+ to_string(points[pos][XAXIS]) + " "  + to_string(points[pos][YAXIS]) + " " 
		+ to_string(points[pos][ZAXIS]) 
		+ "  distance: " + to_string(sqrt(dst));
}

void RaLaNSAlone::throwDistError(const std::vector<double> &point, int TransOrRec, 
		int pos, double dst, const std::vector<std::vector<double> > &points) const{

	std::cerr << genDistError(point, TransOrRec, pos, dst, points) << "\n";
}

bool RaLaNSAlone::checkDistError(const std::vector<double> &point, int TransOrRec, int pos, 
		double dst, const std::vector<std::vector<double> > &points) const{

	if (dst > threshold){
		throwDistError(point, TransOrRec, pos, dst, points);
		return false;
	} else {
		return true;
	}
}

bool RaLaNSAlone::checkDistError(const std::vector<double> &point, int TransOrRec, 
		int pos, const std::vector<std::vector<double> > &points) const{

	return checkDistError(point, TransOrRec, pos, calcDist(point, points[pos]), points);
}

double RaLaNSAlone::calcDist(const std::vector<double> &point1, const std::vector<double> &point2) const{
	return std::pow(point1[XAXIS] - point2[XAXIS],2) 
		+ std::pow(point1[YAXIS] -  point2[YAXIS],2) 
		+ std::pow(point1[ZAXIS] - point2[ZAXIS],2);
}

int RaLaNSAlone::checkOnUp(const std::vector<double> &point, 
		const std::vector<std::vector<double> > &points, int axis) const{

	if(point[axis] == points[points.size() - 1][axis]){
		return ONUP;
	} else{
		return NOTONUP;
	}
}

int RaLaNSAlone::checkIn(const std::vector<double> &point, 
		const std::vector<std::vector<double> > &points, int axis) const{


	if(point[axis] < points[0][axis] || point[axis] > points[points.size() - 1][axis]){
		return OUT;
	} else{
		return IN;
	}
}

int RaLaNSAlone::checkIn(const std::vector<double> &point,
		const std::vector<std::vector<double> > &points) const{

	int checkX = checkIn(point, points, XAXIS);
	int checkY = checkIn(point, points, YAXIS);
	int checkZ = checkIn(point, points, ZAXIS);

	return checkX + checkY + checkZ;
}

double RaLaNSAlone::calcNearestPos(const std::vector<double> &point, int axis, 
		const std::vector<std::vector<double> > &points, int TransOrRec) const{

	double result = point[axis] - points[0][axis]; 

	if(result < 0){
		result = 0;
	} else {

		int h = axis + TransOrRec * LENPOINT;
		result /= step[h];

		int lh = len[h] - 1; 

		if(result > lh ){
			result = lh;
		}
	}

	return result;
}

int RaLaNSAlone::calcNearestPosArea(const std::vector<double> &point, 
		const std::vector<std::vector<double> > &points, int TransOrRec) const{

	DEBUG_MSG("CALCSIG: calcNearestPosArea");
	/**
	 * the position of the nearest point is in y row, so the position according to the y axis,
	 * multiplied with the number of elements in the x axis + the position in the x axis.
	 */
	return ((int) (calcNearestPos(point, XAXIS, points, TransOrRec) + 0.5)) 
		+ ((int) (calcNearestPos(point, YAXIS, points, TransOrRec) + 0.5)) 
		* len[XAXIS + TransOrRec * LENPOINT];
}

int RaLaNSAlone::calcNearestPosCubic(const std::vector<double> &point, 
		const std::vector<std::vector<double> > &points, int TransOrRec) const{

	DEBUG_MSG("CALCSIG: calcNearestPosCubic");
	/**
	 * the position in the cubic is the position in an area + the shift because of the z value. 
	 */
	return calcNearestPosArea(point, points, TransOrRec) 
		+ ((int) (calcNearestPos(point, ZAXIS, points, TransOrRec) + 0.5)) 
		* len[XAXIS + TransOrRec * LENPOINT] * len[YAXIS + TransOrRec * LENPOINT];
}

int RaLaNSAlone::calcNearestPosElse(const std::vector<double> &point, 
		const std::vector<std::vector<double> > &points) const{

	DEBUG_MSG("CALCSIG: calcNearestPosElse");

	int pos;
	double currentMinDst = MAXDST;
	/*
	 * iterate over all transmitter, find the one, which is the nearest to the point a
	 */
	for(std::vector<double>::size_type t=0; t < points.size(); t++) {

		double sqDst = calcDist(point, points[t]);

		if (sqDst < currentMinDst) {
			currentMinDst = sqDst;
			pos=t;
		}
	}

	return pos;
}

int RaLaNSAlone::calcNearestPosList(const std::vector<double> &point, int TransOrRec) const{

	return this->listCalc[TransOrRec].calcNearestPos(point);
}

int RaLaNSAlone::calcNearestPosAll(const std::vector<double> &point, 
		const std::vector<std::vector<double> > &points, int TransOrRec, int type) const{

	int pos;

	if(type == AREA){
		DEBUG_MSG("NOINTERPOL: calc area");
		pos = calcNearestPosArea(point, points, TransOrRec);
	}
	else if(type == CUBIC){
		DEBUG_MSG("NOINTERPOL: calc cubic");
		pos = calcNearestPosCubic(point, points, TransOrRec);
	}
	else if(type == LIST){
		DEBUG_MSG("NOINTERPOL: calc list");
		pos = calcNearestPosList(point, TransOrRec);
	}
	else {
		DEBUG_MSG("NOINTERPOL: calc else");
		pos = calcNearestPosElse(point, points);
	}

	return pos;
}

double RaLaNSAlone::DoCalcNoInterpol(std::vector<double> transmitter, std::vector<double> receiver) const{

	int transPos = calcNearestPosAll(transmitter, transmitters, TRANS, typeTrans);
	int recPos = calcNearestPosAll(receiver, receivers, REC, typeRec);

	//DEBUG_MSG("Before: Get signal from array");
	//DEBUG_MSG("transPos: " << transPos << " vs. size" << signals.size());
	//DEBUG_MSG("recPos: " << recPos);

	double signal = signals[transPos][recPos];
	//DEBUG_MSG("After: Get signal from array");

	/**
	 * if one of the distances to the transmitter or receiver mobility model is too high 
	 * the signal will be set to noSignal
	 */
	if(!checkDistError(transmitter, TRANS, transPos, transmitters)){
		signal = noSignal;
	}
	if(!checkDistError(receiver, REC, recPos, receivers)){
		signal = noSignal;
	}

	DEBUG_MSG("TEST: trans Vec: pos: " << transPos << ", x:" <<
			transmitters[transPos][XAXIS] << ", y:" <<
			transmitters[transPos][YAXIS] <<", z:" <<
			transmitters[transPos][ZAXIS]);
	DEBUG_MSG("TEST: rec Vec: pos: " << recPos << ", x:" <<
			receivers[recPos][XAXIS] << ", y:" <<
			receivers[recPos][YAXIS] <<", z:" <<
			receivers[recPos][ZAXIS]);
	DEBUG_MSG("TEST: signal= " << signal);	

	return signal;
}

std::vector< std::pair <int, double> > RaLaNSAlone::makeNoSignalInterpolPoints() 
	const{

		std::vector< std::pair <int, double> > interpolPoints;

		std::pair <int, double> interpolPoint(-100, 1);
		interpolPoints.push_back(interpolPoint);

		return interpolPoints;
	}

std::vector< std::pair <int, double> > RaLaNSAlone::getInterpolPoints(
		const std::vector<double> &point, int axis, 
		const std::vector<std::vector<double> > &points, int TransOrRec) const{

	/**
	 * If a is inside the grid according to this axis: 
	 * The function calculates the smaller and bigger point according to this axis for the
	 * interpolation.
	 *
	 * If a is outside of the grid according to this axis:
	 * The function calculates 2 positions according to this axis for the interpolation.
	 *
	 * 1. point: the position of the point on the border.
	 *
	 * 2. point: a negative value, which will remain negative even with adding the other axis to
	 * it. That value is to interpolate with an imaginary interpolation point which has an
	 * orthogonal distance of the threshold value from the nearer border, nearer for the mobility
	 * model a, of the axis.
	 *	
	 */
	double pos = calcNearestPos(point, axis, points, TransOrRec);
	std::vector< std::pair <int, double> > interpolPoints;
	int posSmall = (int) pos;

	if(checkIn(point, points, axis) == IN){

		/**
		 * Then also take the higher value, 
		 * because the higher and lower value are the positions according to this axis of the points for 
		 * the interpolation.
		 */
		int posBig = (int) (pos + 1);

		/**
		 * compute the weighting for both positions for this axis
		 */
		double posBigWeight = pos - posSmall;
		double posSmallWeight = 1 - posBigWeight;
		std::pair <int, double> interpolPointSmall(posSmall, posSmallWeight);
		interpolPoints.push_back(interpolPointSmall);

		/**
		 * if a is on the upper border of the area or cubic according to this axis, then the bigger
		 * point will be outside of the area or cubic. Because of that it will not be added to
		 * interpolation points, because otherwise the corresponding position in the cubic or area
		 * will be higher than points.size().
		 *
		 * If that is not the case, the bigger point will be added normally.
		 */
		if(checkOnUp(point, points, axis) == ONUP){

			//set weight to 1, because it is the only interpolation point for this axis
			interpolPoints[0].second = 1;
		} else {

			std::pair <int, double> interpolPointBig(posBig, posBigWeight);
			interpolPoints.push_back(interpolPointBig);
		}

	} else {

		//setting posNoSignal to the negative of points.size() ensures it will remain negative
		int posNoSignal = -points.size();

		double distAToPos;
		if(point[axis] > points[points.size() - 1][axis]){

			distAToPos = point[axis] - points[points.size() - 1][axis];
		} else {

			distAToPos = points[0][axis] - point[axis]; 
		}

		/*
		 * if the threshold is 0, the signal has to be noSignal.
		 *
		 * if the threshold is bigger than 0,
		 * the weight is calculated by the relation between the distance and the maximal
		 * distance.
		 */
		double posNoSignalWeight;
		if(threshold == 0){
			posNoSignalWeight = 1;
		} else {

			posNoSignalWeight = std::pow(distAToPos,2) / threshold;
			if(posNoSignalWeight > 1){
				posNoSignalWeight = 1;
			}
		}
		double posSmallWeight = 1 - posNoSignalWeight;

		std::pair <int, double> interpolPointSmall(posSmall, posSmallWeight);
		interpolPoints.push_back(interpolPointSmall);

		std::pair <int, double> interpolPointNoSignal(posNoSignal, posNoSignalWeight);
		interpolPoints.push_back(interpolPointNoSignal);
	}

	return interpolPoints;
}

std::vector< std::pair <int, double> > RaLaNSAlone::getInterpolPointsArea(
		const std::vector<double> &point, const std::vector<std::vector<double> > &points, int TransOrRec) const{

	std::vector< std::pair <int, double> > interpolPointsX = getInterpolPoints(point, XAXIS, 
			points, TransOrRec);
	std::vector< std::pair <int, double> > interpolPointsY = getInterpolPoints(point, YAXIS, 
			points, TransOrRec);
	std::vector< std::pair <int, double> > interpolPointsArea; 

	/**
	 * compute the position of the interpolation points for the area. 
	 * The calculation of the position is the same as for the normal area position calculation.
	 *
	 * Also calculate the weighting for each interpolation point.
	 */
	for(int i = 0, endX = interpolPointsX.size(); i < endX; i++){
		for(int j = 0, endY = interpolPointsY.size(); j < endY; j++){


			int pos = interpolPointsX[i].first 
				+ interpolPointsY[j].first * len[XAXIS + TransOrRec * LENPOINT];
			double weight = interpolPointsX[i].second * interpolPointsY[j].second;
			std::pair <int, double> interpolPoint (pos, weight);
			interpolPointsArea.push_back(interpolPoint);
		}
	}

	return interpolPointsArea;
}

std::vector< std::pair <int, double> > RaLaNSAlone::getInterpolPointsCubic(
		const std::vector<double> &point, const std::vector<std::vector<double> > &points, int TransOrRec) const{


	std::vector< std::pair <int, double> > interpolPointsX = getInterpolPoints(point, XAXIS, 
			points, TransOrRec);
	std::vector< std::pair <int, double> > interpolPointsY = getInterpolPoints(point, YAXIS, 
			points, TransOrRec);
	std::vector< std::pair <int, double> > interpolPointsZ = getInterpolPoints(point, ZAXIS, 
			points, TransOrRec);
	std::vector< std::pair <int, double> > interpolPointsCubic; 

	//TODO merge with Area, 3 loops for area and cubic
	for(int i = 0, endX = interpolPointsX.size(); i < endX; i++){
		for(int j = 0, endY = interpolPointsY.size(); j < endY; j++){
			for(int k = 0, endZ = interpolPointsZ.size(); k < endZ; k++){

				int pos = interpolPointsX[i].first 
					+ interpolPointsY[j].first * len[XAXIS + TransOrRec * LENPOINT]
					+ interpolPointsZ[k].first * len[XAXIS + TransOrRec * LENPOINT] 
					* len[YAXIS + TransOrRec * LENPOINT];
				double weight = interpolPointsX[i].second 
					* interpolPointsY[j].second
					* interpolPointsZ[k].second;
				std::pair <int, double> interpolPoint (pos, weight);
				interpolPointsCubic.push_back(interpolPoint);
			}
		}
	}

	return interpolPointsCubic;
}

std::vector< std::pair <int, double> > RaLaNSAlone::getInterpolPointsElse(
		const std::vector<double> &point, const std::vector<std::vector<double> > &points) const{


	std::pair <int,double> interpolPoint (calcNearestPosElse(point, points),1);
	std::vector< std::pair <int, double> > interpolPoints;
	interpolPoints.push_back(interpolPoint);
	return interpolPoints;
}

std::vector< std::pair <int, double> > RaLaNSAlone::getInterpolPointsList(
		const std::vector<double> &point, int TransOrRec) const{

	return this->listCalc[TransOrRec].getInterpolPoints(point);
}

std::vector< std::pair <int, double> > RaLaNSAlone::getInterpolPointsAll(
		const std::vector<double> &point, 
		const std::vector<std::vector<double> > &points, int TransOrRec, int type) const{

	std::vector< std::pair <int, double> > interpolPoints;

	if(type == AREA){

		DEBUG_MSG("INTERPOLATE: start area calculation");
		interpolPoints = getInterpolPointsArea(point, points, TransOrRec);
	}
	else if(type == CUBIC){

		DEBUG_MSG("INTERPOLATE: start cubic calculation");
		interpolPoints = getInterpolPointsCubic(point, points, TransOrRec);
	} 
	else if(type == LIST){

		DEBUG_MSG("INTERPOLATE: start list calculation");
		interpolPoints = getInterpolPointsList(point, TransOrRec);
	} else {

		DEBUG_MSG("INTERPOLATE: start else calculation");
		interpolPoints = getInterpolPointsElse(point, points);
	} 

	/**
	 * checks wether all interpolation are too far away. If all interpolation points are too far
	 * away, then the distance error will be thrown with the last saved point in the grid as
	 * information for the distance error
	 */
	bool errorResult = false;
	double distance;
	double minDst = MAXDST;
	int posMin = -1;
	for(int i = 0, t = interpolPoints.size(); i < t && !errorResult; i++){


		int pos = interpolPoints[i].first;
		if(pos >= 0){

			distance = calcDist(point, points[pos]);

			if(distance < minDst){

				posMin = pos;
				minDst = distance;
			}

		}

		if(distance <= threshold){

			errorResult = true;
		}
	}

	/*
	 * if all interpolation points are too far away from the mobility model,
	 * throw the distance error and set the interpolation points to a noSignal interpolation point.
	 */
	if(!errorResult){

		throwDistError(point, TransOrRec, posMin, minDst, points);
		interpolPoints = makeNoSignalInterpolPoints();
	}

	return interpolPoints;
}

double RaLaNSAlone::DoCalcInterpol(
		std::vector<double> transmitter,
		std::vector<double> receiver) const
{


	/**
	 * 1. get all points to interpolate from the transmitters
	 *
	 * 2. get all points to interpolate from the receivers
	 *
	 * 3. compute signal for every combination.
	 *
	 * 4. sum the combinations in consideration of their weight up.
	 *
	 * 5. return the result
	 */
	DEBUG_MSG("INTERPOLATE: start interpolate calculation");
	std::vector< std::pair <int, double> > interpolPointsTrans = getInterpolPointsAll(transmitter, 
			transmitters, TRANS, typeTrans);
	std::vector< std::pair <int, double> > interpolPointsRec = getInterpolPointsAll(receiver, 
			receivers, REC, typeRec);

	DEBUG_MSG("INTERPOLATE: Finished getting interpolation points");

	DEBUG_MSG("interpolation points transmitter size: " << interpolPointsTrans.size());
	DEBUG_MSG("interpolation points receiver size: " << interpolPointsRec.size());

	DEBUG_MSG("all transmitter interpolation points: \n" 
			<< printInterpolPoints(interpolPointsTrans, transmitters));

	DEBUG_MSG("all receiver interpolation points: \n" 
			<< printInterpolPoints(interpolPointsRec, receivers));

	DEBUG_MSG("INTERPOLATE: Finished printing interpolation points");

	double interpolatedSignal = 0;
	int curBadNum = 0;
	double totWeight = 0;
	DEBUG_MSG("");
	for(int i = 0, endTrans = interpolPointsTrans.size(); i < endTrans; i++){

		int transPos = interpolPointsTrans[i].first;
		double transWeight = interpolPointsTrans[i].second;
		DEBUG_MSG("transPos: " << transPos);

		for(int j = 0, endRec = interpolPointsRec.size(); j < endRec; j++){

			int recPos = interpolPointsRec[j].first;
			double recWeight = interpolPointsRec[j].second;

			double signal;
			double weight = transWeight * recWeight;

			/**
			 * if at least one of the positions is smaller 0, 
			 * that means that the signal to this point is noSignal, because a or b are
			 * outside of the grid 
			 * or all interpolation points are too far away from a or
			 * b, although a or b is inside the grid.
			 */
			if(recPos < 0 || transPos < 0){
				signal = noSignal;
			} else {

				signal = signals[transPos][recPos];

				/**
				 * this is to exclude the bad signals from the calculation, 
				 * but limited to a maximum of bad signals, 
				 * which is stated by badNum
				 */
				if(signal <= noSignal){

					if(curBadNum < this->badNum){
						signal = 0;
						curBadNum++;
					} else {
						signal = noSignal;
					}
				}
				totWeight += weight;
			}

			interpolatedSignal += signal * weight;
			DEBUG_MSG("recPos: " << recPos);
			DEBUG_MSG("signal: " << signal << " * Weight:" << weight << " = "<< signal * weight);
			DEBUG_MSG("CurrentTotSignal: "<< interpolatedSignal);
		}
	}

	/*
	 * Do things for the badNumber value:
	 *
	 * if the total weight is 0:
	 * 	That means all interpolation signals are noSignal,
	 * 	then the result value must be noSignal of course.
	 *
	 * if curBadNum > this->badNum:
	 * 	That means that the actual number of bad values was bigger than the parameter.
	 * 	So the bad values have to be included again.
	 *
	 * Else:
	 * 	That means the number of bad values is lower or equal than the parameter.
	 * 	So the signal just needs to be corrected, because there were less signals for the
	 * 	calculation.
	 */
	if(totWeight == 0){
		interpolatedSignal = noSignal;
	} else if(curBadNum > this->badNum){
		double resWeight = 1 - totWeight;
		interpolatedSignal += noSignal * resWeight;
	} else{
		interpolatedSignal /= totWeight;
	}

	DEBUG_MSG("INTERPOLATE: END: interpolatedSignal= "<< interpolatedSignal);
	return interpolatedSignal;
}

double RaLaNSAlone::DoCalc(std::vector<double> transmitter,
		std::vector<double> receiver) const
{

	START;


	DEBUG_MSG("TEST: a: " << transmitter[XAXIS] << " " << transmitter[YAXIS] 
			<< " "<< transmitter[ZAXIS] 
			<< " -> b: " << receiver[XAXIS] << " " << receiver[YAXIS] 
			<< " "<< receiver[ZAXIS]);

	double result;
	if(interpolate){
		DEBUG_MSG("INTERPOLATE: START");
		result = DoCalcInterpol(transmitter, receiver);
	} else{
		DEBUG_MSG("NOINTERPOL: START");
		result = DoCalcNoInterpol(transmitter, receiver);
	}

	//no result can be smaller than the no signal value
	DEBUG_MSG("noSignal= " << noSignal);
	if(result <= noSignal){
		result = noSignal;	
	}
	DEBUG_MSG("result= " << result);

	DIFF("DOCALC");
	return result;
}
