// 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 "measure-mds-based.h"
#include "SVD/matrix_tools.h"
#include <assert.h>

SimilMeasureMDSBased::SimilMeasureMDSBased()
{
	coordinates[0] = nullptr;
	coordinates[1] = nullptr;
	
	with_alignment = true;
	save_matching = false;
	fixedZaxis = 0;
	wMDS = 1;
}

double SimilMeasureMDSBased::getDistance()
{
	double dResult = -1;
	m_iSmaller = models[0]->getPartCount() <= models[1]->getPartCount() ? 0 : 1;

	prepareData();
	
	if (with_alignment)
	{
		if (!getPartPositions())
		{
			logPrintf("SimilMDSBasedMeasure", "getDistance", LOG_ERROR, "Cannot compute part positions");
			return -1;
		}
		
		if (!computePartsPositionsByMDS())
		{
			logPrintf("SimilMDSBasedMeasure", "getDistance", LOG_ERROR, "Cannot perform MDS");
			return -1;
		}

		// tutaj zacznij pętlę po przekształceniach  geometrycznych
		const int NO_OF_TRANSFORM = 8; // liczba transformacji geometrycznych (na razie tylko ID i O_YZ)
		// tablice transformacji współrzędnych; nie są to dokładnie tablice tranformacji, ale raczej tablice PRZEJŚĆ
		// pomiędzy transformacjami; 
		const int dMulX[NO_OF_TRANSFORM] = { 1, -1, -1, 1, -1, 1, -1, -1 };
		const int dMulY[NO_OF_TRANSFORM] = { 1, 1, -1, -1, -1, -1, -1, 1 };
		const int dMulZ[NO_OF_TRANSFORM] = { 1, 1, 1, -1, -1, -1, 1, 1 };

		#ifdef max
		#undef max //this macro would conflict with line below
		#endif
		double dMinSimValue = std::numeric_limits<double>::max(); // minimum value of similarity
		
		int iTransform; // a counter of geometric transformations
		for (iTransform = 0; iTransform < NO_OF_TRANSFORM; iTransform++)
		{
			beforeTransformation();
			
			for (int iPart = 0; iPart < models[m_iSmaller]->getPartCount(); iPart++)
			{
				// for each iPart, a part of the model iMod
				coordinates[m_iSmaller][iPart].x *= dMulX[iTransform];
				coordinates[m_iSmaller][iPart].y *= dMulY[iTransform];
				coordinates[m_iSmaller][iPart].z *= dMulZ[iTransform];
			}
			// now the positions are recomputed
			double dCurrentSim = distanceForTransformation();

			// załóż poprawną wartość podobieństwa
			assert(dCurrentSim >= 0.0);

			// porównaj wartość obliczoną z dotychczasowym minimum
			if (dCurrentSim < dMinSimValue)
			{
			//printf("lesser sim\n");
				dMinSimValue = dCurrentSim;
				if (save_matching)
					copyMatching();
			}
		}
		
		dResult = dMinSimValue;
		
		SAFEDELETEARRAY(coordinates[0]);
		SAFEDELETEARRAY(coordinates[1]);
	}
	else
	{
		dResult = distanceWithoutAlignment();
	}

	cleanData();

	return dResult;
}


/** Gets information about Parts' positions in 3D from models into the arrays
		coordinates.
		Assumptions:
		- Models (models) are created and available.
		- Arrays coordinates are created.
		@return 1 if success, otherwise 0.
		*/
int SimilMeasureMDSBased::getPartPositions()
{	
	// for each model copy its part coordinates
	Part *pPart;
	for (int iMod = 0; iMod < 2; iMod++)
	{
		// utworz tablice dla pozycji 3D Parts (wielkosc tablicy: liczba Parts organizmu)
		coordinates[iMod] = new Pt3D[models[iMod]->getPartCount()];
		assert(coordinates[iMod] != NULL);
		// dla każdego z modeli iMod
		for (int iPart = 0; iPart < models[iMod]->getPartCount(); iPart++)
		{
			// dla każdego iPart organizmu iMod
			// pobierz tego Part
			pPart = models[iMod]->getPart(iPart);
			// zapamietaj jego pozycje 3D w tablicy coordinates
			coordinates[iMod][iPart].x = pPart->p.x;
			coordinates[iMod][iPart].y = pPart->p.y;
			coordinates[iMod][iPart].z = pPart->p.z;
		}
	}

	return 1;
}

bool SimilMeasureMDSBased::computePartsPositionsByMDS()
{
	bool bResult = true;
	
	// use MDS to obtain different point of view on organisms
	// and store the new positions (currently the original ones are still stored)
	for (int iMod = 0; iMod < 2; iMod++)
	{
		// prepare the vector of errors of approximation for the SVD
		std::vector<double> vEigenvalues;
		int nSize = models[iMod]->getPartCount();

		double *pDistances = new double[nSize * nSize];

		for (int i = 0; i < nSize; i++)
		{
			pDistances[i] = 0;
		}

		Model *pModel = models[iMod]; // use the model of the iMod (current) organism
		int iP1, iP2; // indices of Parts in the model
		Part *P1, *P2; // pointers to Parts
		Pt3D P1Pos, P2Pos; // positions of parts 
		double dDistance; // the distance between Parts

		double *weights = new double[nSize];
		for (int i = 0; i < nSize; i++)
		{
			if (wMDS == 1)
				weights[i] = 0;
			else
				weights[i] = 1;
		}

		if (wMDS == 1)
			for (int i = 0; i < pModel->getJointCount(); i++)
			{
				weights[pModel->getJoint(i)->p1_refno]++;
				weights[pModel->getJoint(i)->p2_refno]++;
			}

		for (iP1 = 0; iP1 < pModel->getPartCount(); iP1++)
		{
			// for each iP1, a Part index in the model of organism iMod
			P1 = pModel->getPart(iP1);
			// get the position of the Part
			P1Pos = P1->p;
			if (fixedZaxis == 1)
			{
				P1Pos.z = 0; //fixed vertical axis, so pretend all points are on the xy plane
			}
			for (iP2 = 0; iP2 < pModel->getPartCount(); iP2++)
			{
				// for each (iP1, iP2), a pair of Parts index in the model
				P2 = pModel->getPart(iP2);
				// get the position of the Part
				P2Pos = P2->p;
				if (fixedZaxis == 1)
				{
					P2Pos.z = 0; //fixed vertical axis, so pretend all points are on the xy plane
				}
				// compute the geometric (Euclidean) distance between the Parts
				dDistance = P1Pos.distanceTo(P2Pos);
				// store the distance
				pDistances[iP1 * nSize + iP2] = dDistance;
			} // for (iP2)
		} // for (iP1)
		
		MatrixTools::weightedMDS(vEigenvalues, nSize, pDistances, coordinates[iMod], weights);
		
		if (fixedZaxis == 1) //restore the original vertical coordinate of each Part
		{
			for (int part = 0; part < pModel->getPartCount(); part++)
			{
				coordinates[iMod][part].z = pModel->getPart(part)->p.z;
			}
		}
		
		delete[] pDistances;
		delete[] weights;
	}

	return bResult;
}