#pragma once
#ifdef _WIN32
#include <windows.h> 
#endif
#include "ClothSim.h"
#include "objLoader.h"
#include "BSH.h"
#include "MinSphere.h"
#include "list.h"
#include <stdlib.h>
#include <time.h>
#include <GL/gl.h>
#include <GL/glut.h> 
#include <iostream>
#include <fstream>
using namespace std;

/* Some physics constants */
#define DAMPING 0.01 // how much to damp the cloth simulation each frame
#define TIME_STEPSIZE2 0.35*0.35 // how large time step each particle takes each frame
#define CONSTRAINT_ITERATIONS 15 // how many iterations of constraint satisfaction each frame (more is rigid, less is soft)

static GLint    ImageWidth=1280;
static GLint    ImageHeight=720;



objLoader *objdata;
BSH *bsh;

/*********************************************************************************************************************************************/	

float Vec3:: length()
	{
		return sqrt(f[0]*f[0]+f[1]*f[1]+f[2]*f[2]);
	}
Vec3:: Vec3(float x, float y, float z)
	{
		f[0] =x;
		f[1] =y;
		f[2] =z;
	}
Vec3:: Vec3(){}

Vec3 Vec3:: normalized()
	{
		float l = length();
		return Vec3(f[0]/l,f[1]/l,f[2]/l);
	}

	void Vec3:: operator+= (const Vec3 &v)
	{
		f[0]+=v.f[0];
		f[1]+=v.f[1];
		f[2]+=v.f[2];
	}

	Vec3 Vec3:: operator/ (const float &a)
	{
		return Vec3(f[0]/a,f[1]/a,f[2]/a);
	}

	Vec3 Vec3:: operator- (const Vec3 &v)
	{

		return Vec3(f[0]-v.f[0],f[1]-v.f[1],f[2]-v.f[2]);
	}

	Vec3 Vec3:: operator+ (const Vec3 &v)
	{
		return Vec3(f[0]+v.f[0],f[1]+v.f[1],f[2]+v.f[2]);
	}

	Vec3 Vec3:: operator* (const float &a)
	{
		return Vec3(f[0]*a,f[1]*a,f[2]*a);
	}

	Vec3 Vec3:: operator-()
	{
		return Vec3(-f[0],-f[1],-f[2]);
	}

	Vec3 Vec3:: cross(const Vec3 &v)
	{
		return Vec3(f[1]*v.f[2] - f[2]*v.f[1], f[2]*v.f[0] - f[0]*v.f[2], f[0]*v.f[1] - f[1]*v.f[0]);
	}

	float Vec3:: dot(const Vec3 &v)
	{
		return f[0]*v.f[0] + f[1]*v.f[1] + f[2]*v.f[2];
	}


/* The particle class represents a particle of mass that can move around in 3D space*/
/**************************************************************************************************************************/

	Particle ::Particle(Vec3 pos){
		this->pos=pos; 
		this->old_pos=pos;
		this->acceleration= Vec3(0,0,0);
		this->mass=1; 
		this->movable=true;
		this->accumulated_normal=Vec3(0,0,0);
		this->state=-1; }
	Particle ::Particle(){}

	void Particle :: addForce(Vec3 f)
	{
		acceleration += f/mass;
	}

	/* This is one of the important methods, where the time is progressed a single step size (TIME_STEPSIZE)
	   The method is called by Cloth.time_step()
	   Given the equation "force = mass * acceleration" the next position is found through verlet integration*/
	void Particle ::timeStep()
	{
		if(movable)
		{
			Vec3 temp = pos;
			pos = pos + (pos-old_pos)*(1.0-DAMPING) + acceleration*TIME_STEPSIZE2;
			old_pos = temp;
			acceleration = Vec3(0,0,0); // acceleration is reset since it HAS been translated into a change in position (and implicitely into velocity)	
		}
	}

	 Vec3& Particle ::getPos() {return pos;}

	void Particle :: resetAcceleration() {acceleration = Vec3(0,0,0);}

	void Particle :: offsetPos(const Vec3 v) { if(movable) pos += v;}

	void Particle :: makeUnmovable() {movable = false;}

	void Particle :: addToNormal(Vec3 normal)
	{
		accumulated_normal += normal.normalized();
	}

	 Vec3& Particle :: getNormal() { return accumulated_normal;} // notice, the normal is not unit length

	void Particle :: resetNormal() {accumulated_normal = Vec3(0,0,0);}

/********************************************************************************************************************************/


	Constraint:: Constraint(Particle *p1, Particle *p2) 
	{
		this->p1=p1; this->p2=p2;
		Vec3 vec = p1->getPos()-p2->getPos();
		rest_distance = vec.length();
	}

	/* This is one of the important methods, where a single constraint between two particles p1 and p2 is solved
	the method is called by Cloth.time_step() many times per frame*/
	void Constraint:: satisfyConstraint()
	{
		Vec3 p1_to_p2 = p2->getPos()-p1->getPos(); // vector from p1 to p2
		float current_distance = p1_to_p2.length(); // current distance between p1 and p2
		Vec3 correctionVector = p1_to_p2*(1 - rest_distance/current_distance); // The offset vector that could moves p1 into a distance of rest_distance to p2
		Vec3 correctionVectorHalf = correctionVector*0.5; // Lets make it half that length, so that we can move BOTH p1 and p2.
		p1->offsetPos(correctionVectorHalf); // correctionVectorHalf is pointing from p1 to p2, so the length should move p1 half the length needed to satisfy the constraint.
		p2->offsetPos(-correctionVectorHalf); // we must move p2 the negative direction of correctionVectorHalf since it points from p2 to p1, and not p1 to p2.	
	}
