// This file is a part of Framsticks SDK.  http://www.framsticks.com/
// Copyright (C) 1999-2020  Maciej Komosinski and Szymon Ulatowski.
// See LICENSE.txt for details.

#include <string>
#include <limits>
#include <algorithm>
#include <frams/util/multirange.h>
#include <utility>
#include "fH_general.h"

using std::pair, std::to_string, std::numeric_limits;



// Methods for loading handles

const char *fH_part_names[FH_PART_PROPS_COUNT] = { "dn", "fr", "ing", "as" };

const char *fH_joint_names[FH_JOINT_PROPS_COUNT] = { "stif", "rotstif", "stam" };

void fH_Handle::loadProperties(Param par)
{
	// loading values for vectors
	for (int i = 0; i < dimensions; i++)
	{
		first[i] = par.getDouble(i);
		second[i] = par.getDouble(dimensions + i);
	}
	obj = par.getSelected();
}

void fH_Builder::addHandle(fH_Handle *handle)
{
	switch (handle->type)
	{
	case fHBodyType::JOINT:
		sticks.push_back((fH_StickHandle*)handle);
		break;
	case fHBodyType::NEURON:
		neurons.push_back((fH_NeuronHandle*)handle);
		break;
	case fHBodyType::CONNECTION:
		connections.push_back((fH_ConnectionHandle*)handle);
		break;
	}
}

// Methods for saving properties of handles in params

void fH_Handle::saveProperties(Param &par)
{
	par.select(obj);
	for (int i = 0; i < dimensions; i++)
	{
		par.setDouble(i, first[i]);
		par.setDouble(dimensions + i, second[i]);
	}
}

// Destructor of Builder

fH_Builder::~fH_Builder()
{
	for (fH_StickHandle *obj : sticks)
	{
		delete obj;
	}
	sticks.clear();
	for (fH_NeuronHandle *obj : neurons)
	{
		delete obj;
	}
	neurons.clear();
	for (fH_ConnectionHandle *obj : connections)
	{
		delete obj;
	}
	connections.clear();

	if (stickparamtab) ParamObject::freeParamTab(stickparamtab);
	if (neuronparamtab) ParamObject::freeParamTab(neuronparamtab);
	if (connectionparamtab) ParamObject::freeParamTab(connectionparamtab);

}

// Methods for parsing genotype

void fH_Builder::prepareParams()
{
	for (int i = 0; i < dimensions; i++) // preparing first vector fields
	{
		string x = "x";
		x += to_string(i);
		stickmut.addProperty(NULL, x.c_str(), HANDLE_VECTOR_TYPE, x.c_str(), "", PARAM_CANOMITNAME, 0, -1);
		neuronmut.addProperty(NULL, x.c_str(), HANDLE_VECTOR_TYPE, x.c_str(), "", PARAM_CANOMITNAME, 0, -1);
		connectionmut.addProperty(NULL, x.c_str(), HANDLE_VECTOR_TYPE, x.c_str(), "", PARAM_CANOMITNAME, 0, -1);

	}
	for (int i = 0; i < dimensions; i++) // preparing second vector fields
	{
		string y = "y";
		y += to_string(i);
		stickmut.addProperty(NULL, y.c_str(), HANDLE_VECTOR_TYPE, y.c_str(), "", PARAM_CANOMITNAME, 0, -1);
		neuronmut.addProperty(NULL, y.c_str(), HANDLE_VECTOR_TYPE, y.c_str(), "", PARAM_CANOMITNAME, 0, -1);
		connectionmut.addProperty(NULL, y.c_str(), HANDLE_VECTOR_TYPE, y.c_str(), "", PARAM_CANOMITNAME, 0, -1);

	}

	Part p;
	for (int i = 0; i < FH_PART_PROPS_COUNT; i++)
	{
		stickmut.addProperty(&p.properties().getParamTab()[p.properties().findId(fH_part_names[i]) + p.properties().getGroupCount()], -1);
	}

	Joint j;
	for (int i = 0; i < FH_JOINT_PROPS_COUNT; i++)
	{
		stickmut.addProperty(&j.properties().getParamTab()[j.properties().findId(fH_joint_names[i]) + j.properties().getGroupCount()], -1);
	}
	stickmut.addProperty(NULL, "l", STICKH_LENGTH_TYPE, "length", "", 0, 0, -1);

	Neuro n;
	neuronmut.addProperty(&n.properties().getParamTab()[n.properties().findId(FH_PE_NEURO_DET) + n.properties().getGroupCount()], -1);

	Param tmp(f0_neuroconn_paramtab, NULL);
	connectionmut.addProperty(&tmp.getParamTab()[tmp.findId(FH_PE_CONN_WEIGHT) + tmp.getGroupCount()], -1);

	stickparamtab = ParamObject::makeParamTab((ParamInterface *)&stickmut, 0, 0, stickmut.firstMutableIndex());
	neuronparamtab = ParamObject::makeParamTab((ParamInterface *)&neuronmut, 0, 0, neuronmut.firstMutableIndex());
	connectionparamtab = ParamObject::makeParamTab((ParamInterface *)&connectionmut, 0, 0, connectionmut.firstMutableIndex());
}

