package clothSimulator;

import java.util.LinkedList;
import java.util.List;

import javax.vecmath.Vector3d;

import annas.graph.WeightedInterface;

public class StretchConstraint implements Constraint, Comparable<StretchConstraint> {


	//dist is the distance between the particles
	private double dist;
	//there are two particles involved in a stretch constraint, here we list them
	Particle p1, p2;
	private List<Particle> theParticles= new LinkedList<Particle>();
	//as a constraint, an instance of this object must know where in the constraints list it has been added
	int cLIndex;
	
	//the current value of the constraint itself
	double g;
	//the current values of the gradient of the constraint
	double Gp1x;
	double Gp1y;
	double Gp1z;
	double Gp2x;
	double Gp2y;
	double Gp2z;
	//the current values of the first time derivative of the gradient of the constraint
	double GdotP1x;
	double GdotP1y;
	double GdotP1z;
	double GdotP2x;
	double GdotP2y;
	double GdotP2z;
	//here we store deltax and the length of deltax so that we only compute it once during update and 
	//not every time we perform a matrix multiplication
	Vector3d deltaX=new Vector3d();
	double magDeltaX;
	//we do the same for deltaV
	Vector3d deltaV=new Vector3d();
	
	//here are the get and sets for dist
	public void setDist(double dist) {
		this.dist = dist;
	}
	public double getDist() {
		return dist;
	}

	//to create a new constraint we must supply both particles and the desired distance
	//between the two particles.
	public StretchConstraint(Particle p1, Particle p2, double length){
		this.dist=length;
		this.p1=p1;
		this.p2=p2;
		this.theParticles.add(p1);
		this.theParticles.add(p2);
	}
	
	@Override
	public ConstraintType constraintType() {
		return ConstraintType.STRETCH;
	}

	@Override
	public void updateAll(){
		deltaX.scaleAdd(-1, this.p1.p, this.p2.p);
		magDeltaX=deltaX.length();
		deltaV.scaleAdd(-1, this.p1.v, this.p2.v);
		updateCurrent_G();
		updateCurrent_Gdot();
		updateCurrent_g();
	}
	
	//this updates the entries of the row of the G matrix.  There are as many entries as there are dofs=3*numParticles
	//but they're all zero except for the dofs to which this constraint refers.
	@Override
	public void updateCurrent_G() {
		Gp1x=deltaX.x/magDeltaX;
		Gp1y=deltaX.y/magDeltaX;
		Gp1z=deltaX.z/magDeltaX;
		Gp2x=-deltaX.x/magDeltaX;
		Gp2y=-deltaX.y/magDeltaX;
		Gp2z=-deltaX.z/magDeltaX;
	}
	@Override
	public void updateCurrent_Gdot() {
		GdotP1x=deltaV.x/magDeltaX-((deltaX.x)*(deltaX.dot(deltaV)))/(magDeltaX*magDeltaX*magDeltaX);
		GdotP1y=deltaV.y/magDeltaX-((deltaX.y)*(deltaX.dot(deltaV)))/(magDeltaX*magDeltaX*magDeltaX);
		GdotP1z=deltaV.z/magDeltaX-((deltaX.z)*(deltaX.dot(deltaV)))/(magDeltaX*magDeltaX*magDeltaX);
		GdotP2x=-(deltaV.x/magDeltaX-((deltaX.x)*(deltaX.dot(deltaV)))/(magDeltaX*magDeltaX*magDeltaX));
		GdotP2y=-(deltaV.y/magDeltaX-((deltaX.y)*(deltaX.dot(deltaV)))/(magDeltaX*magDeltaX*magDeltaX));
		GdotP2z=-(deltaV.z/magDeltaX-((deltaX.z)*(deltaX.dot(deltaV)))/(magDeltaX*magDeltaX*magDeltaX));
	}
	@Override
	public void updateCurrent_g() {
		g=this.dist-magDeltaX;
	}
	@Override
	public double GiDot(double[] in) {
		return Gp1x*in[p1.getIndex()*3]+Gp1y*in[p1.getIndex()*3+1]+Gp1z*in[p1.getIndex()*3+2]+Gp2x*in[p2.getIndex()*3]+Gp2y*in[p2.getIndex()*3+1]+Gp2z*in[p2.getIndex()*3+2];
	}
	
