// This file is a part of the Framsticks GDK.
// Copyright (C) 2002-2014  Maciej Komosinski and Szymon Ulatowski.  See LICENSE.txt for details.
// Refer to http://www.framsticks.com/ for further information.

#include "conv_fF.h"
#include "fF_genotype.h"
#include <frams/model/model.h>
#include <common/nonstd_stl.h>

GenoConv_fF0::GenoConv_fF0() 
{
	name = "7-value Foraminifera encoding";
	in_format = 'F';
	out_format = '0';
	mapsupport = 0;
	cosines = new double[fF_LATITUDE_NUM];
	sines = new double[fF_LATITUDE_NUM];
	fill_cos_and_sin();
}

GenoConv_fF0::~GenoConv_fF0() 
{
	delete[] cosines;
	delete[] sines;
}

SString GenoConv_fF0::convert(SString &in, MultiMap *map)
{
	fF_growth_params gp;
	if (!gp.load(in)) //invalid input genotype?
		return ""; //so we return an invalid f0 genotype

	double div_radius_length = 1;//div_radius_length=1 or kx=ky=kz=1
	double radius = 1;

	Model m;
	m.open();
	// subsequent parts (chambers) are placed relative to the previous part's orientation and location
	Part *p1, *p2;

	fF_chamber3d **chambers = new fF_chamber3d*[gp.number_of_chambers];

	for (int i = 0; i < gp.number_of_chambers; i++) {
		createSphere(i, chambers, radius, div_radius_length, gp.translation, gp.angle1, gp.angle2, gp.scalex, gp.scaley, gp.scalez);
	}

	p1 = m.addNewPart(Part::SHAPE_ELLIPSOID);
	p1->p = Pt3D(chambers[0]->centerX, chambers[0]->centerY, chambers[0]->centerZ);


	for (int i = 1; i < gp.number_of_chambers; i++, p1 = p2) {
		p2 = m.addNewPart(Part::SHAPE_ELLIPSOID);
		p2->scale = p1->scale.entrywiseProduct(Pt3D(gp.scalex, gp.scaley, gp.scalez)); //each part's scale is its predecessor's scale * scaling

		p2->p = Pt3D(chambers[i]->centerX, chambers[i]->centerY, chambers[i]->centerZ);

		m.addNewJoint(p1, p2, Joint::SHAPE_SOLID); //all parts must be connected
	}

	for (int i = 0; i < gp.number_of_chambers; i++)
		delete chambers[i];
	delete[]chambers;

	m.close();
	return m.getF0Geno().getGene();
}

