package clothSimulator;


import java.awt.Component;
import java.awt.event.KeyAdapter;
import java.awt.event.KeyEvent;
import java.io.*;
import java.text.DecimalFormat;
import java.util.Calendar;
import java.util.Date;
import java.util.HashSet;
import java.util.LinkedList;
import java.util.List;
import java.util.PriorityQueue;
import java.util.Set;

import javax.media.opengl.GL;
import javax.media.opengl.GLAutoDrawable;
import javax.swing.JPanel;
import javax.vecmath.Point3d;
import javax.vecmath.Vector3d;

import clothSimulator.Constraint.ConstraintType;

import meshTools.PolygonSoup;
import meshTools.Vertex;
import no.uib.cipr.matrix.DenseMatrix;
import no.uib.cipr.matrix.DenseVector;

import meshTools.HEDS;
import meshTools.HalfEdge;
import meshTools.Loop;
import mintools.viewer.Interactor;
import comp599.a1.numerical.ForwardEuler;
import comp599.a1.numerical.Function;
import comp599.a1.numerical.Integrator;
import comp599.a1.numerical.Midpoint;
import comp599.a1.numerical.RK;
import comp599.a1.numerical.SymplecticEuler3D;
import comp599.a1.numerical.TwoThirds;
import comp599.tools.parameters.BooleanParameter;
import comp599.tools.parameters.DoubleParameter;
import comp599.tools.parameters.IntParameter;
import comp599.tools.swing.VerticalFlowPanel;
import comp599.tools.viewer.EasyViewer;
import comp599.tools.viewer.SceneGraphNode;


/**
 * Implementation of a simple particle system.
 * 
 * Note that the particle system implements Function, that is, it evaluates
 * its derivatives to return to the step method which is called by implementations
 * of the Integrator interface.
 * 
 * Note also that it is actually the updateParticles method in this class which 
 * should be calling the Integrator step method! 
 * 
 * @author kry
 */
public class ParticleSystem implements SceneGraphNode, Function, Interactor {
    
	
	//here we have a list of constraints
	//private List<Constraint> theConstraints= new LinkedList<Constraint>();
	/**now here we have an object which is meant to represent a list of constraints */
	ConstraintList constraintList;
	
	
    private List<Particle> particles = new LinkedList<Particle>();
    
    private List<Spring> springs = new LinkedList<Spring>();
    
    private Boolean applyBaumgarte;
    private Boolean applyPostStep;
    
    
    public int intType=5;
    
    public boolean subSteps=false;
    /** this helps during constraints because right now, constraint systems are not quite monolithic and have types like pendulum and
    double pendulum*/
    public int whichSystemType;
    
    /**
     * Creates an empty particle system
     */
    public ParticleSystem() {
        // creates an empty system!
    	
    }

    public Sheet theSheet;

  

    /**
     * Creates one of a number of simple test systems.
     * which == 1 generates an n-link chain
     * which == 2 generates a 2d array of particles
     *
     * The single pair of particles is probably the most useful for debugging 
     *
     * @param which
     */
    public void createSystem( int which ) {    
    	this.whichSystemType=which;
    	//we should start with an nlink chain
    	//we need to keep around the index of the last particle added to the list
    	int prevParticle;
    	if(which ==1){
    		//here we just make a chain of length numLinks
    		//int numLinks=30;
    		//When creating a particle system, one must know the number of particles and thus we create our constraints list
    		//with this number in the case of an N-link chain, the number of dofs is 3*(N+1) since there must be N+1 particles
    		constraintList=new ConstraintList();
    		//below we add the top particle to which is applied a pinning constraint
    		Vector3d topPart=new Vector3d(0,5,0); //the top particle, which will be pinned
			Particle p= new Particle(topPart.x,topPart.y,topPart.z,0,0,0);
			particles.add(p);
			p.setIndex(particles.indexOf(p));
			set3DPinnedConstraint(topPart,p);
			p.pinned=true;
    		// Each new constraint created will have the dist set to d.
    		Point3d pPrime= new Point3d(topPart.x,topPart.y,topPart.z);
    		prevParticle=p.getIndex();
    		for(int i=0; i<numLinks.getValue();i++){
    			pPrime.x-=d.getValue();
    			//pPrime.y-=d.getValue();
    			p=new Particle(pPrime.x,pPrime.y,pPrime.z,0,0,0);
    			particles.add(p);
    			p.setIndex(particles.indexOf(p));
    			StretchConstraint sc=new StretchConstraint(particles.get(prevParticle),p,d.getValue());
    			constraintList.theConstraints.add(sc);
    			sc.setCLIndex(constraintList.theConstraints.indexOf(sc));
    			prevParticle=p.getIndex(); 			
    		}
			if(pinChain.getValue()){
				set3DPinnedConstraint(new Vector3d(p.p.x,p.p.y,p.p.z),p);
				p.pinned=true;
			}
			constraintList.setNumDofs(particles.size()*3);
    	}
    	else if ( which == 2) {
    		//create a sheet object
    		theSheet=new Sheet(sheetHeight.getValue(),sheetWidth.getValue());
        	createSheet();
			constraintList.setNumDofs(particles.size()*3);
        }
    	else if(which ==3){
    		
    		//here we just make a chain of length 1
    		//When creating a particle system, one must know the number of particles and thus we create our constraints list
    		//with this number in the case of an N-link chain, the number of dofs is 3*(N+1) since there must be N+1 particles
    		constraintList=new ConstraintList();
    		//below we add the top particle to which is applied a pinning constraint
    		Vector3d topPart=new Vector3d(0,5,0); //the top particle, which will be pinned
			Particle p= new Particle(topPart.x,topPart.y,topPart.z,0,0,0);
			particles.add(p);
			p.setIndex(particles.indexOf(p));
			set3DPinnedConstraint(topPart,p);
			p=new Particle(topPart.x+d.getValue()*Math.sin(Math.PI/2),topPart.y-d.getValue()*Math.cos(Math.PI/2),topPart.z,0,0,0);
			particles.add(p);
			p.setIndex(particles.indexOf(p));
			StretchConstraint sc=new StretchConstraint(particles.get(0),p,d.getValue());
			constraintList.theConstraints.add(sc);
			sc.setCLIndex(constraintList.theConstraints.indexOf(sc));
    		
			constraintList.setNumDofs(particles.size()*3);
    	}
    	else if(which ==4){
    		double d=2;
    		constraintList=new ConstraintList();
    		//try testing the integrator with just a spring and pinned particle
    		Vector3d topPart=new Vector3d(0,5,0); //the top particle, which will be pinned
			Particle p= new Particle(topPart.x,topPart.y,topPart.z,0,0,0);
			p.pinned=true;
			particles.add(p);
			p.setIndex(particles.indexOf(p));
			p= new Particle(topPart.x +d,topPart.y,topPart.z,0,0,0);
			particles.add(p);
			p.setIndex(particles.indexOf(p));
			Spring spr=new Spring(particles.get(0),particles.get(1));
			springs.add(spr);
    	}
    	else if(which ==5){
    		//we are going to set a parameter so that the T-tester can hand loose or in a relaxed pose
    		double l;
    		l=Math.sqrt(d.getValue()*d.getValue()-t.getValue()*t.getValue()*d.getValue()*d.getValue());
    		constraintList=new ConstraintList();
    		//below we add the top particle to which is applied a pinning constraint
    		Vector3d topPart=new Vector3d(0,5,0); //the top particle, which will be pinned
			Particle p= new Particle(topPart.x,topPart.y,topPart.z,0,0,0);
			particles.add(p);
			p.setIndex(particles.indexOf(p));
			XPinnedConstraint pinX=new XPinnedConstraint(p,topPart.x);
			YPinnedConstraint pinY=new YPinnedConstraint(p,topPart.y);
			constraintList.theConstraints.add(pinX);
			pinX.setCLIndex(constraintList.theConstraints.indexOf(pinX));
			constraintList.theConstraints.add(pinY);
			pinY.setCLIndex(constraintList.theConstraints.indexOf(pinY));
			p.pinned=true;
    		// Each new constraint created will have the dist set to d.
    		prevParticle=p.getIndex();
    		p= new Particle(topPart.x+l,topPart.y-t.getValue()*d.getValue(),topPart.z,0,0,0);
			particles.add(p);
			p.setIndex(particles.indexOf(p));
			StretchConstraint sc= new StretchConstraint(p,particles.get(0),d.getValue());
			constraintList.theConstraints.add(sc);
			sc.cLIndex=constraintList.theConstraints.indexOf(sc);
			p=new Particle(topPart.x+2*l,topPart.y,topPart.z,0,0,0);
			particles.add(p);
			p.pinned=true;
			pinX=new XPinnedConstraint(p,p.p.x);
			pinY=new YPinnedConstraint(p,p.p.y);
			constraintList.theConstraints.add(pinX);
			pinX.setCLIndex(constraintList.theConstraints.indexOf(pinX));
			constraintList.theConstraints.add(pinY);
			pinY.setCLIndex(constraintList.theConstraints.indexOf(pinY));
			p.setIndex(particles.indexOf(p));
			sc = new StretchConstraint(p, particles.get(1),d.getValue());
			constraintList.theConstraints.add(sc);
			sc.setCLIndex(constraintList.theConstraints.indexOf(sc));
			p=new Particle(topPart.x+l,topPart.y-d.getValue()-t.getValue()*d.getValue(),topPart.z,0,0,0);
			particles.add(p);
			p.setIndex(particles.indexOf(p));
			sc = new StretchConstraint(p, particles.get(1),d.getValue());
			constraintList.theConstraints.add(sc);
			sc.setCLIndex(constraintList.theConstraints.indexOf(sc));
			constraintList.setNumDofs(particles.size()*3);
			
			
    	} else if(which ==6){
    		//create a cube
    		//read a cube in from a file
    	}
    }
    

