#include <LBBCKMeansClusterGenerator.h>

namespace LBBC {

void KMeansClusterGenerator::setAlpha(float value)
{
	mAlpha = value;
}

float KMeansClusterGenerator::getAlpha()
{
	return mAlpha;
}

void KMeansClusterGenerator::setNumIterations(unsigned int value)
{
	mNumIterations = value;
}

unsigned int KMeansClusterGenerator::getNumIterations()
{
	return mNumIterations;
}

unsigned int KMeansClusterGenerator::findBestBillboard(Leaf *leaf)
{
	float minError = FLT_MAX;
	unsigned int iMinErrorBillboard = 0;
	
	Ogre::Vector3 normalLeaf = leaf->getLeafNormal();
	Ogre::Vector3 positionLeaf = leaf->getPosition();		
	
	for (unsigned int iBillboard = 0; iBillboard < mBillboardCloud->getNumBillboards(); iBillboard++)
	{
		BBC::BillboardPtr billboard = mBillboardCloud->getBillboard(iBillboard);
		BillboardKMeansClusterData *billboardClusterData = (BillboardKMeansClusterData*)billboard->getBillboardClusterData().get();
		Ogre::Vector3 normalBillboard = billboardClusterData->getNormal();			

		float d = (normalBillboard.dotProduct(positionLeaf) +  billboardClusterData->getD());
		float cosine = normalBillboard.dotProduct(normalLeaf);
		float error = (d*d) + ( 1 - cosine * cosine ) * mAlpha;

		if (error < minError)
		{
			minError = error;
			iMinErrorBillboard = billboard->getBillboardHandle();
		}				
	}
	return iMinErrorBillboard;
}

void KMeansClusterGenerator::assignLeafBillboard(Leaf *leaf, BBC::BillboardPtr billboard)
{
	BillboardKMeansClusterData *billboardClusterData = (BillboardKMeansClusterData*)billboard->getBillboardClusterData().get();			
	BBC::EntityClusterPtr entityCluster = billboardClusterData->getEntityCluster();

	BBC::EntityClusterDataPtr entityClusterData = BBC::EntityClusterDataPtr( (BBC::EntityClusterData*) new LeafKMeansClusterData() );
	entityClusterData->setEntity(leaf);
	entityCluster->addEntityClusterData(entityClusterData);
}

void KMeansClusterGenerator::splitLeafDistribution()
{
	for (unsigned int iLeaf = 0; iLeaf < mEntityDistribution->getNumEntities(); iLeaf++)
	{
		Leaf *leaf = (Leaf*)mEntityDistribution->getEntity(iLeaf).get();
		unsigned int iMinErrorBillboard = findBestBillboard(leaf);

		BBC::BillboardPtr billboardMinError = mBillboardCloud->getBillboard(iMinErrorBillboard);
		assignLeafBillboard(leaf,billboardMinError);
	}
}

void KMeansClusterGenerator::recomputeBillboard(BBC::BillboardPtr billboard)
{
	BillboardKMeansClusterData *billboardClusterData = (BillboardKMeansClusterData*)billboard->getBillboardClusterData().get();			
	BBC::EntityClusterPtr entityCluster = billboardClusterData->getEntityCluster();

	if (entityCluster->getNumEntitiesClusterData() > 1)
	{
		Ogre::Matrix3 nmii = Ogre::Matrix3::ZERO;
		Ogre::Matrix3 nmiiSum = Ogre::Matrix3::ZERO;
		Ogre::Matrix3 miiSum = Ogre::Matrix3::ZERO;
		Ogre::Matrix3 mii = Ogre::Matrix3::ZERO;	
		Ogre::Matrix3 mijSum = Ogre::Matrix3::ZERO;
		Ogre::Matrix3 mij = Ogre::Matrix3::ZERO; 
		Ogre::Vector3 lastY = Ogre::Vector3::ZERO;
		Ogre::Vector3 piSum = Ogre::Vector3::ZERO;
		Ogre::Vector3 normSum = Ogre::Vector3::ZERO;

		for (unsigned int iLeaf = 0; iLeaf < entityCluster->getNumEntitiesClusterData(); iLeaf++)
		{
			Leaf* leaf = (Leaf*)entityCluster->getEntityClusterData(iLeaf)->getEntity().get();
			Ogre::Vector3 pi = leaf->getPosition();	
			Ogre::Vector3 norm2 = leaf->getLeafNormal();	
			piSum = piSum + pi * (1.0 / (float)entityCluster->getNumEntitiesClusterData());
				
			mii[0][0] = pi.x * pi.x;
			mii[0][1] = pi.x * pi.y;
			mii[0][2] = pi.x * pi.z;
			mii[1][0] = pi.y * pi.x;
			mii[1][1] = pi.y * pi.y;
			mii[1][2] = pi.y * pi.z;
			mii[2][0] = pi.z * pi.x;
			mii[2][1] = pi.z * pi.y;
			mii[2][2] = pi.z * pi.z;	
			nmii[0][0] = norm2.x * norm2.x;
			nmii[0][1] = norm2.x * norm2.y;
			nmii[0][2] = norm2.x * norm2.z;
			nmii[1][0] = norm2.y * norm2.x;
			nmii[1][1] = norm2.y * norm2.y;
			nmii[1][2] = norm2.y * norm2.z;
			nmii[2][0] = norm2.z * norm2.x;
			nmii[2][1] = norm2.z * norm2.y;
			nmii[2][2] = norm2.z * norm2.z;	
			
			nmiiSum = nmiiSum + ( nmii * (1.0 / (float)entityCluster->getNumEntitiesClusterData()));

			miiSum = miiSum + ( mii * (1.0 / (float)entityCluster->getNumEntitiesClusterData()));

			// Generate the initial value for the iterative method as the average...
			Ogre::Vector3 norm = leaf->getLeafNormal();
			lastY = lastY + norm;

			// Generate the sum normal of all the leaves associated to the plane...
			normSum = normSum + norm;
		}
		
		mijSum[0][0] = piSum.x * piSum.x;
		mijSum[0][1] = piSum.x * piSum.y;
		mijSum[0][2] = piSum.x * piSum.z;
		mijSum[1][0] = piSum.y * piSum.x;
		mijSum[1][1] = piSum.y * piSum.y;
		mijSum[1][2] = piSum.y * piSum.z;
		mijSum[2][0] = piSum.z * piSum.x;
		mijSum[2][1] = piSum.z * piSum.y;
		mijSum[2][2] = piSum.z * piSum.z;	

		Ogre::Matrix3 mA = miiSum - mijSum;
		mA = mA - (mAlpha * nmiiSum);
		mA = mA.Inverse();

		lastY.normalise();
		normSum.normalise();

		// Apply the iterative approach
		Ogre::Vector3 currY;
		currY = mA * lastY;
		currY.normalise();

		for(unsigned int icount = 0; icount < 100; icount++)
		{
			lastY = currY;
			currY = mA * lastY;
			currY.normalise();
		}

		// Generate the d parameter...
		float sumD = 0;
		for (unsigned int iLeaf = 0; iLeaf < entityCluster->getNumEntitiesClusterData(); iLeaf++)
		{
			Leaf* leaf = (Leaf*)entityCluster->getEntityClusterData(iLeaf)->getEntity().get();
			Ogre::Vector3 pi = leaf->getPosition();	
			sumD = sumD + pi.dotProduct(currY);
		}

		sumD = - sumD / (float)entityCluster->getNumEntitiesClusterData();

		billboardClusterData->setNormal(currY);
		billboardClusterData->setD(sumD);
	}
	else if (entityCluster->getNumEntitiesClusterData() == 1)
	{
		Leaf* leaf = (Leaf*)entityCluster->getEntityClusterData(0)->getEntity().get();
		billboardClusterData->setNormal(leaf->getLeafNormal());
		billboardClusterData->setD(leaf->getLeafD());
	}
}

void KMeansClusterGenerator::recomputeBillboardCloud()
{
	for (unsigned int iBillboard = 0; iBillboard < mBillboardCloud->getNumBillboards(); iBillboard++)
	{
		BBC::BillboardPtr billboard = mBillboardCloud->getBillboard(iBillboard);
		recomputeBillboard(billboard);
	}
}

void KMeansClusterGenerator::iterativeRecomputeBillboardCloud()
{
	for (unsigned int iIteration = 0; iIteration < mNumIterations; iIteration++)
	{
		recomputeBillboardCloud();
	}
}

void KMeansClusterGenerator::generate() 
{
	// Generate the initial random billboards and cluster the leaves with them
	// 1. Create the billboards and assign an initial random orientation
	initializeBillboardCloud();
	// 2. Split the leaf distribution, each leaf in the it's best candidate billboard
	splitLeafDistribution();
	// 3. The billboard is recomputed in order to minimize the total error 
	// for the leaves of this cluster with respect to this plane.
	iterativeRecomputeBillboardCloud();
	// 4. Generate each entity cluster packed entity and compute the bounding quad for each entity cluster
	generateBillboardCloudBounds();
}

BBC::BillboardPtr KMeansClusterGenerator::createBillboard()
{
	BBC::BillboardPtr billboard = BBC::BillboardPtr( new BBC::Billboard() );
	mBillboardCloud->addBillboard(billboard);
	return billboard;
}

void KMeansClusterGenerator::initializeBillboardClusterData(BBC::BillboardPtr billboard)
{
	BBC::BillboardClusterDataPtr billboardClusterData = BBC::BillboardClusterDataPtr( new BillboardKMeansClusterData() );
	billboard->setBillboardClusterData(billboardClusterData);
	BBC::EntityClusterPtr entityCluster = BBC::EntityClusterPtr( new BBC::EntityCluster() );
	billboard->getBillboardClusterData()->setEntityCluster(entityCluster);
}

void KMeansClusterGenerator::generateBillboardCloudBounds()
{
	for (unsigned int iBillboard = 0; iBillboard < this->getMaxNumBillboards(); iBillboard++)
	{
		BBC::BillboardPtr billboard = mBillboardCloud->getBillboard(iBillboard);		
		BBC::EntityClusterPtr entityCluster = billboard->getBillboardClusterData()->getEntityCluster();

		if (entityCluster->getNumEntitiesClusterData() > 0)
		{					
			entityCluster->generateEntityCluster();
			billboard->getBillboardClusterData()->generateBillboardBoundingQuad();
		}
	}
}

void KMeansClusterGenerator::initializeRandomBillboard(BBC::BillboardPtr billboard)
{
	float dMin = ((LeafDistribution*)mEntityDistribution)->getMinD();
	float dMax = ((LeafDistribution*)mEntityDistribution)->getMaxD();

	float d = Ogre::Math::RangeRandom(dMin,dMax);

	//Ogre::Vector3 normalMin = ((LeafDistribution*)mEntityDistribution)->getMinNormal();
	//Ogre::Vector3 normalMax = ((LeafDistribution*)mEntityDistribution)->getMaxNormal();
	Ogre::Vector3 normal;
	normal.x = Ogre::Math::RangeRandom(-1,1); // Ogre::Math::RangeRandom(normalMin.x,normalMax.x);
	normal.y = Ogre::Math::RangeRandom(-1,1); // Ogre::Math::RangeRandom(normalMin.y,normalMax.y);
	normal.z = Ogre::Math::RangeRandom(-1,1); // Ogre::Math::RangeRandom(normalMin.z,normalMax.z);
	
	BillboardKMeansClusterData *mBillboardClusterData = (BillboardKMeansClusterData *)billboard->getBillboardClusterData().get();
	mBillboardClusterData->setNormal(normal);
	mBillboardClusterData->setD(d);
}

void KMeansClusterGenerator::initializeBillboardCloud()
{
	for (unsigned int iBillboard = 0; iBillboard < this->getMaxNumBillboards(); iBillboard++)
	{
		BBC::BillboardPtr billboard = createBillboard();
		initializeBillboardClusterData(billboard);
		initializeRandomBillboard(billboard);
	}
}

void KMeansClusterGenerator::init() 
{
}

KMeansClusterGenerator::KMeansClusterGenerator() 
{
}

KMeansClusterGenerator::~KMeansClusterGenerator() 
{

}


}