int fH_Builder::processLine(SString line, int linenumber, int begin, int end)
{
	// Firstly, method determines if line describes joint, neuron or neural connection
	// and prepares corresponding ParamTab
	fH_Handle *handle = NULL;
	ParamEntry *tab = NULL;
	if (line.startsWith("j:")) //joint
	{
		handle = new fH_StickHandle(dimensions, begin, end);
		tab = stickparamtab;
	}
	else if (line.startsWith("n:")) //neuron
	{
		handle = new fH_NeuronHandle(dimensions, begin, end);
		tab = neuronparamtab;
	}
	else if (line.startsWith("c:")) //connection
	{
		handle = new fH_ConnectionHandle(dimensions, begin, end);
		tab = connectionparamtab;
	}
	else // could not determine type of a handle
	{
		string message = "Cannot determine handle type at line:  " + to_string(linenumber);
		logMessage("fH_Builder", "processLine", LOG_ERROR, message.c_str());
		return begin;
	}
	line = line.substr(2); // skip of "j:", "c:" or "n:"

	// Secondly, ParamObject for holding handle properties is created
	void *obj = ParamObject::makeObject(tab);
	Param par(tab, obj);
	par.setDefault();
	ParamInterface::LoadOptions opts;

	// After preparing Param objects, vector values and body properties are parsed
	par.load(ParamInterface::FormatSingleLine, line, &opts);

	// If parsing failed, method writes error message and ends processing
	if (opts.parse_failed)
	{
		string message = "Error in parsing handle parameters at line:  " + to_string(linenumber);
		logMessage("fH_Builder", "processLine", LOG_ERROR, message.c_str());
		delete handle;
		ParamObject::freeObject(obj);
		return begin;
	}

	// If parsing ended successfully, parsed properties are loaded into handle fields
	handle->loadProperties(par);

	// In the end, ready handle is stored in an appropriate vector
	addHandle(handle);
	return 0;
}

int fH_Builder::parseGenotype(const SString &genotype)
{
	// Firstly, number of dimensions is parsed
	int pos = 0;
	SString numdimensions;
	genotype.getNextToken(pos, numdimensions, '\n');
	if (!ExtValue::parseInt(numdimensions.c_str(), dimensions, true, false))
	{
		logMessage("fH_Builder", "parseGenotype", LOG_ERROR, "Could not parse number of dimensions");
		return 1;
	}
	if (dimensions < 1)
	{
		logMessage("fH_Builder", "parseGenotype", LOG_ERROR, "Number of dimensions cannot be lower than 1");
		return 1;
	}
	SString line;
	int linenumber = 2;

	// With known number of dimensions ParamTabs for handles are prepared
	prepareParams();

	// After preparing Builder for parsing, each line is processed with processLine
	int lastpos = pos;
	while (genotype.getNextToken(pos, line, '\n'))
	{
		if (line.len() > 0)
		{
			int res = processLine(line, linenumber, lastpos, pos - 1);
			if (res != 0)
			{
				return res;
			}
		}
		lastpos = pos;
		linenumber++;
	}
	if (sticks.size() == 0)
	{
		logMessage("fH_Builder", "parseGenotype", LOG_ERROR, "Genotype does not contain any stick");
		return 1;
	}
	return 0;
}