    /** creates a 2d sheet that hangs in a relaxed pose */
  private void createSheet(){
	  		//even
	  		//the angle
	  		double alpha;
			constraintList=new ConstraintList();
			Vector3d topPart=new Vector3d(-3,20,0); //the top particle, which will be pinned
			Particle p;
			double h=0;
			double w=0;
			int verticalRow=0;
			StretchConstraint sc;
			for(int i =0; i< sheetWidth.getValue()*sheetHeight.getValue(); i++){
				if(i!=0){
					/**we are creating a sheet of particles with a single variable, so we use some modular arithmetic. 
					 * If i passes over a length equal to the sheet height, then we reset the height variable and
					 *  increase the width, otherwise we increase the heigh variable.
					 */					
					if(i%sheetHeight.getValue()==0){
						verticalRow++;
						w=w+d.getValue();
						h=0;
					}else  h=h+d.getValue();
					//testing
					p=new Particle(0,0,0,0,0,0);
					if(sheetWidth.getValue()<3){
						//just build a square
						p=new Particle(topPart.x+verticalRow*d.getValue(),topPart.y,topPart.z-h,0,0,0);
					}else{
						//otherwise, you hve to see if the width si odd or even
						if(sheetWidth.getValue()%2!=0){
							alpha=Math.asin(t.getValue()*2d/(d.getValue()*(sheetWidth.getValue()-1)));
							if(verticalRow> (sheetWidth.getValue()-1)/2){
								//p=new Particle(topPart.x+w,topPart.y,topPart.z+h,0,0,0);
								p=new Particle(topPart.x+verticalRow*d.getValue()*Math.cos(alpha),topPart.y,topPart.z-h-(sheetWidth.getValue()-1-verticalRow)*d.getValue()*Math.sin(alpha),0,0,0);
							}else{
								p=new Particle(topPart.x+verticalRow*d.getValue()*Math.cos(alpha),topPart.y,topPart.z-h-verticalRow*d.getValue()*Math.sin(alpha),0,0,0);
							}
						}else{
							alpha=Math.asin(t.getValue()*2d/(d.getValue()*(sheetWidth.getValue())));
							if(verticalRow> (sheetWidth.getValue()/2)){
								//way up
								p=new Particle(topPart.x+verticalRow*d.getValue()*Math.cos(alpha),topPart.y,topPart.z-h-(sheetWidth.getValue()-1-verticalRow)*d.getValue()*Math.sin(alpha),0,0,0);
							}
							if(verticalRow == (sheetWidth.getValue()/2)){
								//middle link
								p=new Particle(topPart.x+(verticalRow)*d.getValue()*Math.cos(alpha),topPart.y,topPart.z-h-(verticalRow-1)*d.getValue()*Math.sin(alpha),0,0,0);
							}
							if(verticalRow<= (sheetWidth.getValue()/2-1) ){
								//way down
								p=new Particle(topPart.x+verticalRow*d.getValue()*Math.cos(alpha),topPart.y,topPart.z-h-verticalRow*d.getValue()*Math.sin(alpha),0,0,0);
							}
						}
					}
					particles.add(p);
		  			p.setIndex(particles.indexOf(p));
		  			//we need to pin the other corner
		  			if(i==(sheetWidth.getValue()-1)*(sheetHeight.getValue())){
		  				if(h==0d){
		  					set3DPinnedConstraint(new Vector3d(p.p.x,p.p.y,p.p.z), p);
		  					p.pinned=true;
		  				}
		  			}
		  			if(h>0){
		  				sc=new StretchConstraint(particles.get(i),particles.get(i-1),d.getValue());
		  				constraintList.theConstraints.add(sc);
		      			sc.setCLIndex(constraintList.theConstraints.indexOf(sc));
		  			}
		  			if(w>0){
		  				sc=new StretchConstraint(particles.get(i),particles.get(i-sheetHeight.getValue()),d.getValue());
		  				constraintList.theConstraints.add(sc);
		      			sc.setCLIndex(constraintList.theConstraints.indexOf(sc));
		  			}
				} else{
					p= new Particle(topPart.x,topPart.y,topPart.z,0,0,0);
					particles.add(p);
					p.setIndex(particles.indexOf(p));
					set3DPinnedConstraint(topPart,p);
					p.pinned=true;
				}
			}
	  		addCrossSprings();
		constraintList.setNumDofs(particles.size()*3);		
  }
  
  /**the temporary cross spring */
  Spring crossSpring;
  /**This adds the cross springs to the fabric to give it only a little bit of shear */
  private void addCrossSprings(){

	int verticalRow=0;
	  for(int i=0; i<particles.size()-sheetHeight.getValue();i++){
		  if(i%sheetHeight.getValue()==0){
				verticalRow++;
		  }
		  if(i%sheetHeight.getValue()!=sheetHeight.getValue()-1){  //at the bottom do nothing 
			  crossSpring=new Spring(particles.get(i),particles.get(i+sheetHeight.getValue()+1));
			  springs.add(crossSpring);
			  crossSpring=new Spring(particles.get(i+1),particles.get(i+sheetHeight.getValue()));
			  springs.add(crossSpring);
		  }
	  }
  }
  
    
  /**pins particles to a point with constraints  */
  private void set3DPinnedConstraint(Vector3d pinPosition, Particle p){
		XPinnedConstraint pinX=new XPinnedConstraint(p,pinPosition.x);
		YPinnedConstraint pinY=new YPinnedConstraint(p,pinPosition.y);
		ZPinnedConstraint pinZ=new ZPinnedConstraint(p,pinPosition.z);
		constraintList.theConstraints.add(pinX);
		pinX.setCLIndex(constraintList.theConstraints.indexOf(pinX));
		constraintList.theConstraints.add(pinY);
		pinY.setCLIndex(constraintList.theConstraints.indexOf(pinY));
		constraintList.theConstraints.add(pinZ);
		pinZ.setCLIndex(constraintList.theConstraints.indexOf(pinZ));
  }
  
    
  /** here we store the pinned particle positions for constrained systems 4 and 5 */
    public Vector3d pinnedPos=new Vector3d();
    
    
    
    /**
     * Gets the particles in the system
     * @return the particle set
     */
    public List<Particle> getParticles() {
        return particles;
    }
    
    /**
     * Resets the positions of all particles to their initial states
     */
    public void resetParticles() {
        for ( Particle p : particles ) {
            p.reset();
        }
        time = 0;
    }
    
    /**
     * Deletes all particles, and as such removes all springs too.
     */
    public void clearParticles() {        
        particles.clear();
        springs.clear();
        
    }    
       
