#include <boost/foreach.hpp>
#include "cinder/app/AppBasic.h"
#include "cinder/Rand.h"
#include "cinder/Vector.h"
#include "Controller.h"
#include "SpringCam.h"

using namespace ci;
using std::list;
using std::vector;

Controller::Controller()
{
}

void Controller::init( Room *room )
{
	mRoom			 = room;

	mNumMatter		= 0;
	mNumAntimatter	= 0;

	mPreset			= 0;
	mRecycle		= false;

	createSphere( mSphereLo, 2 );
	createSphere( mSphereHi, 3 );
}

void Controller::createSphere( gl::VboMesh &vbo, int res )
{
	float X = 0.525731112119f;
	float Z = 0.850650808352f;

	static Vec3f verts[12] = {
		Vec3f( -X, 0.0f, Z ), Vec3f( X, 0.0f, Z ), Vec3f( -X, 0.0f, -Z ), Vec3f( X, 0.0f, -Z ),
		Vec3f( 0.0f, Z, X ), Vec3f( 0.0f, Z, -X ), Vec3f( 0.0f, -Z, X ), Vec3f( 0.0f, -Z, -X ),
		Vec3f( Z, X, 0.0f ), Vec3f( -Z, X, 0.0f ), Vec3f( Z, -X, 0.0f ), Vec3f( -Z, -X, 0.0f ) };

	static GLuint triIndices[20][3] = {
		{0,4,1}, {0,9,4}, {9,5,4}, {4,5,8}, {4,8,1}, {8,10,1}, {8,3,10}, {5,3,8}, {5,2,3}, {2,7,3},
		{7,10,3}, {7,6,10}, {7,11,6}, {11,0,6}, {0,1,6}, {6,1,10}, {9,0,11}, {9,11,2}, {9,2,5}, {7,2,11} };

	gl::VboMesh::Layout layout;
	layout.setStaticIndices();
	layout.setStaticPositions();
	layout.setStaticNormals();
	layout.setStaticTexCoords2d();

	mIndex = 0;
	mPosCoords.clear();
	mNormals.clear();
	mIndices.clear();
	mTexCoords.clear();
	for( int i=0; i<20; i++ ){
		drawSphereTri( verts[triIndices[i][0]], verts[triIndices[i][1]], verts[triIndices[i][2]], res );
	}
	vbo = gl::VboMesh( mPosCoords.size(), mIndices.size(), layout, GL_TRIANGLES );
	vbo.bufferIndices( mIndices );
	vbo.bufferPositions( mPosCoords );
	vbo.bufferNormals( mNormals );
	vbo.bufferTexCoords2d( 0, mTexCoords );
}

void Controller::drawSphereTri( Vec3f va, Vec3f vb, Vec3f vc, int div )
{
	if( div <= 0 ){
		mPosCoords.push_back( va );
		mPosCoords.push_back( vb );
		mPosCoords.push_back( vc );
		mIndices.push_back( mIndex++ );
		mIndices.push_back( mIndex++ );
		mIndices.push_back( mIndex++ );
		Vec3f vn = ( va + vb + vc ) * 0.3333f;
		mNormals.push_back( va );
		mNormals.push_back( vb );
		mNormals.push_back( vc );

		float u1 = ( (float)atan( va.y/(va.x+0.0001f) ) + (float)M_PI * 0.5f ) / (float)M_PI;
		float u2 = ( (float)atan( vb.y/(vb.x+0.0001f) ) + (float)M_PI * 0.5f ) / (float)M_PI;
		float u3 = ( (float)atan( vc.y/(vc.x+0.0001f) ) + (float)M_PI * 0.5f ) / (float)M_PI;

		if( va.x <= 0.0f ){
			u1 = u1 * 0.5f + 0.5f;
		} else {
			u1 *= 0.5f;
		}

		if( vb.x <= 0.0f ){
			u2 = u2 * 0.5f + 0.5f;
		} else {
			u2 *= 0.5f;
		}

		if( vc.x <= 0.0f ){
			u3 = u3 * 0.5f + 0.5f;
		} else {
			u3 *= 0.5f;
		}

		float v1 = (float)acos( va.z ) / (float)M_PI;
		float v2 = (float)acos( vb.z ) / (float)M_PI;
		float v3 = (float)acos( vc.z ) / (float)M_PI;

		mTexCoords.push_back( Vec2f( u1, v1 ) );
		mTexCoords.push_back( Vec2f( u2, v2 ) );
		mTexCoords.push_back( Vec2f( u3, v3 ) );
	} else {
		Vec3f vab = ( ( va + vb ) * 0.5f ).normalized();
		Vec3f vac = ( ( va + vc ) * 0.5f ).normalized();
		Vec3f vbc = ( ( vb + vc ) * 0.5f ).normalized();
		drawSphereTri( va, vab, vac, div-1 );
		drawSphereTri( vb, vbc, vab, div-1 );
		drawSphereTri( vc, vac, vbc, div-1 );
		drawSphereTri( vab, vbc, vac, div-1 );
	}
}

void Controller::applyShockwavesToTime()
{
	for( vector<Shockwave>::iterator shockIt = mShockwaves.begin(); shockIt != mShockwaves.end(); ++shockIt )
	{
		Vec3f roomCenter = Vec3f::zero();
		Vec3f dirToParticle = shockIt->mPos - roomCenter;
		float dist = dirToParticle.length();
		if( dist > shockIt->mRadiusPrev && dist < shockIt->mRadius ){
			mRoom->mTimeElapsed += ( dist - shockIt->mRadiusPrev )/( shockIt->mRadius - shockIt->mRadiusPrev ) * 10.0f;
		}
	}
}

void Controller::applyShockwavesToCam( SpringCam &cam )
{
	for( vector<Shockwave>::iterator shockIt = mShockwaves.begin(); shockIt != mShockwaves.end(); ++shockIt )
	{
		Vec3f dirToParticle = shockIt->mPos - cam.mEyeNode.mPos;
		float dist = dirToParticle.length();
		if( dist > shockIt->mRadiusPrev && dist < shockIt->mRadius ){
			Vec3f dirToParticleNorm = dirToParticle.normalized();
			cam.mEyeNode.mAcc -= dirToParticleNorm * shockIt->mImpulse * 5.0f;
		}

		dirToParticle = shockIt->mPos - cam.mCenNode.mPos;
		dist = dirToParticle.length();
		if( dist > shockIt->mRadiusPrev && dist < shockIt->mRadius ){
			Vec3f dirToParticleNorm = dirToParticle.normalized();
			cam.mCenNode.mAcc -= dirToParticleNorm * shockIt->mImpulse * 1.0f;
		}

		dirToParticle = shockIt->mPos - cam.mUpNode.mPos;
		dist = dirToParticle.length();
		if( dist > shockIt->mRadiusPrev && dist < shockIt->mRadius ){
			Vec3f dirToParticleNorm = dirToParticle.normalized();
			cam.mUpNode.mAcc -= dirToParticleNorm * shockIt->mImpulse * 1.0f;
		}
	}
}

void Controller::applyForceToParticles()
{
	float prevNumMatter		= mNumMatter;
	float prevNumAntimatter = mNumAntimatter;
	float matterRange		= 1000.0f - math<float>::min( prevNumMatter, 400.0f ) * 2.0f;
	matterRange				*= matterRange * 3.0f;
	float antimatterRange	= 1000.0f - math<float>::min( prevNumAntimatter, 400.0f ) * 2.0f;
	antimatterRange			*= antimatterRange * 3.0f;

//	matterRange = 1000.0f;
//	antimatterRange = 1000.0f;

	mNumMatter		= 0;
	mNumAntimatter	= 0;

	for( list<Particle>::iterator it1 = mParticles.begin(); it1 != mParticles.end(); ++it1 )
	{
		float zoneRadiusSqrd;
		if( it1->mCharge < 0.0f ){
			mNumAntimatter ++;
			zoneRadiusSqrd = antimatterRange;
		} else {
			mNumMatter ++;
			zoneRadiusSqrd = matterRange;
		}

		// APPLY SHOCKWAVES TO OTHER PARTICLES
		for( vector<Shockwave>::iterator shockIt = mShockwaves.begin(); shockIt != mShockwaves.end(); ++shockIt )
		{
			Vec3f dirToParticle = shockIt->mPos - it1->mPos;
			float dist = dirToParticle.length();
			if( dist > shockIt->mRadiusPrev && dist < shockIt->mRadius ){
				Vec3f dirToParticleNorm = dirToParticle.normalized();
				it1->mAcc -= dirToParticleNorm * shockIt->mImpulse * 2.5f;
				it1->mFusionThresh = 1.0f;
			}
		}

		list<Particle>::iterator it2 = it1;
		for( std::advance( it2, 1 ); it2 != mParticles.end(); ++it2 ) {
			Vec3f dir = it1->mPos - it2->mPos;
			float distSqrd = dir.lengthSquared();

			if( distSqrd < zoneRadiusSqrd ){
				float totalCharge = it1->mCharge * it2->mCharge;
				bool shouldAnnihilate = false;
				if( totalCharge < 0.0f )
					shouldAnnihilate = distSqrd < ( 2.0f + ( it1->mFusionThresh + it2->mFusionThresh ) * 10.5f );

				// lit room
				if( mRoom->getPower() < 0.5f ){
					Vec3f moveVec		= it2->mVel - it1->mVel;
					Vec3f dirNormal		= dir.normalized();

					float dist			= sqrt( distSqrd );
					float invDistSqrd	= 1.0f/distSqrd;

					float sumRadii		= ( it1->mRadius + it2->mRadius );
					float sumRadiiSqrd	= sumRadii * sumRadii;
					float sumMass		= it1->mMass + it2->mMass + 0.001f;

					bool collision		= didParticlesCollide( dir, dirNormal, dist, sumRadii, sumRadiiSqrd, &moveVec );

					if( collision )
					{
						float invSumMass	= 1.0f/(float)sumMass;

						float a1	= it1->mVel.dot( dirNormal );
						float a2	= it2->mVel.dot( dirNormal );
						float pVar	= ( 2.0f * ( a1 - a2 ) ) * invSumMass;

						dist -= sumRadii;

						if( dist < 0.0f ){
							float per1	= it1->mMass * invSumMass;
							float per2	= 1.0f - per1;
							Vec3f off	= dirNormal * dist;

							it1->mPos -= off * per2;
							it1->mVel -= off * per2 * 0.5f;

							it2->mPos += off * per1;
							it2->mVel += off * per1 * 0.5f;
						}

						float collisionDecay = 0.75f;
						Vec3f newDir = pVar * dirNormal * collisionDecay;
						it1->mVel -= it2->mMass * newDir;
						it2->mVel += it1->mMass * newDir;

//						float cDecay = 0.75f;
//						Vec3f newDir = pVar * dirNormal * cDecay;
////							it1->mPos	-= moveVec * 0.5f;
////							it2->mPos	+= moveVec * 0.5f;
//						it1->mVel	-= it2->mMass * newDir;
//						it2->mVel	+= it1->mMass * newDir;
					}

					float F = ( it1->mMass * it2->mMass ) * invDistSqrd * 0.000015 * mRoom->getTimeDelta();
					it1->mVel -= F*dirNormal*it1->mInvMass;
					it2->mVel += F*dirNormal*it2->mInvMass;

				// dark room
				} else {

					if( it1->mDistToClosestNeighbor > distSqrd ){
						it1->mDistToClosestNeighbor = distSqrd;
					}

					if( it2->mDistToClosestNeighbor > distSqrd ){
						it2->mDistToClosestNeighbor = distSqrd;
					}

					if( shouldAnnihilate ) annihilate(  &(*it1), &(*it2) );
				}

				float q = totalCharge / distSqrd;
				dir		= dir.normalized() * 5.0f * q * mRoom->getPower();

				it1->mAcc += dir;
				it2->mAcc -= dir;
			}
		}

		it1->mAcc += Vec3f::yAxis() * mRoom->getGravity();
	}
}

bool Controller::didParticlesCollide( const ci::Vec3f &dir, const ci::Vec3f &dirNormal, const float dist, const float sumRadii, const float sumRadiiSqrd, ci::Vec3f *moveVec )
{
	float moveVecLength = sqrtf( moveVec->x * moveVec->x + moveVec->y * moveVec->y + moveVec->z * moveVec->z );
	float newDist = dist - sumRadii;

	if( newDist < 0 )
		return true;

	if( moveVecLength < dist )
		return false;

	moveVec->normalize();
	float D	= moveVec->dot(dir);

	if( D <= 0 )
		return false;

	float F	= ( newDist * newDist ) - ( D * D );

	if( F >= sumRadiiSqrd )
		return false;

	float T = sumRadiiSqrd - F;

	if( T < 0 )
		return false;

	float distance = D - sqrtf(T);

	if( moveVecLength < distance )
		return false;

	*moveVec *= distance;
	return true;
}

void Controller::annihilate( Particle *p1, Particle *p2 )
{
	p1->mIsDead = true;
	p2->mIsDead = true;
	Vec3f p		= ( p1->mPos + p2->mPos ) * 0.5f;
	Vec3f v		= ( p1->mVel + p2->mVel ) * 0.5f;
	Vec3f dir	= p1->mPos - p2->mPos;
	Vec3f axis	= dir.normalized();
	Vec3f right	= axis.cross( Vec3f::zAxis() ).normalized();
	Vec3f up	= axis.cross( right ).normalized();
	right		= axis.cross( up ).normalized();

	float lifespan	= 25.0f;
	float speed		= 20.0f;
	mShockwaves.push_back( Shockwave( p, up, lifespan, speed ) );

	// MAKE PHOTONS
	int numPhotons = 20;
	float perSeg = 1.0f/(float)numPhotons;
	for( int i=0; i<numPhotons; i++ ){
		float angle = (float)i*perSeg*M_PI*2.0f;
		Vec3f dir	= ( cos( angle ) * right + sin( angle ) * up );
		Vec3f vel	= v + dir * 15.0f;

		mPhotons.push_back( Photon( p + dir * 50.0f, vel ) );
	}

	// MAKE SMOKES
	for( int i=0; i<3; i++ ){
		mSmokes.push_back( Smoke( p, Vec3f::zero(), Rand::randFloat( 50.0f, 120.0f ), Rand::randFloat( 4.0f, 8.0f ) ) );
	}
}

