package clothSimulator;


import java.text.DecimalFormat;
import java.util.LinkedList;
import java.util.List;

import no.uib.cipr.matrix.DenseMatrix;
import no.uib.cipr.matrix.DenseVector;
import no.uib.cipr.matrix.sparse.CompRowMatrix;
import no.uib.cipr.matrix.io.*;
import no.uib.cipr.matrix.sparse.*;


/**of course, a constraintList object will have a list of constraints */
public class ConstraintList {
	
	/** It is reasonable to expect a constraints list to know the number of degrees of freedom to which the constraint list refers.
	 * This number is basically the number of particles times 3 (for three dimensions).  It also refers to the number of columns in the
	 * G and Gdot matrices, and also to the number of rows of the Gtranspose matrix.
	 */
	private int numberOfDofs;

	public ConstraintList(){
		//create an empty list
		//this.numberOfDofs=numDofs;
	}
	
	/** of course, a constraintList object will have a list of constraints */
	public List<Constraint> theConstraints= new LinkedList<Constraint>();
	
	/** the constraints list object will be responsible for returning the constraints force vector
	 * To do this, it will have to solve a linear system and so the constraints list needs a linear system solver
	 * 
	 */
	private linearSolver theSolver;
	
	/** but it also has a method which is meant to represent matrix vector multiplication
	 * the constraint list holds all the data which we would expect to find in G and Gdot and g.
	 * So, it makes sense that an object which holds all the data for a matrix G, should also have
	 * methods which represent matrix vector multiplication.  in and out are understood in the sense
	 * of G in = out and while in and out are seen as vectors abstractly, in fact they are passed in just
	 * as double arrays.
	 */
	public arrayAsVector multG(arrayAsVector in){
		/** we loop over constraints in the variable theConstraints
		 * 
		 */
		out=new arrayAsVector(this.theConstraints.size());
		for(int i=0; i<theConstraints.size();i++){
			out.theVector[i]=theConstraints.get(i).GiDot(in.theVector);
			//in.theVector[i]=theConstraints.get(i).GiDot(in.theVector);
		}
		//in.set(out);
		return out;
	}
	
	/** We also have methods that are meant to represent Gtranspose matrix multiplication
	 */
	public arrayAsVector multGTrans(arrayAsVector in){
		out=new arrayAsVector(this.numberOfDofs);
		for(int i=0; i<theConstraints.size();i++){
			theConstraints.get(i).GtransiDot(in.theVector, out.theVector);
			//theConstraints.get(i).GtransiDot(in.theVector, in.theVector);
		}
		return out;		
	}
	
	/** out is what comes out of the Matrix-vector-multiplication methods */
	private arrayAsVector out;
	/**
	 * We also need a method that represents multiplying a vector by Gdot 
	 */
	public arrayAsVector multGdot(arrayAsVector in){
		out=new arrayAsVector(theConstraints.size());
		for(int i=0; i<theConstraints.size();i++){
			out.theVector[i]=theConstraints.get(i).GdotiDot(in.theVector);
		}
		//in.set(out);
		return out;
	}
	
	
	/**here we have a method to multiply by the inverse of the mass matrix.  However, for now we just leave all the masses 
	 * as unit masses and so the method does nothing
	 * 
	 */
	public void multMassMatrixInverse(arrayAsVector theVector){
		//do nothing for now
	}
	
	/** A constraint list object should have a method to update all of the constraints
	 * 
	 */
	
	public void updateConstraints(){
		for(Constraint c: theConstraints){
			c.updateAll();
		}
	}
	