    /** here we set position and velocity of pinned particles to zero
    we also invoke walls */
    private void setPinnedParticles2(double[] y, double[] dydt){
    	int i=0;
    	for(Particle p: this.particles){
    		if (p.pinned ){
    			y[i+3]=0;
    			y[i+4]=0;
    			y[i+5]=0;
    			if( this.integrator.getName() == "Symplectic3D"){
    				dydt[i]=0;
    				dydt[i+1]=0;
    				dydt[i+2]=0;
    				dydt[i+3]=0;
    				dydt[i+4]=0;
    				dydt[i+5]=0;
    			}
    		}
    		//for now we don't have walls
//    		if (y[i+1]>height && y[i+3]>0){
//    			y[i+1]=height;
//    			y[i+3]=0;
//    			
//    		}
//    		else if (y[i+1] <0 && y[i+3]<0){
//    			y[i+1]=0;
//    			y[i+3]=0;
//    		}
//    		if(y[i] > width && y[i+2]>0){
//    			y[i]=width;
//    			y[i+2]=0;
//    		}
//    		else if(y[i]<=0 && y[i+2]<0){
//    			y[i]=0;
//    			y[i+2]=0;
//    		}
    	i=i+6;
    	}
    }
    
    @Override
    public void derivs(double t, double[] y, double[] dydt) {
          
        
    	/**so take the particles data and shove it into y, then go through all pairs of particles
    	 * that have springs or just go through all springs and compute forces for each particle.
    	 * Enter this force data into the particle system and then pull data into the derivative dydt
    	 *  
    	 */   	
    	this.clearForces();
    	this.computeForces2(y, dydt);
    	this.setPinnedParticles2(y,dydt);
    	for(int i=0; i<y.length;i=i+6){
    		dydt[i]=y[i+3];
    		dydt[i+1]=y[i+4];
    		dydt[i+2]=y[i+5];
    	}
        // You want to unpack the provided state, into all the objects that have state,
        // compute forces, such as gravity, and spring forces, and then, pack up all the 
        // derivatives into dydt to be returned to the integration method
    }
    
    /** this sub jsut clears forces of all accumulators*/
    private void clearForces(){
    	for(Particle p: this.particles){
    		p.f=new Vector3d();
    	}
    }
  
  
    private Vector3d grav;
	private Vector3d Vdrag;
	private Point3d pos1;
	
    private double[] constraintForces;
	private Point3d pos2=new Point3d();
	private Vector3d v1=new Vector3d();
	private Vector3d v2=new Vector3d();
	private Vector3d fSpr=new Vector3d();
    //a new compute forces function
    /**The trick we do, in order to compute forcse given a state y, is to note that we loaded particles
     * from p into y sequentially, and particles in p have an index.  So the particle of index 0 in the particles
     * list is loaded into y positions 0 to 3.  So we are looping over particles, and then at each particle
     * we are looping over each spring which is attached to that particle.  We do this with the p.springs array.
     * For each spring, we get the index of each particle that is attached, and then get the position information
     * , via the index j, from y.
     *
     * To compute constraint forces, we need to assemble a basic x vector which is just the positions of the
     * particles stacked up.  We also need to assemble a separate v vector which is just just the velocities
     * stacked up.  Finally we have to assemble a force vector which is just the forces on the particles
     * (from springs, gravity and other force objects) stacked up by particle number. 
     * 
     * 
     */
    private void computeForces2(double[] y, double dydt[]){
    	int i=0;
    	int j=0;
    	grav=new Vector3d();
		grav.set(0,(useg.getValue() ? g.getValue() : 0),0);
		//grav.set(0,0,(useg.getValue() ? g.getValue() : 0));
		Vdrag= new Vector3d();
		pos1=new Point3d();
		pos2=new Point3d();
		v1=new Vector3d();
		v2=new Vector3d();
		fSpr=new Vector3d();	
    	for(Particle p: this.particles){
    		fSpr= new Vector3d();
    		for(Spring spr: p.springs){
    			if(this.particles.indexOf(spr.p2)!=i){
    				j=this.particles.indexOf(spr.p2);
    			}
    			else{
    				j=this.particles.indexOf(spr.p1);
    			}
    			pos1.set(y[6*i], y[6*i+1], y[6*i+2]);
    			pos2.set(y[6*j], y[6*j+1],y[6*j+2]);
    			v1.set(y[6*i+3], y[6*i+4],y[6*i+5]);
    			v2.set(y[6*j+3],y[6*j+4], y[6*j+5]);		
    			fSpr.add(spr.compSpringForce2(pos1, pos2));
    			fSpr.add(spr.compDampForce2(v1, v2,pos1,pos2));

    		}
    		
			Vdrag.set(-1d*c.getValue()*p.v.x, -1d*c.getValue()*p.v.y,-1d*c.getValue()*p.v.z);
			fSpr.add(Vdrag);
			
    		fSpr.add(grav);
    		p.f.set(fSpr);
   // 		dydt[4*i+2]=fSpr.x/p.mass;
   // 		dydt[4*i+3]=fSpr.y/p.mass;
    		i=i+1;
    		
    	}
		/** here we compute constraint forces and then add them to the particles force accumulators.  We 
		 * do this by first creating state vectors x and xdot.  We continue by 
		 * calling a constraintList function called computeConstraintForces which returns a force vector.
		 * This force vector is just the stack of particles' forces by d.o.f.  We then call a little
		 * method to load the forces into the particles' force accumulators.  The next step just loads
		 * the force accumulators into dydt.
		 * 
		 */
		if(applyConstraints && constraintList.theConstraints.size()!=0){
			computeXVandF();
			constraintForces =constraintList.computeConstraintForces(velocityVector, forceVector, BGks.getValue(),BGkd.getValue(), applyBaumgarte, applyPostStep);
			//now load the constraint forces into the particles' force accumulators
			loadConstraintForces();
		}
		//we need to compute the constraint forces first before loading dydt
		//Let's try fixing dydt after everything is done
		i=0;
		for(Particle p: this.particles){
    		dydt[6*i+3]=p.f.x/p.mass;
    		dydt[6*i+4]=p.f.y/p.mass;
    		dydt[6*i+5]=p.f.z/p.mass;
			i=i+1;
		}
    }
    
    /** just loads the constraint forces into the force accumulators */
    private void loadConstraintForces(){
    	for(int i=0; i<particles.size();i++){
    		particles.get(i).f.x += constraintForces[i*3];
    		particles.get(i).f.y += constraintForces[i*3+1];
    		particles.get(i).f.z += constraintForces[i*3+2];
    	}
    }
    

    private arrayAsVector velocityVector;
    private arrayAsVector forceVector;
    /** a method to load the position vector, the velocity vector and the force vector for the constraints solver
     * 
     */
    private void computeXVandF(){
    	velocityVector= new arrayAsVector(particles.size()*3);
    	forceVector= new arrayAsVector(particles.size()*3);
    	for( int i=0; i< particles.size(); i++){
    		velocityVector.theVector[i*3]=particles.get(i).v.x;
    		velocityVector.theVector[i*3+1]=particles.get(i).v.y;
    		velocityVector.theVector[i*3+2]=particles.get(i).v.z;
    		forceVector.theVector[i*3]=particles.get(i).f.x;
    		forceVector.theVector[i*3+1]=particles.get(i).f.y;
    		forceVector.theVector[i*3+2]=particles.get(i).f.z;
    	}
    }
//    /**
//     *  a method to compute v
//     */
//    private double[] computeV(){
//    	velocityVector= new double[particles.size()*3];
//    	return velocityVector;
//    }    
//    /**
//     *  a method to compute F
//     */
    
    double time = 0;
    /**
     * The integrator 
     */
    public Integrator integrator;    
    /** use this to store the state of the system */
    private double[] systemState = null;
    /**  here is the getstate method which will take the particle
     * system, particles, and dump the state of the system into the flat array systemState 
     * @param particles
     */
    public void getState(double[] y){
    	int i=0;
    	for(Particle p: this.particles){
    		y[i]=p.p.x;
    		y[i+1]=p.p.y;
    		y[i+2]=p.p.z;
    		y[i+3]=p.v.x;
    		y[i+4]=p.v.y;
    		y[i+5]=p.v.z;
    		i=i+6;
    	}    	
    }    
    
    /** here is the setState method which takes the systemState array and dumps the data
     * into the particle system.  We are just setting the position and velocity of each 
     * particle to whatever is in the systemState
     */
    public void setState(double[] yout){
    	int i=0;
    	for(Particle p: this.particles){
    		p.p.x=yout[i];
    		p.p.y=yout[i+1];
    		p.p.z=yout[i+2];
    		p.v.x=yout[i+3];
    		p.v.y=yout[i+4];
    		p.v.z=yout[i+5];
    		i=i+6;
    	}
    	
    }
        
