#include "particle_buffer.h"
 
 void particleBuffer_init( ParticleBuffer* particleBuffer, int maxParticles )
 {
 	if( quadric == NULL )
 	{
 		quadric = gluNewQuadric();;
 	}
 
 	particleBuffer->particles = (Particle*)_aligned_malloc( maxParticles * sizeof( Particle ), 16 );
 
 	particleBuffer->maxParticles = maxParticles;
 	particleBuffer->curParticles = 0;
 }
 
 
 void particleBuffer_copy( ParticleBuffer* particleBuffer, ParticleBuffer* particleBuffer_out, bool new_pb )
 {
 	if( new_pb )
 	{
 		*particleBuffer_out = *particleBuffer;
 		particleBuffer_out->particles = (Particle*)_aligned_malloc( sizeof( Particle ) * particleBuffer->maxParticles, 16 );
 		particleBuffer_out->constraints = particleBuffer->constraints;//(Particle*)_aligned_malloc( sizeof( Particle ) * particleBuffer->maxParticles, 16 );
 	}
 
 	memcpy( particleBuffer_out->particles,
 			particleBuffer->particles,
 			particleBuffer->maxParticles * sizeof( Particle ) );
 }
 
 
 
 
 ParticleBuffer* s_pParticleBuffer;
 float s_fTimeStep;
 
 void particles_force_kernel( int thread_index, struct thread_group* thread_group )
 {	
 	int curParticles    = s_pParticleBuffer->curParticles;
 	Particle* particles = s_pParticleBuffer->particles;
 
 	int lowerbounds = thread_index * total_particles / thread_group->thread_no;
 	int upperbounds = lowerbounds + total_particles / thread_group->thread_no;
 	
 	for( int i = lowerbounds; i < upperbounds; ++i )
 	{
 		solve_forces_mp( &particles[i], s_pParticleBuffer, s_fTimeStep );
 	}
 }
 
 void particles_position_kernel( int thread_index, struct thread_group* thread_group )
 {	
 	int curParticles    = s_pParticleBuffer->curParticles;
 	Particle* particles = s_pParticleBuffer->particles;
 
 	int lowerbounds = thread_index * total_particles / thread_group->thread_no;
 	int upperbounds = lowerbounds + total_particles / thread_group->thread_no;
 	
 	for( int i = lowerbounds; i < upperbounds; ++i )
 	{
 		solve_position( &particles[i],
 			s_pParticleBuffer->constraints,
 			s_pParticleBuffer->constraints_dim,
 			s_fTimeStep );
 	}
 }
 
 
 
 
 void particleBuffer_solve( ParticleBuffer* particleBuffer, float timeStep )
 {
 #if defined( SINGLE_PASS )
 	int curParticles   = particleBuffer->curParticles;
 	Particle* particle = particleBuffer->particles;
 	
 	for( int i = 0; i < curParticles ; ++i )
 	{
 		particle_solve( particle++, particleBuffer, timeStep );		
 	}
 
 #elif defined( MULTI_PASS )
 	
 	int curParticles   = particleBuffer->curParticles;
 	Particle* particles = particleBuffer->particles;
 	
 	//-----------------------------------------------------------------
 	// Solve the forces acting on the particle
 	//-----------------------------------------------------------------
 	for( int i = 0; i < curParticles ; ++i )
 	{
 		solve_forces_mp( &particles[i], particleBuffer, timeStep );
 	}
 
 	
 	//-----------------------------------------------------------------
 	// Solve the position of the particles
 	//-----------------------------------------------------------------
 	for( int i = 0; i < curParticles ; ++i )
 	{
 		solve_position( &particles[i], particleBuffer->constraints, particleBuffer->constraints_dim, timeStep );
 	}
 
 #elif defined( MULTI_THREAD )
 	s_pParticleBuffer = particleBuffer;
 	s_fTimeStep = timeStep;
 
 	//-----------------------------------------------------------------
 	// Solve the forces acting on the particle
 	//-----------------------------------------------------------------
 	thread_group.thread_run = particles_force_kernel;
 	Thread_Group_execute( &thread_group );
 	Thread_Group_wait( &thread_group );
 	
 	//-----------------------------------------------------------------
 	// Solve the position of the particles
 	//-----------------------------------------------------------------
 	thread_group.thread_run = particles_position_kernel;
 	Thread_Group_execute( &thread_group );
 	Thread_Group_wait( &thread_group );
 #endif
 }
 
 
 
 void particleBuffer_draw( ParticleBuffer* particleBuffer )
 {
 	int curParticles = particleBuffer->curParticles;
 	Particle* particle = particleBuffer->particles;
 #if defined( FACING_PARTICLES )
 	glDisable( GL_LIGHTING );
 
 	Vector3 left_vec;
 	Vector3 look;
 	Vector3 up;
 
 	vector3_sub( &g_camera.target, &g_camera.position, &look );
 	vector3_crs( &g_camera.up, &look, &left_vec );
 	vector3_crs( &left_vec, &look, &up );
 
 	vector3_normalize( &left_vec );
 	vector3_mul( &left_vec, PARTICLE_RADIUS * 2 );
 
 	vector3_normalize( &up );
 	vector3_mul( &up, PARTICLE_RADIUS * 2 );
 
 	Vector3 ptn1, ptn2, ptn3, ptn4;
 
 	vector3_add( &left_vec, &up, &ptn1 );
 
 	vector3_mul( &left_vec, -1.0f );
 	vector3_mul( &up, -1.0f );
 
 	vector3_add( &left_vec, &ptn1, &ptn2 );
 	vector3_add( &left_vec, &ptn2, &ptn2 );
 
 	vector3_add( &up, &ptn2, &ptn3 );
 	vector3_add( &up, &ptn3, &ptn3 );
 
 	vector3_add( &up, &ptn1, &ptn4 );
 	vector3_add( &up, &ptn4, &ptn4 );
 
 
 	glEnable (GL_BLEND); glBlendFunc (GL_ONE, GL_ZERO);
 	glDepthFunc( GL_ALWAYS );
 	glEnable( GL_TEXTURE_2D );
 	glDisable( GL_LIGHTING );
 	glBindTexture(GL_TEXTURE_2D, point_texture);	
 	for( int i = 0; i < curParticles ; ++i )
 	{
 		glPushMatrix();
 		glTranslatef( particle[i].position.x_com,
 			particle[i].position.y_com,
 			particle[i].position.z_com );
 
 		glBegin( GL_QUADS );
 			glTexCoord2f(1.0f, 1.0f);glColor4f( 1.0f, 1.0f, 1.0f, 0.7f );glVertex3fv( ptn1.data );
 			glTexCoord2f(0.0f, 1.0f);glColor4f( 1.0f, 1.0f, 1.0f, 0.7f );glVertex3fv( ptn2.data );
 			glTexCoord2f(0.0f, 0.0f);glColor4f( 1.0f, 1.0f, 1.0f, 0.7f );glVertex3fv( ptn3.data );
 			glTexCoord2f(1.0f, 0.0f);glColor4f( 1.0f, 1.0f, 1.0f, 0.7f );glVertex3fv( ptn4.data );
 		glEnd();
 		glPopMatrix();
 	}
 	glDisable( GL_TEXTURE_2D );
 	glDisable( GL_TEXTURE_2D );
 	glDepthFunc( GL_LEQUAL );
 #elif defined( MESH_PARTICLES )
 	for( int i = 0; i < curParticles ; ++i )
 	{
 		particle_draw( particle + i );
 	}
 #endif
 
 	glEnable( GL_LIGHTING );
 
 	glEnable (GL_BLEND); glBlendFunc (GL_SRC_ALPHA, GL_ONE);
 	glColor4f( 0.0f, 0.0f, 1.0f, 0.5f );
 
 	glMatrixMode( GL_MODELVIEW );
 	glPushMatrix();
 	glTranslatef( force_point.x_com, 
 		force_point.y_com, 
 		force_point.z_com );
 	gluSphere( quadric, FB_MAX_DISTANCE, 5, 5 );
 	glPopMatrix();
 
 	if( particleBuffer->constraints )
 	{
 		Constraint* constraint = particleBuffer->constraints;
 
 		for( int i = 0; i < particleBuffer->constraints_dim; ++i )
 		{
 			constraint[i].constraintDraw( constraint + i );
 		}
 	}
 }
 
 
 
 void particle_cube( int rect_x, int rect_y, int rect_z, float radius, float mass, ParticleBuffer* particleBuffer )
 {
 	particleBuffer->curParticles = particleBuffer->maxParticles;
 
 	for( int x = 0; x < rect_x; ++x )
 	{
 		for( int y = 0; y < rect_y; ++y )
 		{
 			for( int z = 0; z < rect_z; ++z )
 			{
 				Particle *p = &(particleBuffer->particles[ x + y * rect_x + z * rect_x * rect_y ]);
 				VECTOR_SET( p->position,
 					x * radius * 2 - ( rect_x - 1 ) * radius,
 					y * radius * 2 + HEIGHT_OFFSET,
 					z * radius * 2 - ( rect_z - 1 ) * radius );				
 				VECTOR_SET( p->velocity,
 					0.00f,
 					0.00f,
 					0.00f );
 			}
 		}
 	}
 }