void Controller::update()
{
	float dt = mRoom->getTimeDelta();

	// PARTICLES
	int numNewParticles = 0;
	for( list<Particle>::iterator p = mParticles.begin(); p != mParticles.end(); ){
		if( p->mIsDead ){
			p = mParticles.erase( p );
			numNewParticles ++;
		} else {
			p->update( mRoom, dt );
			++p;
		}
	}

	// PHOTONS
	for( list<Photon>::iterator p = mPhotons.begin(); p != mPhotons.end(); ){
		if( p->mIsDead ){
			p = mPhotons.erase( p );
		} else {
			p->update( dt );
			++p;
		}
	}

	// SHOCKWAVES
	for( vector<Shockwave>::iterator it = mShockwaves.begin(); it != mShockwaves.end(); ){
		if( it->mIsDead ){
			it = mShockwaves.erase( it );
		} else {
			it->update( dt );
			++it;
		}
	}

	// SMOKES
	for( vector<Smoke>::iterator it = mSmokes.begin(); it != mSmokes.end(); ){
		if( it->mIsDead ){
			it = mSmokes.erase( it );
		} else {
			it->update( dt );
			++it;
		}
	}

	if( mRecycle )
		addParticles( numNewParticles );
}

void Controller::draw( gl::GlslProg *shader )
{
	// DRAW PARTICLES
	gl::color( ColorA( 1.0f, 1.0f, 1.0f, 1.0f ) );
	for( list<Particle>::iterator p = mParticles.begin(); p != mParticles.end(); ++p ){
		shader->uniform( "pos", p->mPos );
		shader->uniform( "radius", p->mRadius );
		shader->uniform( "charge", p->mCharge * 0.5f + 0.5f );
//		shader->uniform( "distPer", p->mDistPer + ( p->mCharge * 0.5f + 0.5f ) );
		p->draw( mSphereLo, mSphereHi );
	}
}

void Controller::drawParticleShadows()
{
	float yFloor = mRoom->getFloorLevel();

	// DRAW PARTICLES
	gl::color( ColorA( 1.0f, 1.0f, 1.0f, 1.0f ) );
	for( list<Particle>::iterator p = mParticles.begin(); p != mParticles.end(); ++p ){
		float yBase				= p->mPos.y - p->mRadius;
		float yDistFromFloor	= ( yBase - yFloor );
		float maxDist			= p->mRadius * 4.0f;
		float perDist			= yDistFromFloor/maxDist;
		if( perDist < 1.0f ){
//			float per = ( p->mPos.y - p->mRadius - y )/(yFloor * -0.25f );
			gl::color( ColorA( 0.0f, 0.0f, 0.0f, 1.0f - perDist ) );
			gl::drawBillboard( Vec3f( p->mPos.x, yFloor, p->mPos.z ), Vec2f( p->mRadius, p->mRadius ) * ( 4.0f + perDist * 7.0f ), 0.0f, Vec3f::xAxis(), Vec3f::zAxis() );
		}
	}
}

void Controller::drawPhotons()
{
	glBegin( GL_LINES );
	for( list<Photon>::iterator p = mPhotons.begin(); p != mPhotons.end(); ++p ){
		p->drawTail();
	}
	glEnd();
}

void Controller::drawShockwaves( const Vec3f &camPos )
{
	for( vector<Shockwave>::iterator it = mShockwaves.begin(); it != mShockwaves.end(); ++it ){
//		gl::color( ColorA( 1.0f, 1.0f, 1.0f, it->mAgePer ) );
//		drawSphericalBillboard( camPos, it->mPos, Vec2f( it->mRadius, it->mRadius ), 0.0f );

		if( it->mAge < 1.5f ){
			gl::color( ColorA( 1.0f, 0.05f, 0.05f, 1.0f ) );
			drawSphericalBillboard( camPos, it->mPos, Vec2f( 800.0f, 800.0f ), it->mRot );
//		} else if( it->mAge < 2.5f ){
//			gl::color( ColorA( 0.0f, 1.0f, 0.0f, 1.0f ) );
//			drawSphericalBillboard( camPos, it->mPos, Vec2f( 800.0f, 800.0f ), 0.0f );
		} else if( it->mAge < 2.5f ){
			gl::color( ColorA( 0.05f, 0.05f, 1.0f, 1.0f ) );
			drawSphericalBillboard( camPos, it->mPos, Vec2f( 600.0f, 600.0f ), it->mRot );
		}
	}
}

void Controller::drawSphericalBillboard( const Vec3f &camEye, const Vec3f &objPos, const Vec2f &scale, float rotInRadians )
{
	glPushMatrix();
	glTranslatef( objPos.x, objPos.y, objPos.z );

	Vec3f lookAt = Vec3f::zAxis();
	Vec3f upAux;
	float angleCosine;

	Vec3f objToCam = ( camEye - objPos ).normalized();
	Vec3f objToCamProj = Vec3f( objToCam.x, 0.0f, objToCam.z );
	objToCamProj.normalize();

	upAux = lookAt.cross( objToCamProj );

	// Cylindrical billboarding
	angleCosine = constrain( lookAt.dot( objToCamProj ), -1.0f, 1.0f );
	glRotatef( toDegrees( acos(angleCosine) ), upAux.x, upAux.y, upAux.z );

	// Spherical billboarding
	angleCosine = constrain( objToCamProj.dot( objToCam ), -1.0f, 1.0f );
	if( objToCam.y < 0 )	glRotatef( toDegrees( acos(angleCosine) ), 1.0f, 0.0f, 0.0f );
	else					glRotatef( toDegrees( acos(angleCosine) ),-1.0f, 0.0f, 0.0f );

	Vec3f verts[4];
	GLfloat texCoords[8] = { 0, 0, 0, 1, 1, 0, 1, 1 };

	glEnableClientState( GL_VERTEX_ARRAY );
	glEnableClientState( GL_TEXTURE_COORD_ARRAY );
	glVertexPointer( 3, GL_FLOAT, 0, &verts[0].x );
	glTexCoordPointer( 2, GL_FLOAT, 0, texCoords );

	float sinA = ci::math<float>::sin( rotInRadians );
	float cosA = ci::math<float>::cos( rotInRadians );

	float scaleXCosA = 0.5f * scale.x * cosA;
	float scaleXSinA = 0.5f * scale.x * sinA;
	float scaleYSinA = 0.5f * scale.y * sinA;
	float scaleYCosA = 0.5f * scale.y * cosA;

	verts[0] = Vec3f( ( -scaleXCosA - scaleYSinA ), ( -scaleXSinA + scaleYCosA ), 0.0f );
	verts[1] = Vec3f( ( -scaleXCosA + scaleYSinA ), ( -scaleXSinA - scaleYCosA ), 0.0f );
	verts[2] = Vec3f( (  scaleXCosA - scaleYSinA ), (  scaleXSinA + scaleYCosA ), 0.0f );
	verts[3] = Vec3f( (  scaleXCosA + scaleYSinA ), (  scaleXSinA - scaleYCosA ), 0.0f );

	glDrawArrays( GL_TRIANGLE_STRIP, 0, 4 );

	glDisableClientState( GL_VERTEX_ARRAY );
	glDisableClientState( GL_TEXTURE_COORD_ARRAY );

	glPopMatrix();
}

void Controller::drawSmokes( const Vec3f &right, const Vec3f &up )
{
	BOOST_FOREACH( Smoke &smoke, mSmokes ){
		smoke.draw( right, up );
	}
}

void Controller::finish()
{
	for( list<Particle>::iterator it = mParticles.begin(); it != mParticles.end(); ++it )
	{
		it->finish();
	}
}

void Controller::reset()
{
	clearRoom();
	preset( mPreset );
}

void Controller::clearRoom()
{
	mParticles.clear();
	mPhotons.clear();
	mShockwaves.clear();
	mSmokes.clear();
}

void Controller::addParticles( int amt )
{
	for( int i=0; i<amt; i++ )
	{
		Vec3f bounds = mRoom->getDims();

		Vec3f pos = Vec3f( Rand::randFloat( -bounds.x, bounds.x ),
						   Rand::randFloat( -bounds.y, bounds.y ),
						   Rand::randFloat( -bounds.z, bounds.z ) );
		Vec3f vel = Vec3f::zero();
		mParticles.push_back( Particle( pos, vel ) );
	}
}

void Controller::addParticle( float charge, const Vec3f &pos )
{
	mParticles.push_back( Particle( pos, Vec3f::zero(), charge ) );
}

void Controller::preset( int i )
{
	clearRoom();
	mRecycle	= false;
	mPreset		= i;

	float yFloor = mRoom->getFloorLevel() + 20.0f;

	if( i == 1 ){				// 2 Matter
		float d = 5.0f;
		addParticle( 1.0f, Vec3f( d, yFloor, 0.0f ) );
		addParticle( 1.0f, Vec3f(-d, yFloor, 0.0f ) );

	} else if( i == 2 ){		// 100 Matter
		float dim = 12.0f;
		for( int z=-12; z<12; z++ ){
			for( int x=-12; x<12; x++ ){
				float charge = 1.0f;
				Vec3f pos = Vec3f( (float)x * dim, yFloor, (float)z * dim );
				addParticle( charge, pos );
			}
		}

	} else if( i == 3 ){		// 1 Matter 1 Antimatter
		float d = 5.0f;
		addParticle( 1.0f, Vec3f( d, yFloor, 0.0f ) );
		addParticle(-1.0f, Vec3f(-d, yFloor, 0.0f ) );

	} else if( i == 4 ){		// Segregated grid
		float dim = 12.0f;
		for( int z=-4; z<4; z++ ){
			for( int x=-4; x<4; x++ ){
				float charge = 1.0f;
				if( x >= 0 ) charge = -1.0f;
				Vec3f pos = Vec3f( (float)x * dim, yFloor, (float)z * dim );
				addParticle( charge, pos );
			}
		}
	} else if( i == 5 ){		// 1 row of Matter 1 row of Antimatter
		for( int i=0; i<30; i++ ){
			float x1 = -20.0f;
			float x2 = 20.0f;
			float z  = ( (float)i - 15.0f ) * 15.0f;
			addParticle( 1.0, Vec3f( z, yFloor, x1 ) );
			addParticle(-1.0, Vec3f( z, yFloor, x2 ) );
		}
	} else if( i == 6 ){		// 2 concentric rings, 1 of Matter, 1 of Antimatter
		float numParticles = 100;
		for( int k=0; k<2; k++ ){
			float charge = 1.0f;
			if( k == 1 ) charge = -1.0f;

			for( int i=0; i<numParticles; i++ ){
				float per	 = (float)i/(float)(numParticles);
				float angle  = per * M_PI * 2.0f;
				float cosA	 = cos( angle );
				float sinA	 = sin( angle );
				float radius = 100.0f + k * 25.0f;

				addParticle( charge, Vec3f( cosA * radius, yFloor, sinA * radius ) );
			}
		}
	} else if( i == 7 ){		// 2 concentric rings, 1 of Matter, 1 of Antimatter
		float numParticles = 200;
		for( int k=0; k<2; k++ ){
			float charge = 1.0f;
			if( k == 1 ) charge = -1.0f;

			for( int i=0; i<numParticles; i++ ){
				float per	 = (float)i/(float)(numParticles);
				float angle  = per * M_PI * 2.0f;
				float cosA	 = cos( angle );
				float sinA	 = sin( angle );
				float radius = 180.0f + k * 25.0f;

				addParticle( charge, Vec3f( cosA * radius, yFloor, sinA * radius ) );
			}
		}

	} else if( i == 8 ){		// MATTER grid
		float dim = 16.0f;
		for( int z=-12; z<12; z++ ){
			for( int x=-12; x<12; x++ ){
				float charge = 1.0f;
				if( x < 3 && x > -3 && z < 3 && z > -3 ) charge = -1.0f;
				Vec3f pos = Vec3f( (float)x * dim, yFloor, (float)z * dim );
				addParticle( charge, pos );
			}
		}
	} else if( i == 9 ){		// RANDOM grid
		float dim = 16.0f;
		for( int z=-12; z<12; z++ ){
			for( int x=-12; x<12; x++ ){
				float charge = 1.0f;
				if( Rand::randBool() ) charge = -1.0f;
				Vec3f pos = Vec3f( (float)x * dim, yFloor, (float)z * dim );
				addParticle( charge, pos );
			}
		}

	} else if( i == 10 ){		// RANDOM grid with recycling
		float dim = 16.0f;
		for( int z=-12; z<12; z++ ){
			for( int x=-12; x<12; x++ ){
				float charge = 1.0f;
				if( Rand::randBool() ) charge = -1.0f;
				Vec3f pos = Vec3f( (float)x * dim, yFloor, (float)z * dim );
				addParticle( charge, pos );
			}
		}

//		float dim = 8.0f;
//		for( int z=-24; z<24; z++ ){
//			for( int x=-24; x<24; x++ ){
//				float charge = 1.0f;
//				if( Rand::randBool() ) charge = -1.0f;
//				Vec3f pos = Vec3f( (float)x * dim, -mRoom->getBounds().y + 20.0f, (float)z * dim );
//				addParticle( charge, pos );
//			}
//		}

		mRecycle = true;
	}
}


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_DEC_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_DEC_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-arithmetic
    This function object returns  its parameter decremented by 1.

    @par Header <boost/simd/function/dec.hpp>

    @par Decorators

    - saturated_ ensures that @c saturated_(dec)(x) will never be
      strictly greater than @c x,
      avoiding the wrap around from @ref Valmin to
      @ref Valmax with integer types

    @see inc, minus

    @par Example:

      @snippet dec.cpp dec

    @par Possible output:

      @snippet dec.txt dec
  **/
  Value dec(Value const& x);
} }
#endif

#include <boost/simd/function/scalar/dec.hpp>
#include <boost/simd/function/simd/dec.hpp>

#endif