    /** here is a little method which returns the dimension of the phase space of the particle system */
    public int particlDim(){
    	
    	int dim=6*this.particles.size();
    	return dim;
    }
    

    
    /** use this to store the state of the system after a step, i.e., pass it ot the integrator */
    private double[] systemStateOut = null;
    private Boolean applyConstraints;
    double maxError;
    int numGraphFixIters;
    /**
     * Updates the position of all particles
     * @param h 
     */
    public void advanceTime( double h, BooleanParameter imposeConstraints,BooleanParameter applyBaumgarte,BooleanParameter applyPostStep) {
    	
        frame++;
    	this.applyBaumgarte=applyBaumgarte.getValue();
    	this.applyPostStep=applyPostStep.getValue();
    	applyConstraints=imposeConstraints.getValue(); 	
    	/**
    	 * It makes sense to update the constraints immediately
    	 */    	
    	constraintList.updateConstraints();   	
        // set the global spring properties
        Spring.k = k.getValue();
        Spring.b = b.getValue();       
        //check if you have arrays y and yout that are of an appropriate size for the current number of particles
        systemState=new double[1];
        if(systemState.length<this.particles.size()*6){
        	systemState=new double[this.particles.size()*6];
        	systemStateOut=new double[this.particles.size()*6];
        }
        this.getState(systemState);
        //integrator= new ForwardEuler(subSteps);
        switch(intType){
        case 1: integrator=new ForwardEuler(subSteps); break;
        case 2: integrator= new Midpoint(subSteps);break;
        case 3: integrator= new TwoThirds(subSteps);break;
        case 4: integrator= new RK();break;
        case 5: integrator= new SymplecticEuler3D(subSteps); break;
        }
        integrator.step(systemState, systemState.length, time, h, systemStateOut,(Function) this);
//        this.setPinnedParticles3(systemStateOut);
        this.setState(systemStateOut);
        //here we are going to apply a post step
        if(applyPostStep.getValue()) applyPostStep();   
        //once the system is stepped, we go ahead and build the MST and use it to do post step corrections
        int whichCorner=0;
        if(graphFix.getValue()){
        	double error;
        	constraintList.updateConstraints();
        	arrayAsVector gVec=constraintList.make_g_Vector();
        	error=gVec.getAbsValNorm();
        	maxError=gVec.findMax();
        	numGraphFixIters=0;
        	while( error>MSTError.getValue() && numGraphFixIters <1){  
        		numGraphFixIters++;
        		constraintList.updateConstraints();
        		gVec=constraintList.make_g_Vector();
        		error=gVec.getAbsValNorm();
        		//define the root
        		theRoot=particles.get(0);
        		switch(whichCorner){
        			case 0:
        				theRoot=particles.get(0);
        				break;
        			case 1:
        				theRoot=particles.get((this.sheetWidth.getValue()-1)*this.sheetHeight.getValue());
        				break;
        			case 2:
        				theRoot=particles.get(this.sheetHeight.getValue()-1);
        				break;
        			case 3:
        				theRoot=particles.get((this.sheetHeight.getValue())*(this.sheetHeight.getValue())-1);
        				break;
        		}
        		doGraphFix();
        		constraintList.updateConstraints();
        		gVec=constraintList.make_g_Vector();
        		error=gVec.getAbsValNorm();
            	maxError=gVec.findMax();
        		if(whichCorner>3){
        			whichCorner=0;
        		}else whichCorner++;

        	}

        }
        // INSTEAD, use the integrator interface to switch between integration methods:        
        //- pack up current system state (stored in all the particles) into systemState
        //- integrator.step( systemState, n, time, h, systemStateOut, this);
        //- unpack systemStateOut so that the particles are in the correct position when drawn               
        time = time + h;        
        //testing
//        constraintList.printGmatrix();
//        constraintList.print_g();
//        constraintList.printGDotmatrix(); 
        //compute the total energy and print it: kinetic plus potential
    }
    
    
    private Calendar cal = Calendar.getInstance();
    private Particle theRoot;
    private ConstraintGraph theBigGraph;
    String theTime;
	double aTime;
    /**here is a method to stabilize the system or do a complete fix using a minimum spanning tree.
     * 
     */    
	private void doGraphFix(){
    	//we can use Prim or Kruskal via swtich
    	if(!kruskalNotPrim.getValue()){
        	//we need to choose a root for the minimum spanning tree
        	//theRoot=particles.get(0);
        	
        	if(theBigGraph==null){
        		theBigGraph=new ConstraintGraph(constraintList, particles);	
        	}
        	//record a time
        	//aTime= new Date().getTime();
        	aTime=System.nanoTime();
        	theBigGraph.mstByPrim(theRoot, constraintList);
        	aTime= System.nanoTime()-aTime;
        	//write said time to a file        	
        	try {
            	theTime=sheetWidth.getValue()+","+aTime;            	
            	writeToFile("output/PrimNumbers"+cal.get(Calendar.MONTH)+"-"+cal.get(Calendar.DATE)+"-"+".txt",theTime);
			} catch (IOException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
			
        	aTime=System.nanoTime();
        	mstByKruskal(theRoot, constraintList);
        	aTime= System.nanoTime()-aTime;
        	try {
        		// USE RELATIVE PATHS!  you want this to work on different machines...
            	theTime=sheetWidth.getValue()+","+aTime;
				writeToFile("output/KruskalNumbers"+cal.get(Calendar.MONTH)+"-"+cal.get(Calendar.DATE)+"-"+".txt",theTime);
			} catch (IOException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
    	}else{
    		//theBigGraph=new ConstraintGraph(constraintList, particles);
        	aTime=System.nanoTime();
        	mstByKruskal(theRoot, constraintList);
        	aTime= System.nanoTime()-aTime;
        	try {
            	theTime=sheetWidth.getValue()+","+aTime;
				writeToFile("output/KruskalNumbers"+cal.get(Calendar.MONTH)+"-"+cal.get(Calendar.DATE)+"-"+new Date().getTime()+".txt",theTime);
			} catch (IOException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
       	   // theBigGraph.graphFromKruskal(kruskalTree);       	    
    	}

		if(usetToFix.getValue()){
	    	/** now we want to do a post step correction but only to the constraints in the graph.    	  
	    	 */
			for(ConstraintGraphNode n: theBigGraph.theNodeList){
				if(n.p==theRoot){
					rootNode=n;
					break;
				}
			}
	    	fixConstraints(rootNode);			
		}

    	
    }
    File aFile;
    /** this method takes a file name and path and writes the string "data" to that file */
    public void writeToFile(String fileNameNPath, String data) throws IOException {
    	//if(aFile==null)
    		aFile = new File(fileNameNPath);
    	FileOutputStream pwdOut = new FileOutputStream( aFile, true );
    	data=data+"\n";
    	pwdOut.write(data.getBytes());
    	//FileWriter theFileWriter= new FileWriter(aFile,true);
    	//PrintWriter thePrintWriter= new PrintWriter(pwdOut,true);
    	//thePrintWriter.write(data);
    	//thePrintWriter.append(data);
        pwdOut.close();
    }
    
    /**A helper method to fix all the constraints
     * To do this, we pick some node to be the root.  For now we will pick the top left pinned particle arbitrarily
     * to be the root, though we can do experiments on that too.  We do depth first tree traversal and correct each constraint
     * by changing the position of the forward (further down the tree) particle along the vector between it and the rear (further up the tree) particle.
     *
     */
//	private ConstraintGraphNode prevNode;
	private ConstraintGraphNode rootNode;
    public void fixConstraints(ConstraintGraphNode root){
    	
		//we are going to step along the graph, parent to child, until we get to a fork.  Then we recursively call this function on each of the branches
    	ConstraintGraphNode currNode;
		currNode=root;
		while(currNode.children.size()!=0){
			if(currNode.children.size()>1){
				for(ConstraintGraphNode cgn: currNode.children){
					//fix the child
					fixChild(currNode, cgn);
					//now recursively call this function
					fixConstraints(cgn);
				}
				return;
			}else {
				if(currNode!=null){
					//fix the child
					fixChild(currNode, currNode.children.get(0));
				}
				currNode=currNode.children.get(0);
			}
		}
    }
    
    


    /** this method moves a particle along the vector defined by the child relationship (also a constraint relationship) */
    private void fixChild(ConstraintGraphNode parent, ConstraintGraphNode child){
        double constraintLength;
        int commonConstr;
        Vector3d fixTemp;
        Vector3d fixDir;
    	//get the constraint value...for a sickening hack we can for now just use link length
    		try {
				constraintLength=constraintList.theConstraints.get(
						findCommonConstr(parent.CListLocations,child.CListLocations)
				).setPoint();
			} catch (Exception e) {
				return;
			}

    	if(constraintLength<10000000){    		
        	fixDir=new Vector3d();
        	fixDir.scaleAdd(-1, child.p.p , parent.p.p);
        	fixDir.normalize();
        	fixDir.scale(-1);
        	Point3d temp=new Point3d();
        	temp.set(parent.p.p);
        	if(!child.p.pinned){
        		child.p.p.scaleAdd( constraintLength,fixDir,temp);
        	}else{
        		temp.set(child.p.p);
        		fixDir.scale(-1);
        		parent.p.p.scaleAdd( constraintLength,fixDir,temp);
        	}
    	}else{
    		System.out.println("hello");
    	}

    }
    
/** look through two lists of ints and find the collision or intersection (with a promise that there will be at most 1) */
    private int findCommonConstr(List<Integer> cLLoc1, List<Integer> cLLoc2) throws Exception{ 
    	for(int i =0 ; i<cLLoc1.size(); i++){
    		for(int j=0; j<cLLoc2.size(); j++){
    			if(Math.floor(cLLoc1.get(i))==Math.floor(cLLoc2.get(j))){
    				return cLLoc1.get(i);
    			}
    		}
    	}
    	throw new Exception( );

    }
	
	/** and we need yet another constraint list which we be the final tree */
	private List <Constraint> kruskalTree;
	
	/** here we try to calculate the mst via kruskal's algorithm.
	 * We might use the primkey as the weight.  
	 * @param rootParticle
	 * @param theCList
	 */
	public void mstByKruskal(Particle rootParticle, ConstraintList theCList){
		
		// we need another constraint list which will be sorted w.r.t. the stretch constraint value 
//		List<Constraint> A = new LinkedList<Constraint>();
		PriorityQueue<StretchConstraint> Q = new PriorityQueue<StretchConstraint>();
		
		kruskalTree= new LinkedList<Constraint>();
		
	    //we need two temporary particles for the Kruskal
	    Particle u,v;

		int i=0;
		for(Particle p: this.particles){
			Set<Particle> s = new HashSet<Particle>();
			s.add( p );
			p.setSet( s );
			i++;
		}
		i=0;
		//now put only the stretch constraints in a sortedlist
		for(Constraint c: theCList.theConstraints){
			if(c.constraintType() == ConstraintType.STRETCH){
				Q.add( (StretchConstraint) c );
//				for(Constraint cA: A){
//					if(-Math.abs(c.get_g())>-Math.abs(cA.get_g())){
//						i=i+1;
//					}else break;
//				}
//				A.add(i, c);
//				i=0;
			}
		}
		
		while ( ! Q.isEmpty() ) {
			Constraint uv = Q.poll();
			u=uv.ParticleList().get(0);
			v=uv.ParticleList().get(1);

			Set<Particle> s1 = u.getSet();
			Set<Particle> s2 = v.getSet();
			
			if( s1 != s2 ){
				kruskalTree.add(uv);
				//now we want to change the sets set(U) and set(v) into one new set
				
				if ( s1.size() < s2.size() ) {
					for ( Particle p : s1 ) {
						p.theSet = s2;
					}
					s2.addAll( s1 );
				} else {
					for ( Particle p : s2 ) {
						p.theSet = s1;
					}
					s1.addAll( s2 );
				}
				
//				union(u.getSet(), v.getSet());
			}
		}
		
//		for(Constraint uv: A){
//			//temp particle
//			u=uv.ParticleList().get(0);
//			v=uv.ParticleList().get(1);
//			if(u.getSet()!=v.getSet()){
//				kruskalTree.add(uv);
//				//now we want to change the sets set(U) and set(v) into one new set
//				union(u.getSet(), v.getSet());
//			}
//		}
		
	}
    
//	//this is a helper method for Kruskal which fuses sets two sets into the union of those sets.  To do this we have to go through all particles and find all the particles in the set then change the set 
//	//integer
//	public void union(int uSet, int vSet){
//		for(Particle p: particles){
//			if(p.getSet()==vSet || p.getSet()==uSet) p.setSet(uSet);
//		}
//	}
//    
    
	
	
	
    
    // some variables used in the post step calculation
	arrayAsVector the_g_Vector;
	double currPSError;
	arrayAsVector dp, dpPrime;    
    //this will calculate the post step
    private void applyPostStep(){
    	//maybe we should update the constraints given there has been a time step
    	constraintList.updateConstraints();
    	ConjugateGradientSolver theSolver= new ConjugateGradientSolver();
    	//we need the g vector
    	the_g_Vector=constraintList.make_g_Vector();
    	//first calculate the step
    	//here we want to do multiple iterations but based on some kind of gross error
    	currPSError=the_g_Vector.getAbsValNorm();
    	int psIters=0;
    	while(currPSError>1E-6){
        	dpPrime=theSolver.solveLinearSystem(constraintList, the_g_Vector);
        	dp=constraintList.multGTrans(dpPrime);
        	//then adjust every particle's position
        	for(int i=0; i<particles.size(); i++){
        		particles.get(i).p.x+=-dp.theVector[i*3];
        		particles.get(i).p.y+=-dp.theVector[i*3+1];
        		particles.get(i).p.z+=-dp.theVector[i*3+2];
        	}
        	constraintList.updateConstraints();
        	the_g_Vector=constraintList.make_g_Vector();
        	currPSError=the_g_Vector.getAbsValNorm();
        	psIters++;
        	if(psIters>15){
        		System.out.println("2 many iters");
        	}
    	}
    }
    
    
    //we want to do a comparison using the mtj package
    private void postSTepMTJCompare(){
    	//compute
    	DenseVector gMTJ,dpPrime,dp;
    	gMTJ=the_g_Vector.convertToMTJVector();
    	dpPrime=constraintList.computeWithMTJCompare(gMTJ);
    	//create GT
    	DenseMatrix G,GT;
    	GT=new DenseMatrix(particles.size()*3,constraintList.theConstraints.size());
    	G=constraintList.getG_AsMTJMatrix();
    	G.transpose(GT);
    	dp=new DenseVector(particles.size()*3);
    	GT.mult(dpPrime, dp);
    	GT.mult(dpPrime, dp);
    }
    
    //total energy
    private double E;
    
	//clears the console screen
    public  void  clearConsole(){
	    int i = 0;
	
	    for(i=0;i<300;i++)
	    {
	    System.out.println("\n");
	    }

    }
    
    /**
     * Creates a new particle and adds it to the system
     * @param x
     * @param y
     * @param vx
     * @param vy
     * @return the new particle
     */
    public Particle createParticle( double x, double y, double z, double vx, double vy, double vz ) {
        Particle p = new Particle( x, y, z, vx, vy, vz );
        particles.add( p );
        return p;
    }
    
    /**
     * Creates a new spring between two particles and adds it to the system.
     * @param p1
     * @param p2
     * @return the new spring
     */
    public Spring createSpring( Particle p1, Particle p2 ) {
        Spring s = new Spring( p1, p2 ); 
        springs.add( s );         
        return s;
    }
    
    @Override
    public void init(GLAutoDrawable drawable) {
        // do nothing
    }
    
    
    
    /** here we attempt to fix all the normals on the cloth
     * 
     */
    Vector3d aTempNorm, bTempNorm, cTempNorm,dTempNorm,n1,n2,n3,n4;
    public void fixClothNormals(){
    	aTempNorm=new Vector3d();
    	bTempNorm=new Vector3d();
    	cTempNorm=new Vector3d();
    	dTempNorm=new Vector3d();
    	n1=new Vector3d();
    	n2=new Vector3d();
    	n3=new Vector3d();
    	n4=new Vector3d();
    	//go through all particles
    	for(int pnum=0; pnum<sheetHeight.getValue()*sheetHeight.getValue();pnum++){
    		@SuppressWarnings("unused")
			int temp=(pnum+1)%sheetHeight.getValue();
    		if(pnum==0){
    			//case1
    			aTempNorm.scaleAdd(-1, particles.get(pnum).p, particles.get(pnum+1).p);
    			bTempNorm.scaleAdd(-1,  particles.get(pnum).p, particles.get(pnum+sheetHeight.getValue()).p);
    			aTempNorm.normalize();
    			bTempNorm.normalize();
    			particles.get(pnum).n.cross(aTempNorm, bTempNorm);
    		}else if(pnum==sheetHeight.getValue()*sheetHeight.getValue()-sheetHeight.getValue()){
    			//case2
    			aTempNorm.scaleAdd(-1, particles.get(pnum).p, particles.get(pnum-sheetHeight.getValue()).p);
    			bTempNorm.scaleAdd(-1,  particles.get(pnum).p,particles.get(pnum+1).p);
    			aTempNorm.normalize();
    			bTempNorm.normalize();
    			particles.get(pnum).n.cross(aTempNorm, bTempNorm);
    		}else if(pnum== sheetHeight.getValue()-1){
    			//case3
    			aTempNorm.scaleAdd(-1, particles.get(pnum).p,particles.get(pnum-1).p);
    			bTempNorm.scaleAdd(-1,  particles.get(pnum).p,particles.get(pnum+sheetHeight.getValue()).p);
    			aTempNorm.normalize();
    			bTempNorm.normalize();
    			particles.get(pnum).n.cross(bTempNorm, aTempNorm);
    		}else if(pnum== sheetHeight.getValue()*sheetHeight.getValue()-1){
    			//case4
    			aTempNorm.scaleAdd(-1, particles.get(pnum).p,particles.get(pnum-1).p);
    			bTempNorm.scaleAdd(-1,  particles.get(pnum).p,particles.get(pnum-sheetHeight.getValue()).p);
    			aTempNorm.normalize();
    			bTempNorm.normalize();
    			particles.get(pnum).n.cross(aTempNorm, bTempNorm);
    		}else if(pnum>0 && pnum<sheetWidth.getValue()-1){
    			//case5
    			aTempNorm.scaleAdd(-1, particles.get(pnum).p,particles.get(pnum-1).p);
    			bTempNorm.scaleAdd(-1,  particles.get(pnum).p,particles.get(pnum+1).p);  
    			cTempNorm.scaleAdd(-1,  particles.get(pnum).p,particles.get(pnum+sheetHeight.getValue()).p);
    			aTempNorm.normalize();
    			bTempNorm.normalize();
    			cTempNorm.normalize();
    			n1.cross(cTempNorm, aTempNorm);    			
    			n2.cross(bTempNorm, cTempNorm);
    			n1.add(n2);
    			n1.normalize();
    			particles.get(pnum).n.set(n1);
    		}else if((pnum>sheetHeight.getValue()*sheetHeight.getValue()-sheetHeight.getValue()-1)&&pnum !=sheetHeight.getValue()*sheetHeight.getValue()-1 ){
    			//case6
        			aTempNorm.scaleAdd(-1, particles.get(pnum).p,particles.get(pnum-1).p);
        			bTempNorm.scaleAdd(-1,  particles.get(pnum).p,particles.get(pnum+1).p);  
        			cTempNorm.scaleAdd(-1,  particles.get(pnum).p,particles.get(pnum-sheetHeight.getValue()).p);
        			aTempNorm.normalize();
        			bTempNorm.normalize();
        			cTempNorm.normalize();
        			n1.cross(aTempNorm,cTempNorm);    			
        			n2.cross(cTempNorm,bTempNorm);
        			n1.add(n2);
        			n1.normalize();
        			particles.get(pnum).n.set(n1);
    		}else if((pnum+1)%sheetHeight.getValue()==0 && pnum>sheetHeight.getValue()&& pnum< sheetHeight.getValue()*sheetHeight.getValue()-sheetHeight.getValue()){
    			//case7
    			aTempNorm.scaleAdd(-1, particles.get(pnum).p,particles.get(pnum-1).p);
    			bTempNorm.scaleAdd(-1,  particles.get(pnum).p,particles.get(pnum-sheetHeight.getValue()).p);  
    			cTempNorm.scaleAdd(-1,  particles.get(pnum).p,particles.get(pnum+sheetHeight.getValue()).p);
    			aTempNorm.normalize();
    			bTempNorm.normalize();
    			cTempNorm.normalize();
    			n1.cross(aTempNorm,bTempNorm);    			
    			n2.cross(cTempNorm,aTempNorm);
    			n1.add(n2);
    			n1.normalize();
    			particles.get(pnum).n.set(n1);
    		}else if(pnum%sheetHeight.getValue()==0 && pnum>0 && pnum< sheetHeight.getValue()*sheetHeight.getValue()-sheetHeight.getValue()){
    			//case8
    			aTempNorm.scaleAdd(-1, particles.get(pnum).p,particles.get(pnum+1).p);
    			bTempNorm.scaleAdd(-1,  particles.get(pnum).p,particles.get(pnum-sheetHeight.getValue()).p);  
    			cTempNorm.scaleAdd(-1,  particles.get(pnum).p,particles.get(pnum+sheetHeight.getValue()).p);
    			aTempNorm.normalize();
    			bTempNorm.normalize();
    			cTempNorm.normalize();
    			n1.cross(bTempNorm,aTempNorm);    			
    			n2.cross(aTempNorm,cTempNorm);
    			n1.add(n2);
    			n1.normalize();
    			particles.get(pnum).n.set(n1);
    		}else  {
    			//case 9
    			aTempNorm.scaleAdd(-1, particles.get(pnum).p,particles.get(pnum+1).p);
    			bTempNorm.scaleAdd(-1,  particles.get(pnum).p,particles.get(pnum-1).p);  
    			cTempNorm.scaleAdd(-1,  particles.get(pnum).p,particles.get(pnum+sheetHeight.getValue()).p);
    			dTempNorm.scaleAdd(-1,  particles.get(pnum).p,particles.get(pnum-sheetHeight.getValue()).p);
    			aTempNorm.normalize();
    			bTempNorm.normalize();
    			cTempNorm.normalize();
    			dTempNorm.normalize();
    			n1.cross(cTempNorm,bTempNorm);    			
    			n2.cross(bTempNorm,dTempNorm);
    			n3.cross(dTempNorm,aTempNorm);
    			n4.cross(aTempNorm,cTempNorm);
    			n1.add(n2);
    			n1.add(n3);
    			n1.add(n4);
    			n1.normalize();
    			particles.get(pnum).n.set(n1);
    		}
    	}
    }
    
    /**
     * We need to draw normals
     */
    Vector3d vec0, vec1, vec2, normal;
    /**
     * The current half edge to display (to help with debugging)
     */
    private meshTools.HalfEdge currentHE;
    /** this is a polygon soup for drawing
     */
    private PolygonSoup theSoup;
    /** a half edge data structure for nicely drawing the sheet
     */
    meshTools.HEDS[] heds;    
    /**
     * Height of the canvas, useful for wall collisions
     */
    private int height;
    /**
     * Width of the canvas, useful for wall collisions
     */
    private int width;
    public int frame=0;
    @Override
    public void display(GLAutoDrawable drawable) {
    	constraintList.updateConstraints();
        String text="";
        GL gl = drawable.getGL();
        DecimalFormat df = new DecimalFormat("#.##");
        //System.out.print(df.format(d));
        E=(0.5)*particles.get(1).v.length()*particles.get(1).v.length()+ 9.8*(particles.get(1).p.y-particles.get(1).p0.y);
        Vector3d length= new Vector3d(particles.get(0).p);
        length.scaleAdd(-1,particles.get(1).p);
        double theLength=length.length();

        if(frame>0){
        	int particleNum=obsWhichParticle.getValue();
            text = "\n \n \n fx="+ this.particles.get(particleNum).f.x + "\n fy="+ this.particles.get(particleNum).f.y + "\n fz="+ this.particles.get(particleNum).f.z + "\n px "+ this.particles.get(particleNum).p.x + "\n py "+ this.particles.get(particleNum).p.y + "\n pz "+ this.particles.get(particleNum).p.z;
            if(this.constraintList.theConstraints.size()>0&& this.applyConstraints){
                text= text+"\n \n   Consfx: "+constraintForces[particleNum*3]+ "\n Consfy: "+constraintForces[particleNum*3+1] +"\n Consfz: "+constraintForces[particleNum*3+2];
                text= text+ "\n g"+this.constraintList.theConstraints.get(this.constraintList.theConstraints.size()-1).get_g();	
            }
            text= text+  "\n \n \n BGks:" + this.BGks.getValue() + "\n BGkd:" +this.BGkd.getValue();
            text=text +"\n  Total Energy: " +E;
            text=text +"\n  error on rod Length: " + (theLength-d.getValue());
           // EasyViewer.printTextLines(drawable, text);

            text = text + "\n \n \n frame number:"+ frame;       	
        }
        EasyViewer.beginOverlay(drawable);
        text = text + "\n \n \n frame number:"+ frame;
        text= text+ "\n  constraint Loc 3:  "+constraintList.theConstraints.get(3).get_g();
        text= text+ "\n  constraint Loc 4:  "+constraintList.theConstraints.get(4).get_g();
        text= text+ "\n  constraint Loc 5:  "+constraintList.theConstraints.get(5).get_g();
        text= text+ "\n  constraint Loc 6:  "+constraintList.theConstraints.get(6).get_g();
        text= text+ "\n  constraint Loc 7:  "+constraintList.theConstraints.get(7).get_g();
        text= text+ "\n  constraint Loc 8:  "+constraintList.theConstraints.get(8).get_g();
        text= text+ "\n  constraint Loc 9:  "+constraintList.theConstraints.get(9).get_g();
        text= text+ "\n  constraint Loc 13:  "+constraintList.theConstraints.get(13).get_g();
        text= text+ "\n  constraint Loc 14:  "+constraintList.theConstraints.get(14).get_g();
        if(particles.get(6).pinned){
            text= text+ "\n  constraint Loc 15:  "+constraintList.theConstraints.get(15).get_g();
            text= text+ "\n  constraint Loc 16:  "+constraintList.theConstraints.get(17).get_g();
        }

//        text= text+ "\n  constraint Loc 10:  "+constraintList.theConstraints.get(10).get_g();
//        text= text+ "\n  constraint Loc 11:  "+constraintList.theConstraints.get(11).get_g();
//        text= text+ "\n  constraint Loc 12:  "+constraintList.theConstraints.get(12).get_g();
        
        //print the total error
        arrayAsVector g=new arrayAsVector(constraintList.theConstraints.size());
        g=constraintList.make_g_Vector();
        text= text+ "\n  \n  Total Error:  "+g.getAbsValNorm();
        //print the max error and its location
        text= text+ "\n  \n  Max Error :  "+g.findMax() +"     Occurs at ConstLoc" + g.maxLoc;

        
        
        EasyViewer.printTextLines(drawable, text);
        EasyViewer.endOverlay(drawable);
        
        
        // We'll keep track of the width and the height 
        // of the drawable as this might be useful for 
        // processing collisions with walls
        height = drawable.getHeight();
        width = drawable.getWidth();
        if(triangles.getValue()&& this.whichSystemType==2){
        	gl.glDisable(GL.GL_CULL_FACE);
            gl.glEnable(GL.GL_LIGHTING);
            float[] frontColor = { 1, 1.0f, 0, 1 };
            float[] backColor = { 1, 0f, 1f, 1 };
            float[] black = {0,0,0,1};
            gl.glLightModeli(GL.GL_LIGHT_MODEL_TWO_SIDE, 1);
            gl.glMaterialfv(GL.GL_FRONT, GL.GL_AMBIENT_AND_DIFFUSE, frontColor, 0);
            gl.glMaterialfv(GL.GL_BACK, GL.GL_AMBIENT_AND_DIFFUSE, backColor, 0);
            gl.glPolygonMode(GL.GL_FRONT_AND_BACK, GL.GL_FILL);
        	gl.glBegin(GL.GL_TRIANGLES);
        		for(int i=theSheet.getSheetHeight(); i< particles.size();i++){
        			if(i%theSheet.getSheetHeight()!=theSheet.getSheetHeight()-1){
        				//draw the first triangle
        				gl.glNormal3d(particles.get(i).n.x, particles.get(i).n.y, particles.get(i).n.z);
        				gl.glVertex3d(particles.get(i).p.x,particles.get(i).p.y,particles.get(i).p.z);
        				gl.glNormal3d(particles.get(i-theSheet.getSheetHeight()).n.x, particles.get(i-theSheet.getSheetHeight()).n.y, particles.get(i-theSheet.getSheetHeight()).n.z);
        				gl.glVertex3d(particles.get(i-theSheet.getSheetHeight()).p.x,particles.get(i-theSheet.getSheetHeight()).p.y,particles.get(i-theSheet.getSheetHeight()).p.z);
        				gl.glNormal3d(particles.get(i-theSheet.getSheetHeight()+1).n.x, particles.get(i-theSheet.getSheetHeight()+1).n.y, particles.get(i-theSheet.getSheetHeight()+1).n.z);
        				gl.glVertex3d(particles.get(i-theSheet.getSheetHeight()+1).p.x,particles.get(i-theSheet.getSheetHeight()+1).p.y,particles.get(i-theSheet.getSheetHeight()+1).p.z);     				        				
        				//draw second triangle
        				gl.glNormal3d(particles.get(i).n.x, particles.get(i).n.y, particles.get(i).n.z);
        				gl.glVertex3d(particles.get(i).p.x,particles.get(i).p.y,particles.get(i).p.z);
        				gl.glNormal3d(particles.get(i-theSheet.getSheetHeight()+1).n.x, particles.get(i-theSheet.getSheetHeight()+1).n.y, particles.get(i-theSheet.getSheetHeight()+1).n.z);
        				gl.glVertex3d(particles.get(i-theSheet.getSheetHeight()+1).p.x,particles.get(i-theSheet.getSheetHeight()+1).p.y,particles.get(i-theSheet.getSheetHeight()+1).p.z);
        				gl.glNormal3d(particles.get(i+1).n.x, particles.get(i+1).n.y, particles.get(i+1).n.z);
        				gl.glVertex3d(particles.get(i+1).p.x,particles.get(i+1).p.y,particles.get(i+1).p.z);
        			}
        		}
        	gl.glEnd();
        }else {
	        gl.glDisable( GL.GL_LIGHTING );
	        gl.glPointSize( 5 );
	        gl.glColor3f(1, 0, 0);
	        gl.glBegin( GL.GL_POINTS );
	        for ( Particle p : particles ) {
	            // transparency is used to get smooth edges on the particles
	            double alpha = 0.5;
	            if ( p.pinned ) {
	                gl.glColor4d( 1, 0, 0, alpha );
	            } else {
	                gl.glColor4d( 0d, 0.95d,0d, alpha );
	            }
	            gl.glVertex3d( p.p.x, p.p.y ,p.p.z);
	        }
	        gl.glEnd();
	        
	        if (graphFix.getValue()&& drawMST.getValue()){                  
                gl.glColor4d(1.0,0.5,0.0, 0.5 );
                gl.glLineWidth( 6 );
                if(!kruskalNotPrim.getValue()){
                    if(theBigGraph!=null && theBigGraph.root!=null ){
                            theBigGraph.root.drawChildren(gl);                      
                    }
                }else{
                    if(kruskalTree!= null){
                        for(Constraint c: kruskalTree){
                            gl.glBegin(GL.GL_LINES);
                                gl.glVertex3d(c.ParticleList().get(0).p.x,c.ParticleList().get(0).p.y,c.ParticleList().get(0).p.z);
                                gl.glVertex3d(c.ParticleList().get(1).p.x,c.ParticleList().get(1).p.y,c.ParticleList().get(1).p.z);
                            gl.glEnd();
                        }
                    }
                }
            }
	        
	        //here we draw the springs
	        gl.glColor4d( 0, 0.5, 0.5, 0.5 );
	        gl.glLineWidth( 2 );
	        gl.glBegin( GL.GL_LINES );
	        for (Spring s : springs) {
	            gl.glVertex3d( s.p1.p.x, s.p1.p.y, s.p1.p.z );
	            gl.glVertex3d( s.p2.p.x, s.p2.p.y, s.p2.p.z );
	        }
	        gl.glEnd();
	        //here we want to draw blue lines to indicate where a constraint exists between two particles
	        gl.glColor4d( 0.2, 0, 1, 0.5 );
	        gl.glBegin( GL.GL_LINES );
	        	for(Constraint c:constraintList.theConstraints){
	        		if(c.constraintType()==ConstraintType.STRETCH){
	        	       	gl.glVertex3d( c.ParticleList().get(0).p.x,c.ParticleList().get(0).p.y, c.ParticleList().get(0).p.z);
	        	       	gl.glVertex3d( c.ParticleList().get(1).p.x,c.ParticleList().get(1).p.y, c.ParticleList().get(1).p.z);
	        		}
	        	}
	        gl.glEnd();
	        //here we draw the minimum spanning tree we need to fix the switches to include the different ways to draw the kruskal vs. the prim tree
	        
        }

    }

    //Here we are going to convert a sheet of particles into a polygon soup so that we can subdivide it
    //we need to generalize the input to the constraints system
    public PolygonSoup SheetToPolygonSoup(){
    	PolygonSoup theSoup= new PolygonSoup();
    	Vertex theVert;
    	int[] aFace;
    	int height=theSheet.getSheetHeight();
    	//go through the particles and add each particle as a vertex to the vertexList
    	for(Particle p: particles){
    		theVert=new Vertex();
    		theVert.p=p.p;
    		theVert.n=new Vector3d();
    		//if the vertex is on the boundary, set the flag
    		if(p.getIndex()<sheetHeight.getValue() || p.getIndex()%sheetHeight.getValue()==0 || p.getIndex()%sheetHeight.getValue()==sheetHeight.getValue()-1||p.getIndex()>=sheetHeight.getValue()*(sheetWidth.getValue()-1)){
    			theVert.isOnBound=true;
    		}else{
    			theVert.isOnBound=false;
    		}
    		theSoup.vertexList.add(theVert);    		
    	}
    	for(int i=height; i< particles.size();i++){
			if(i%theSheet.getSheetHeight()!=theSheet.getSheetHeight()-1){
	    		//for each particle in the list there are two triangles
	    		//here is the first
	    		aFace=new int[3];
	    		aFace[0]=i;
	    		aFace[1]=i-height;
	    		aFace[2]=i-height+1;
	    		theSoup.faceList.add(aFace);
	    		//here is the second
	    		aFace= new int[3];
	    		aFace[0]=i;
	    		aFace[1]=i-height+1;
	    		aFace[2]=i+1;
	    		theSoup.faceList.add(aFace);
			}
    	}
    	return theSoup;
    	
    }
    
    
    /** we are now going to do the fast projection step from the cloth paper and this means iterating the post steps and _this_
     * requires a maximum number of post step iterations and alternatively a minimum distance to the constraint manifold.
     * These actually mean the same thing, except that we are going to do an experiment to test the error from post step
     * after doing only one iteration.
     */
    public IntParameter maxPostStepIters= new IntParameter("Maximum post step iterations",1,1,300);    
    public DoubleParameter minPostStepDelta= new DoubleParameter("Min. distance to constraint manifold", 0.1, 1E-16,10);    
    /**We are adding the option to do a minimum spanning tree post step.  This involves finding a subtree of the constraint graph
     * which maximizes the constraint error.  We will have two algorithms to find this MST, Prims and Kruskal's.  Kruskal's is preferred since
     * it is more useful for a sparse graph, which our graph is.
     * 
     */
    //if we want to draw the MST but not use it to fix the constraints (
    public BooleanParameter usetToFix = new BooleanParameter("Stabilize with an MST?",false);
    //we fix the sheet with an MST up to an error parameter
    public DoubleParameter MSTError=new DoubleParameter("MST Error", 1E-16, 1E-16, 1);
    //we definitely will want to turn off the drawing of the MST
    public BooleanParameter drawMST= new BooleanParameter("Draw the MST?", true);
    //this is the main boolean for the minimum spanning tree
    public BooleanParameter graphFix= new BooleanParameter("Compute the MST?",true);
    //there are two different algorithms to calculate the mst and here is a flag to switch between them.  True is Kruskal, while false is Prim
    public BooleanParameter kruskalNotPrim= new BooleanParameter("Use the Kruskal and not the Prim",false);
  //draws triangles with bad normals ie it uses the particle system display 
    private BooleanParameter triangles= new BooleanParameter("Draw Simple Triangles?", true);
    //allows for a relaxed pose on the T-tester
    private DoubleParameter t= new DoubleParameter("Sheet relaxation parameter:",0.25,0,3);
    //allows one to observe a single particle
    private IntParameter obsWhichParticle= new IntParameter("Observe Which Particle?", 0,0,100);
    //pins the last particle of the n-link chain
    private BooleanParameter pinChain=new BooleanParameter("Pin chain?", false);
    //the width of a link
    private DoubleParameter d = new DoubleParameter("Link Spacing", 0.6, 0.01,10);   
    //this is the width and height of the sheet as a number of particles
    //you will probably want to put these on the control panel
 //   public IntParameter sheetWidth=  new IntParameter("Sheet Width", 9, 3,100);
    public IntParameter sheetHeight=new IntParameter("Sheet Height", 11, 3,100);
    public IntParameter sheetWidth= sheetHeight;
    
    private IntParameter numLinks= new IntParameter("Number of Links in Chain", 38, 1, 50);
    
    private BooleanParameter useg = new BooleanParameter( "use gravity", true );
    
    private DoubleParameter g = new DoubleParameter( "gravity", -9.8, -20, 20 );
    
    public DoubleParameter k = new DoubleParameter( "spring stiffness", 100,0.01, 100000 );
    
    private DoubleParameter b = new DoubleParameter( "spring damping", 20, 0, 10 );
    
   private DoubleParameter c = new DoubleParameter( "viscous damping", 01E-9, 1E-9, 10 );
    
    public DoubleParameter BGks=new DoubleParameter( "Baumgarte spring constant", 57, 0, 250000);
    
    public DoubleParameter BGkd=new DoubleParameter("Baumgarte damping constant", 4, 0.1, 1000);
    //here we can record the error on the velocity Gxdot and the constraint g 
    private BooleanParameter recordError=new BooleanParameter("Record the Error",false);

     
    public JPanel getControls() {    
        VerticalFlowPanel vfp = new VerticalFlowPanel();
        vfp.add(usetToFix.getControls());
        vfp.add(MSTError.getSliderControls(true));
        vfp.add(drawMST.getControls());
        vfp.add(kruskalNotPrim.getControls());
        vfp.add(graphFix.getControls());
        vfp.add(triangles.getControls());
        vfp.add(obsWhichParticle.getSliderControls());
        vfp.add(pinChain.getControls());
        vfp.add(c.getSliderControls(true));
        vfp.add(useg.getControls());
        vfp.add(recordError.getControls());
        vfp.add(t.getSliderControls(false));
        vfp.add( g.getSliderControls(false) );
        vfp.add( k.getSliderControls(true) );
        vfp.add( b.getSliderControls(false) );
        vfp.add(BGks.getSliderControls(false));
        vfp.add(BGkd.getSliderControls(true));
        vfp.add(numLinks.getSliderControls());
        vfp.add(sheetHeight.getSliderControls());
        vfp.add(sheetWidth.getSliderControls());
        vfp.add(d.getSliderControls(true));
        return vfp.getPanel();        
    }
    
    @Override
    public String toString() {
//        return "Ben Sprott\n" +
//               comments.getText() + "\n" +
//        	   "particles = " + particles.size() + "\n" + 
//               "integrator = " + integrator.getName() + "\n" + 
//               "stiffness = " + k.getValue() + "\n" +
//               "damping = " + b.getValue() + "\n" +
//               "time = " + time;
     return "Ben Sprott\n";
//      comments.getText() + "\n" +
//	   "particles = " + particles.size() + "\n" + 
//      "integrator = " + integrator.getName() + "\n" + 
//      "time = " + time;

        }

    public void resetFrame(){
    	frame=0;
    }

    /**
     * this clears all the parent child relationships
     */
    public void resetMST(){
    	theBigGraph=null;
//    	for(ConstraintGraphNode cgn: theBigGraph.theNodeList){
//    		theBigGraph=null;
//    		cgn.parent=null;
//    		cgn.children=new LinkedList<ConstraintGraphNode>();
//    	}
    }

    
    @Override
    public void attach(Component component) {
        component.addKeyListener(new KeyAdapter() {
            @Override
            public void keyPressed(KeyEvent e) {
                if (e.getKeyCode() == KeyEvent.VK_T) {
                    if ( currentHE.twin != null ) 
                    	currentHE = currentHE.twin;                    
                } else if (e.getKeyCode() == KeyEvent.VK_UP) {
                    if ( currentHE.parent != null ) currentHE = currentHE.parent;
                } else if (e.getKeyCode() == KeyEvent.VK_N) {
                    if ( currentHE.next != null ) currentHE = currentHE.next;
                } else if (e.getKeyCode() == KeyEvent.VK_LEFT) {
                    if ( currentHE.child1 != null ) currentHE = currentHE.child1;                    
                } else if (e.getKeyCode() == KeyEvent.VK_RIGHT) {
                    if ( currentHE.child2 != null ) currentHE = currentHE.child2;
                } else if (e.getKeyCode() == KeyEvent.VK_S) {
                    // walks along the mesh two triangles at a time
                    if ( currentHE.next != null ) currentHE = currentHE.next;
                    if ( currentHE.next != null ) currentHE = currentHE.next;
                    if ( currentHE.twin != null ) currentHE = currentHE.twin;
                    if ( currentHE.next != null ) currentHE = currentHE.next;
                    if ( currentHE.twin != null ) currentHE = currentHE.twin;
                }               
            }
        });
    }
    
}