void GenoConv_fF0::createSphere(int which, fF_chamber3d **chambers, double radius_, double div_radius_length_, double div_vector_length_,
	double alpha_, double gamma_, double kx_, double ky_, double kz_)
{
	chambers[which] = new fF_chamber3d(0.0f, 0.0f, 0.0f,
		(float)radius_, (float)radius_ * (float)kx_, 0.0f, 0.0f,
		(float)(radius_ * div_vector_length_), 0.0f, 0.0f, 0.0f, 0.0f);
	if (which == 0)
		chambers[which]->points = generate_points(chambers[which], which, kx_, ky_, kz_);
	if (which > 0) {
		/* old radius */
		double radiusOld, radius;
		radiusOld = chambers[which - 1]->radius;
		radius = div_radius_length_ * radiusOld;
		/* new growth vector length */
		double len = radius * div_vector_length_;
		if (radius < fF_TOO_LITTLE) {
			radius = fF_TOO_LITTLE;
			if (fabs(len) >(fF_TOO_MUCH * radius)) {
				len = ((len < 0) ? (-1) : 1) * fF_TOO_MUCH * radius;
			}
		}
		if (len == 0) {
			len = -0.0000001;
		}

		/* aperture of the previous chamber */
		double pzx = chambers[which - 1]->holeX;
		double pzy = chambers[which - 1]->holeY;
		double pzz = chambers[which - 1]->holeZ;

		//center of the previous chamber
		double pcx = chambers[which - 1]->centerX;
		double pcy = chambers[which - 1]->centerY;
		double pcz = chambers[which - 1]->centerZ;

		/* aperture of the next to last chamber */
		double ppx;
		double ppy;
		double ppz;

		if (which == 1) {
			ppx = pcx;
			ppy = pcy;
			ppz = pcz;
		}
		else {
			ppx = chambers[which - 2]->holeX;
			ppy = chambers[which - 2]->holeY;
			ppz = chambers[which - 2]->holeZ;
		}

		double pzxprim = pzx - ppx;
		double pzyprim = pzy - ppy;
		double angle;

		angle = atan2(pzyprim, pzxprim);
		double alpha = angle - alpha_;


		double gamma = chambers[which - 1]->phi + gamma_;

		/* x */
		double wx = len * cos(alpha);
		/* y */
		double wy = len * sin(alpha);
		/* y */
		double wz = len * sin(alpha) * sin(gamma);

		/*center of the new sphere*/
		double x = pzx + wx;
		double y = pzy + wy;
		double z = pzz + wz;

		chambers[which]->centerX = (float)x;
		chambers[which]->centerY = (float)y;
		chambers[which]->centerZ = (float)z;
		chambers[which]->radius = (float)radius;
		chambers[which]->vectorTfX = (float)wx;
		chambers[which]->vectorTfY = (float)wy;
		chambers[which]->vectorTfZ = (float)wz;
		chambers[which]->beta = (float)alpha;
		chambers[which]->phi = (float)gamma;

		chambers[which]->points = generate_points(chambers[which], which, kx_, ky_, kz_);
		search_hid(which, chambers, kx_, ky_, kz_);
		int pun;
		pun = find_hole(which, pzx, pzy, pzz, chambers, kx_, ky_, kz_);

		chambers[which]->holeX = (float)chambers[which]->points[pun][0];
		chambers[which]->holeY = (float)chambers[which]->points[pun][1];
		chambers[which]->holeZ = (float)chambers[which]->points[pun][2];
	}
}

void GenoConv_fF0::fill_cos_and_sin()
{
	int i;
	double pi = acos(-1.0);
	double angle = pi / (((double)fF_LATITUDE_NUM)*0.5);
	for (i = 0; i < fF_LATITUDE_NUM; i++)
	{
		cosines[i] = cos((double)i * angle);
		sines[i] = sin((double)i * angle);
	}
}

double** GenoConv_fF0::generate_points(fF_chamber3d *chamber, int which, double kx_, double ky_, double kz_)
{
	float radius = chamber->radius;
	float cenx = chamber->centerX;
	float ceny = chamber->centerY;
	float cenz = chamber->centerZ;

	double maxX = 0;
	double maxY = 0;
	double minX = 0;
	double minY = 0;
	double minZ = 0;

	double kx = 1;
	double ky = 1;
	double kz = 1;

	if (which > 0) {
		for (int kt = 1; kt < (which + 1); kt++) {
			kx = kx * kx_;
			ky = ky * ky_;
			kz = kz * kz_;
		}
	}

	int i, j;
	double x, y, z;

	double **points = new double*[fF_SIZE];
	for (int i = 0; i < fF_SIZE; i++) {
		points[i] = new double[4];
	}

	for (i = 0; i < fF_LONGITUDE_NUM; i++) {
		if (kx_ == 1 && ky_ == 1 && kz_ == 1) {
			y = ceny + radius * cosines[i];
		}
		else {
			y = ceny + ky * cosines[i];
		}
		for (j = 0; j < fF_LATITUDE_NUM; j++) {
			if (kx_ == 1 && ky_ == 1 && kz_ == 1) {
				points[(i * fF_LATITUDE_NUM) + j][0] = x = cenx + radius * cosines[j] * sines[i];
				points[(i * fF_LATITUDE_NUM) + j][1] = y;
				points[(i * fF_LATITUDE_NUM) + j][2] = z = cenz + radius * sines[j] * sines[i];
			}
			else {
				points[(i * fF_LATITUDE_NUM) + j][0] = x = cenx + kx * cosines[j] * sines[i];
				points[(i * fF_LATITUDE_NUM) + j][1] = y;
				points[(i * fF_LATITUDE_NUM) + j][2] = z = cenz + kz * sines[j] * sines[i];
			}

			points[(i * fF_LATITUDE_NUM) + j][3] = 1.0;
			if (x < minX) minX = x;
			if (x > maxX) maxX = x;
			if (y < minY) minY = y;
			if (y > maxY) maxY = y;

			if (z < minZ) minZ = z;
		};
	};
	return points;

}