#include <iostream>
#include <fstream>
#include <sstream>
#include <algorithm>
#include <cmath>
#include <numeric>
#include <chrono>
#include <limits>
#include <float.h>
#include <lamure/mesh/bvh.h>
#include <memory>

#include <CGAL/Polygon_mesh_processing/measure.h>

#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string/classification.hpp>
#include <boost/algorithm/string/trim.hpp>

#include "Utils.h"
#include "OBJ_printer.h"
#include "SymMat.h"
#include "cluster_settings.h"
#include "ErrorQuadric.h"
#include "Chart.h"
#include "JoinOperation.h"
#include "polyhedron_builder.h"
#include "eig.h"
#include "ClusterCreator.h"
#include "GridClusterCreator.h"
#include "ParallelClusterCreator.h"

#include "CGAL_typedefs.h"

#include <lamure/mesh/bvh.h>

#include "kdtree.h"
#include <lamure/mesh/tools.h>

#include "lodepng.h"
#include "texture.h"
#include "frame_buffer.h"

#include "chart_packing.h"

int render_to_texture_width_ = 4096;
int render_to_texture_height_ = 4096;

int full_texture_width_ = 4096;
int full_texture_height_ = 4096;

struct viewport {
  scm::math::vec2f normed_dims;
  scm::math::vec2f normed_offset;
};

std::vector<viewport> viewports_;

std::vector< std::shared_ptr<texture_t>> textures_;

GLuint shader_program_;
GLuint vertex_buffer_;

GLuint dilation_shader_program_;
GLuint dilation_vertex_buffer_;

std::vector<std::shared_ptr<frame_buffer_t>> frame_buffers_;

std::shared_ptr<texture_t> load_image(const std::string& filepath) {
  std::vector<unsigned char> img;
  unsigned int width = 0;
  unsigned int height = 0;
  int tex_error = lodepng::decode(img, width, height, filepath);
  if (tex_error) {
    std::cout << "ERROR: unable to load image file " << filepath << std::endl;
    exit(1);
  }
  else {
    std::cout << "Loaded texture from " << filepath << std::endl;}

  auto texture = std::make_shared<texture_t>(width, height, GL_LINEAR);
  texture->set_pixels(&img[0]);

  return texture;
}

void save_image(std::string filename, std::vector<uint8_t> image, int width, int height) {
  int tex_error = lodepng::encode(filename, image, width, height);
  if (tex_error) {
    std::cout << "ERROR: unable to save image file " << filename << std::endl;
  }
  std::cout << "Saved image to " << filename << std::endl;

}

void save_framebuffer_to_image(std::string filename, std::shared_ptr<frame_buffer_t> frame_buffer) {

  std::vector<uint8_t> pixels;
  frame_buffer->get_pixels(0, pixels);

  int tex_error = lodepng::encode(filename, pixels, frame_buffer->get_width(), frame_buffer->get_height());
  if (tex_error) {
    std::cout << "ERROR: unable to save image file " << filename << std::endl;
    exit(1);
  }
  std::cout << "Framebuffer written to image " << filename << std::endl;

}

//subroutine for error-checking during shader compilation
GLint compile_shader(const std::string& _src, GLint _shader_type) {

  const char* shader_src = _src.c_str();
  GLuint shader = glCreateShader(_shader_type);

  glShaderSource(shader, 1, &shader_src, NULL);
  glCompileShader(shader);

  GLint status;
  glGetShaderiv(shader, GL_COMPILE_STATUS, &status);
  if (status == GL_FALSE) {
    GLint log_length;
    glGetShaderiv(shader, GL_INFO_LOG_LENGTH, &log_length);

    GLchar* log = new GLchar[log_length + 1];
    glGetShaderInfoLog(shader, log_length, NULL, log);

    const char* type = NULL;
    switch (_shader_type) {
      case GL_VERTEX_SHADER: type = "vertex"; break;
      case GL_FRAGMENT_SHADER: type = "fragment"; break;
      default: break;
    }

    std::string error_message = "ERROR: Compile shader failure in " + std::string(type) + " shader:\n" + std::string(log);
    delete[] log;
    std::cout << error_message << std::endl;
    exit(1);
  }

  return shader;

}

//compile and link the shader programs
void make_shader_program() {

  //locates vertices at new uv position on screen
  //passes old uvs in order to read from the texture

  std::string vertex_shader_src = "#version 420\n\
    layout (location = 0) in vec2 vertex_old_coord;\n\
    layout (location = 1) in vec2 vertex_new_coord;\n\
    \n\
    uniform vec2 viewport_offset;\n\
    uniform vec2 viewport_scale;\n\
    \n\
    varying vec2 passed_uv;\n\
    \n\
    void main() {\n\
      vec2 coord = vec2(vertex_new_coord.x, vertex_new_coord.y);\n\
      vec2 coord_translated = coord - viewport_offset; \n\
      vec2 coord_scaled = coord_translated / viewport_scale; \n\
      gl_Position = vec4((coord_scaled-0.5)*2.0, 0.5, 1.0);\n\
      passed_uv = vertex_old_coord;\n\
    }";

  std::string fragment_shader_src = "#version 420\n\
    uniform sampler2D image;\n\
    varying vec2 passed_uv;\n\
    \n\
    layout (location = 0) out vec4 fragment_color;\n\
    \n\
    void main() {\n\
      vec4 color = texture(image, passed_uv).rgba;\n\
      fragment_color = vec4(color.rgb, 1.0);\n\
    }";

  //compile shaders
  GLint vertex_shader = compile_shader(vertex_shader_src, GL_VERTEX_SHADER);
  GLint fragment_shader = compile_shader(fragment_shader_src, GL_FRAGMENT_SHADER);

  //create the GL resource and save the handle for the shader program
  shader_program_ = glCreateProgram();
  glAttachShader(shader_program_, vertex_shader);
  glAttachShader(shader_program_, fragment_shader);
  glLinkProgram(shader_program_);

  //since the program is already linked, we do not need to keep the separate shader stages
  glDetachShader(shader_program_, vertex_shader);
  glDeleteShader(vertex_shader);
  glDetachShader(shader_program_, fragment_shader);
  glDeleteShader(fragment_shader);
}

//compile and link the shader programs
void make_dilation_shader_program() {

  std::string vertex_shader_src = "#version 420\n\
    layout (location = 0) in vec2 vertex_old_coord;\n\
    layout (location = 1) in vec2 vertex_new_coord;\n\
    \n\
    varying vec2 passed_uv;\n\
    \n\
    void main() {\n\
      vec2 coord = vec2(vertex_old_coord.x, vertex_old_coord.y);\n\
      gl_Position = vec4((coord), 0.5, 1.0);\n\
      passed_uv = vertex_new_coord;\n\
    }";

  std::string fragment_shader_src = "#version 420\n\
    uniform sampler2D image;\n\
    uniform int image_width;\n\
    uniform int image_height;\n\
    varying vec2 passed_uv;\n\
    \n\
    layout (location = 0) out vec4 fragment_color;\n\
    \n\
    vec4 weighted_dilation() {\n\
      vec4 fallback_color = vec4(1.0, 0.0, 1.0, 1.0);\n\
      vec4 accumulated_color = vec4(0.0, 0.0, 0.0, 1.0);\n\
      float accumulated_weight = 0.0;\n\
      for(int y_offset = -1; y_offset < 2; ++y_offset) {\n\
        if( 0 == y_offset) {\n\
          continue;\n\
        }\n\
        ivec2 sampling_frag_coord = ivec2(gl_FragCoord.xy) + ivec2(0, y_offset);\n\
\n\
        if( \n\
           sampling_frag_coord.y >= image_height || sampling_frag_coord.y < 0 ) {\n\
            continue;\n\
           }\n\
\n\
        vec3 sampled_color = texelFetch(image, ivec2(sampling_frag_coord), 0).rgb;\n\
\n\
        if( !(1.0 == sampled_color.r && 0.0 == sampled_color.g && 1.0 == sampled_color.b) ) {\n\
          ivec2 pixel_dist = ivec2(gl_FragCoord.xy) - sampling_frag_coord;\n\
          float eucl_distance = sqrt(pixel_dist.x * pixel_dist.x + pixel_dist.y * pixel_dist.y);\n\
          float weight = 1.0 / eucl_distance;\n\
          accumulated_color += vec4(sampled_color, 1.0) * weight;\n\
          accumulated_weight += weight;\n\
        }\n\
\n\
    }\n\
\n\
      for(int x_offset = -1; x_offset < 2; ++x_offset) {\n\
        if( 0 == x_offset) {\n\
          continue;\n\
        }\n\
        ivec2 sampling_frag_coord = ivec2(gl_FragCoord.xy) + ivec2(x_offset, 0);\n\
\n\
        if( \n\
           sampling_frag_coord.x >= image_width || sampling_frag_coord.x < 0 ) {\n\
            continue;\n\
           }\n\
\n\
        vec3 sampled_color = texelFetch(image, ivec2(sampling_frag_coord), 0).rgb;\n\
\n\
        if( !(1.0 == sampled_color.r && 0.0 == sampled_color.g && 1.0 == sampled_color.b) ) {\n\
          ivec2 pixel_dist = ivec2(gl_FragCoord.xy) - sampling_frag_coord;\n\
          float eucl_distance = sqrt(pixel_dist.x * pixel_dist.x + pixel_dist.y * pixel_dist.y);\n\
          float weight = 1.0 / eucl_distance;\n\
          accumulated_color += vec4(sampled_color, 1.0) * weight;\n\
          accumulated_weight += weight;\n\
        }\n\
\n\
    }\n\
\n\
\n\
      if(accumulated_weight > 0.0) {\n\
        //return vec4(0.0, 1.0, 0.0, 1.0);\n\
        return vec4( (accumulated_color / accumulated_weight).rgb, 1.0);\n\
      } else {\n\
        return fallback_color;\n\
      }\n\
    }\n\
\n\
    void main() {\n\
      vec4 color = texture(image, passed_uv).rgba;\n\
    \n\
      if(1.0 == color.r && 0.0 == color.g && 1.0 == color.b) {\n\
      \n\
        fragment_color = weighted_dilation();\n\
       //fragment_color = vec4(1.0, 0.0, 0.0, 1.0); \n\
      } else {\n\
        fragment_color = texelFetch(image, ivec2(gl_FragCoord.xy), 0);\n\
      }\n\
    }";

  //compile shaders
  GLint vertex_shader = compile_shader(vertex_shader_src, GL_VERTEX_SHADER);
  GLint fragment_shader = compile_shader(fragment_shader_src, GL_FRAGMENT_SHADER);

  //create the GL resource and save the handle for the shader program
  dilation_shader_program_ = glCreateProgram();
  glAttachShader(dilation_shader_program_, vertex_shader);
  glAttachShader(dilation_shader_program_, fragment_shader);
  glLinkProgram(dilation_shader_program_);

  //since the program is already linked, we do not need to keep the separate shader stages
  glDetachShader(dilation_shader_program_, vertex_shader);
  glDeleteShader(vertex_shader);
  glDetachShader(dilation_shader_program_, fragment_shader);
  glDeleteShader(fragment_shader);
}