/********************************************************************************************************************************/

	ClothFace::ClothFace(Particle *p1, Particle *p2,Particle *p3, obj_face *c/*, int j*/){
      
		this->p1=p1; this->p2=p2;  this->p3= p3; 
        // cout<<"in construct"<<endl;
		this->Clothfaces=c;
	}

	
	void Cloth:: getFaces(Particle *p1,Particle *p2,Particle *p3, int t[]){
		
				obj_face *f= new obj_face;
		            // int tab[4]={0,0,0,0};
		           /* for(int i=0;i<4;i++)
		               {tab[i]=t[i];	 
		                f->normal_index[i]=0;
	                       f->texture_index[i]=0;
		                 }*/
		               f->material_index=f->vertex_count=0;
		              for(int i=0;i<4;i++)
					  {f->vertex_index[i]=t[i];
					   f->normal_index[i]=0;
	                   f->texture_index[i]=0;
					  }
                      Faces.push_back(ClothFace(p1,p2,p3,f)); 
					  //cout<<"Faces= "<<Faces.size()<<endl;

				}


	Particle* Cloth:: getParticle(int x, int y) {return &particles[y*num_particles_width + x];}
	Particle* Cloth:: getParticle1(int x, int y,int col1) {
		//cout<<RefinedParticles[y*col+ x].pos.f[1]<<endl;
		return &RefinedParticles[y*col1+ x];}
	Particle* Cloth:: getParticle2(int x, int y) {return &particleActiveState[y*num_particles_width + x];}

    void Cloth:: getvertexindex(int t[],int x,int y,int x1,int y1,int x2,int y2) {

		//int t[]={0,0,0,0};
		t[0]=y*num_particles_width + x;
		t[1]=y1*num_particles_width + x1;
		t[2]=y2*num_particles_width + x2;
 		t[3]=-1;
    }


	void Cloth:: makeConstraint(Particle *p1, Particle *p2) {constraints.push_back(Constraint(p1,p2));}
	void Cloth:: makeConstraint1(Particle *p1, Particle *p2) {constraints1.push_back(Constraint(p1,p2));}


	/* A private method used by drawShaded() and addWindForcesForTriangle() to retrieve the  
	normal vector of the triangle defined by the position of the particles p1, p2, and p3.
	The magnitude of the normal vector is equal to the area of the parallelogram defined by p1, p2 and p3
	*/
	Vec3 Cloth:: calcTriangleNormal(Particle *p1,Particle *p2,Particle *p3)
	{
		Vec3 pos1 = p1->getPos();
		Vec3 pos2 = p2->getPos();
		Vec3 pos3 = p3->getPos();

		Vec3 v1 = pos2-pos1;
		Vec3 v2 = pos3-pos1;
        
		return v1.cross(v2);
	}

	/* A private method used by windForce() to calcualte the wind force for a single triangle 
	defined by p1,p2,p3*/
	void Cloth:: addWindForcesForTriangle(Particle *p1,Particle *p2,Particle *p3, const Vec3 direction)
	{
		Vec3 normal = calcTriangleNormal(p1,p2,p3);
		Vec3 d = normal.normalized();
		Vec3 force = normal*(d.dot(direction));
		p1->addForce(force);
		p2->addForce(force);
		p3->addForce(force);
	}

	/* A private method used by drawShaded(), that draws a single triangle p1,p2,p3 with a color*/
	void Cloth:: drawTriangle(Particle *p1, Particle *p2, Particle *p3, const Vec3 color)
	{
		glColor3fv( (GLfloat*) &color );

		glNormal3fv((GLfloat *) &(p1->getNormal().normalized() ));
		glVertex3fv((GLfloat *) &(p1->getPos() ));

		glNormal3fv((GLfloat *) &(p2->getNormal().normalized() ));
		glVertex3fv((GLfloat *) &(p2->getPos() ));

		glNormal3fv((GLfloat *) &(p3->getNormal().normalized() ));
		glVertex3fv((GLfloat *) &(p3->getPos() ));	
	}


	/* This is a important constructor for the entire system of particles and constraints*/
	Cloth:: Cloth(float width, float height, int num_particles_width, int num_particles_height,int Reslevel) 
	{
        num_particles = num_particles_width * num_particles_height; 
		this->num_particles_width=num_particles_width; this->num_particles_height=num_particles_height;
		particleActiveState.resize(num_particles_width*num_particles_width); //I am essentially using this vector as an array with room for num_particles_width*num_particles_height particles
        //vector<Vec3> *vec;
		vec vectr;  
		//vector<int> t ; 
		t=new ListeSommet;
		//t->resize((num_particles_width* (Reslevel+1)-Reslevel)*(num_particles_width* (Reslevel+1)-Reslevel)); 
		int i=0;
		// creating particles in a grid of particles from (0,0,0) to (width,-height,0)
		for(int x=0; x<num_particles_width; x++)
		{
			for(int y=0; y<num_particles_height; y++)
			{
				Vec3 pos = Vec3(width * (x/(float)num_particles_width),
								-height * (y/(float)num_particles_height),
								0);
				particleActiveState[y*(num_particles_width)+x]= Particle(pos); // insert particle in column x at y'th row
				particleActiveState[y*(num_particles_width)+x].particleIndex=y*(num_particles_width)+x;
				vectr.push_back(particleActiveState[y*(num_particles_width)+x].pos);
				IndiceVertex S;
				S.index=y*(num_particles_width)+x;
				S.x=x;
				S.y=y;
				t->push_back(S);				 
			}
		}
		/*Settinp up the resolution matrix */
		int k=0;
		 for(int i=0;i<=num_particles_width-1;i++)
		  for(int j=0;j<=num_particles_height-1;j++)		
		  { MatResol1[i][j]=MatResol[i][j]=t->at(k++).index;  }
		    
       /* for(int i=0;i<=num_particles_width*2-1;i++)
		
		{ printf("\n");
		 for(int j=0;j<=num_particles_height*2-1;j++)
			 printf("%d -",MatResol[i][j]);
		}*/
         k=0;
       /* Activate the particles of the first level*/
		for(int i=0;i<num_particles_width;i=i+4)	//<=
		 for(int j=0;j<num_particles_height;j=j+4)//<=
		 {
			 ActiveParticles.push_back(MatResol[i][j]);		   
		 }

		for(int i=0;i<ActiveParticles.size();i++)
			particleActiveState[ActiveParticles[i]].state=1;
		
		for(int i=0;i<particleActiveState.size();i++)
		{
			if(particleActiveState[i].state==1)   
				particles.push_back(particleActiveState.at(i));			
		}
		/////////////////////////////////
		for(int x = 0; x<num_particles_width-4; x=x+4)
		{		
			for(int y=0; y<num_particles_height-4; y=y+4)
			{  
				int t[]={0,0,0,0}; 
			   getvertexindex(t,x+4,y,x,y,x,y+4); 
		       getFaces(getParticle2(x+4,y),getParticle2(x,y),getParticle2(x,y+4),t); 
			   for(int i=0;i<4;i++)
				        t[i]=0;
			   getvertexindex(t,x+4,y+4,x+4,y,x,y+4); 
               getFaces(getParticle2(x+4,y+4),getParticle2(x+4,y),getParticle2(x,y+4),t);  
			}
		}    
		obj_face **Clothface = new obj_face*[vecteurtail];
		obj_face *listface= new obj_face[vecteurtail];

        for (int i=0;i<Faces.size();i++)
		{
			obj_face *f;
			f=Faces.at(i).Clothfaces;
			listface[i] = *f; 
		}   

		for (int i=0;i<Faces.size();i++)
		{
			Clothface[i] = &listface[i];            
		}  
		

		for (int i=0;i<Faces.size();i++)
		{
			for(int j=0;j<4;j++){
				Clothface[i]->vertex_index[j] = Faces.at(i).Clothfaces->vertex_index[j];	
			}
		}		
		this->Indexface=Clothface;
		
         obj_vector *listvertex= new obj_vector[vecteurtail];
              ClothvertexList1Taille=0;  int l=0;
			  
	 for (k=0;k<vectr.size();k=k+276)
	  {  
      	for (;l<69+k;l=l+4)
		{		
			
			listvertex[l].e[0]= vectr.at(l).f[0];
			listvertex[l].e[1]= vectr.at(l).f[1];  
			listvertex[l].e[2]= vectr.at(l).f[2];          
			 ClothvertexList1Taille++;			
		}
		l=l-3;		
        l=l+207;
	 }
		//cout<<"ClothvertexList1Taille= "<<ClothvertexList1Taille<<endl;
         obj_vector **ClothvertexList = new obj_vector*[vecteurtail];
		// ClothvertexList1Taille=vectr.size();
		 //j=0;
       ////////////////////////////////////////////////////////////////////////
		 l=0;
	for (k=0;k<vectr.size();k=k+276)
	  {  
      	for (;l<69+k;l=l+4)
		{		
			ClothvertexList[l] = &listvertex[l];			    			
		}
		l=l-3;		
        l=l+207;
	 }
	  /////////////////////////////////////////////////////////////////////////

	  // for( int i = 0; i<ClothvertexList1Taille;i++)
		//ClothvertexList[i] = &listvertex[i];
	  //j=0;
	l=0;
	for (k=0;k<vectr.size();k=k+276)
	  {  
      	for (;l<69+k;l=l+4)
		{		
			ClothvertexList[l]->e[0] = listvertex[l].e[0];
			ClothvertexList[l]->e[1] = listvertex[l].e[1];
			ClothvertexList[l]->e[2] = listvertex[l].e[2];			    			
		}
		l=l-3;		
        l=l+207;
	 }
    /* for (int i=0;i<ClothvertexList1Taille;i++)
		{
			ClothvertexList[i]->e[0] = listvertex[i].e[0];
			ClothvertexList[i]->e[1] = listvertex[i].e[1];
			ClothvertexList[i]->e[2] = listvertex[i].e[2];
			//j++;
		}*/
	 this->ClothvertexList1=ClothvertexList;
   
		// Connecting immediate neighbor particles with constraints (distance 1 and sqrt(2) in the grid)
		for(int x=0; x<num_particles_width; x+=4)
		{
			for(int y=0; y<num_particles_height; y+=4)
			{
				if (x<num_particles_width-4) makeConstraint(getParticle2(x,y),getParticle2(x+4,y));
				if (y<num_particles_height-4) makeConstraint(getParticle2(x,y),getParticle2(x,y+4));
				if (x<num_particles_width-4 && y<num_particles_height-4) makeConstraint(getParticle2(x,y),getParticle2(x+4,y+4));
				if (x<num_particles_width-4 && y<num_particles_height-4) makeConstraint(getParticle2(x+4,y),getParticle2(x,y+4));
			}
		}
	// Connecting secondary neighbors with constraints (distance 2 and sqrt(4) in the grid)
		for(int x=0; x<num_particles_width; x+=4)
		{
			for(int y=0; y<num_particles_height; y+=4)
			{
				if (x<num_particles_width-8) makeConstraint(getParticle2(x,y),getParticle2(x+8,y));
				if (y<num_particles_height-8) makeConstraint(getParticle2(x,y),getParticle2(x,y+8));
				if (x<num_particles_width-8 && y<num_particles_height-8) makeConstraint(getParticle2(x,y),getParticle2(x+8,y+8));
				if (x<num_particles_width-8 && y<num_particles_height-8) makeConstraint(getParticle2(x+8,y),getParticle2(x,y+8));			}
		}


		// making the upper left most three and right most three particles unmovable
		for(int i=0;i<5; i++)
		{
			getParticle2(0+i ,0)->offsetPos(Vec3(0.5,0.0,0.0)); // moving the particle a bit towards the center, to make it hang more natural - because I like it ;)
			getParticle2(0+i ,0)->makeUnmovable(); 

			getParticle2(0+i ,0)->offsetPos(Vec3(-0.5,0.0,0.0)); // moving the particle a bit towards the center, to make it hang more natural - because I like it ;)
			getParticle2(num_particles_width-1-i ,0)->makeUnmovable();
		}   
	}

	/* drawing the cloth as a smooth shaded (and colored according to column) OpenGL triangular mesh
	Called from the display() method
	The cloth is seen as consisting of triangles for four particles in the grid as follows:

	(x,y)   *--* (x+1,y)
	        | /|
	        |/ |
	(x,y+1) *--* (x+1,y+1)

	*/
	void Cloth:: drawShaded()
	{
		// reset normals (which where written to last frame)
		std::vector<Particle>::iterator particle;
		for(particle = particleActiveState.begin(); particle != particleActiveState.end(); particle++)
		{
			(*particle).resetNormal();
		}

		//create smooth per particle normals by adding up all the (hard) triangle normals that each particle is part of
		for(int x = 0; x<num_particles_width-4; x+=4)
		{
			for(int y=0; y<num_particles_height-4; y+=4)
			{
				Vec3 normal = calcTriangleNormal(getParticle2(x+4,y),getParticle2(x,y),getParticle2(x,y+4));
				getParticle2(x+4,y)->addToNormal(normal);
				getParticle2(x,y)->addToNormal(normal);
				getParticle2(x,y+4)->addToNormal(normal);

				normal = calcTriangleNormal(getParticle2(x+4,y+4),getParticle2(x+4,y),getParticle2(x,y+4));
				getParticle2(x+4,y+4)->addToNormal(normal);
				getParticle2(x+4,y)->addToNormal(normal);
				getParticle2(x,y+4)->addToNormal(normal);
			}
		}
       glColor3f(0.0f,0.0f,0.0f);
		glBegin(GL_LINES);
		//glBegin(GL_TRIANGLES);
//glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
		for(int x = 0; x<num_particles_width-4; x+=4)
		{
			for(int y=0; y<num_particles_height-4; y+=4)
			{
				Vec3 color(0,0,0);
				if (x%2) // red and white color is interleaved according to which column number
					color = Vec3(0.6f,0.2f,0.2f);
				else
					color = Vec3(1.0f,1.0f,1.0f);

				drawTriangle(getParticle2(x+4,y),getParticle2(x,y),getParticle2(x,y+4),color);
				drawTriangle(getParticle2(x+4,y+4),getParticle2(x+4,y),getParticle2(x,y+4),color);
				drawTriangle(getParticle2(x,y),getParticle2(x+4,y+4),getParticle2(x+4,y),color);
				drawTriangle(getParticle2(x+4,y+4),getParticle2(x+4,y),getParticle2(x,y),color);
			}
		} 
		glEnd();
	}

	void Cloth:: drawShaded1(int col, int line,int col1)
	{	//glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	//glLoadIdentity();

	/*glDisable(GL_LIGHTING); // drawing some smooth shaded background - because I like it ;)
	glBegin(GL_POLYGON);
	glColor3f(0.8f,0.8f,1.0f);
	glVertex3f(-200.0f,-100.0f,-100.0f);
	glVertex3f(200.0f,-100.0f,-100.0f);
	glColor3f(0.4f,0.4f,0.8f);	
	glVertex3f(200.0f,100.0f,-100.0f);
	glVertex3f(-200.0f,100.0f,-100.0f);
	glEnd();
	glEnable(GL_LIGHTING);*/	
		// reset normals (which where written to last frame)
		std::vector<Particle>::iterator particle; 
	  for(int i=0;i<RefinedParticles.size();i++) {
		for(particle = particleActiveState.begin(); particle != particleActiveState.end() && (RefinedParticles.at(i).particleIndex!=(*particle).particleIndex); particle++);
		  if(particle<particleActiveState.end())
			(*particle).resetNormal(); 		
	  }
		//create smooth per particle normals by adding up all the (hard) triangle normals that each particle is part of
		for(int x = 0; x<col-1; x++)
		{
			for(int y=0; y<line-1; y++)
			{
                /*Particle *p1,*p2,*p3,*p4; 
				p1=getParticle1(x+1,y,col1); p1=&particleActiveState[p1->particleIndex];
				p2=getParticle1(x,y,col1);   p2=&particleActiveState[p2->particleIndex];
				p3=getParticle1(x,y+1,col1); p3=&particleActiveState[p3->particleIndex];
				p4=getParticle1(x+1,y+1,col1); p4=&particleActiveState[p4->particleIndex];

				Vec3 normal = calcTriangleNormal(p1,p2,p3);
				p1->addToNormal(normal);
				p2->addToNormal(normal);
				p3->addToNormal(normal);
				
				normal = calcTriangleNormal(p4,p1,p3);
				p4->addToNormal(normal);
				p1->addToNormal(normal);
				p3->addToNormal(normal);*/
				Vec3 normal = calcTriangleNormal(getParticle1(x+1,y,col1),getParticle1(x,y,col1),getParticle1(x,y+1,col1));
				getParticle1(x+1,y,col1)->addToNormal(normal);
				getParticle1(x,y,col1)->addToNormal(normal);
				getParticle1(x,y+1,col1)->addToNormal(normal);

				normal = calcTriangleNormal(getParticle1(x+1,y+1,col1),getParticle1(x+1,y,col1),getParticle1(x,y+1,col1));
				getParticle1(x+1,y+1,col1)->addToNormal(normal);
				getParticle1(x+1,y,col1)->addToNormal(normal);
				getParticle1(x,y+1,col1)->addToNormal(normal);
			}
		}
       glColor3f(0.0f,0.0f,0.0f);
		glBegin(GL_LINES);
		//glBegin(GL_TRIANGLES);
//glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
		for(int x = 0; x<col-1; x++)
		{
			for(int y=0; y<line-1; y++)
			{
				Vec3 color(0,0,0);
				if (x%2) // red and white color is interleaved according to which column number
					color = Vec3(0.6f,0.2f,0.2f);
				else
					color = Vec3(1.0f,0.0f,0.0f);
				/* Particle *p1,*p2,*p3,*p4; 
				p1=getParticle1(x+1,y,col1); p1=&particleActiveState[p1->particleIndex];
				p2=getParticle1(x,y,col1);   p2=&particleActiveState[p2->particleIndex];
				p3=getParticle1(x,y+1,col1); p3=&particleActiveState[p3->particleIndex];
				p4=getParticle1(x+1,y+1,col1); p4=&particleActiveState[p4->particleIndex];

				drawTriangle(p1,p2,p3,color);
				drawTriangle(p4,p1,p3,color);
				drawTriangle(p2,p4,p1,color);
				drawTriangle(p4,p1,p2,color);*/
				drawTriangle(getParticle1(x+1,y,col1),getParticle1(x,y,col1),getParticle1(x,y+1,col1),color);
				drawTriangle(getParticle1(x+1,y+1,col1),getParticle1(x+1,y,col1),getParticle1(x,y+1,col1),color);
				drawTriangle(getParticle1(x,y,col1),getParticle1(x+1,y+1,col1),getParticle1(x+1,y,col1),color);
				drawTriangle(getParticle1(x+1,y+1,col1),getParticle1(x+1,y,col1),getParticle1(x,y,col1),color);
			}
		} 
		glEnd();
		glutSwapBuffers(); 
	}

	/* this is an important methods where the time is progressed one time step for the entire cloth.
	This includes calling satisfyConstraint() for every constraint, and calling timeStep() for all particles
	*/

	void Cloth:: timeStep()
	{
		std::vector<Constraint>::iterator constraint;
		for(int i=0; i<CONSTRAINT_ITERATIONS; i++) // iterate over all constraints several times
		{
			for(constraint = constraints.begin(); constraint != constraints.end(); constraint++ )
			{
				(*constraint).satisfyConstraint(); // satisfy constraint.
			}
		}

		std::vector<Particle>::iterator particle;
		for(particle = particleActiveState.begin(); particle != particleActiveState.end(); particle++)
		{
			(*particle).timeStep(); // calculate the position of each particle at the next time step.
		}      
		
		/*int j=0;
		for(int i=0;i<particleActiveState.size();i++)
		{
			if(particleActiveState[i].state==1)   
				particles[j++]=particleActiveState.at(i);	
		}*/

	    /*for (int k=0;k<particleActiveState.size();k=k+276)
	    {  
        	for (;l<69+k;l=l+4)
		    {	
              particles.push_back(particleActiveState[l]);

			//ClothvertexList[l] = &listvertex[l];			    			
		    }
		   l=l-3;		
           l=l+207;
	    }*/
	}

	void Cloth:: timeStep1()
	{  //cout<<"taille constr= "<<constraints1.size()<<endl;
		std::vector<Constraint>::iterator constraint; //int j;
		for(int i=0; i<CONSTRAINT_ITERATIONS; i++) // iterate over all constraints several times
		{		
		  //for(j=0;j<constraints1.size();j++)
		  //{
			for(constraint = constraints.begin(); constraint != constraints.end(); constraint++)
			{
				//if(((*constraint).p1->particleIndex==constraints1.at(j).p1->particleIndex)&&((*constraint).p2->particleIndex==constraints1.at(j).p2->particleIndex))
				(*constraint).satisfyConstraint(); // satisfy constraint.
			}
		  //}
		}
        std::vector<Particle>::iterator particle;
		for(int i=0;i<RefinedParticles.size();i++) {
		for(particle = particleActiveState.begin(); particle != particleActiveState.end() && (RefinedParticles.at(i).particleIndex!=(*particle).particleIndex); particle++);
		  if(particle<particleActiveState.end())
			(*particle).timeStep(); 	// calculate the position of each particle at the next time step.	
	    }
		    
	}


	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	void Cloth::BSH_up_date()
	{
	     vec vectr1;
         vector<Particle> Newparticles;
         Newparticles=particleActiveState;


		 /*cout<<"------------------------dans ballcollision"<<endl;
   	for(int i=0;i< particles.size();i++)
		{
			cout<< particles.at(i).pos.f[0]; cout<< "-";
			cout<< particles.at(i).pos.f[1]; cout<< "-";
			cout<< particles.at(i).pos.f[2]; cout<< "-";
			cout<<endl;
		}*/


		// creating particles in a grid of particles from (0,0,0) to (width,-height,0)
		 Newparticles.resize(particleActiveState.size());
		for(int x=0; x<num_particles_width; x++)
		{
			for(int y=0; y<num_particles_height; y++)
			{
				vectr1.push_back(Newparticles[y*num_particles_width+x].pos);
			}
		}
	
		/////////////////////////////////
		this->Faces.clear();
		/*for(int x = 0; x<num_particles_width; x++)
		{  cout<<"x= ";cout<<x<<endl;
			for(int y=0; y<num_particles_height; y++)
			{   
				cout<<"y= ";cout<<y<<endl;
				int t[]={0,0,0,0}; 
			   getvertexindex(t,x+1,y,x,y,x,y+1); 
		       getFaces(getParticle(x+1,y),getParticle(x,y),getParticle(x,y+1),t); 
			   for(int i=0;i<4;i++)
				        t[i]=0;
			   getvertexindex(t,x+1,y+1,x+1,y,x,y+1); 
               getFaces(getParticle(x+1,y+1),getParticle(x+1,y),getParticle(x,y+1),t);  
			}
		}*/
		for(int x = 0; x<num_particles_width-4; x=x+4)
		{		
			for(int y=0; y<num_particles_height-4; y=y+4)
			{  
				int t[]={0,0,0,0}; 
			   getvertexindex(t,x+4,y,x,y,x,y+4); 
		       getFaces(getParticle2(x+4,y),getParticle2(x,y),getParticle2(x,y+4),t); 
			   for(int i=0;i<4;i++)
				        t[i]=0;
			   getvertexindex(t,x+4,y+4,x+4,y,x,y+4); 
               getFaces(getParticle2(x+4,y+4),getParticle2(x+4,y),getParticle2(x,y+4),t);  
			}
		}  
          		
		obj_face **Clothface = new obj_face*[vecteurtail];
		obj_face *listface= new obj_face[vecteurtail];

        for (int i=0;i<this->Faces.size();i++)
		{
			obj_face *f;
			f=this->Faces.at(i).Clothfaces;
			listface[i] = *f; 
		}   

		for (int i=0;i<this->Faces.size();i++)
		{
			Clothface[i] = &listface[i];            
		}  
		

		for (int i=0;i<this->Faces.size();i++)
		{
			for(int j=0;j<4;j++){
				Clothface[i]->vertex_index[j] = this->Faces.at(i).Clothfaces->vertex_index[j];	
			}
		}	
		this->Indexface=NULL;

		this->Indexface=Clothface;
		
          
         obj_vector *listvertex= new obj_vector[vecteurtail];
              
      /*	for (int i=0;i<vectr1.size();i++)
		{
			listvertex[i].e[0]= vectr1.at(i).f[0];
			listvertex[i].e[1]= vectr1.at(i).f[1];
			listvertex[i].e[2]= vectr1.at(i).f[2];
		}*/

        int l=0;
			  
	 for (int k=0;k<vectr1.size();k=k+276)
	  {  
      	for (;l<69+k;l=l+4)
		{		
			
			listvertex[l].e[0]= vectr1.at(l).f[0];
			listvertex[l].e[1]= vectr1.at(l).f[1];  
			listvertex[l].e[2]= vectr1.at(l).f[2];          			 	
		}
		l=l-3;		
        l=l+207;
	 }
         obj_vector **ClothvertexList = new obj_vector*[vecteurtail];


		 l=0;
	for (int k=0;k<vectr1.size();k=k+276)
	  {  
      	for (;l<69+k;l=l+4)
		{		
			ClothvertexList[l] = &listvertex[l];			    			
		}
		l=l-3;		
        l=l+207;
	 }
	  /////////////////////////////////////////////////////////////////////////

	  // for( int i = 0; i<ClothvertexList1Taille;i++)
		//ClothvertexList[i] = &listvertex[i];
	  //j=0;
	 l=0;
	for (int k=0;k<vectr1.size();k=k+276)
	  {  
      	for (;l<69+k;l=l+4)
		{		
			ClothvertexList[l]->e[0] = listvertex[l].e[0];
			ClothvertexList[l]->e[1] = listvertex[l].e[1];
			ClothvertexList[l]->e[2] = listvertex[l].e[2];			    			
		}
		l=l-3;		
        l=l+207;
	 }

		// ClothvertexList1Taille=vectr1.size();
	 /*  for( int i = 0; i < vectr1.size(); i++)
		ClothvertexList[i] = &listvertex[i];
	
     for (int i=0;i<vectr1.size();i++)
		{
			ClothvertexList[i]->e[0] = listvertex[i].e[0];
			ClothvertexList[i]->e[1] = listvertex[i].e[1];
			ClothvertexList[i]->e[2] = listvertex[i].e[2];
		}*/
	 this->ClothvertexList1=NULL;
	 this->ClothvertexList1=ClothvertexList;
	}
	////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

	/* used to add gravity (or any other arbitrary vector) to all particles*/
	void Cloth:: addForce(const Vec3 direction)
	{
		std::vector<Particle>::iterator particle;
		for(particle = particles.begin(); particle != particles.end(); particle++)
		{
			(*particle).addForce(direction); // add the forces to each particle
		}

	}
  void Cloth:: addForce1(const Vec3 direction)
	{

		std::vector<Particle>::iterator particle;
		for(int i=0;i<RefinedParticles.size();i++) {
		for(particle = particleActiveState.begin(); particle != particleActiveState.end() && (RefinedParticles.at(i).particleIndex!=(*particle).particleIndex); particle++);
		  if(particle<particleActiveState.end())
			(*particle).addForce(direction); // add the forces to each particle	
	    }
	}

	/* used to add wind forces to all particles, is added for each triangle since the final force is proportional to the triangle area as seen from the wind direction*/
	void Cloth:: windForce(const Vec3 direction)
	{
		for(int x = 0; x<num_particles_width-4; x+=4)
		{
			for(int y=0; y<num_particles_height-4; y+=4)
			{
				addWindForcesForTriangle(getParticle2(x+4,y),getParticle2(x,y),getParticle2(x,y+4),direction);
				addWindForcesForTriangle(getParticle2(x+4,y+4),getParticle2(x+4,y),getParticle2(x,y+4),direction);
			}
		}
	}
	void Cloth:: windForce1(const Vec3 direction,int col, int line,int col1)
	{
		for(int x = 0; x<col-1; x++)
		{
			for(int y=0; y<line-1; y++)
			{  
				Particle *p1,*p2,*p3,*p4; 
				p1=getParticle1(x+1,y,col1); p1=&particleActiveState[p1->particleIndex];
				p2=getParticle1(x,y,col1);   p2=&particleActiveState[p2->particleIndex];
				p3=getParticle1(x,y+1,col1); p3=&particleActiveState[p3->particleIndex];
				p4=getParticle1(x+1,y+1,col1); p4=&particleActiveState[p4->particleIndex];


				addWindForcesForTriangle(p1,p2,p3,direction);
				addWindForcesForTriangle(p4,p1,p3,direction);
			}
		}
	}

	/* used to detect and resolve the collision of the cloth with the ball.
	This is based on a very simples scheme where the position of each particle is simply compared to the sphere and corrected.
	This also means that the sphere can "slip through" if the ball is small enough compared to the distance in the grid bewteen particles
	*/
	void Cloth:: ballCollision(const Vec3 center,const float radius )
	{		 
		std::vector<Particle>::iterator particle;
		//std::vector<Particle>::iterator it;
		/*for(particle = particleActiveState.begin(); particle != particleActiveState.end(); particle++)   
		{
		  if((*particle).state==1)
		  {
			Vec3 v = (*particle).getPos()-center;
			float l = v.length();
			if ( v.length() < radius) // if the particle is inside the ball
			{
				(*particle).offsetPos(v.normalized()*(radius-l)); // project the particle to the surface of the ball
			}
		  }
		}*/
        int m=0; 
		for (int k=0;k<particleActiveState.size();k=k+276)
	    {  
          for (;m<69+k;m=m+4)
		  {	
			  Vec3 v = particleActiveState.at(m).getPos()-center;
			float l = v.length();
			if ( v.length() < radius) // if the particle is inside the ball
			{
				particleActiveState.at(m).offsetPos(v.normalized()*(radius-l)); // project the particle to the surface of the ball
			}		
		  }
		   m=m-3;
           m=m+207; 
	    }



	}

	void Cloth:: ballCollision1(const Vec3 center,const float radius )
	{
		std::vector<Particle>::iterator particle;
		for(int i=0;i<RefinedParticles.size();i++) {
		for(particle = particleActiveState.begin(); particle != particleActiveState.end() && (RefinedParticles.at(i).particleIndex!=(*particle).particleIndex); particle++);
		if(particle<particleActiveState.end()){
			Vec3 v = (*particle).getPos()-center;
			float l = v.length();
			if ( v.length() < radius) // if the particle is inside the ball
			{
				(*particle).offsetPos(v.normalized()*(radius-l)); // project the particle to the surface of the ball
			}
		}
			
	    }
	}