// Distance calculations

double fH_Handle::dist(vector<double> left, vector<double> right)
{
	double sum = 0;
	for (unsigned int i = 0; i < left.size(); i++)
	{
		sum += (left[i] - right[i]) * (left[i] - right[i]);
	}
	return sqrt(sum);
}

vector<double> fH_Handle::getVectorsAverage()
{
	vector<double> result(dimensions, 0);
	for (int i = 0; i < dimensions; i++)
	{
		result[i] = (first[i] + second[i]) / 2;
	}
	return result;
}

double fH_StickHandle::distance(fH_Handle *right)
{
	double distance = 0;
	switch (right->type)
	{
	case fHBodyType::JOINT:
		// distance is computed between second vector of current handle and first
		// vector of second handle
		distance = dist(second, right->first);
		break;
	case fHBodyType::NEURON:
	{
		// if neuron has to be connected to joint, then distance is calculated
		// between averages of both handles
		vector<double> avgs = getVectorsAverage();
		vector<double> avgn = right->getVectorsAverage();
		distance = dist(avgs, avgn);
		break;
	}
	case fHBodyType::CONNECTION:
		// it is impossible to calculate distance between Joint and Connection
		return numeric_limits<double>::quiet_NaN();
	}
	return distance;
}

double fH_NeuronHandle::distance(fH_Handle *right)
{
	double distance = 0;
	switch (right->type)
	{
	case fHBodyType::JOINT:
	{
		// if neuron has to be connected to joint, then distance is calculated
		// between averages of both handles
		vector<double> avgs = right->getVectorsAverage();
		vector<double> avgn = getVectorsAverage();
		distance = dist(avgs, avgn);
		break;
	}
	case fHBodyType::CONNECTION:
		// this calculation is meant for input neuron - it compares second vector
		// of neuron and first vector of connection
		distance = dist(second, right->first);
		break;
	case fHBodyType::NEURON:
		// it is impossible to calculate distance between two Neurons
		return numeric_limits<double>::quiet_NaN();
	}
	return distance;
}

double fH_NeuronHandle::distance(fH_StickHandle *right, bool first)
{
	vector<double> avgn = getVectorsAverage();
	double distance = 0;
	if (first)
	{
		distance = dist(avgn, right->firstparthandle);
	}
	else
	{
		distance = dist(avgn, right->secondparthandle);
	}
	return distance;
}

double fH_ConnectionHandle::distance(fH_Handle *right)
{
	double distance = 0;
	switch (right->type)
	{
	case fHBodyType::NEURON:
		// this calculation is meant for output neuron - it compares second vector
		// of connection and first vector of neuron
		distance = dist(second, right->first);
		break;
	case fHBodyType::JOINT:
	case fHBodyType::CONNECTION:
		// it is impossible to calculate distance between Connection and other
		// Connection or Joint
		return numeric_limits<double>::quiet_NaN();
	}
	return distance;
}

// Creature build functions