	private double[] temp_b;
	private arrayAsVector b;
	private arrayAsVector lagrangeMults;
	private arrayAsVector constraintForces;
	/** here we have the core of the constraints mechanism.  It is a conjugate gradients solver.
	 * The steps in solving for the constraint forces are simply to create a new solver then compute b
	 * then pass the constraints and b to the solver which will return...what....for now it is going 
	 * to do everything and just return a force vector.  This breaks the abstraction of the linear solver
	 * object, but then considering we are passing it a constraints list and not a matrix, how perfectly could
	 * the solver be an abstraction of a linear solver anyway.
	 * 
	 */
	public double[] computeConstraintForces( arrayAsVector xdot,arrayAsVector F, double BG_spring_const, double BG_damp_const, Boolean applyBaumgarte, Boolean applyPostStep){
		theSolver=new ConjugateGradientSolver();
		compute_b(xdot,F,BG_spring_const,BG_damp_const,  applyBaumgarte);
		//we now call the conjugate gradients and for now it will return a force vector		
		lagrangeMults= theSolver.solveLinearSystem(this, b);
		constraintForces=new arrayAsVector(this.numberOfDofs);
		constraintForces=multGTrans(lagrangeMults);
		return constraintForces.theVector;
	}
	
	
	/**here we are going to compute various quantities using MTJ and compare
	 * 
	 */
	public DenseVector computeWithMTJCompare(DenseVector b){
		//get GMGT and pass that to the conjgrad
		//
		DenseMatrix G, GT,GMGT;
		DenseVector lambda,forces;	
		forces=new DenseVector(numberOfDofs);
		GT=new DenseMatrix(numberOfDofs,theConstraints.size());
		GMGT=new DenseMatrix(theConstraints.size(),theConstraints.size());
		G=this.getG_AsMTJMatrix();
		G.transpose(GT);
		G.mult(GT, GMGT);
		lambda=doConjGrad(GMGT,b);
		GT.mult(lambda,forces);
		compareForces(forces);
		return lambda;
	}
	
	private double[] Force_Diff_FromMTJ;
	/** a method to compute the differences between the forces given by mtj and the clothSim system */
	public void compareForces(DenseVector forces){
		Force_Diff_FromMTJ=new double[numberOfDofs];
		for(int i=0; i<numberOfDofs; i++){
			Force_Diff_FromMTJ[i]= constraintForces.theVector[i]-forces.get(i);			
		}
		
	}
	
	/** a method to handle the conjugate gradient solve */
    public DenseVector doConjGrad(DenseMatrix theTrix, DenseVector b){
    	DenseVector x= new DenseVector(b.size());
       	CompRowMatrix A;
    	A=new CompRowMatrix(theTrix);
    	// Allocate storage for Conjugate Gradients
    	IterativeSolver solver = new CG(x);
    	// Create a Cholesky preconditioner
    	Preconditioner M = new ICC(A.copy());
    	// Set up the preconditioner, and attach it
    	M.setMatrix(A);
    	solver.setPreconditioner(M);
    	// Add a convergence monitor
    	solver.getIterationMonitor().setIterationReporter(new OutputIterationReporter());
    	// Start the solver, and check for problems
    	try {
    	  solver.solve(A, b, x);
    	} catch (IterativeSolverNotConvergedException e) {
    	  System.err.println("Iterative solver failed to converge");
    	}
    	return x;
    }
	
	
	