/*****************************************************************************************************************************************************/


/***** Above are method's definitions of classes; Vec3, Particle, Constraint, and Cloth *****/




// Just below are three global variables holding the actual animated stuff; Cloth and Ball 
Cloth cloth1(10,10,69,69,3); // one Cloth object of the Cloth class
Vec3 ball_pos(7,-5,0); // the center of our one ball
float ball_radius = 2; // the radius of our one ball



/***** Below are functions Init(), display(), reshape(), keyboard(), arrow_keys(), main() *****/

/* This is where all the standard Glut/OpenGL stuff is, and where the methods of Cloth are called; 
addForce(), windForce(), timeStep(), ballCollision(), and drawShaded()*/


void init(GLvoid)
{
	glShadeModel(GL_SMOOTH);
	glClearColor(0.2f, 0.2f, 0.4f, 0.5f);				
	glClearDepth(1.0f);
	glEnable(GL_DEPTH_TEST);
	glDepthFunc(GL_LEQUAL);
	glEnable(GL_COLOR_MATERIAL);
	glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST);
	
	glEnable(GL_LIGHTING);
	glEnable(GL_LIGHT0);
	GLfloat lightPos[4] = {-1.0,1.0,0.5,0.0};
	glLightfv(GL_LIGHT0,GL_POSITION,(GLfloat *) &lightPos);

	glEnable(GL_LIGHT1);

	GLfloat lightAmbient1[4] = {0.0,0.0,0.0,0.0};
	GLfloat lightPos1[4] = {1.0,0.0,-0.2,0.0};
	GLfloat lightDiffuse1[4] = {0.5,0.5,0.3,0.0};

	glLightfv(GL_LIGHT1,GL_POSITION,(GLfloat *) &lightPos1);
	glLightfv(GL_LIGHT1,GL_AMBIENT,(GLfloat *) &lightAmbient1);
	glLightfv(GL_LIGHT1,GL_DIFFUSE,(GLfloat *) &lightDiffuse1);

	glLightModeli(GL_LIGHT_MODEL_TWO_SIDE,GL_TRUE);
}