Part * fH_StickHandle::createPart(ParamEntry *tab, std::vector<fH_StickHandle *> *children, Model *model, bool createmapping)
{
	Param par(tab, obj);
	double partprops[FH_PART_PROPS_COUNT];
	for (int i = 0; i < FH_PART_PROPS_COUNT; i++)
	{
		partprops[i] = par.getDouble(2 * getDimensions() + i);
	}

	unsigned int stickscount = children->size() + 1;

	MultiRange ranges;
	ranges.add(begin, end);

	for (fH_StickHandle *child : (*children))
	{
		par.select(child->obj);
		for (int i = 0; i < FH_PART_PROPS_COUNT; i++)
		{
			partprops[i] += par.getDouble(2 * getDimensions() + i);
		}
		ranges.add(child->begin, child->end);
	}

	for (int i = 0; i < FH_PART_PROPS_COUNT; i++)
	{
		partprops[i] /= stickscount;
	}

	Part *newpart = new Part();

	model->addPart(newpart);

	newpart->density = partprops[0];
	newpart->friction = partprops[1];
	newpart->ingest = partprops[2];
	newpart->assim = partprops[3];

	if (createmapping) newpart->addMapping(ranges);

	return newpart;
}

Joint* fH_StickHandle::createJoint(ParamEntry *tab, Model *model, bool createmapping)
{
	Param par(tab, obj);
	if (firstpart == NULL || secondpart == NULL)
	{
		return NULL;
	}
	Joint *newjoint = new Joint();

	model->addJoint(newjoint);

	newjoint->stif = par.getDoubleById("stif");
	newjoint->rotstif = par.getDoubleById("rotstif");
	newjoint->stamina = par.getDoubleById("stam");
	newjoint->attachToParts(firstpart, secondpart);
	if (createmapping) newjoint->addMapping(IRange(begin, end));
	return newjoint;
}

void fH_Builder::buildBody()
{
	// stickconnections vector holds information about connections between sticks.
	// Left side of pair should hold pointer to stick that is connected with second
	// vector, and right side of pair should hold pointer to stick that is connected
	// with first vector
	stickconnections.clear();

	// if body consists of single stick, just add it to body
	if (sticks.size() == 1)
	{
		stickconnections.push_back(pair<fH_StickHandle *, fH_StickHandle *>(nullptr, sticks[0]));
		sticksorder.push_back(0);
		return;
	}

	vector<bool> remainingsticks(sticks.size(), true);

	// first we find two handles that have minimal distances between their second
	// and first vector
	fH_StickHandle *left = sticks[0];
	fH_StickHandle *right = sticks[1];
	double mindist = left->distance(right);
	int leftid = 0;
	int rightid = 1;
	for (unsigned int i = 0; i < sticks.size(); i++)
	{
		for (unsigned int j = i + 1; j < sticks.size(); j++)
		{
			double distance = sticks[i]->distance(sticks[j]);
			if (distance < mindist)
			{
				mindist = distance;
				left = sticks[i];
				right = sticks[j];
				leftid = i;
				rightid = j;
			}
			distance = sticks[j]->distance(sticks[i]);
			if (distance < mindist)
			{
				mindist = distance;
				left = sticks[j];
				right = sticks[i];
				leftid = j;
				rightid = i;
			}
		}
	}

	// two found handles are the beginning of creature body
	stickconnections.push_back(pair<fH_StickHandle *, fH_StickHandle *>(nullptr, left));
	stickconnections.push_back(pair<fH_StickHandle *, fH_StickHandle *>(left, right));

	// after selecting two handles as beginning of body, they are marked as used
	// in the list of remaining sticks
	remainingsticks[leftid] = false;
	remainingsticks[rightid] = false;

	sticksorder.push_back(leftid);
	sticksorder.push_back(rightid);

	// next stick is selected by minimum distance between first vector of its handle
	// and second vector of any existing StickHandle in body
	int remaining = sticks.size() - 2;
	while (remaining > 0)
	{
		leftid = -1;
		rightid = -1;
		mindist = numeric_limits<double>::max();
		for (unsigned int i = 0; i < sticks.size(); i++)
		{
			// if stick is not already in
			if (remainingsticks[i])
			{
				for (int stickid : sticksorder)
				{
					double distance = sticks[stickid]->distance(sticks[i]);
					if (distance < mindist)
					{
						mindist = distance;
						leftid = stickid;
						rightid = i;
					}
				}
			}
		}
		stickconnections.push_back(pair<fH_StickHandle *, fH_StickHandle *>(sticks[leftid], sticks[rightid]));
		remainingsticks[rightid] = false;
		sticksorder.push_back(rightid);
		remaining--;
	}
}

