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

#include "f9_conv.h"
#include <frams/model/model.h>
#include <string.h>

#define APPLY_DETERMINISTIC_BODY_NOISE //this genetic representation easily produces perfectly vertical sticks that would stay upright forever in simulation. In most cases such infinite perfection is not desired, so we make the construct less perfect by perturbing its coordinates.

GenoConv_f90::GenoConv_f90()
{
	name = "Turtle3D-ortho encoding";
	in_format = '9';
	out_format = '0';
	mapsupport = 1;
}


const char* turtle_commands_f9 = "LRBFDU";
int turtle_commands_f9_count = 6; //keep in sync, must equal strlen(turtle_commands_f9)

//const char* turtle_commandsX_f9="-+0000";
//const char* turtle_commandsY_f9="00-+00";
//const char* turtle_commandsZ_f9="0000-+";

SString GenoConv_f90::convert(SString &in, MultiMap *map, bool using_checkpoints)
{
	vector<XYZ_LOC> vertices;
	XYZ_LOC current;
	Model m;
	m.open(using_checkpoints);
	int recently_added = addSegment(m, 0, vertices, current, 0xDead);
	for (int i = 0; i < in.length(); i++)
	{
		char command = in[i];
		char *ptr = strchr((char*)turtle_commands_f9, command);
		if (ptr)
		{
			int delta[] = { 0, 0, 0 };
			int pos = ptr - turtle_commands_f9;
			int axis = pos / 2;
			int dir = pos % 2;
			(*(delta + axis)) += dir * 2 - 1; //+1 or -1 in the given axis
			current.add(delta);
			recently_added = addSegment(m, i, vertices, current, recently_added);
			m.checkpoint();
		}
	}
#ifdef APPLY_DETERMINISTIC_BODY_NOISE
	perturbPartLocations(m);
#endif
	setColors(m, recently_added);
	m.close();
	if (m.getPartCount() < 2) //only one part <=> there were no valid turtle commands in the input genotype
		return ""; //so we return an invalid f0 genotype
	if (map != NULL)
		m.getCurrentToF0Map(*map);
	return m.getF0Geno().getGenes();
}

int GenoConv_f90::addSegment(Model &m, int genenr, vector<XYZ_LOC> &vertices, const XYZ_LOC &new_vertex, int recently_added)
{
	if (vertices.size() < 1) //empty model?
	{
		return addNewVertex(m, vertices, new_vertex);
	}
	else
	{
		int vertex_here = findVertexAt(vertices, new_vertex);
		if (vertex_here < 0) //need to create a new Part
		{
			vertex_here = addNewVertex(m, vertices, new_vertex);
		} //else there already exists a Part in new_vertex; new Joint may or may not be needed
		Part *p1 = m.getPart(recently_added);
		Part *p2 = m.getPart(vertex_here);
		p1->addMapping(MultiRange(genenr, genenr));
		p2->addMapping(MultiRange(genenr, genenr));

		int j12 = m.findJoint(p1, p2);
		int j21 = m.findJoint(p2, p1);
		if (j12 >= 0)
			m.getJoint(j12)->addMapping(MultiRange(genenr, genenr));
		else if (j21 >= 0)
			m.getJoint(j21)->addMapping(MultiRange(genenr, genenr));
		else //both j12<0 and j21<0. New Joint needed. Should always happen if we just created a new Part (vertex_here was <0)
			m.addNewJoint(p1, p2)->addMapping(MultiRange(genenr, genenr));
		return vertex_here;
	}
}

int GenoConv_f90::findVertexAt(vector<XYZ_LOC> &vertices, const XYZ_LOC &vertex)
{
	for (int i = 0; i < (int)vertices.size(); i++)
		if (vertices[i].same_coordinates(vertex)) return i;
	return -1;
}


int GenoConv_f90::addNewVertex(Model &m, vector<XYZ_LOC> &vertices, const XYZ_LOC &new_vertex)
{
	Part *p = new Part;
	p->p.x = new_vertex.x;
	p->p.y = new_vertex.y;
	p->p.z = new_vertex.z;
	m.addPart(p);

	vertices.push_back(new_vertex);
	return int(vertices.size()) - 1;
}

double mix(int *colortab, int maxind, double ind)
{
	int indpre = (int)ind;
	int indpost = indpre + 1;
	if (indpost > maxind) indpost = maxind;
	int v1 = colortab[indpre];
	int v2 = colortab[indpost];
	double d1 = ind - indpre;
	double d2 = indpost - ind;
	double v = indpre == indpost ? v1 : d2 * v1 + d1 * v2; //d1+d2==1
	return v;
}