float ball_time = 0; // counter for used to calculate the z position of the ball below

/* display method called each frame*/
void display(void)
{

	objdata->load1(cloth1);
	bsh->BuidBSH(objdata,15,cloth1);  
	bsh->SearchBSH(15);
	ball_time++;
	ball_pos.f[2] = cos(ball_time/50.0)*7;

    cloth1.addForce(Vec3(0,-0.2,0)*TIME_STEPSIZE2); // add gravity each frame, pointing down
	cloth1.windForce(Vec3(0.5,0,0.2)*TIME_STEPSIZE2); // generate some wind each frame
	cloth1.timeStep(); // calculate the particle positions of the next frame       
	// drawing
    cloth1.ballCollision(ball_pos,ball_radius); 
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	glLoadIdentity();

	glDisable(GL_LIGHTING); // drawing some smooth shaded background - because I like it ;)
	glBegin(GL_POLYGON);
	glColor3f(0.8f,0.8f,1.0f);
	glVertex3f(-200.0f,-100.0f,-100.0f);
	glVertex3f(200.0f,-100.0f,-100.0f);
	glColor3f(0.4f,0.4f,0.8f);	
	glVertex3f(200.0f,100.0f,-100.0f);
	glVertex3f(-200.0f,100.0f,-100.0f);
	glEnd();
	glEnable(GL_LIGHTING);
    
	glTranslatef(-6.5,6,-9.0f); // move camera out and center on the cloth
	glRotatef(140,0,1,0); // rotate a bit to see the cloth from the side
   

		///////////////////////////////////////////
    //draw the sphere hierarchy
	
	/*for (int i=0;i<bsh->output->size();i++)
	{
		BSN *current=bsh->output->at(i);

		glPushMatrix();
		glTranslatef(current->BS->center.x,current->BS->center.y,current->BS->center.z);
		glutWireSphere(current->BS->radius,40,40);
		glPopMatrix();
	}*/
	///////////////////////////////////////////
    
	cloth1.drawShaded(); // finally draw the cloth with smooth shading
	
	glPushMatrix(); // to draw the ball we use glutSolidSphere, and need to draw the sphere at the position of the ball
	glTranslatef(ball_pos.f[0],ball_pos.f[1],ball_pos.f[2]); // hence the translation of the sphere onto the ball position
	glColor3f(0.4f,0.8f,0.5f);
	glutSolidSphere(ball_radius-0.1,50,50); // draw the ball, but with a slightly lower radius, otherwise we could get ugly visual artifacts of cloth penetrating the ball slightly
	glPopMatrix();

	glutSwapBuffers(); 
    
     cloth1.BSH_up_date();//updates the tree 
     bsh->traverse(cloth1,bsh->root,ball_pos,ball_radius,3,TIME_STEPSIZE2); 

	glutPostRedisplay();
}

