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

#include "measure-distribution.h"
#include <common/nonstd_math.h>
#include <limits>
#include "EMD/emd.c"
#include <iostream>

#define FIELDSTRUCT SimilMeasureDistribution

static ParamEntry simil_distribution_paramtab[] = {
		{ "Creature: Similarity: Descriptor distribution", 1, 4, "SimilMeasureDistribution", "Evaluates morphological dissimilarity using distribution measure.", },
		{ "simil_density", 0, 0, "Density of surface sampling", "f 1 100 10", FIELD(density), "", },
		{ "simil_bin_num", 0, 0, "Number of bins", "d 1 1000 128", FIELD(bin_num), "", },
		{ "simil_samples_num", 0, 0, "Number of samples", "d 1 1048576 10000", FIELD(samples_num), "", }, //based on experiments, not much accuracy to gain when this is increased above 1000
		{ "evaluateDistance", 0, PARAM_DONTSAVE | PARAM_USERHIDDEN, "Evaluate model dissimilarity", "p f(oGeno,oGeno)", PROCEDURE(p_evaldistance), "Calculates dissimilarity between two models created from Geno objects.", },
		{ 0, },
};

#undef FIELDSTRUCT

SimilMeasureDistribution::SimilMeasureDistribution() : localpar(simil_distribution_paramtab, this)
{
	localpar.setDefault();
	SimilMeasureDistribution::distribution_fun = &SimilMeasureDistribution::D2; //D1 and D2 are the best descriptors
}

double SimilMeasureDistribution::getDistance()
{
	double dist = 0;
	for (int i = 0; i < 2; i++)
	{
		funs[i] = new std::pair<double, float>[bin_num]();
		for (int j = 0; j < bin_num; j++)
			funs[i][j] = std::make_pair(0, 0);
	}

	for (int i = 0; i < 2; i++)
		sst_models[i] = new SolidsShapeTypeModel((*models[i]));

	SimilMeasureDistribution::calculateFuns();
	dist = SimilMeasureDistribution::compareFuns();

	for (int i = 0; i < 2; i++)
	{
		SAFEDELETE(sst_models[i]);
		SAFEDELETEARRAY(funs[i]);
	}
	return dist;
}

int SimilMeasureDistribution::setParams(std::vector<double> params)
{
	for (unsigned int i = 0; i < params.size(); i++)
		if (params.at(i) <= 0)
		{
			logPrintf("SimilDistributionMeasure", "setParams", LOG_ERROR, "Param values should be larger than 0.");
			return -1;
		}

	density = params.at(0);
	bin_num = params.at(1);
	samples_num = params.at(2);

	return 0;
}

void SimilMeasureDistribution::calculateFun(std::pair<double, float> *fun, const Model &sampled)
{
	int samples_taken = samples_num;

	//Check if total number of point pairs is smaller than samples number (just to avoid the calculation of the same stats for the same pairs of points).
	//This optimization turned out to have a minor effect, only present for very high simil_samples_num, and was not perfect anyway:
	//- samples are selected randomly so there are no guarantees that they will be repeated,
	//- even if they do, it has the benefit of averaging the result that becomes more stable,
	//- the concept of "point pairs" is not relevant when we randomly select more than two points, as some descriptors do.
	//int size = sampled.getPartCount();
	//if (size < (int) sqrt((double) std::numeric_limits<int>::max())) //prevent exceeding int limits
	//	samples_taken = std::min(samples_num, size*size);

	rndgen.seed(55); //For determinism. Otherwise the descriptors (that choose samples pseudo-randomly) for the same Model can yield different values and so the dissimilarity between the object and its copy will not be 0.
	std::uniform_int_distribution<> uniform_distrib(0, sampled.getPartCount() - 1);

	//Get sampled distribution
	std::vector<double> dist_vect;
	dist_vect.reserve(samples_taken); //we will add up to samples_taken elements to this vector
	(this->*SimilMeasureDistribution::distribution_fun)(samples_taken, uniform_distrib, sampled, dist_vect);

	auto result = std::minmax_element(dist_vect.begin(), dist_vect.end());
	double min = *result.first;
	double max = *result.second;

	//Create histogram
	vector<int> hist(bin_num);
	int ind = 0;

	for (unsigned int j = 0; j < dist_vect.size(); j++)
	{
		ind = (int)std::floor((dist_vect.at(j) - min) * 1 / (max - min) * bin_num);
		if (ind <= (bin_num - 1))
			hist[ind]++;
		else if (ind == bin_num)
			hist[bin_num - 1]++;
	}

	//Create pairs
	for (int j = 0; j < bin_num; j++)
	{
		fun[j] = std::make_pair(min + (max - min) / bin_num * (j + 0.5), hist[j]);
	}

	//Normalize
	float total_mass = 0;
	for (int j = 0; j < bin_num; j++)
	{
		total_mass += fun[j].second;
	}

	for (int j = 0; j < bin_num; j++)
		fun[j].second /= total_mass;
}

void SimilMeasureDistribution::calculateFuns()
{
	for (int i = 0; i < 2; i++)
	{
		Model sampled = SimilMeasureDistribution::sampleSurface(&sst_models[i]->getModel(), density);
		SimilMeasureDistribution::calculateFun(funs[i], sampled);
	}
}