int main( int argc, char** argv )
{

  glutInit(&argc, argv);
  glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA);
  glutInitWindowSize(render_to_texture_width_, render_to_texture_height_);
  glutInitWindowPosition(64, 64);
  glutCreateWindow(argv[0]);
  glutSetWindowTitle("Lamure Mesh Preprocessing");
  glEnable(GL_DEPTH_TEST);
  glEnable(GL_TEXTURE_2D);
  glDisable(GL_CULL_FACE);
  glewExperimental = GL_TRUE;
  glewInit();
  glutHideWindow();

  std::string obj_filename = "dino.obj";
  if (Utils::cmdOptionExists(argv, argv+argc, "-f")) {
    obj_filename = std::string(Utils::getCmdOption(argv, argv + argc, "-f"));
  }
  else {
    std::cout << "Please provide an obj filename using -f <filename.obj>" << std::endl;
    std::cout << "Optional: -of specifies outfile name" << std::endl;

    std::cout << "Optional: -ch specifies chart threshold (=infinite)" << std::endl;
    std::cout << "Optional: -co specifies cost threshold (=infinite)" << std::endl;

    std::cout << "Optional: -ef specifies error fit coefficient (=1)" << std::endl;
    std::cout << "Optional: -eo specifies error orientation coefficient (=1)" << std::endl;
    std::cout << "Optional: -es specifies error shape coefficient (=1)" << std::endl;

    std::cout << "Optional: -cc specifies cell resolution for grid splitting, along longest axis. When 0, no cell splitting is used. (=0)" << std::endl;
    std::cout << "Optional: -ct specifies threshold for grid chart splitting by normal variance (=0.001)" << std::endl;

    std::cout << "Optional: -debug writes charts to obj file, as colours that override texture coordinates (=false)" << std::endl;

    std::cout << "Optional: -tkd num triangles per kdtree node (default: 32000)" << std::endl;

    std::cout << "Optional: -tbvh num triangles per kdtree node (default: 8192)" << std::endl;

    std::cout << "Optional: -single-max: specifies largest possible single output texture (=4096)" << std::endl;

    std::cout << "Optional: -multi-max: specifies largest possible output texture (=8192)" << std::endl;

    return 1;
  }

  std::string out_filename = obj_filename.substr(0,obj_filename.size()-4) + "_charts.obj";
  if (Utils::cmdOptionExists(argv, argv+argc, "-of")) {
    out_filename = Utils::getCmdOption(argv, argv + argc, "-of");
  }

  double cost_threshold = std::numeric_limits<double>::max();
  if (Utils::cmdOptionExists(argv, argv+argc, "-co")) {
    cost_threshold = atof(Utils::getCmdOption(argv, argv + argc, "-co"));
  }
  uint32_t chart_threshold = std::numeric_limits<uint32_t>::max();;
  if (Utils::cmdOptionExists(argv, argv+argc, "-ch")) {
    chart_threshold = atoi(Utils::getCmdOption(argv, argv + argc, "-ch"));
  }
  double e_fit_cf = 1.0;
  if (Utils::cmdOptionExists(argv, argv+argc, "-ef")) {
    e_fit_cf = atof(Utils::getCmdOption(argv, argv + argc, "-ef"));
  }
  double e_ori_cf = 1.0;
  if (Utils::cmdOptionExists(argv, argv+argc, "-eo")) {
    e_ori_cf = atof(Utils::getCmdOption(argv, argv + argc, "-eo"));
  }
  double e_shape_cf = 1.0;
  if (Utils::cmdOptionExists(argv, argv+argc, "-es")) {
    e_shape_cf = atof(Utils::getCmdOption(argv, argv + argc, "-es"));
  }
  double cst = 0.001;
  if (Utils::cmdOptionExists(argv, argv+argc, "-ct")) {
    cst = atof(Utils::getCmdOption(argv, argv + argc, "-ct"));
  }

  CLUSTER_SETTINGS cluster_settings (e_fit_cf, e_ori_cf, e_shape_cf, cst);

  int cell_resolution = 0;
  if (Utils::cmdOptionExists(argv, argv+argc, "-cc")) {
    cell_resolution = atoi(Utils::getCmdOption(argv, argv + argc, "-cc"));
  }

  if (Utils::cmdOptionExists(argv, argv+argc, "-debug")) {
    cluster_settings.write_charts_as_textures = true;
  }
  int num_tris_per_node_kdtree = 1024*32;
  if (Utils::cmdOptionExists(argv, argv+argc, "-tkd")) {
    num_tris_per_node_kdtree = atoi(Utils::getCmdOption(argv, argv + argc, "-tkd"));
  }
  int num_tris_per_node_bvh = 8*1024;
  if (Utils::cmdOptionExists(argv, argv+argc, "-tbvh")) {
    num_tris_per_node_bvh = atoi(Utils::getCmdOption(argv, argv + argc, "-tbvh"));
  }

  int single_tex_limit = 4096;
  if (Utils::cmdOptionExists(argv, argv+argc, "-single-max")) {
    single_tex_limit = atoi(Utils::getCmdOption(argv, argv+argc, "-single-max"));
    std::cout << "Single output texture limited to " << single_tex_limit << std::endl;
  }

  int multi_tex_limit = 8192;
  if (Utils::cmdOptionExists(argv, argv+argc, "-multi-max")) {
    multi_tex_limit = atoi(Utils::getCmdOption(argv, argv+argc, "-multi-max"));
    std::cout << "Multi output texture limited to " << multi_tex_limit << std::endl;
  }

  std::vector<lamure::mesh::triangle_t> all_triangles;
  std::vector<std::string> all_materials;

  Utils::load_obj(obj_filename, all_triangles, all_materials);

  std::vector<indexed_triangle_t> all_indexed_triangles;

  if (all_triangles.empty()) {
    std::cout << "ERROR: Didnt find any triangles" << std::endl;
    exit(1);
  }

  std::cout << "Mesh loaded (" << all_triangles.size() << " triangles)" << std::endl;
  std::cout << "Loading mtl..." << std::endl;
  std::string mtl_filename = obj_filename.substr(0, obj_filename.size()-4)+".mtl";
  std::map<std::string, std::pair<std::string, int>> material_map;
  bool load_mtl_success = Utils::load_mtl(mtl_filename, material_map);

  if (all_materials.size() != all_triangles.size()) {
    std::cout << "ERROR: incorrect amount of materials found, given number of incoming triangles.";
    exit(1);
  }

  //check materials
  std::map<uint32_t, texture_info> texture_info_map;

  if (load_mtl_success) {

    for (auto mat_it : material_map) {
      std::string texture_filename = mat_it.second.first;
      uint32_t material_id = mat_it.second.second;
      std::cout << "Material " << mat_it.first << " : " << texture_filename << " : " << material_id << std::endl;

      if (boost::filesystem::exists(texture_filename) && boost::algorithm::ends_with(texture_filename, ".png")) {
        texture_info_map[material_id] = {texture_filename, Utils::get_png_dimensions(texture_filename)};
      }
      else if (texture_filename == "") {
        //ok
      }
      else {
        std::cout << "ERROR: texture " << texture_filename << " was not found or is not a .png" << std::endl;
        exit(1);
      }
    }

    for (uint32_t i = 0; i < all_triangles.size(); ++i) {
      auto& tri = all_triangles[i];
      std::string mat = all_materials[i];

      indexed_triangle_t idx_tri;
      idx_tri.v0_ = tri.v0_;
      idx_tri.v1_ = tri.v1_;
      idx_tri.v2_ = tri.v2_;
      idx_tri.tex_idx_ = material_map[mat].second;

      all_indexed_triangles.push_back(idx_tri);
    }
  }
  else {
    std::cout << "ERROR: Failed to load .mtl" << std::endl;
    exit(1);
  }

  all_triangles.clear();

  std::cout << "Building kd tree..." << std::endl;
  std::shared_ptr<kdtree_t> kdtree = std::make_shared<kdtree_t>(all_indexed_triangles, num_tris_per_node_kdtree);

  uint32_t first_leaf_id = kdtree->get_first_node_id_of_depth(kdtree->get_depth());
  uint32_t num_leaf_ids = kdtree->get_length_of_depth(kdtree->get_depth());

  std::vector<uint32_t> node_ids;
  for (uint32_t i = 0; i < num_leaf_ids; ++i) {
    node_ids.push_back(i + first_leaf_id);
  }

  std::map<uint32_t, std::map<uint32_t, uint32_t>> per_node_chart_id_map;
  std::map<uint32_t, Polyhedron> per_node_polyhedron;

  int prev_percent = -1;

  auto lambda_chartify = [&](uint64_t i, uint32_t id)->void{

    int percent = (int)(((float)i / (float)node_ids.size())*100.f);
    if (percent != prev_percent) {
      prev_percent = percent;
      std::cout << "Chartification: " << percent << " %" << std::endl;
    }

    //build polyhedron from node.begin to node.end in accordance with indices

    uint32_t node_id = node_ids[i];
    const auto& node = kdtree->get_nodes()[node_id];
    const auto& indices = kdtree->get_indices();

    BoundingBoxLimits limits;
    limits.min = scm::math::vec3f(std::numeric_limits<float>::max());
    limits.max = scm::math::vec3f(std::numeric_limits<float>::lowest());

    std::vector<indexed_triangle_t> node_triangles;
    for (uint32_t idx = node.begin_; idx < node.end_; ++idx) {

      const auto& tri = all_indexed_triangles[indices[idx]];
      node_triangles.push_back(tri);

      limits.min.x = std::min(limits.min.x, tri.v0_.pos_.x);
      limits.min.y = std::min(limits.min.y, tri.v0_.pos_.y);
      limits.min.z = std::min(limits.min.z, tri.v0_.pos_.z);

      limits.min.x = std::min(limits.min.x, tri.v1_.pos_.x);
      limits.min.y = std::min(limits.min.y, tri.v1_.pos_.y);
      limits.min.z = std::min(limits.min.z, tri.v1_.pos_.z);

      limits.min.x = std::min(limits.min.x, tri.v2_.pos_.x);
      limits.min.y = std::min(limits.min.y, tri.v2_.pos_.y);
      limits.min.z = std::min(limits.min.z, tri.v2_.pos_.z);

      limits.max.x = std::max(limits.max.x, tri.v0_.pos_.x);
      limits.max.y = std::max(limits.max.y, tri.v0_.pos_.y);
      limits.max.z = std::max(limits.max.z, tri.v0_.pos_.z);

      limits.max.x = std::max(limits.max.x, tri.v1_.pos_.x);
      limits.max.y = std::max(limits.max.y, tri.v1_.pos_.y);
      limits.max.z = std::max(limits.max.z, tri.v1_.pos_.z);

      limits.max.x = std::max(limits.max.x, tri.v2_.pos_.x);
      limits.max.y = std::max(limits.max.y, tri.v2_.pos_.y);
      limits.max.z = std::max(limits.max.z, tri.v2_.pos_.z);
    }

    Polyhedron polyMesh;
    polyhedron_builder<HalfedgeDS> builder(node_triangles);
    polyMesh.delegate(builder);

    if (!CGAL::is_triangle_mesh(polyMesh) || !polyMesh.is_valid(false)){
      std::cerr << "ERROR: Input geometry is not valid / not triangulated." << std::endl;
      return;
    }

    //key: face_id, value: chart_id
    std::map<uint32_t, uint32_t> chart_id_map;
    uint32_t active_charts;

    if (cell_resolution > 0) { //do grid clustering
      active_charts = GridClusterCreator::create_grid_clusters(polyMesh, chart_id_map, limits, cell_resolution, cluster_settings);
    }

    active_charts = ParallelClusterCreator::create_charts(chart_id_map, polyMesh, cost_threshold, chart_threshold, cluster_settings);
    //std::cout << "After creating chart clusters: " << active_charts << std::endl;

    per_node_chart_id_map[node_id] = chart_id_map;
    per_node_polyhedron[node_id] = polyMesh;

  };

  uint32_t num_threads = std::min((size_t)24, node_ids.size());
  lamure::mesh::parallel_for(num_threads, node_ids.size(), lambda_chartify);

  //convert back to triangle soup
  std::vector<lamure::mesh::Triangle_Chartid> triangles;

  uint32_t num_areas = 0;
  for (auto& per_node_chart_id_map_it : per_node_chart_id_map) {
    uint32_t node_id = per_node_chart_id_map_it.first;
    auto polyMesh = per_node_polyhedron[node_id];

    //create index
    typedef CGAL::Inverse_index<Polyhedron::Vertex_const_iterator> Index;
    Index index(polyMesh.vertices_begin(), polyMesh.vertices_end());

    //compute normals
    std::map<face_descriptor,Vector> fnormals;
    std::map<vertex_descriptor,Vector> vnormals;
    CGAL::Polygon_mesh_processing::compute_normals(polyMesh,
      boost::make_assoc_property_map(vnormals),
      boost::make_assoc_property_map(fnormals));

    uint32_t nml_id = 0;

    //extract triangle soup
    for(Polyhedron::Facet_const_iterator fi = polyMesh.facets_begin(); fi != polyMesh.facets_end(); ++fi) {
      Polyhedron::Halfedge_around_facet_const_circulator hc = fi->facet_begin();
      Polyhedron::Halfedge_around_facet_const_circulator hc_end = hc;

      if (circulator_size(hc) != 3) {
        std::cout << "ERROR: mesh corrupt!" << std::endl;
        exit(1);
      }

      lamure::mesh::Triangle_Chartid tri;

      Polyhedron::Vertex_const_iterator it = polyMesh.vertices_begin();
      std::advance(it, index[Polyhedron::Vertex_const_iterator(hc->vertex())]);
      tri.v0_.pos_ = scm::math::vec3f(it->point().x(), it->point().y(), it->point().z());
      tri.v0_.tex_ = scm::math::vec2f(fi->t_coords[0].x(), fi->t_coords[0].y());
      auto nml_it = vertices(polyMesh).begin();
      std::advance(nml_it, nml_id);
      tri.v0_.nml_ = scm::math::vec3f(vnormals[*nml_it].x(), vnormals[*nml_it].y(), vnormals[*nml_it].z());
      ++hc; ++nml_it;

      it = polyMesh.vertices_begin();
      std::advance(it, index[Polyhedron::Vertex_const_iterator(hc->vertex())]);
      tri.v1_.pos_ = scm::math::vec3f(it->point().x(), it->point().y(), it->point().z());
      tri.v1_.tex_ = scm::math::vec2f(fi->t_coords[1].x(), fi->t_coords[1].y());
      nml_it = vertices(polyMesh).begin();
      std::advance(nml_it, nml_id);
      tri.v1_.nml_ = scm::math::vec3f(vnormals[*nml_it].x(), vnormals[*nml_it].y(), vnormals[*nml_it].z());
      ++hc; ++nml_it;

      it = polyMesh.vertices_begin();
      std::advance(it, index[Polyhedron::Vertex_const_iterator(hc->vertex())]);
      tri.v2_.pos_ = scm::math::vec3f(it->point().x(), it->point().y(), it->point().z());
      tri.v2_.tex_ = scm::math::vec2f(fi->t_coords[2].x(), fi->t_coords[2].y());
      nml_it = vertices(polyMesh).begin();
      std::advance(nml_it, nml_id);
      tri.v2_.nml_ = scm::math::vec3f(vnormals[*nml_it].x(), vnormals[*nml_it].y(), vnormals[*nml_it].z());
      ++hc; ++nml_it;

      tri.area_id = num_areas;
      tri.chart_id = per_node_chart_id_map_it.second[fi->id()];
      tri.tex_id = fi->tex_id;

      triangles.push_back(tri);

    }
    ++num_areas;

  }

  std::cout << "Creating LOD hierarchy..." << std::endl;

  auto bvh = std::make_shared<lamure::mesh::bvh>(triangles, num_tris_per_node_bvh);

  //here, we make sure that triangles is in the same ordering as the leaf level triangles

  first_leaf_id = bvh->get_first_node_id_of_depth(bvh->get_depth());
  num_leaf_ids = bvh->get_length_of_depth(bvh->get_depth());

  triangles.clear();
  for (uint32_t node_id = first_leaf_id; node_id < first_leaf_id+num_leaf_ids; ++node_id) {
    triangles.insert(triangles.end(), bvh->get_triangles(node_id).begin(), bvh->get_triangles(node_id).end());
  }

  std::cout << "Reordered " << triangles.size() << " triangles" << std::endl;

  std::cout << "Preparing charts..." << std::endl;

  //prep charts for projection
  std::map<uint32_t, std::map<uint32_t, chart>> chart_map;

  for (uint32_t tri_id = 0; tri_id < triangles.size(); ++tri_id) {
    const auto& tri = triangles[tri_id];
    uint32_t area_id = tri.area_id;
    uint32_t chart_id = tri.chart_id;

    if (chart_id != -1 && tri.get_area() > 0.f) {
      chart_map[area_id][chart_id].id_ = chart_id;
      chart_map[area_id][chart_id].original_triangle_ids_.insert(tri_id);
    }
  }

  for (uint32_t area_id = 0; area_id < num_areas; ++area_id) {

    //init charts
    for (auto& it : chart_map[area_id]) {

      it.second.rect_ = rectangle{
        scm::math::vec2f(std::numeric_limits<float>::max()),
        scm::math::vec2f(std::numeric_limits<float>::lowest()),
        it.first,
        false};

      it.second.box_ = lamure::bounding_box(
        scm::math::vec3d(std::numeric_limits<float>::max()),
        scm::math::vec3d(std::numeric_limits<float>::lowest()));

    }

  }

  std::cout << "Expanding charts throughout BVH..." << std::endl;

  //grow chart boxes by all triangles in all levels of bvh
  for (uint32_t node_id = 0; node_id < bvh->get_num_nodes(); node_id++) {

    const std::vector<lamure::mesh::Triangle_Chartid>& tris = bvh->get_triangles(node_id);

    for (const auto& tri : tris) {
      chart_map[tri.area_id][tri.chart_id].box_.expand(scm::math::vec3d(tri.v0_.pos_));
      chart_map[tri.area_id][tri.chart_id].box_.expand(scm::math::vec3d(tri.v1_.pos_));
      chart_map[tri.area_id][tri.chart_id].box_.expand(scm::math::vec3d(tri.v2_.pos_));
    }
  }

  std::cout << "Assigning additional triangles to charts..." << std::endl;

  std::vector<uint32_t> area_ids;
  for (uint32_t area_id = 0; area_id < num_areas; ++area_id) {
    area_ids.push_back(area_id);
  }

  auto lambda_append = [&](uint64_t i, uint32_t id)->void{

    //compare all triangles with chart bounding boxes

    uint32_t area_id = area_ids[i];

    for (auto& it : chart_map[area_id]) {

      int chart_id = it.first;
      auto& chart = it.second;

      chart.all_triangle_ids_.insert(chart.original_triangle_ids_.begin(), chart.original_triangle_ids_.end());

      //add any triangles that intersect chart
      for (uint32_t tri_id = 0; tri_id < triangles.size(); ++tri_id) {
        const auto& tri = triangles[tri_id];
        if (tri.chart_id != -1 && tri.get_area() > 0.f) {
          if (chart.box_.contains(scm::math::vec3d(tri.v0_.pos_))
            || chart.box_.contains(scm::math::vec3d(tri.v1_.pos_))
            || chart.box_.contains(scm::math::vec3d(tri.v2_.pos_))) {

            chart.all_triangle_ids_.insert(tri_id);
          }
        }
      }

      //report chart parameters
      /*
      std::cout << "chart id " << chart_id << std::endl;
      std::cout << "box min " << chart.box_.min() << std::endl;
      std::cout << "box max " << chart.box_.max() << std::endl;
      std::cout << "original triangles " << chart.original_triangle_ids_.size() << std::endl;
      std::cout << "all triangles " << chart.all_triangle_ids_.size() << std::endl;
      */
    }

  };

  num_threads = std::min((size_t)24, area_ids.size());
  lamure::mesh::parallel_for(num_threads, area_ids.size(), lambda_append);

  const float packing_scale = 400.f;

  std::vector<rectangle> area_rects;

  for (uint32_t area_id = 0; area_id < num_areas; ++area_id) {
    calculate_chart_tex_space_sizes(chart_map[area_id], triangles, texture_info_map);

    project_charts(chart_map[area_id], triangles);

    std::cout << "Projected " << chart_map[area_id].size() << " charts for area " << area_id << std::endl;

    std::cout << "Running rectangle packing for area " << area_id << std::endl;

    //init the rectangles
    std::vector<rectangle> rects;
    for (auto& chart_it : chart_map[area_id]) {
      chart& chart = chart_it.second;
      if (chart.original_triangle_ids_.size() > 0) {
        rectangle rect = chart.rect_;
        rect.max_ *= packing_scale;
        rects.push_back(rect);
      }
    }

    //rectangle packing
    rectangle area_rect = pack(rects);
    area_rect.id_ = area_id;
    area_rect.flipped_ = false;
    area_rects.push_back(area_rect);

    std::cout << "Packing of area " << area_id << " complete (" << area_rect.max_.x << ", " << area_rect.max_.y << ")" << std::endl;

    //apply rectangles
    for (const auto& rect : rects) {
      chart_map[area_id][rect.id_].rect_ = rect;
      chart_map[area_id][rect.id_].projection.tex_space_rect = rect;//save for rendering from texture later on
    }

  }

  std::cout << "Packing " << area_rects.size() << " areas..." << std::endl;
  rectangle image_rect = pack(area_rects);
  std::cout << "Packing of all areas complete (" << image_rect.max_.x << ", " << image_rect.max_.y << ")" << std::endl;

  std::cout << "Applying texture space transformation..." << std::endl;

  for (auto& area_rect : area_rects) {
    std::cout << "Area " << area_rect.id_ << " min: (" << area_rect.min_.x << ", " << area_rect.min_.y << ")" << std::endl;
    std::cout << "Area " << area_rect.id_ << " max: (" << area_rect.max_.x << ", " << area_rect.max_.y << ")" << std::endl;

    //next, apply the global transformation from area packing onto all individual chart rects per area
    for (auto& chart_it : chart_map[area_rect.id_]) {
      auto& chart = chart_it.second;
      chart.rect_.min_ += area_rect.min_;

      //apply this transformation to the new parameterization

      for (auto tri_id : chart.all_triangle_ids_) {
        if ((chart.rect_.flipped_ && !area_rect.flipped_) || (area_rect.flipped_ && !chart.rect_.flipped_)) {
          float temp = chart.all_triangle_new_coods_[tri_id][0].x;
          chart.all_triangle_new_coods_[tri_id][0].x = chart.all_triangle_new_coods_[tri_id][0].y;
          chart.all_triangle_new_coods_[tri_id][0].y = temp;

          temp = chart.all_triangle_new_coods_[tri_id][1].x;
          chart.all_triangle_new_coods_[tri_id][1].x = chart.all_triangle_new_coods_[tri_id][1].y;
          chart.all_triangle_new_coods_[tri_id][1].y = temp;

          temp = chart.all_triangle_new_coods_[tri_id][2].x;
          chart.all_triangle_new_coods_[tri_id][2].x = chart.all_triangle_new_coods_[tri_id][2].y;
          chart.all_triangle_new_coods_[tri_id][2].y = temp;
        }

        for (uint32_t i = 0; i < 3; ++i) {
          chart.all_triangle_new_coods_[tri_id][i] *= packing_scale;
          chart.all_triangle_new_coods_[tri_id][i].x += chart.rect_.min_.x;
          chart.all_triangle_new_coods_[tri_id][i].y += chart.rect_.min_.y;
          chart.all_triangle_new_coods_[tri_id][i].x /= image_rect.max_.x;
          chart.all_triangle_new_coods_[tri_id][i].y /= image_rect.max_.x;
        }
      }
    }
  }

  struct blit_vertex_t {
    scm::math::vec2f old_coord_;
    scm::math::vec2f new_coord_;
  };

  //use 2D array to account for different textures (if no textures were found, make sure it has at least one row)
  std::vector<std::vector<blit_vertex_t>> to_upload_per_texture;
  to_upload_per_texture.resize(texture_info_map.size());

  //replacing texture coordinates in LOD file
  //...and at the same time, we will fill the upload per texture list
  std::cout << "Updating texture coordinates in leaf-level LOD nodes..." << std::endl;
  uint32_t num_invalid_tris = 0;
  uint32_t num_dropped_tris = 0;

  for (uint32_t node_id = first_leaf_id; node_id < first_leaf_id+num_leaf_ids; ++node_id) {

    auto& tris = bvh->get_triangles(node_id);
    for (int local_tri_id = 0; local_tri_id < tris.size(); ++local_tri_id) {
      int32_t tri_id = ((node_id-first_leaf_id)*(bvh->get_primitives_per_node()/3))+local_tri_id;

      auto& tri = tris[local_tri_id];
      if (tri.chart_id != -1 && tri.get_area() > 0.f) {

        auto& chart = chart_map[tri.area_id][tri.chart_id];

        if (chart.all_triangle_ids_.find(tri_id) != chart.all_triangle_ids_.end()) {

          if (tri.tex_id != -1) { //create render list
            to_upload_per_texture[tri.tex_id].push_back(blit_vertex_t{tri.v0_.tex_, chart.all_triangle_new_coods_[tri_id][0]});
            to_upload_per_texture[tri.tex_id].push_back(blit_vertex_t{tri.v1_.tex_, chart.all_triangle_new_coods_[tri_id][1]});
            to_upload_per_texture[tri.tex_id].push_back(blit_vertex_t{tri.v2_.tex_, chart.all_triangle_new_coods_[tri_id][2]});
          }
          else {
            ++num_dropped_tris;
          }

          //override texture coordinates
          tri.v0_.tex_ = chart.all_triangle_new_coods_[tri_id][0];
          tri.v1_.tex_ = chart.all_triangle_new_coods_[tri_id][1];
          tri.v2_.tex_ = chart.all_triangle_new_coods_[tri_id][2];

          tri.v0_.tex_.y = 1.0-tri.v0_.tex_.y; //flip y coord
          tri.v1_.tex_.y = 1.0-tri.v1_.tex_.y;
          tri.v2_.tex_.y = 1.0-tri.v2_.tex_.y;

        }
        else {
          ++num_dropped_tris;
        }
      }
      else {
        ++num_invalid_tris;
      }
    }
  }

  std::cout << "Updating texture coordinates in inner LOD nodes..." << std::endl;
  for (uint32_t node_id = 0; node_id < first_leaf_id; ++node_id) {

    auto& tris = bvh->get_triangles(node_id);
    for (int local_tri_id = 0; local_tri_id < tris.size(); ++local_tri_id) {
      auto& tri = tris[local_tri_id];
      if (tri.chart_id != -1) {
        auto& proj_info = chart_map[tri.area_id][tri.chart_id].projection;
        rectangle& chart_rect = chart_map[tri.area_id][tri.chart_id].rect_;
        rectangle& area_rect = area_rects[tri.area_id];

        //at this point we will need to project all triangles of inner nodes to their respective charts using the corresponding chart plane

        scm::math::vec3f original_v;

        for (uint32_t i = 0; i < 3; ++i) {
          switch (i) {
            case 0: original_v = tri.v0_.pos_; break;
            case 1: original_v = tri.v1_.pos_; break;
            case 2: original_v = tri.v2_.pos_; break;
            default: break;
          }
          scm::math::vec2f projected_v = project_to_plane(original_v, proj_info.proj_normal, proj_info.proj_centroid, proj_info.proj_world_up);

          projected_v -= proj_info.tex_coord_offset; //correct by offset (so that min uv coord = 0)

          projected_v /= proj_info.largest_max; //apply normalisation factor
          if ((chart_rect.flipped_ && !area_rect.flipped_) || (area_rect.flipped_ && !chart_rect.flipped_)) { //flip if needed
            float temp = projected_v.x;
            projected_v.x = projected_v.y;
            projected_v.y = temp;
          }
          projected_v *= packing_scale; //scale
          projected_v += chart_rect.min_; //offset position in texture
          projected_v /= image_rect.max_; //scale down to normalised image space
          projected_v.y = 1.0 - projected_v.y; //flip y coord

          //replace existing coords
          switch (i) {
            case 0: tri.v0_.tex_ = projected_v; break;
            case 1: tri.v1_.tex_ = projected_v; break;
            case 2: tri.v2_.tex_ = projected_v; break;
            default: break;
          }
        }
      }
      else {
        ++num_invalid_tris;
      }
    }

  }

  std::cout << "Num tris with invalid chart ids encountered: " << num_invalid_tris << std::endl;
  std::cout << "Num dropped tris encountered: " << num_dropped_tris << std::endl;

  std::string bvh_filename = obj_filename.substr(0, obj_filename.size()-4)+".bvh";
  bvh->write_bvh_file(bvh_filename);
  std::cout << "Bvh file written to " << bvh_filename << std::endl;

  std::string lod_filename = obj_filename.substr(0, obj_filename.size()-4)+".lod";
  bvh->write_lod_file(lod_filename);
  std::cout << "Lod file written to " << lod_filename << std::endl;

  //cleanup
  std::cout << "Cleanup bvh" << std::endl;
  bvh.reset();

  std::cout << "Single texture size limit: " << single_tex_limit << std::endl;
  std::cout << "Multi texture size limit: " << multi_tex_limit << std::endl;

  render_to_texture_width_ = std::max(single_tex_limit, 4096);
  render_to_texture_height_ = std::max(single_tex_limit, 4096);

  multi_tex_limit = std::max(render_to_texture_width_, multi_tex_limit);

  full_texture_width_ = render_to_texture_width_;
  full_texture_height_ = render_to_texture_height_;

  //double texture size up to 8k if a given percentage of charts do not have enough pixels
  const double target_percentage_charts_with_enough_pixels = 0.90;
  std::cout << "Adjusting final texture size (" << full_texture_width_ << " x " << full_texture_height_ << ")" << std::endl;

  calculate_new_chart_tex_space_sizes(chart_map, triangles, scm::math::vec2i(full_texture_width_, full_texture_height_));
  while (!is_output_texture_big_enough(chart_map, target_percentage_charts_with_enough_pixels)) {

    if (std::max(full_texture_width_, full_texture_height_) >= multi_tex_limit) {
      std::cout << "Maximum texture size limit reached (" << full_texture_width_ << " x " << full_texture_height_ << ")" << std::endl;
      break;
    }

    full_texture_width_ *= 2;
    full_texture_height_ *= 2;

    std::cout << "Not enough pixels! Adjusting final texture size (" << full_texture_width_ << " x " << full_texture_height_ << ")" << std::endl;

    calculate_new_chart_tex_space_sizes(chart_map, triangles, scm::math::vec2i(full_texture_width_, full_texture_height_));
  }

  //if output texture is bigger than 8k, create a set if viewports that will be rendered separately
  if (full_texture_width_ > render_to_texture_width_ || full_texture_height_ > render_to_texture_height_) {
    //calc num of viewports needed from size of output texture
    int viewports_w = std::ceil(full_texture_width_ / render_to_texture_width_);
    int viewports_h = std::ceil(full_texture_height_ / render_to_texture_height_);

    scm::math::vec2f viewport_normed_size(1.f / viewports_w, 1.f / viewports_h);

    //create a vector of viewports needed
    for (int y = 0; y < viewports_h; ++y) {
      for (int x = 0; x < viewports_w; ++x) {
        viewport vp;
        vp.normed_dims = viewport_normed_size;
        vp.normed_offset = scm::math::vec2f(viewport_normed_size.x * x, viewport_normed_size.y * y);
        viewports_.push_back(vp);

        //std::cout << "Viewport " << viewports.size() -1 << ": " << viewport_normed_size.x << "x" << viewport_normed_size.y << " at (" << vp.normed_offset.x << "," << vp.normed_offset.y << ")\n";
      }
    }
  }
  else {
    viewport single_viewport;
    single_viewport.normed_offset = scm::math::vec2f(0.f,0.f);
    single_viewport.normed_dims = scm::math::vec2f(1.0,1.0);
    viewports_.push_back(single_viewport);

    //std::cout << "Viewport " << viewports.size() -1 << ": " << single_viewport.normed_dims.x << "x" << single_viewport.normed_dims.y << " at (" << single_viewport.normed_offset.x << "," << single_viewport.normed_offset.y << ")\n";
  }
  std::cout << "Created " << viewports_.size() << " viewports to render multiple output textures" << std::endl;

  std::cout << "Loading all textures..." << std::endl;
  for (auto tex_it : texture_info_map) {
    textures_.push_back(load_image(tex_it.second.filename_));
  }

  std::cout << "Compiling shaders..." << std::endl;
  make_shader_program();
  make_dilation_shader_program();

  std::cout << "Creating framebuffers..." << std::endl;

  //create output frame buffers
  for (int i = 0; i < 2; ++i) {
    frame_buffers_.push_back(std::make_shared<frame_buffer_t>(1, render_to_texture_width_, render_to_texture_height_, GL_RGBA, GL_LINEAR));
  }

  //create vertex buffer for dilation
  float screen_space_quad_geometry[30] {
    -1.0, -1.0, 0.0, 0.0,
     1.0, -1.0, 1.0, 0.0,
    -1.0,  1.0, 0.0, 1.0,

      1.0, -1.0, 1.0, 0.0,
      1.0,  1.0, 1.0, 1.0,
     -1.0,  1.0, 0.0, 1.0
  };
  glGenBuffers(1, &dilation_vertex_buffer_);
  glBindBuffer(GL_ARRAY_BUFFER, dilation_vertex_buffer_);
  glBufferData(GL_ARRAY_BUFFER, 6*4*sizeof(float), &screen_space_quad_geometry[0], GL_STATIC_DRAW);
  glBindBuffer(GL_ARRAY_BUFFER, 0);

  //set the viewport size
  glViewport(0, 0, (GLsizei)render_to_texture_width_, (GLsizei)render_to_texture_height_);

  //set background colour
  glClearColor(1.0f, 0.0f, 1.0f, 1.0f);

  glGenBuffers(1, &vertex_buffer_);

  std::vector<std::vector<uint8_t>> area_images(viewports_.size());

  for (uint32_t view_id = 0; view_id < viewports_.size(); ++view_id) {
    std::cout << "Rendering into viewport " << view_id << "..." << std::endl;

    viewport vport = viewports_[view_id];

    std::cout << "Viewport start: " << vport.normed_offset.x << ", " << vport.normed_offset.y << std::endl;
    std::cout << "Viewport size: " << vport.normed_dims.x << ", " << vport.normed_dims.y << std::endl;

    frame_buffers_[0]->enable();

    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    for (uint32_t i = 0; i < to_upload_per_texture.size(); ++i) {

      uint32_t num_vertices = to_upload_per_texture[i].size();

      if (num_vertices == 0) {
        std::cout << "Nothing to render for texture " << i << " ("<< texture_info_map[i].filename_ << ")" << std::endl;
        continue;
      }

      std::cout << "Rendering from texture " << i << " ("<< texture_info_map[i].filename_ << ")" << std::endl;

      glUseProgram(shader_program_);

      //upload this vector to GPU
      glBindBuffer(GL_ARRAY_BUFFER, vertex_buffer_);
      glBufferData(GL_ARRAY_BUFFER, num_vertices*sizeof(blit_vertex_t), &to_upload_per_texture[i][0], GL_STREAM_DRAW);

      //define the layout of the vertex buffer:
      //setup 2 attributes per vertex (2x texture coord)
      glEnableVertexAttribArray(0);
      glEnableVertexAttribArray(1);
      glVertexAttribPointer(0, 2, GL_FLOAT, GL_FALSE, sizeof(blit_vertex_t), (void*)0);
      glVertexAttribPointer(1, 2, GL_FLOAT, GL_FALSE, sizeof(blit_vertex_t), (void*)(2*sizeof(float)));

      //get texture location
      int slot = 0;
      glUniform1i(glGetUniformLocation(shader_program_, "image"), slot);
      glUniform2f(glGetUniformLocation(shader_program_, "viewport_offset"), vport.normed_offset[0], vport.normed_offset[1]);
      glUniform2f(glGetUniformLocation(shader_program_, "viewport_scale"), vport.normed_dims[0], vport.normed_dims[1]);

      glActiveTexture(GL_TEXTURE0 + slot);

      //here, enable the current texture
      textures_[i]->enable(slot);

      //draw triangles from the currently bound buffer
      glDrawArrays(GL_TRIANGLES, 0, num_vertices);

      //unbind, unuse
      glBindBuffer(GL_ARRAY_BUFFER, 0);
      glUseProgram(0);

      textures_[i]->disable();

    } //end for each texture

    frame_buffers_[0]->disable();

    std::cout << "Dilating view " << view_id << "..." << std::endl;

    uint32_t num_dilations = render_to_texture_width_/2;

    uint32_t current_framebuffer = 0;
    for (int i = 0; i < num_dilations; ++i) {

      current_framebuffer = (i+1)%2;

      frame_buffers_[current_framebuffer]->enable();
      int current_texture = 0;
      if (current_framebuffer == 0) {
        current_texture = 1;
      }

      glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

      glUseProgram(dilation_shader_program_);
      glBindBuffer(GL_ARRAY_BUFFER, dilation_vertex_buffer_);

      glEnableVertexAttribArray(0);
      glEnableVertexAttribArray(1);
      glVertexAttribPointer(0, 2, GL_FLOAT, GL_FALSE, sizeof(blit_vertex_t), (void*)0);
      glVertexAttribPointer(1, 2, GL_FLOAT, GL_FALSE, sizeof(blit_vertex_t), (void*)(2*sizeof(float)));

      int slot = 0;
      glUniform1i(glGetUniformLocation(dilation_shader_program_, "image"), slot);
      glUniform1i(glGetUniformLocation(dilation_shader_program_, "image_width"), render_to_texture_width_);
      glUniform1i(glGetUniformLocation(dilation_shader_program_, "image_height"), render_to_texture_height_);
      glActiveTexture(GL_TEXTURE0 + slot);
      frame_buffers_[current_texture]->bind_texture(slot);

      glDrawArrays(GL_TRIANGLES, 0, 6);

      glBindBuffer(GL_ARRAY_BUFFER, 0);
      glUseProgram(0);

      frame_buffers_[current_texture]->unbind_texture(slot);

      frame_buffers_[current_framebuffer]->disable();
    }

    std::vector<uint8_t> pixels;
    frame_buffers_[current_framebuffer]->get_pixels(0, pixels);
    area_images[view_id] = pixels;

  } //end for each viewport

  std::cout << "Producing final texture..." << std::endl;

  //concatenate all area images to one big texture
  uint32_t num_bytes_per_pixel = 4;
  std::vector<uint8_t> final_texture(full_texture_width_*full_texture_height_*num_bytes_per_pixel);

  uint32_t num_lookups_per_line = full_texture_width_ / render_to_texture_width_;

  for (uint32_t y = 0; y < full_texture_height_; ++y) { //for each line
    for (uint32_t tex_x = 0; tex_x < num_lookups_per_line; ++tex_x) {
      void* dst = ((void*)&final_texture[0]) + y*full_texture_width_*num_bytes_per_pixel + tex_x*render_to_texture_width_*num_bytes_per_pixel;
      uint32_t tex_y = y / render_to_texture_height_;
      uint32_t tex_id = tex_y * num_lookups_per_line + tex_x;
      void* src = ((void*)&area_images[tex_id][0]) + (y % render_to_texture_height_)*render_to_texture_width_*num_bytes_per_pixel; //+ 0;

      memcpy(dst, src, render_to_texture_width_*num_bytes_per_pixel);
    }
  }

  std::string image_filename = bvh_filename.substr(0, bvh_filename.size()-4) + "_texture.png";
  save_image(image_filename, final_texture, full_texture_width_, full_texture_height_);