double GenoConv_fF0::dist(double x1, double y1, double z1, double x2, double y2, double z2)
{
	return sqrt((x2 - x1)*(x2 - x1) + (y2 - y1)*(y2 - y1) + (z2 - z1)*(z2 - z1));
}

void GenoConv_fF0::search_hid(int nr, fF_chamber3d **spheres, double kx_, double ky_, double kz_)
{
	int i, j;
	if (nr != 0) {
		for (i = 0; i < nr; i++) {
			for (j = 0; j < fF_AMOUNT; j++) {
				double X = spheres[nr]->points[j][0];
				double Y = spheres[nr]->points[j][1];
				double Z = spheres[nr]->points[j][2];

				double srX0 = spheres[i]->centerX;
				double srY0 = spheres[i]->centerY;
				double srZ0 = spheres[i]->centerZ;

				double a2;
				double b2;
				double c2;

				if (kx_ != 1) {
					a2 = (kx_ * kx_);
				}
				else {
					a2 = (spheres[i]->radius * spheres[i]->radius);
				}

				if (ky_ != 1) {
					b2 = (ky_ * ky_);

				}
				else {
					b2 = (spheres[i]->radius * spheres[i]->radius);
				}

				c2 = (kz_ * spheres[i]->radius) * (kz_ * spheres[i]->radius);

				double up1 = (X - srX0) * (X - srX0);
				double up2 = (Y - srY0) * (Y - srY0);
				double up3 = (Z - srZ0) * (Z - srZ0);

				double exp = up1 / a2;
				double exp2 = up2 / b2;
				double exp3 = up3 / c2;

				double result = exp + exp2 + exp3;

				if (result < (fF_THICK_RATIO)
					) {
					spheres[nr]->points[j][3] = 0;
				}
			}
		}
	}
}

int GenoConv_fF0::find_hole(int which, double x, double y, double z, fF_chamber3d **chambers, double kx_, double ky_, double kz_)
{
	int i;
	double distance;
	int found = 0;
	double dist_found = 0;
	int first = 1;

	for (i = 0; i < fF_AMOUNT; i++) {
		if (chambers[which]->points[i][3] != 0) //it is not inside another chamber
		{
			distance = sqrt((chambers[which]->points[i][0] - x)*(chambers[which]->points[i][0] - x) +
				(chambers[which]->points[i][1] - y)*(chambers[which]->points[i][1] - y) +
				(chambers[which]->points[i][2] - z)*(chambers[which]->points[i][2] - z));
			if (first != 0) {
				found = i;
				dist_found = distance;
				first = 0;
			}
			if (distance < dist_found) {
				if (which != 0) {
					bool good = true;
					for (int j = 0; j < which; j++) {
						{
							double X = chambers[which]->points[i][0];
							double Y = chambers[which]->points[i][1];
							double Z = chambers[which]->points[i][2];

							double srX0 = chambers[j]->centerX;
							double srY0 = chambers[j]->centerY;
							double srZ0 = chambers[j]->centerZ;

							double a2 = (kx_ * chambers[j]->radius) * (kx_ * chambers[j]->radius);
							double b2 = (ky_ * chambers[j]->radius) * (ky_ * chambers[j]->radius);
							double c2 = (kz_ * chambers[j]->radius) * (kz_ * chambers[j]->radius);

							double up1 = (X - srX0) * (X - srX0);
							double up2 = (Y - srY0) * (Y - srY0);
							double up3 = (Z - srZ0) * (Z - srZ0);

							double exp1 = up1 / a2;
							double exp2 = up2 / b2;
							double exp3 = up3 / c2;

							double result = exp1 + exp2 + exp3;
							if (result < 1.0)
							{
								good = false;
							}
						}
					}
					if (good) {
						found = i;
						dist_found = distance;
					}
				}
			}
		}
	}

	return (found);
}