	private arrayAsVector temp;
	private arrayAsVector temp_in;
	private arrayAsVector temp_out;
	/** this computes the rhs of Ax=b for the constraint forces equation.  namely it computes the value of the
	 * vector equal to
	 * -Gdot*qdot-G*M^-1 *F- ks*g -kd*gdot
	 * 
	 */
	private void compute_b(arrayAsVector xdot, arrayAsVector F,double BG_spring_const,double BG_damp_const, Boolean applyBaumgarte){
		
		b=new arrayAsVector(this.theConstraints.size());
		temp_in=new arrayAsVector(this.numberOfDofs);
		temp_out=new arrayAsVector(this.theConstraints.size());
		//-Gdot*xdot
		temp_in.set(xdot);
		temp_in.scaleVector(-1d);
		temp_out=multGdot(temp_in);
		b.addVectors(temp_out);
		//-G*Minv*F
		temp_in.set(F);
		temp_in.scaleVector(-1d);
		multMassMatrixInverse(temp_in);
		temp_out=multG(temp_in);
		b.addVectors(temp_out);
		if(applyBaumgarte){
			//-ks*g
			the_g_Vector=make_g_Vector();
			temp_out.set(the_g_Vector);
			temp_out.scaleVector(-BG_spring_const);
			b.addVectors(temp_out);
			
			//-kdG*xdot
			temp_in.set(xdot);
			temp_out=multG(temp_in);
			temp_out.scaleVector(-BG_damp_const);
			b.addVectors(temp_out);			
		}

	}
	
	
	private arrayAsVector the_g_Vector,a_g_Vector;
	/** this method just makes the g vector 
	 * 
	 */
	public arrayAsVector make_g_Vector(){
		a_g_Vector=new arrayAsVector(this.theConstraints.size());
		for(int i=0; i<this.theConstraints.size();i++){
			a_g_Vector.theVector[i]=this.theConstraints.get(i).get_g();
		}
		return a_g_Vector;
	}
	/** Since we are treating double arrays as vectors, we need some simple methods to do vector math
	 * with double arrays.  These methods include adding two vectors and scaling a vector
	 */
	private void scaleArray(double[] inVector, double[] outVector, double scalar){
		for(int i=0; i< inVector.length;i++){
			outVector[i]=inVector[i]*scalar;
		}	
	}	
	private void addArrays(double[] inVector1, double[] inVector2, double[] outVector) {

		for(int i=0; i< inVector1.length;i++){
			outVector[i]=inVector1[i]+inVector2[i];
		}
	}
	public void setNumDofs(int numDofs){
		this.numberOfDofs=numDofs;
	}
	public int getNumDofs(){
		return this.numberOfDofs;
	}
	private DecimalFormat df=new DecimalFormat("#.###");
	public void printGmatrix(){
		String text="\n  G: \n \n";
		double[] aRow;
		for(int i=0; i<theConstraints.size();i++){
			aRow=theConstraints.get(i).get_G(numberOfDofs);
			for(int j=0; j<numberOfDofs;j++){
				text=text +"   " + df.format(aRow[j]);	
			}
			text=text+"\n";	
		}
		System.out.println(text);
	}
	public void printGDotmatrix(){
		String text="\n  Gdot: \n \n";
		double[] aRow;
		for(int i=0; i<theConstraints.size();i++){
			aRow=theConstraints.get(i).get_GDot(numberOfDofs);
			for(int j=0; j<numberOfDofs;j++){
				text=text +"   "+ df.format(aRow[j]);	
			}
			text=text+"\n";
		}
		System.out.println(text);
	}
	public void print_g(){
		String text="\n  g: \n \n";
		for(int i=0; i<theConstraints.size();i++){
			text=text+"\n " + theConstraints.get(i).get_g();
		}
		System.out.println(text);
	}
	public void print_b(){
		String text="\n  b: \n \n";
		for(int i=0; i<theConstraints.size();i++){
			text=text+"\n " + b.theVector[i];
		}
		System.out.println(text);
	}
	
	/** we are going to test the system rigorously by comparing calculations in this cloth sim
	 * code to those performed by the mtj matrix package.  Here we have a method which will spit out
	 * the G matrix as an mtj DenseMatrix
	 * 
	 */
	public DenseMatrix getG_AsMTJMatrix(){
		DenseMatrix A= new DenseMatrix(theConstraints.size(),numberOfDofs);
		double[] Grow;
		//i are the rows
		for(int i=0; i<theConstraints.size();i++){
			Grow=theConstraints.get(i).get_G(numberOfDofs);
			for(int j=0; j< numberOfDofs; j++){
				A.set(i, j, Grow[j]);
			}
		}
		return A;
	}
	/** Further in the vein of comparing to mtj computations, we also want to get Gdot as an mtj matrix
	 * 
	 */	
	public DenseMatrix getGdot_AsMTJMatrix(){
			DenseMatrix A= new DenseMatrix(0,0);
			double[] Grow;
			//i are the columns
			for(int i=0; i<numberOfDofs;i++){
				Grow=theConstraints.get(i).get_GDot(numberOfDofs);
				for(int j=0; j< numberOfDofs; j++){
					A.set(i, j, Grow[j]);
				}
			}
			return A;
		}


	Constraint commonConstr;
	/** this method can take two particles and return the constraint to which these particles refer
	 * 
	 */
	public Constraint getConstForParticles(Particle p1, Particle p2){
		
		
		return commonConstr;
	}
}