void reshape(int w, int h)  
{
	glViewport(0, 0, w, h);
	glMatrixMode(GL_PROJECTION); 
	glLoadIdentity();  
	if (h==0)  
		gluPerspective(80,(float)w,1.0,5000.0);
	else
		gluPerspective (80,( float )w /( float )h,1.0,5000.0 );
	glMatrixMode(GL_MODELVIEW);  
	glLoadIdentity(); 
}

void keyboard( unsigned char key, int x, int y ) 
{
	switch ( key ) {
	case 27:    
		exit ( 0 );
		break;  
	default: 
		break;
	}
}

void arrow_keys( int a_keys, int x, int y ) 
{
	switch(a_keys) {
	case GLUT_KEY_UP:
		glutFullScreen();
		break;
	case GLUT_KEY_DOWN: 
		glutReshapeWindow (1280, 720 );
		break;
	default:
		break;
	}
}


int main ( int argc, char** argv ) 
{
	glutInit( &argc, argv );

	
	glutInitDisplayMode( GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH ); 
	glutInitWindowSize(ImageWidth, ImageHeight ); 

	glutCreateWindow( "Cloth Simulation" );
	init();
	  	//cloth1.getFaces();// Here we build the faces of the cloth, in the vector Faces.

	objdata = new objLoader();
	bsh=new BSH();
	/*objdata->load1(cloth1);

	bsh=new BSH();
	bsh->BuidBSH(objdata,6);

	bsh->SearchBSH(6); */

	glutDisplayFunc(display);  
	glutReshapeFunc(reshape);
	glutKeyboardFunc(keyboard);
	glutSpecialFunc(arrow_keys);

	glutMainLoop();
}