	/** Here is a method to multiply a vector by Gtranspose.  This method is only part of the multiplication implementation
	 * which begins as a method in the constraintList class.  In the constraintList class, we have a simple loop
	 * over constraints in the list, and the method here in StretchConstraint adds to the elements in the
	 *  resulting vector that are affected by each constraint
	 */
	@Override
	public void GtransiDot(double[] in, double[] out) {
		out[p1.getIndex()*3]+=Gp1x*in[this.getCLIndex()];
		out[p1.getIndex()*3+1]+=Gp1y*in[this.getCLIndex()];
		out[p1.getIndex()*3+2]+=Gp1z*in[this.getCLIndex()];
		out[p2.getIndex()*3]+=Gp2x*in[this.getCLIndex()];
		out[p2.getIndex()*3+1]+=Gp2y*in[this.getCLIndex()];
		out[p2.getIndex()*3+2]+=Gp2z*in[this.getCLIndex()];
	}
	
	/** here is a method to return the inner product of the vector Gdoti and an arbitrary vector in
	 *where Gdoti is the first time derivative of the gradient of the ith constraint 
	 */
	@Override
	public double GdotiDot(double[] in) {
		return GdotP1x*in[p1.getIndex()*3]+GdotP1y*in[p1.getIndex()*3+1]+GdotP1z*in[p1.getIndex()*3+2]+GdotP2x*in[p2.getIndex()*3]+GdotP2y*in[p2.getIndex()*3+1]+GdotP2z*in[p2.getIndex()*3+2];
	}
	
	@Override
	public int getCLIndex() {
		// TODO Auto-generated method stub
		return this.cLIndex;
	}
	@Override
	public void setCLIndex(int cLIndex) {
		// TODO Auto-generated method stub
		this.cLIndex=cLIndex;
	}
	@Override
	public List<Particle> ParticleList() {
		return theParticles;
	}

	@Override
	public double get_g(){
		return this.g;
	}
	@Override
	public double[] get_G(int length) {
		double[] Grow=new double[length];
		Grow[p1.getIndex()*3]=Gp1x;
		Grow[p1.getIndex()*3+1]=Gp1y;
		Grow[p1.getIndex()*3+2]=Gp1z;
		Grow[p2.getIndex()*3]=Gp2x;
		Grow[p2.getIndex()*3+1]=Gp1y;
		Grow[p2.getIndex()*3+2]=Gp1z;
		return Grow;
	}
	@Override
	public double[] get_GDot(int length) {
		double[] Grow=new double[length];
		Grow[p1.getIndex()*3]=GdotP1x;
		Grow[p1.getIndex()*3+1]=GdotP1y;
		Grow[p1.getIndex()*3+2]=GdotP1z;
		Grow[p2.getIndex()*3]=GdotP2x;
		Grow[p2.getIndex()*3+1]=GdotP2y;
		Grow[p2.getIndex()*3+2]=GdotP2z;
		return Grow;
	}
	
	
	
	
	/**These methods will be used by graph handling methods which see constraints as edges, or arcs, in a graph
	 * A stretch constraint will have a weight and that weight will be the value of the constraint
	 * 
	 */
	@Override
	public Particle getHead() {
		// TODO Auto-generated method stub
		return p1;
	}
	@Override
	public Particle getTail() {
		// TODO Auto-generated method stub
		return p2;
	}
	@Override
	public Double getWeight() {
		// here we gather the weight of the graph and this weight is given as the negative of the constraint value.  The reason we return
		//the negative is such taht the minimum spanning tree algorithm will find the the graph with lowest (ie most negative number).
		//This graph will actually be a maximum spanning tree, which is computed by finding a minimum spanning tree over negative numbers.
		updateCurrent_g();
		double weight=-Math.abs(g);
		return weight;
	}
	@Override
	public void setWeight(WeightedInterface wi) {
		// TODO Auto-generated method stub		
	}
	@Override
	public WeightedInterface getWeightedInterface() {
		// TODO Auto-generated method stub
		return null;
	}
	@Override
	public double setPoint() {
		// TODO Auto-generated method stub
		return this.dist;
	}

	@Override
	public int compareTo(StretchConstraint o) {
		if ( g < o.g ) return -1;
		if ( g > o.g ) return 1;
		return 0;		
	}





}