double SimilMeasureDistribution::compareFuns()
{
	return SimilMeasureDistribution::EMD(funs[0], funs[1]);
}

void SimilMeasureDistribution::D1(int samples_taken, std::uniform_int_distribution<> &distribution, const Model &sampled, std::vector<double> &dist_vect)
{
	int size = sampled.getPartCount();
	double x = 0;
	double y = 0;
	double z = 0;

	for (int i = 0; i < size; i++)
	{
		Pt3D pos = sampled.getPart(i)->p;
		x += pos.x;
		y += pos.y;
		z += pos.z;
	}

	x = x / size;
	y = y / size;
	z = z / size;

	Pt3D centroid = { x, y, z };

	for (int i = 0; i < samples_taken; i++)
	{
		int p1 = distribution(rndgen);
		double dist = sampled.getPart(p1)->p.distanceTo(centroid);
		if (dist > 0)
			dist_vect.push_back(dist);
	}
}

void SimilMeasureDistribution::D2(int samples_taken, std::uniform_int_distribution<> &distribution, const Model &sampled, std::vector<double> &dist_vect)
{
	for (int i = 0; i < samples_taken; i++)
	{
		int p1 = distribution(rndgen);
		int p2 = distribution(rndgen);
		double dist = sampled.getPart(p1)->p.distanceTo(sampled.getPart(p2)->p);
		if (dist > 0)
			dist_vect.push_back(dist);
	}
}

void SimilMeasureDistribution::D3(int samples_taken, std::uniform_int_distribution<> &distribution, const Model &sampled, std::vector<double> &dist_vect)
{
	for (int i = 0; i < samples_taken; i++)
	{
		int p1 = distribution(rndgen);
		int p2 = distribution(rndgen);
		int p3 = distribution(rndgen);

		Pt3D v(sampled.getPart(p2)->p);
		Pt3D w(sampled.getPart(p3)->p);
		v -= sampled.getPart(p1)->p;
		w -= sampled.getPart(p1)->p;
		Pt3D cross_prod(0);
		cross_prod.vectorProduct(v, w);

		double dist = 0.5 * cross_prod.length();
		if (dist > 0)
			dist_vect.push_back(dist);
	}
}

void SimilMeasureDistribution::D4(int samples_taken, std::uniform_int_distribution<> &distribution, const Model &sampled, std::vector<double> &dist_vect)
{
	for (int i = 0; i < samples_taken; i++)
	{
		int a = distribution(rndgen);
		int b = distribution(rndgen);
		int c = distribution(rndgen);
		int d = distribution(rndgen);

		Pt3D ad(sampled.getPart(a)->p);
		Pt3D bd(sampled.getPart(b)->p);
		Pt3D cd(sampled.getPart(c)->p);

		ad -= sampled.getPart(d)->p;
		bd -= sampled.getPart(d)->p;
		cd -= sampled.getPart(d)->p;

		Pt3D cross_prod(0);
		cross_prod.vectorProduct(bd, cd);
		cross_prod.entrywiseProduct(ad);

		double dist = cross_prod.length() / 6;
		if (dist > 0)
			dist_vect.push_back(dist);
	}
}

void SimilMeasureDistribution::A3(int samples_taken, std::uniform_int_distribution<> &distribution, const Model &sampled, std::vector<double> &dist_vect)
{
	for (int i = 0; i < samples_taken; i++)
	{
		int p1 = distribution(rndgen);
		int p2 = distribution(rndgen);
		int p3 = distribution(rndgen);
		double a = sampled.getPart(p1)->p.distanceTo(sampled.getPart(p3)->p);
		double b = sampled.getPart(p3)->p.distanceTo(sampled.getPart(p2)->p);
		double c = sampled.getPart(p1)->p.distanceTo(sampled.getPart(p2)->p);
		double beta = acos((a * a + b * b - c * c) / (2 * a * b));

		if (!std::isnan(beta))
			dist_vect.push_back(beta);
	}
}


float dist(feature_t* F1, feature_t* F2)
{
	return abs((*F1) - (*F2));
}


void SimilMeasureDistribution::fillPointsWeights(std::pair<double, float> *fun, feature_t *points, float *weights)
{
	for (int j = 0; j < bin_num; j++)
	{
		points[j] = { fun[j].first };
		weights[j] = fun[j].second;
	}
}

double SimilMeasureDistribution::EMD(std::pair<double, float> *fun1, std::pair<double, float> *fun2)
{
	feature_t *points[2];
	float *weights[2];

	for (int i = 0; i < 2; i++)
	{
		points[i] = new feature_t[bin_num];
		weights[i] = new float[bin_num]();
	}
	SimilMeasureDistribution::fillPointsWeights(fun1, points[0], weights[0]);
	SimilMeasureDistribution::fillPointsWeights(fun2, points[1], weights[1]);

	signature_t sig1 = { bin_num, points[0], weights[0] },
		sig2 = { bin_num, points[1], weights[1] };

	float e = emd(&sig1, &sig2, dist, 0, 0, bin_num, bin_num);

	for (int i = 0; i < 2; i++)
	{
		delete[] points[i];
		delete[] weights[i];
	}

	return e;
}