void GenoConv_f90::setColors(Model &m, int last_added_part) //sets fixed (independent from genes) colors and widths on a model, purely for aesthetic purposes
{
	static const bool OLD = false; //old "rainbow" hue gradient
	if (OLD)
	{
		//a rainbow on Joints: from the first one red, through middle green, to blue or violet - last
		static int r[] = { 1, 1, 0, 0, 0, 1 };
		static int g[] = { 0, 1, 1, 1, 0, 0 };
		static int b[] = { 0, 0, 0, 1, 1, 1 };
		int maxind = int(std::size(r)) - 1;

		int joints_count = m.getJointCount();
		for (int i = 0; i < joints_count; i++)
		{
			Joint *j = m.getJoint(i);
			double x = joints_count < 2 ? 0 : (double)i / (joints_count - 1); //0..1, position in the rainbow
			double ind = x * maxind;
			j->vcolor.x = mix(r, maxind, ind);
			j->vcolor.y = mix(g, maxind, ind);
			j->vcolor.z = mix(b, maxind, ind);
		}
	}
	else
	{
		int joints_count = m.getJointCount();
		for (int i = 0; i < joints_count; i++)
		{
			Joint *j = m.getJoint(i);
			Pt3D d = j->part2->p - j->part1->p; //dx,dy,dz
			double ax = fabs(d.x), ay = fabs(d.y), az = fabs(d.z);
			// Pairs of colors should be easy to associate as "one family" at a glance, but different so that we use all main six parts of the spectrum.
			// Colors are slightly brightened to make them more pastel/plastic; extreme saturation pure red=1,0,0 or blue 0,0,1 do not look good.
			// Find the longest axis (i.e., recover the information from the genotype: was this joint created by LR, BF, or DU?)
			if ((ax > ay) && (ax > az)) // x
			{
				j->vcolor = d.x < 0 ? Pt3D(1, 0.2, 0.2) : Pt3D(1, 0.2, 0.6); // red, purple red
			}
			else
				if ((ay > ax) && (ay > az)) // y
				{
					j->vcolor = d.y < 0 ? Pt3D(0.2, 1, 0.2) : Pt3D(0.7, 1, 0.2); //green, yellowish green
				}
				else // z
				{
					j->vcolor = d.z < 0 ? Pt3D(0.2, 0.2, 1) : Pt3D(0.4, 0.9, 1); //blue, cyanish blue
				}
		}
	}

	int parts_count = m.getPartCount();
	if (OLD)
	{
		SList jlist;
		for (int i = 0; i < parts_count; i++)
		{
			Part *p = m.getPart(i);
			jlist.clear();
			int count = m.findJoints(jlist, p);
			Pt3D averagecolor(0, 0, 0); //Parts will get averaged colors from all attached Joints
			FOREACH(Joint*, j, jlist)
				averagecolor += j->vcolor;
			p->vcolor = averagecolor / count;
		}
	}
	else
	{
		//Parts will get gray colors from darker to brighter, according to their order of creation.
		for (int i = 0; i < parts_count; i++)
		{
			Part *p = m.getPart(i);
			p->vcolor.x = p->vcolor.y = p->vcolor.z = 0.3 + 0.4 * i / (parts_count - 1); //0.3..0.7, so first (black) and last (white) stand out more
		}
	}
	//The first Part will be black, the last Part will be white - a visual aid for easier matching of the genotype and the corresponding phenotype.
	if (!OLD)
		m.getPart(0)->vcolor = Pt3D(0, 0, 0); //mark first Part black - not attractive in OLD sweet and positive color scheme
	m.getPart(last_added_part)->vcolor = Pt3D(1, 1, 1); //mark last Part white
}