int fH_Builder::developBrain(Model *model, bool createmapping)
{
	Param par(neuronparamtab, NULL);
	// First of all, neurons are attached to body
	for (fH_NeuronHandle *currneu : neurons)
	{
		par.select(currneu->obj);
		// create Neuro object and set details
		currneu->neuron = new Neuro();
		SString det = par.getStringById("d");
		if (det != "")
		{
			currneu->neuron->setDetails(det);
		}
		else
		{
			currneu->neuron->setDetails("N");
		}

		// get class of neuron. If class with given name does not exist - return error
		NeuroClass *nclass = currneu->neuron->getClass();
		if (!nclass)
		{
			SString msg = "NeuroClass given in details \"";
			msg += det + "\" does not exist";
			logMessage("fH_Builder", "developBrain", LOG_ERROR, msg.c_str());
			delete currneu->neuron;
			return -1;
		}
		// add neuron to model -> required before attaching to body part
		model->addNeuro(currneu->neuron);
		if (nclass->getPreferredLocation() == 2) // attach to Joint
		{
			// find stick that has closest average handle to average handle of
			// neuron
			double mindist = currneu->distance(sticks[0]);
			fH_StickHandle *minstick = sticks[0];
			for (unsigned int i = 1; i < sticks.size(); i++)
			{
				double distance = currneu->distance(sticks[i]);
				if (distance < mindist)
				{
					mindist = distance;
					minstick = sticks[i];
				}
			}
			currneu->neuron->attachToJoint(minstick->joint);
		}
		else if (nclass->getPreferredLocation() == 1) // attach to Part
		{
			// in the beginning we take first part of first stick to calculate
			// distance between them as initial minimal distance
			double mindist = currneu->distance(sticks[0], true);
			Part *minpart = sticks[0]->firstpart;
			for (unsigned int i = 0; i < sticks.size(); i++)
			{
				// after this we take only second parts of following sticks to
				// avoid repetition (thats why we start from i = 0)
				double distance = currneu->distance(sticks[i], false);
				if (distance < mindist)
				{
					mindist = distance;
					minpart = sticks[i]->secondpart;
				}
			}
			currneu->neuron->attachToPart(minpart);
		}
		if (createmapping) currneu->neuron->addMapping(IRange(currneu->begin, currneu->end));
		model->checkpoint();
	}

	par.setParamTab(connectionparamtab);
	// Secondly, connections are created
	for (fH_ConnectionHandle *currcon : connections)
	{
		par.select(currcon->obj);
		// Connection is created as follows:
		//   beginneu ---> endneu
		// distance between beginneu and connection is calculated as distance
		// between second handle of beginneu and first handle of connection.
		// This is why calculation is written as beginneu->distance(currcon).
		// In case of connection and endneu distance between them is calculated
		// as distance between second handle of connection and first handle of
		// endneu. This is why calculation is written as currcon->distance(endneu).

		fH_NeuronHandle *beginneu = NULL;
		double mindist = numeric_limits<double>::max();
		// find beginning of connection
		for (fH_NeuronHandle *neuron : neurons)
		{
			// These method checked earlier if all neurons have valid classes.
			// If a neuron does not have output, then it's skipped from comparison.
			// Otherwise:
			if (neuron->neuron->getClass()->getPreferredOutput() > 0)
			{
				double distance = neuron->distance(currcon);
				if (distance < mindist)
				{
					mindist = distance;
					beginneu = neuron;
				}
			}
		}
		// if there was no neuron that could begin a connection, then return warning
		if (!beginneu)
		{
			// due to often appearance of connection genes in fB encoding, this
			// log message is commented
			// logMessage("fH_Builder", "developBrain", LOG_DEBUG, "There are no available neurons with outputs, connection could not be established");
			continue;
		}

		fH_NeuronHandle *endneu = NULL;
		mindist = numeric_limits<double>::max();
		// find ending of connection
		for (fH_NeuronHandle *neuron : neurons)
		{
			// Method checked earlier if all neurons have valid classes.
			// If neuron does not accept input or all inputs are already connected,
			// then it's skipped from comparison.
			// Otherwise:
			if (neuron->neuron->getClass()->getPreferredInputs() == -1 ||
				neuron->neuron->getClass()->getPreferredInputs() > neuron->neuron->getInputCount())
			{
				double distance = currcon->distance(neuron);
				if (distance < mindist)
				{
					mindist = distance;
					endneu = neuron;
				}
			}
		}
		// if there was no neuron that could end connection, then return warning
		if (!endneu)
		{
			// due to often appearance of connection genes in fB encoding, this
			// log message is commented
			// logMessage("fH_Builder", "developBrain", LOG_DEBUG, "There are no available neurons with free inputs, connection could not be established");
			continue;
		}
		endneu->neuron->addInput(beginneu->neuron, par.getDoubleById("w"));
		if (createmapping) endneu->neuron->addMapping(IRange(currcon->begin, currcon->end));
		model->checkpoint();
	}
	return 0;
}