#if 0

  //write obj

  typedef typename Polyhedron::Vertex_const_iterator                  VCI;
  typedef typename Polyhedron::Facet_const_iterator                   FCI;
  typedef typename Polyhedron::Halfedge_around_facet_const_circulator HFCC;
  typedef CGAL::Inverse_index< VCI> Index;

  std::cout << "Writing obj... " << out_filename << std::endl;

  std::ofstream ofs(out_filename);
  File_writer_wavefront_xtnd writer(ofs);

  uint32_t num_vertices = 0;
  uint32_t num_halfedges = 0;
  uint32_t num_facets = 0;
  for (auto& it : per_node_polyhedron) {
    num_vertices += it.second.size_of_vertices();
    num_halfedges += it.second.size_of_halfedges();
    num_facets += it.second.size_of_facets();
  }

  writer.write_header(ofs, num_vertices, num_halfedges, num_facets);

  for (auto& it : per_node_chart_id_map) {
    uint32_t node_id = it.first;
    //auto chart_id_map = it.second;
    auto polyMesh = per_node_polyhedron[node_id];

    for (VCI vi = polyMesh.vertices_begin(); vi != polyMesh.vertices_end(); ++vi) {
      writer.write_vertex(::CGAL::to_double( vi->point().x()),
                          ::CGAL::to_double( vi->point().y()),
                          ::CGAL::to_double( vi->point().z()));
    }
  }

  writer.write_tex_coord_header(num_facets * 3);

  for (auto& it : per_node_chart_id_map) {
    uint32_t node_id = it.first;
    //auto chart_id_map = it.second;
    auto polyMesh = per_node_polyhedron[node_id];

    //for each face, write tex coords
    for (FCI fi = polyMesh.facets_begin(); fi != polyMesh.facets_end(); ++fi) {
      writer.write_tex_coord(fi->t_coords[0].x(), fi->t_coords[0].y());
      writer.write_tex_coord(fi->t_coords[1].x(), fi->t_coords[1].y());
      writer.write_tex_coord(fi->t_coords[2].x(), fi->t_coords[2].y());
    }
  }

  writer.write_normal_header(num_facets);

  for (auto& it : per_node_chart_id_map) {
    uint32_t node_id = it.first;
    //auto chart_id_map = it.second;
    auto polyMesh = per_node_polyhedron[node_id];

    std::map<face_descriptor,Vector> fnormals;
    std::map<vertex_descriptor,Vector> vnormals;
    CGAL::Polygon_mesh_processing::compute_normals(polyMesh,
      boost::make_assoc_property_map(vnormals),
      boost::make_assoc_property_map(fnormals));

    for (face_descriptor fd: faces(polyMesh)){
      writer.write_normal((double)fnormals[fd].x(),(double)fnormals[fd].y(),(double)fnormals[fd].z());
    }

  }

  writer.write_facet_header();

  int32_t face_counter = 0;
  int32_t face_id = 0;

  for (auto& it : per_node_chart_id_map) {
    uint32_t node_id = it.first;
    auto chart_id_map = it.second;
    auto polyMesh = per_node_polyhedron[node_id];

    Index index(polyMesh.vertices_begin(), polyMesh.vertices_end());
    uint32_t highest_idx = 0;

    for(FCI fi = polyMesh.facets_begin(); fi != polyMesh.facets_end(); ++fi) {

      HFCC hc = fi->facet_begin();
      HFCC hc_end = hc;
      std::size_t nn = circulator_size(hc);
      CGAL_assertion(nn >= 3);
      writer.write_facet_begin(n);

      const int id = fi->id();
      int chart_id = chart_id_map[id];
      int edge = 0;
      do {
          uint32_t idx = index[VCI(hc->vertex())];
          highest_idx = std::max(idx, highest_idx);
          writer.write_facet_vertex_index(face_counter+idx, (face_id*3)+edge, face_id); // for uv coords

          ++hc;
          edge++;
      } while (hc != hc_end);
      writer.write_facet_end();
      face_id++;
    }

    face_counter += highest_idx+1;

  }

  writer.write_footer();

  ofs.close();

  std::cout << "Obj file written to:  " << out_filename << std::endl;

  //write charty charts

  std::string chart_filename = out_filename.substr(0,out_filename.size()-4) + ".chart";
  std::ofstream ocfs(chart_filename);

  int32_t chart_id_counter = 0;

  for (auto& it : per_node_chart_id_map) {
    uint32_t node_id = it.first;
    auto chart_id_map = it.second;
    auto polyMesh = per_node_polyhedron[node_id];

    int32_t highest_chart_id = 0;

    for (FCI fi = polyMesh.facets_begin(); fi != polyMesh.facets_end(); ++fi) {
      const int32_t id = fi->id();
      const int32_t chart_id = chart_id_map[id];
      ocfs << chart_id_counter+chart_id << " ";
      highest_chart_id = std::max(highest_chart_id, chart_id);
    }

    chart_id_counter += highest_chart_id;

  }

  ocfs.close();

  std::cout << "Chart file written to:  " << chart_filename << std::endl;

  std::string tex_file_name = out_filename.substr(0, out_filename.size()-4) + ".texid";
  std::ofstream otfs(tex_file_name);
  for (auto& it : per_node_chart_id_map) {
    uint32_t node_id = it.first;
    auto chart_id_map = it.second;
    auto polyMesh = per_node_polyhedron[node_id];

    for (Facet_iterator fi = polyMesh.facets_begin(); fi != polyMesh.facets_end(); ++fi) {
      otfs << fi->tex_id << " ";
    }

  }
  otfs.close();
  std::cout << "Texture id per face file written to:  " << tex_file_name << std::endl;

  std::cout << "TOTAL NUM CHARTS: " << chart_id_counter << std::endl;