void GenoConv_f90::perturbPartLocations(Model &m) //deterministic "body noise", see APPLY_DETERMINISTIC_BODY_NOISE
{
	// If you want to know why some structures that have different f9 genotypes, but "look the same", still get different fitness values, read on!
	//
	// For the purpose of the following discussion, consider 4 genotypes and the resulting structures as examples:
	// 1. square                        /*9*/LFRB
	// 2. square overlapping twice      /*9*/LFRBLFRB
	// 3. reverse square                /*9*/LRFLB
	// 4. cyclic permutation square     /*9*/FRBL
	//
	// All these structures "look the same" (i.e., the phenotypes are squares and a human does not distinguish any differences).
	// However, due to noise added to location of Parts based on the consecutive number of a Part,
	// only the phenotypes of 1. and 2. are truly, perfectly identical.
	//
	// Since the noise is determined by consecutive Part numbers, it is possible to create structures that "look the same",
	// but since Parts in them are created in different order, the structures would have slightly differently disturbed
	// coordinates (and thus fitness).
	//
	// To avoid this effect, the noise could instead be generated from x,y,z (i.e., XYZ_LOC) of Parts so that structures that
	// "look the same" have exactly the same noise added to each Part (and thus identical fitness), but then the structures that are 
	// shifted, rotated, or mirrored would still have different noise added to each Part, even though they could also be considered as
	// "looking the same".
	// In that approach, 3. would be the same as 1. and 2., because it is specifically constructed to use the same x,y,z Part coordinates as 1.
	// However, 4. would still be different from 1. and 2., because it is a rotated version of 1, and coordinates of Parts are different.
	//
	// So there is no quick and simple way to add exactly the same noise to structures that "look the same" independently of
	// various potential transformations. Also, what is considered "the same" may vary: is the structure (e.g. a tetrahedron) turned
	// upside-down "the same" and do we require the same noise disturbance? It depends on the context and what we use this structure for.
	// So that's the question of identifying transformation-invariant features in the structure that would become the source of the noise.
	//
	// With the current approach, structures that have precisely the same randomly perturbed Part coordinates (and thus fitness) are
	// those that have the same Part coordinates before random perturbations AND the order of creating Parts is the same (their genotypes
	// may still be different though, like 1. and 2.). Structures which have the same order of created Parts and are only shifted
	// relative to each other (i.e., all Parts uniformly shifted) can also be considered precisely the same (ignoring floating point
	// differences). There is no such example provided here because it is impossible to make in f9, because we always start
	// "drawing" Part #0 from (0,0,0) - it is not possible in f9 to start "drawing" Part #0 from another initial point in space.
	//
	// A simple alternative (quite opposite to the current approach) could be to add the same amount of shift (noise) to each Part
	// depending only on their vertical coordinate - then only vertical transformations would influence that shift, while Part creation
	// order and all horizontal shifts, rotations, and reflections would not matter.
	// Another determinant of randomness could be the structure of the phenotype graph, traversed depth-first, from some specific Part
	// (i.e., closest to the centroid).
	// Another determinant of randomness could be the integer (avoiding floating point operations if possible) squared distance from
	// the structure center/centroid of Parts (likely displaced by some constant to avoid symmetry and ties in distances, e.g. for
	// a vertical stick).
	// Another (best?) approach: [start] calculate centroid of current structure, calculate distances from centroid to all Parts, take closest Part, add noise to its coordinates based on its distance from centroid and stddev(x),stddev(y),stddev(z) for all Parts in the entire phenotype (this ensures that noise depends on the orientation of the entire phenotype, i.e., a "horizontal square" and a "vertical square", after added noise, still "look the same"), then go back to [start]. In case many Parts have identical distance, take Part with lowest number (id) - i.e., use the same number as we currently use as the only source of noise, assuming that the same distance from centroid for many Parts means "any of them could be taken as first, it does not matter which one we take". Note that this approach starts to resemble some mechanisms used in the similarity estimation measures - PCA to align/"normalize" the phenotype in 3D etc.

	// For now, the source of the noise stays as it is because:
	// 1) the source is integer (consecutive number of a Part), not distances or other stats prone to floating point inaccuracies
	// 2) the idea is extremely simple
	// 3) "complaints/surprises" detailed above are only due to the fact that we know what is the phenotype (solution) and we have some specific expectations (like "rotated or mirrored or shifted or alternatively encoded structures should be equivalent, should behave the same way in simulation, and thus should have the same fitness"). For the optimization algorithm it does not matter and it has no such assumptions, because genotypes for "rotated or mirrored or shifted or alternatively encoded structures" are actually different, so since they are formally different solutions, it is perfectly fine that they may get a different fitness value. Also, while they are extremely close in the phenotype space, they are not neccessarily close in the f9 genotype space where the search takes place.
	for (int i = 0; i < m.getPartCount(); i++)
	{
		Part *p = m.getPart(i);
		Pt3D noise(
			((i + 1) % 10) - 4.5,
			((3 * i + 5) % 10) - 4.5,
			((7 * i + 2) % 10) - 4.5
		); //-4.5 .. 4.5 in each axis
		p->p += noise / 1000;
	}
}