Pt3D fH_Builder::getNextDirection(int count, int number)
{
	// In order to get evenly distributed sticks coming from the same Part, the method
	// uses an algorithm for even distribution of points on a sphere. There are several
	// methods to perform this, usually iterative. The method introduced
	// below offers not fully accurate, yet quite satisfying results. This is
	// the RSZ method (Rakhmanov, Saff and Zhou) with the use of the golden angle.
	// This method is based on the distribution of points along a spiral that covers
	// the sphere surface.

	// The following method works partially on spherical coordinates (r and theta is used).
	// The Z coordinate is from Cartesian coordinate system. The golden angle is used
	// to "iterate" along the spiral, while the Z coordinate is used to move down the
	// sphere.

	double golden_angle = M_PI * (3.0 - sqrt(5));
	double dz = 2.0 / (double)count;
	double z = 1 - ((double)number + 0.5) * dz;
	double r = sqrt(1 - z * z);
	double theta = golden_angle * number;
	Pt3D vec;
	// In the end X and Y coordinates are calculated with current values of
	// r and theta. Value z is already calculated
	vec.x = r * cos(theta);
	vec.y = r * sin(theta);
	vec.z = z;
	vec.normalize();
	return vec;
}

Orient fH_Builder::getRotationMatrixToFitVector(Pt3D currdir, Pt3D expecteddir)
{
	Orient res;
	// first method normalizes vectors for easy calculations
	currdir.normalize();
	expecteddir.normalize();
	double c = currdir.dotProduct(expecteddir); // dot product of both vectors
	// if the dot product of both vectors equals 0
	if (c == 0)
	{
		res.x.x = -1;
		res.x.y = 0;
		res.x.z = 0;

		res.y.x = 0;
		res.y.y = -1;
		res.y.z = 0;

		res.z.x = 0;
		res.z.y = 0;
		res.z.z = -1;
	}
	Pt3D v = Pt3D(0); // cross product of both vectors
	v.x = currdir.y * expecteddir.z - currdir.z * expecteddir.y;
	v.y = currdir.z * expecteddir.x - currdir.x * expecteddir.z;
	v.z = currdir.x * expecteddir.y - currdir.y * expecteddir.x;

	// Rotation matrix that enables aligning currdir to expecteddir comes from
	// following calculation
	// R = I + [v]_x + ([v]_x)^2 / (1+c)
	// where [v]_x is the skew-symmetric cross-product matrix of v
	res.x.x = 1 - (v.y * v.y + v.z * v.z) / (1 + c);
	res.x.y = v.z + (v.x * v.y) / (1 + c);
	res.x.z = -v.y + (v.x * v.z) / (1 + c);
	res.y.x = -v.z + (v.x * v.y) / (1 + c);
	res.y.y = 1 - (v.x * v.x + v.z * v.z) / (1 + c);
	res.y.z = v.x + (v.y * v.z) / (1 + c);
	res.z.x = v.y + (v.x * v.z) / (1 + c);
	res.z.y = -v.x + (v.y * v.z) / (1 + c);
	res.z.z = 1 - (v.x * v.x + v.y * v.y) / (1 + c);

	return res;
}

