// This file is a part of Framsticks SDK.  http://www.framsticks.com/
// Copyright (C) 1999-2023  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] = { "fr" }; // , "dn", "ing", "as" };

const char *fH_joint_names[FH_JOINT_PROPS_COUNT] = { "rotstif" }; //"stif", "stam" }; //actually we would want to disable "rotstif" too (we assume the default value is the best, lower values are good to test technical abilities of phenotypes, but in most cases not useful for evolution - the structures and muscles get weaker and even more elastic), but having no joint properties was never tested, so better leave at least one

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.length() > 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);
	}

	size_t 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->friction = partprops[0]; //Macko 2023-06: TODO partprops[0] is different from par.getDoubleById("fr"); (and par.getDoubleById() is used in createJoint() below, investigate why this difference and whether hardcoded references to fields like here could be avoided? Only use fH_part_names if possible.)
	//newpart->density = 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->rotstif = par.getDoubleById("rotstif");
	//newjoint->stif = par.getDoubleById("stif");
	//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 = int(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 = (int)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 - (int)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;
	}
}