#endif

  return 0 ;
}


#include "QuadKey.hpp"
#include "heightmap/FlatElevationProvider.hpp"
#include "builders/terrain/LineGridSplitter.hpp"
#include "mapcss/ColorGradient.hpp"
#include "builders/MeshBuilder.hpp"

#include <boost/test/unit_test.hpp>

using namespace utymap::builders;
using namespace utymap::heightmap;
using namespace utymap::mapcss;
using namespace utymap::math;

namespace {
typedef Vector2 DPoint;
struct Meshing_MeshingFixture {
  Meshing_MeshingFixture() :
      eleProvider(),
      builder(utymap::QuadKey(1, 1, 0), eleProvider),
      gradient(),
      textureRegion(),
      geometryOptions(0, 0, 0, 0),
      appearanceOptions(gradient, 0, 0, textureRegion, 0) {
  }

  FlatElevationProvider eleProvider;
  MeshBuilder builder;
  ColorGradient gradient;
  TextureRegion textureRegion;
  MeshBuilder::GeometryOptions geometryOptions;
  MeshBuilder::AppearanceOptions appearanceOptions;
};
}

BOOST_FIXTURE_TEST_SUITE(Meshing_MeshBuilder, Meshing_MeshingFixture)

BOOST_AUTO_TEST_CASE(GivenPolygon_WhenAddPolygon_ThenRefinesCorrectly) {
  Mesh mesh("");
  Polygon polygon(4, 0);
  geometryOptions.area = 5;
  polygon.addContour(std::vector<DPoint>
                         {
                             DPoint(0, 0),
                             DPoint(10, 0),
                             DPoint(10, 10),
                             DPoint(0, 10)
                         });

  builder.addPolygon(mesh, polygon, geometryOptions, appearanceOptions);

  BOOST_CHECK_EQUAL(mesh.vertices.size()/3, 23);
  BOOST_CHECK_EQUAL(mesh.triangles.size()/3, 34);
}

