package clothSimulator;



public class ConjugateGradientSolver implements linearSolver{

	/** so here we go with the solver
	 * 
	 */
	
	@Override
	public String solverType() {
		return "ConjugateGradients";
	}

	private arrayAsVector r, prev_r, prev_prev_r;
	private arrayAsVector x, prev_x;
	private arrayAsVector p, prev_p;
	private double alpha, beta;
	private arrayAsVector temp_dofs, temp_constr;
	@Override
	public arrayAsVector solveLinearSystem(ConstraintList constraintList, arrayAsVector b) {
		
		r=new arrayAsVector(constraintList.theConstraints.size());
		r.set(b);
		prev_r=new arrayAsVector(constraintList.theConstraints.size());
		prev_prev_r=new arrayAsVector(constraintList.theConstraints.size());
		x= new arrayAsVector(constraintList.theConstraints.size());
		prev_x=new arrayAsVector(constraintList.theConstraints.size());
		p=new arrayAsVector(constraintList.theConstraints.size());
		prev_p=new arrayAsVector(constraintList.theConstraints.size());
		temp_dofs=new arrayAsVector(constraintList.getNumDofs());
		temp_constr=new arrayAsVector(constraintList.theConstraints.size());
		int k=0;
		while(!r.isZero()){
			//r.set(new arrayAsVector(r.length()));
			k++;
			prev_prev_r.set(prev_r);
			prev_r.set(r);
			prev_p.set(p);
			prev_x.set(x);
			if(k==1) p.set(prev_r);
			else{
				beta=prev_r.inner(prev_r)/(prev_prev_r.inner(prev_prev_r));
				p.set(prev_p);
				p.scaleVector(beta);
				p.addVectors(prev_r);
			}
			//compute Ap
			temp_constr.set(p);
			temp_dofs=constraintList.multGTrans(temp_constr);
			constraintList.multMassMatrixInverse(temp_dofs);
			temp_constr=constraintList.multG(temp_dofs);
			//divide it into |prev_r|^2
			alpha=(prev_r.inner(prev_r))/(p.inner(temp_constr));
			//compute x;
			x.set(p);
			x.scaleVector(alpha);
			x.addVectors(prev_x);
			//finally compute r
			r.set(p);
			temp_dofs=constraintList.multGTrans(r);
			constraintList.multMassMatrixInverse(temp_dofs);
			r=constraintList.multG(temp_dofs);
			r.scaleVector(-alpha);
			r.addVectors(prev_r);	
			if(k>100){
				r.set(new arrayAsVector(r.length()));
			}
		}
		
		
		/**here we return the solution to Ax=b which is just the vector of lagrange multipliers
		 * and we leave it up to the calling method to compute the forces.
		 */
		
		return x;
	}
	


}