Model* fH_Builder::buildModel(bool using_checkpoints)
{
	Model *model = new Model();

	// At first, floating sticks are connected
	buildBody();

	model->open(using_checkpoints);

	// Secondly, parts and joints are created
	// For every stick in body, starting with initial
	Param par(stickparamtab, NULL);
	for (int currid : sticksorder)
	{
		fH_StickHandle *currstick = sticks[currid];
		fH_StickHandle *parent = NULL;
		// find parent of current stick - it is first element of pair, in which
		// current stick is second
		for (pair<fH_StickHandle *, fH_StickHandle *> conn : stickconnections)
		{
			if (conn.second == currstick)
			{
				parent = conn.first;
				break;
			}
		}

		// if parent is NULL, then create Part with current stick properties and
		// location at (0,0,0)
		if (!parent)
		{
			vector<fH_StickHandle *> emptylist;
			Part *firstpart = currstick->createPart(stickparamtab, &emptylist, model, createmapping);
			firstpart->p = Pt3D(0);
			currstick->firstpart = firstpart;
			currstick->firstparthandle = currstick->first; // this is used to calculate later distance between
			model->checkpoint();
		}
		else //otherwise first part of current stick is the second part of previous stick
		{
			currstick->firstpart = parent->secondpart;
			currstick->firstparthandle = parent->secondparthandle;
		}
		// position of second part depends on two things
		//  1. direction of previous joint
		//  2. how many sticks are connected to the same parent
		// default direction of growth (without parent) is (1,0,0)
		Pt3D direction(1, 0, 0);
		Pt3D secondposition(currstick->firstpart->p);
		// if parent does exist, then determine how many sticks are connected to
		// parent and distribute them evenly on a sphere surrounding second part
		if (parent)
		{
			// improved RSZ method creates vectors that starts in
			// center of sphere (which will act as shared part), so direction
			// calculated below should point from shared part to previous part
			// in order to perform proper aligning
			direction = parent->secondpart->p - parent->firstpart->p;
			direction.normalize();
			// determine how many sticks are connected to parent and when connection
			// between parent and current stick appear
			int count = 0;
			int id = -1;
			for (unsigned int i = 0; i < stickconnections.size(); i++)
			{
				if (stickconnections[i].first == parent)
				{
					if (stickconnections[i].second == currstick)
					{
						id = count;
					}
					count++;
				}
			}
			if (id == -1)
			{
				logMessage("fH_Builder", "buildModel", LOG_ERROR, "Invalid behaviour");
				delete model;
				return NULL;
			}

			// if there is only one child, then don't change direction - continue
			// along axis of parent. Otherwise calculate direction of id-th stick
			// (that is currstick) with use of RSZ/Vogel method of distributing points
			// evenly on a sphere
			if (count > 1)
			{
				direction = parent->firstpart->p - parent->secondpart->p;
				direction.normalize();
				// there has to be count+1 directions, so method needs to generate
				// count+1 evenly distributed points on a sphere to make vectors
				// from point (0,0,0) to those points. First generated vector
				// will act as parent joint direction vector
				Pt3D sphere0direction = getNextDirection(count + 1, 0);

				// First generated vector needs to be aligned to parent vector
				Orient rotmatrix = getRotationMatrixToFitVector(sphere0direction, direction);

				// Calculation of direction from sphere for currstick
				direction = getNextDirection(count + 1, id + 1);
				// Rotation matrix aligning
				direction = rotmatrix.transform(direction);
				direction.normalize();
			}
		}

		// calculate second position
		par.select(currstick->obj);
		secondposition += direction * par.getDoubleById("l");

		// find every stick connected to current stick in order to calculate second
		// part properties
		vector<fH_StickHandle *> children;
		currstick->secondparthandle = currstick->second;
		for (pair<fH_StickHandle *, fH_StickHandle *> conn : stickconnections)
		{
			if (conn.first == currstick)
			{
				children.push_back(conn.second);
				for (int i = 0; i < dimensions; i++)
				{
					currstick->secondparthandle[i] += conn.second->first[i];
				}
			}
		}
		// create part from current stick and other sticks connected to this part
		Part *secondpart = currstick->createPart(stickparamtab, &children, model, createmapping);
		secondpart->p = secondposition;
		currstick->secondpart = secondpart;
		double count = (double)children.size() + 1;
		for (int i = 0; i < dimensions; i++)
		{
			currstick->secondparthandle[i] /= count;
		}

		//after creating second part connect two parts with joint
		Joint * joint = currstick->createJoint(stickparamtab, model, createmapping);
		if (!joint)
		{
			logMessage("fH_Builder", "buildModel", LOG_ERROR, "Joint cannot be created");
			delete model;
			return NULL;

		}
		currstick->joint = joint;
		model->checkpoint();
	}
	// after creating a body, attach neurons to body and link them according to
	// connections
	if (developBrain(model, createmapping) == -1)
	{
		delete model;
		return NULL;
	}
	model->close();
	return model;
}