BOOST_AUTO_TEST_CASE(GivenPolygonWithHole_WhenAddPolygon_ThenRefinesCorrectly) {
  Mesh mesh("");
  Polygon polygon(8, 1);
  geometryOptions.area = 1;
  polygon.addContour(std::vector<DPoint>
                         {
                             DPoint(0, 0),
                             DPoint(10, 0),
                             DPoint(10, 10),
                             DPoint(0, 10)
                         });
  polygon.addHole(std::vector<DPoint>
                      {
                          DPoint(3, 3),
                          DPoint(6, 3),
                          DPoint(6, 6),
                          DPoint(3, 6)
                      });

  builder.addPolygon(mesh, polygon, geometryOptions, appearanceOptions);

  BOOST_CHECK(mesh.vertices.size() > 0);
  BOOST_CHECK(mesh.triangles.size() > 0);
}

BOOST_AUTO_TEST_CASE(GivenPolygonProcessedByGridSplitter_WhenAddPolygon_ThenRefinesCorrectly) {
  std::vector<Vector2> contour;
  LineGridSplitter splitter;
  int scale = 10;
  geometryOptions.area = 1./scale;
  splitter.setParams(scale, 1);
  std::vector<IntPoint> inputPoints
      {
          IntPoint(0*scale, 0*scale),
          IntPoint(10*scale, 0*scale),
          IntPoint(10*scale, 10*scale),
          IntPoint(0*scale, 10*scale)
      };
  std::size_t lastItemIndex = inputPoints.size() - 1;
  for (std::size_t i = 0; i <= lastItemIndex; i++) {
    IntPoint start = inputPoints[i];
    IntPoint end = inputPoints[i==lastItemIndex ? 0 : i + 1];
    splitter.split(start, end, contour);
  }

  Polygon polygon(contour.size(), 0);
  polygon.addContour(contour);

  Mesh mesh("");
  builder.addPolygon(mesh, polygon, geometryOptions, appearanceOptions);

  BOOST_CHECK(mesh.vertices.size() > 0);
  BOOST_CHECK(mesh.triangles.size() > 0);
}

BOOST_AUTO_TEST_CASE(GivenPolygonWithSharePoint_WhenAddPolygon_ThenRefinesCorrectly) {
  Polygon polygon(8, 0);
  polygon.addContour({{0, 0}, {10, 0}, {10, 10}, {0, 10}});
  polygon.addContour({{10, 10}, {15, 10}, {15, 15}, {10, 15}});
  Mesh mesh("");
  geometryOptions.area = 1;

  builder.addPolygon(mesh, polygon, geometryOptions, appearanceOptions);

  BOOST_CHECK(mesh.vertices.size() > 0);
}

BOOST_AUTO_TEST_CASE(GivenSimpleSquareWithTextureOptions_WhenAddPolygon_ThenTextureIsApplied) {

  Mesh mesh("");
  Polygon polygon(4, 0);
  polygon.addContour(std::vector<DPoint> {{0, 0}, {10, 0}, {10, 10}, {0, 10}});
  TextureRegion newTextureRegion(100, 100, 0, 0, 100, 100);
  MeshBuilder::AppearanceOptions newAppearanceOptions(gradient, 0, 0, newTextureRegion, 1);

  builder.addPolygon(mesh, polygon, geometryOptions, newAppearanceOptions);

  BOOST_CHECK(mesh.uvs.size() > 0);
  BOOST_CHECK_EQUAL(mesh.vertices.size()*2/3, mesh.uvs.size());
}

BOOST_AUTO_TEST_CASE(GivenSimpleSquareWithScale_WhenAddPolygon_ThenScaledTextureIsApplied) {
  Mesh mesh("");
  Polygon polygon(4, 0);
  MeshBuilder localBuilder(utymap::QuadKey(16, 35205, 21489), eleProvider);
  polygon
      .addContour(std::vector<DPoint> {{13.3874549, 52.530385}, {13.38747790, 52.53038981}, {13.38764498, 52.53042475},
                                       {13.38781206, 52.53045970}});
  TextureRegion newTextureRegion(1025, 1025, 513, 513, 512, 512);
  MeshBuilder::AppearanceOptions newAppearanceOptions(gradient, 0, 0, newTextureRegion, 100);

  localBuilder.addPolygon(mesh, polygon, geometryOptions, newAppearanceOptions);

  BOOST_CHECK(mesh.uvs.size() > 0);
  BOOST_CHECK_EQUAL(mesh.vertices.size()*2/3, mesh.uvs.size());
}

BOOST_AUTO_TEST_SUITE_END()


// ====================================================================
// This file is part of FlexibleSUSY.
//
// FlexibleSUSY is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published
// by the Free Software Foundation, either version 3 of the License,
// or (at your option) any later version.
//
// FlexibleSUSY is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with FlexibleSUSY.  If not, see
// <http://www.gnu.org/licenses/>.
// ====================================================================

// File generated at Thu 10 May 2018 14:47:56

#ifndef HSSUSY_TWO_SCALE_LOW_SCALE_CONSTRAINT_H
#define HSSUSY_TWO_SCALE_LOW_SCALE_CONSTRAINT_H

#include "HSSUSY_low_scale_constraint.hpp"
#include "HSSUSY_input_parameters.hpp"
#include "single_scale_constraint.hpp"
#include "lowe.h"
#include <Eigen/Core>

namespace flexiblesusy {

template <class T>
class HSSUSY;

class Two_scale;

template<>
class HSSUSY_low_scale_constraint<Two_scale> : public Single_scale_constraint {
public:
   HSSUSY_low_scale_constraint() = default;
   HSSUSY_low_scale_constraint(HSSUSY<Two_scale>*, const softsusy::QedQcd&);
   virtual ~HSSUSY_low_scale_constraint() = default;
   virtual void apply() override;
   virtual double get_scale() const override;
   virtual std::string name() const override { return "HSSUSY low-scale constraint"; }
   virtual void set_model(Model*) override;

   void clear();
   const Eigen::Matrix<std::complex<double>,3,3>& get_ckm();
   const Eigen::Matrix<std::complex<double>,3,3>& get_pmns();
   double get_initial_scale_guess() const;
   void initialize();
   const softsusy::QedQcd& get_sm_parameters() const;
   void set_sm_parameters(const softsusy::QedQcd&);

private:
   double scale{0.};
   double initial_scale_guess{0.};
   HSSUSY<Two_scale>* model{nullptr};
   softsusy::QedQcd qedqcd{};
   Eigen::Matrix<std::complex<double>,3,3> ckm{Eigen::Matrix<std::complex<double>,3,3>::Identity()};
   Eigen::Matrix<std::complex<double>,3,3> pmns{Eigen::Matrix<std::complex<double>,3,3>::Identity()};
   Eigen::Matrix<double,3,3> neutrinoDRbar{Eigen::Matrix<double,3,3>::Zero()};
   double mW_run{0.};
   double mZ_run{0.};
   double AlphaS{0.};
   double e_run{0.};
   double ThetaWDRbar{0.};
   double new_g1{0.}, new_g2{0.}, new_g3{0.};

   double calculate_theta_w();
   void calculate_threshold_corrections();
   void calculate_DRbar_gauge_couplings();
   void calculate_DRbar_yukawa_couplings();
   void calculate_Yu_DRbar();
   void calculate_Yd_DRbar();
   void calculate_Ye_DRbar();
   void calculate_MNeutrino_DRbar();
   double calculate_delta_alpha_em(double) const;
   double calculate_delta_alpha_s(double) const;
   double calculate_alpha_s_SM5_at(softsusy::QedQcd, double) const;
   void check_model_ptr() const;
   void update_scale();
};

} // namespace flexiblesusy

#endif


#include <cctbx/boost_python/flex_fwd.h>

#include <cctbx/xray/sampled_model_density.h>
#include <boost/python/class.hpp>
#include <boost/python/return_value_policy.hpp>
#include <boost/python/copy_const_reference.hpp>

namespace cctbx { namespace xray { namespace boost_python {

namespace {

  struct sampled_model_density_wrappers
  {
    typedef sampled_model_density<> w_t;
    typedef w_t::grid_point_type grid_point_type;

    static void
    wrap()
    {
      using namespace boost::python;
      typedef return_value_policy<copy_const_reference> ccr;
      class_<w_t, bases<w_t::base_t> >("sampled_model_density", no_init)
        .def(init<uctbx::unit_cell const&,
                  af::const_ref<scatterer<> > const&,
                  scattering_type_registry const&,
                  grid_point_type const&,
                  grid_point_type const&,
                  optional<double const&,
                           double const&,
                           double const&,
                           bool,
                           bool,
                           double const&,
                           bool,
                           int> >(
          (arg("unit_cell"),
           arg("scatterers"),
           arg("scattering_type_registry"),
           arg("fft_n_real"),
           arg("fft_m_real"),
           arg("u_base")=0.25,
           arg("wing_cutoff")=1.e-3,
           arg("exp_table_one_over_step_size")=-100,
           arg("force_complex")=false,
           arg("sampled_density_must_be_positive")=false,
           arg("tolerance_positive_definite")=1.e-5,
           arg("use_u_base_as_u_extra")=false,
           arg("store_grid_indices_for_each_scatterer")=0)))
        .def("real_map", &w_t::real_map)
        .def("complex_map", &w_t::complex_map)
        .def("eliminate_u_extra_and_normalize",
          &w_t::eliminate_u_extra_and_normalize,
          (arg("miller_indices"), arg("structure_factors")))
        .def("grid_indices_for_each_scatterer",
          &w_t::grid_indices_for_each_scatterer, ccr())
      ;
    }
  };

} // namespace <anoymous>

  void wrap_sampled_model_density()
  {
    sampled_model_density_wrappers::wrap();
  }

}}} // namespace cctbx::xray::boost_python


// Copyright (c) 2015-2021 Daniel Cooke
// Use of this source code is governed by the MIT license that can be found in the LICENSE file.

#ifndef trio_model_hpp
#define trio_model_hpp

#include <vector>
#include <functional>
#include <cstddef>

#include <boost/optional.hpp>

#include "basics/trio.hpp"
#include "core/types/haplotype.hpp"
#include "core/types/indexed_haplotype.hpp"
#include "core/models/mutation/denovo_model.hpp"
#include "core/models/haplotype_likelihood_array.hpp"
#include "core/types/genotype.hpp"
#include "population_prior_model.hpp"
#include "logging/logging.hpp"

namespace octopus { namespace model {

class TrioModel
{
public:
    using GenotypeVector = MappableBlock<Genotype<IndexedHaplotype<>>>;

    struct Latents
    {
        struct JointProbability
        {
            using GenotypeIndex = unsigned;
            double log_probability, probability;
            GenotypeIndex maternal, paternal, child;
        };
        using JointProbabilityVector = std::vector<JointProbability>;
        JointProbabilityVector joint_genotype_probabilities;
    };

    struct InferredLatents
    {
        Latents posteriors;
        double log_evidence;
        boost::optional<double> estimated_lost_log_posterior_mass = boost::none;
    };

    struct Options
    {
        boost::optional<std::size_t> max_genotype_combinations = boost::none;
        double max_individual_log_probability_loss = -1'000, max_joint_log_probability_loss = -10'000;
    };

    TrioModel() = delete;

    TrioModel(const Trio& trio,
              const PopulationPriorModel& prior_model,
              const DeNovoModel& mutation_model,
              Options options,
              boost::optional<logging::DebugLogger> debug_log = boost::none);

    TrioModel(const TrioModel&)            = delete;
    TrioModel& operator=(const TrioModel&) = delete;
    TrioModel(TrioModel&&)                 = delete;
    TrioModel& operator=(TrioModel&&)      = delete;

    ~TrioModel() = default;

    static unsigned max_ploidy() noexcept;

    const PopulationPriorModel& prior_model() const noexcept;

    InferredLatents
    evaluate(const GenotypeVector& maternal_genotypes,
             const GenotypeVector& paternal_genotypes,
             const GenotypeVector& child_genotypes,
             const HaplotypeLikelihoodArray& haplotype_likelihoods) const;

    // Use if all samples have same ploidy
    InferredLatents
    evaluate(const GenotypeVector& genotypes,
             const HaplotypeLikelihoodArray& haplotype_likelihoods) const;

private:
    const Trio& trio_;
    const PopulationPriorModel& prior_model_;
    const DeNovoModel& mutation_model_;
    Options options_;
    mutable boost::optional<logging::DebugLogger> debug_log_;

    InferredLatents evaluate_allosome(const GenotypeVector& parent_genotypes,
                                      const GenotypeVector& child_genotypes,
                                      const HaplotypeLikelihoodArray& haplotype_likelihoods) const;
};

} // namespace model
} // namespace octopus

#endif


#pragma once

#include <mbgl/annotation/annotation.hpp>
#include <mbgl/util/geo.hpp>

#include <string>

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-function"
#pragma GCC diagnostic ignored "-Wunused-parameter"
#pragma GCC diagnostic ignored "-Wunused-variable"
#pragma GCC diagnostic ignored "-Wshadow"
#ifdef __clang__
#pragma GCC diagnostic ignored "-Wunknown-pragmas"
#endif
#pragma GCC diagnostic ignored "-Wpragmas"
#pragma GCC diagnostic ignored "-Wdeprecated-register"
#pragma GCC diagnostic ignored "-Wshorten-64-to-32"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/register/point.hpp>
#include <boost/geometry/geometries/register/box.hpp>
#include <boost/geometry/index/rtree.hpp>
#pragma GCC diagnostic pop

// Make Boost Geometry aware of our LatLng type
BOOST_GEOMETRY_REGISTER_POINT_2D(mbgl::LatLng, double, boost::geometry::cs::cartesian, longitude, latitude)
BOOST_GEOMETRY_REGISTER_BOX(mbgl::LatLngBounds, mbgl::LatLng, southwest(), northeast())

namespace mbgl {

class AnnotationTileLayer;
class CanonicalTileID;

class SymbolAnnotationImpl {
public:
    SymbolAnnotationImpl(AnnotationID, SymbolAnnotation);

    void updateLayer(const CanonicalTileID&, AnnotationTileLayer&) const;

    const AnnotationID id;
    const SymbolAnnotation annotation;
};

} // namespace mbgl

// Tell Boost Geometry how to access a std::shared_ptr<mbgl::SymbolAnnotation> object.
namespace boost {
namespace geometry {
namespace index {

template <>
struct indexable<std::shared_ptr<const mbgl::SymbolAnnotationImpl>> {
    using result_type = mbgl::LatLng;
    mbgl::LatLng operator()(const std::shared_ptr<const mbgl::SymbolAnnotationImpl>& v) const {
        const mbgl::Point<double>& p = v->annotation.geometry;
        return mbgl::LatLng(p.y, p.x);
    }
};

} // end namespace index
} // end namespace geometry
} // end namespace boost


#include <Configuration.h> // Package Path
#include <avatar_locomanipulation/enable_pinocchio_with_hpp_fcl.h> // Enable HPP FCL
// Multibody
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/multibody/geometry.hpp"
// Parsers
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/parsers/srdf.hpp"
// Algorithms
#include "pinocchio/algorithm/geometry.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
// Standard
#include <math.h>
#include <iostream>
#include <iomanip>
#include <vector>
#include <boost/shared_ptr.hpp>

int main(int argc, char ** argv){
  std::string filename = (argc<=1) ? THIS_PACKAGE_PATH"models/valkyrie_simplified.urdf" : argv[1];
  //const std::string srdf_filename = PINOCCHIO_SOURCE_DIR"/models/romeo/romeo_description/srdf/romeo.srdf";
  std::vector < std::string > packageDirs;
  std::string meshDir  = THIS_PACKAGE_PATH"../val_model/";
  packageDirs.push_back(meshDir);

  // build URDF model
  pinocchio::Model model;
  pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(),model);

  // build Geometry model
  pinocchio::GeometryModel geomModel;
  pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geomModel, packageDirs );

  geomModel.addAllCollisionPairs();

  pinocchio::Data data(model);
  pinocchio::GeometryData geomData(geomModel);
  pinocchio::fcl::DistanceResult result;

    // Initialize Configuration -----------------------
  Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq); // Config
  Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv); // Config Velocity
  Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv); // Config Acceleration

  // floating base joints: x, y, z
  q[0] = 0.0;  q[1] = 0.0;  q[2] = 0.0;

  // floating base quaternion: qx, qy, qz, qw
  double theta = M_PI/4.0;
  Eigen::AngleAxis<double> aa(theta, Eigen::Vector3d(0.0, 0.0, 1.0)); // yaw pi/4 to the left
  Eigen::Quaternion<double> quat_init; quat_init =  aa;

  q[3] = quat_init.x();// 0.0;
  q[4] = quat_init.y(); //0.0;
  q[5] = quat_init.z(); //sin(theta/2.0);
  q[6] = quat_init.w(); //cos(theta/2.0);

  // std::cout << "Initial " << q.transpose() << std::endl;
  // std::cout << "q = " << q.transpose() << std::endl;
  int j;
  pinocchio::PairIndex PairId;

  pinocchio::updateGeometryPlacements(model, data, geomModel, geomData, q);
  pinocchio::Index idx = geomModel.findCollisionPair(pinocchio::CollisionPair(1,10));

  // this calls computeDistance for evey collision pair
  	// computeDistance Computes the minimal distance between collision objects of a *SINGLE* collison pair
  //the results are located in geomData.distanceResults
  	// if result pos: no collision; if result neg: collision
  	// distanceResult struct: https://github.com/flexible-collision-library/fcl/blob/9dba579158109c0164bfe0e8b4a75c16bfc788f6/include/fcl/narrowphase/distance_result.h
  pinocchio::computeDistances(model,data,geomModel,geomData,q);

  std::cout << "------------Distance Query Results: " << std::endl;
  for(j=0; j<geomData.distanceResults.size(); j++)
  {
    //We iterate through the distanceResults vector so we can determine each pairs distance
      result = geomData.distanceResults[j];
      // We can print the collision objects, but this is quite useless
      	// Unsure how to go from collision object, which is an address to the name of the link it represents
      std::cout << "collision object 1: " << result.o1 << std::endl;
      std::cout << "collision object 2: " << result.o2 << std::endl;
      std::cout << "result.min_distance: " << result.min_distance << std::endl;
      //std::cout << "result.nearest_points[1]" << result.nearest_points[1] << std::endl;
  }
}


/* Boost interval/rounded_transc.hpp template implementation file
 *
 * Copyright 2002-2003 Hervé Brönnimann, Guillaume Melquiond, Sylvain Pion
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or
 * copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_NUMERIC_INTERVAL_ROUNDED_TRANSC_HPP
#define BOOST_NUMERIC_INTERVAL_ROUNDED_TRANSC_HPP

#include <boost/numeric/interval/rounding.hpp>
#include <boost/numeric/interval/detail/bugs.hpp>
#include <boost/config/no_tr1/cmath.hpp>

namespace boost {
namespace numeric {
namespace interval_lib {

template<class T, class Rounding>
struct rounded_transc_exact: Rounding
{
# define BOOST_NUMERIC_INTERVAL_new_func(f) \
    T f##_down(const T& x) { BOOST_NUMERIC_INTERVAL_using_math(f); return f(x); } \
    T f##_up  (const T& x) { BOOST_NUMERIC_INTERVAL_using_math(f); return f(x); }
  BOOST_NUMERIC_INTERVAL_new_func(exp)
  BOOST_NUMERIC_INTERVAL_new_func(log)
  BOOST_NUMERIC_INTERVAL_new_func(sin)
  BOOST_NUMERIC_INTERVAL_new_func(cos)
  BOOST_NUMERIC_INTERVAL_new_func(tan)
  BOOST_NUMERIC_INTERVAL_new_func(asin)
  BOOST_NUMERIC_INTERVAL_new_func(acos)
  BOOST_NUMERIC_INTERVAL_new_func(atan)
  BOOST_NUMERIC_INTERVAL_new_func(sinh)
  BOOST_NUMERIC_INTERVAL_new_func(cosh)
  BOOST_NUMERIC_INTERVAL_new_func(tanh)
# undef BOOST_NUMERIC_INTERVAL_new_func
# define BOOST_NUMERIC_INTERVAL_new_func(f) \
    T f##_down(const T& x) { BOOST_NUMERIC_INTERVAL_using_ahyp(f); return f(x); } \
    T f##_up  (const T& x) { BOOST_NUMERIC_INTERVAL_using_ahyp(f); return f(x); }
  BOOST_NUMERIC_INTERVAL_new_func(asinh)
  BOOST_NUMERIC_INTERVAL_new_func(acosh)
  BOOST_NUMERIC_INTERVAL_new_func(atanh)
# undef BOOST_NUMERIC_INTERVAL_new_func
};

template<class T, class Rounding>
struct rounded_transc_std: Rounding
{
# define BOOST_NUMERIC_INTERVAL_new_func(f) \
    T f##_down(const T& x) \
    { BOOST_NUMERIC_INTERVAL_using_math(f); \
      this->downward(); return this->force_rounding(f(x)); } \
    T f##_up  (const T& x) \
    { BOOST_NUMERIC_INTERVAL_using_math(f); \
      this->upward(); return this->force_rounding(f(x)); }
  BOOST_NUMERIC_INTERVAL_new_func(exp)
  BOOST_NUMERIC_INTERVAL_new_func(log)
  BOOST_NUMERIC_INTERVAL_new_func(sin)
  BOOST_NUMERIC_INTERVAL_new_func(cos)
  BOOST_NUMERIC_INTERVAL_new_func(tan)
  BOOST_NUMERIC_INTERVAL_new_func(asin)
  BOOST_NUMERIC_INTERVAL_new_func(acos)
  BOOST_NUMERIC_INTERVAL_new_func(atan)
  BOOST_NUMERIC_INTERVAL_new_func(sinh)
  BOOST_NUMERIC_INTERVAL_new_func(cosh)
  BOOST_NUMERIC_INTERVAL_new_func(tanh)
# undef BOOST_NUMERIC_INTERVAL_new_func
# define BOOST_NUMERIC_INTERVAL_new_func(f) \
    T f##_down(const T& x) \
    { BOOST_NUMERIC_INTERVAL_using_ahyp(f); \
      this->downward(); return this->force_rounding(f(x)); } \
    T f##_up  (const T& x) \
    { BOOST_NUMERIC_INTERVAL_using_ahyp(f); \
      this->upward(); return this->force_rounding(f(x)); }
  BOOST_NUMERIC_INTERVAL_new_func(asinh)
  BOOST_NUMERIC_INTERVAL_new_func(acosh)
  BOOST_NUMERIC_INTERVAL_new_func(atanh)
# undef BOOST_NUMERIC_INTERVAL_new_func
};

template<class T, class Rounding>
struct rounded_transc_opp: Rounding
{
# define BOOST_NUMERIC_INTERVAL_new_func(f) \
    T f##_down(const T& x) \
    { BOOST_NUMERIC_INTERVAL_using_math(f); \
      this->downward(); T y = this->force_rounding(f(x)); \
      this->upward(); return y; } \
    T f##_up  (const T& x) \
    { BOOST_NUMERIC_INTERVAL_using_math(f); \
      return this->force_rounding(f(x)); }
  BOOST_NUMERIC_INTERVAL_new_func(exp)
  BOOST_NUMERIC_INTERVAL_new_func(log)
  BOOST_NUMERIC_INTERVAL_new_func(cos)
  BOOST_NUMERIC_INTERVAL_new_func(acos)
  BOOST_NUMERIC_INTERVAL_new_func(cosh)
# undef BOOST_NUMERIC_INTERVAL_new_func
# define BOOST_NUMERIC_INTERVAL_new_func(f) \
    T f##_down(const T& x) \
    { BOOST_NUMERIC_INTERVAL_using_math(f); \
      return -this->force_rounding(-f(x)); } \
    T f##_up  (const T& x) \
    { BOOST_NUMERIC_INTERVAL_using_math(f); \
      return this->force_rounding(f(x)); }
  BOOST_NUMERIC_INTERVAL_new_func(sin)
  BOOST_NUMERIC_INTERVAL_new_func(tan)
  BOOST_NUMERIC_INTERVAL_new_func(asin)
  BOOST_NUMERIC_INTERVAL_new_func(atan)
  BOOST_NUMERIC_INTERVAL_new_func(sinh)
  BOOST_NUMERIC_INTERVAL_new_func(tanh)
# undef BOOST_NUMERIC_INTERVAL_new_func
# define BOOST_NUMERIC_INTERVAL_new_func(f) \
    T f##_down(const T& x) \
    { BOOST_NUMERIC_INTERVAL_using_ahyp(f); \
      this->downward(); T y = this->force_rounding(f(x)); \
      this->upward(); return y; } \
    T f##_up  (const T& x) \
    { BOOST_NUMERIC_INTERVAL_using_ahyp(f); \
      return this->force_rounding(f(x)); }
  BOOST_NUMERIC_INTERVAL_new_func(asinh)
  BOOST_NUMERIC_INTERVAL_new_func(atanh)
# undef BOOST_NUMERIC_INTERVAL_new_func
# define BOOST_NUMERIC_INTERVAL_new_func(f) \
    T f##_down(const T& x) \
    { BOOST_NUMERIC_INTERVAL_using_ahyp(f); \
      return -this->force_rounding(-f(x)); } \
    T f##_up  (const T& x) \
    { BOOST_NUMERIC_INTERVAL_using_ahyp(f); \
      return this->force_rounding(f(x)); }
  BOOST_NUMERIC_INTERVAL_new_func(acosh)
# undef BOOST_NUMERIC_INTERVAL_new_func
};

} // namespace interval_lib
} // namespace numeric
} // namespace boost

#endif // BOOST_NUMERIC_INTERVAL_ROUNDED_TRANSC_HPP