int fH_Builder::removeNeuronsWithInvalidClasses()
{
	int count = neurons.size();
	if (count == 0)
	{
		return 0;
	}
	vector<fH_NeuronHandle *>::iterator it = neurons.begin();
	Param par(neuronparamtab, NULL);
	while (it != neurons.end())
	{
		par.select((*it)->obj);
		SString det = par.getStringById("d");
		if (det == "")
		{
			it++;
		}
		else
		{
			Neuro *neu = new Neuro();
			neu->setDetails(det);
			if (neu->getClass())
			{
				it++;
			}
			else
			{
				fH_NeuronHandle *tmp = (*it);
				it = neurons.erase(it);
				delete tmp;
			}
			delete neu;
		}

	}
	return count - neurons.size();
}

SString fH_Builder::toString()
{
	SString result = "";
	result += to_string(dimensions).c_str();
	result += "\n";
	// first method stringifies parts
	Param par(stickparamtab, NULL);
	void *def = ParamObject::makeObject(stickparamtab);
	par.select(def);
	par.setDefault();
	for (fH_StickHandle *currstick : sticks)
	{
		currstick->saveProperties(par);
		SString props;
		par.saveSingleLine(props, def, true, false);
		result += "j:";
		result += props;
	}
	ParamObject::freeObject(def);
	par.setParamTab(neuronparamtab);
	def = ParamObject::makeObject(neuronparamtab);
	par.select(def);
	par.setDefault();
	for (fH_NeuronHandle *currneuron : neurons)
	{
		currneuron->saveProperties(par);
		SString props;
		par.saveSingleLine(props, def, true, false);
		result += "n:";
		result += props;
	}
	ParamObject::freeObject(def);
	par.setParamTab(connectionparamtab);
	def = ParamObject::makeObject(connectionparamtab);
	par.select(def);
	par.setDefault();
	for (fH_ConnectionHandle *currconnection : connections)
	{
		currconnection->saveProperties(par);
		SString props;
		par.saveSingleLine(props, def, true, false);
		result += "c:";
		result += props;
	}
	ParamObject::freeObject(def);
	return result;
}

ParamEntry* fH_Builder::getParamTab(fHBodyType type)
{
	switch (type)
	{
	case fHBodyType::JOINT:
		return stickparamtab;
		break;
	case fHBodyType::NEURON:
		return neuronparamtab;
		break;
	default:
		return connectionparamtab;
		break;
	}
}