
import java.awt.*;

/**
 * PSystem - DESCRIPTION
 * <p/>
 * An extension to the Processing project - http://Proce55ing.net
 * <p/>
 * Copyright (c) 2004 Simon Greenwold
 * Created: Feb 11, 2004
 * <p/>
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * <p/>
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 * <p/>
 * You should have received a copy of the GNU Lesser General
 * Public License along with the Processing project; if not,
 * write to the Free Software Foundation, Inc., 59 Temple Place,
 * Suite 330, Boston, MA  02111-1307  USA
 */

public class PSystem implements MPConstants, Plugin {


  private static final float DEFAULT_GX = 0.0f;
  private static final float DEFAULT_GY = 0.03f;
  private static final float DEFAULT_GZ = 0.0f;

  private static final float DEFAULT_DRAG = 0.03f;
  private static final float FIXED_MASS = 1000000.0f;
 
  private static final float DEFAULT_SPRING_DAMPING = 0.01f;
  private static final float DEFAULT_SPRING_STRENGTH = 0.005f;
  private static final float DEFAULT_SPRING_REST_LENGTH = 18.0f;

  private static final float DEFAULT_TIME_STEP = 0.5f;

  private static final float DEFAULT_MASS = 1.0f;
  private static final float DEFAULT_COLLISION_RADIUS = 10.0f;
  private static final float DEFAULT_MIN_COLLISION_FORCE = 1.0f;
  private static final float DEFAULT_MAX_COLLISION_FORCE = 2.0f;

  float time;  // accumulated system time

  int nForces;
  int nParticles;
  boolean drawForces = true;
  Particle particles[] = null;
  Force forces[] = null;
  boolean useImplicitEuler = true;

  Surface surfaces[] = null;

  private double zeros[];
  private int    nVectorElements;
  private double x[];
  private double v[];
  private double dv[];
  private double tmpV[];
  private double F0[];
  private double b[];
  private double r[];
  private double d[];
  private double q[];
  private double p[];
  private double s[];
  private double t[];
  private double rtilde[];
  
  float gX, gY, gZ;
  float drag, timeStep;
  float defaultMass;
  float defaultSpringDamping;
  float defaultSpringRestLength;
  float defaultSpringStrength;
  float defaultCollisionRadius;
  float defaultMinCollisionForce;
  float defaultMaxCollisionForce;

  void initGlobals() {
    gX = DEFAULT_GX;
    gY = DEFAULT_GY;
    gZ = DEFAULT_GZ;
    drag = DEFAULT_DRAG;
    timeStep = DEFAULT_TIME_STEP;

    defaultMass = DEFAULT_MASS;
    defaultSpringDamping = DEFAULT_SPRING_DAMPING;
    defaultSpringStrength =  DEFAULT_SPRING_STRENGTH;
    defaultSpringRestLength = DEFAULT_SPRING_REST_LENGTH;
    defaultCollisionRadius = DEFAULT_COLLISION_RADIUS;
    defaultMinCollisionForce = DEFAULT_MIN_COLLISION_FORCE;
    defaultMaxCollisionForce = DEFAULT_MAX_COLLISION_FORCE;
  }

  private void preloop() {
    update();
  }

  private void postloop() {

  }

  public void draw() {
    drawParticles();
    drawForces();
  }

  public void drawParticles() {
    for (int i = 0; i < nParticles; i++) {
      if (particles[i].visible)
        particles[i].draw();
    }
  }

  public void drawForces() {
    if (!drawForces) return;
    for (int i = 0; i < nForces; i++) {
      if (forces[i].visible)
        forces[i].draw();
    }
  }

  public void update() {
    if (nParticles > 0) {
      
      if (!useImplicitEuler) {  // Old-style Runge-Kutta      
	      zeroForces();
	      applyForces();
	      calcNextState();
	      updateState();
	  }
	  else {
  	  	  collectState();
    	  clearForces();
    	  calculateForces();
    	  calculateJacobians();
    	  impEulerStep(timeStep);
    	  scatterState();
	  }
    }

    if (surfaces != null) {
      for (int i = 0; i < surfaces.length; i++) {
        surfaces[i].update();
      }
    }
  }

  void drawSprings() {
    for (int i = 0; i < nForces; i++) {
      if (forces[i] instanceof Spring) {
        ((Spring)forces[i]).draw();
      }
    }
  }

  void addParticle(Particle p) {
    p.pSystem = this;
    if (particles == null) {
      particles = new Particle[START_PARTICLES];
    }

    if (nParticles >= particles.length) {
      Particle[] newParticles = new Particle[nParticles + START_PARTICLES];
      System.arraycopy(particles, 0, newParticles, 0, nParticles);
      particles = newParticles;
    }

    particles[nParticles] = p;
    p.init();
    p.index = nParticles;
//    if (p.collisionForce != null)
//      addForce(p.collisionForce);
    nParticles++;
    resizeTempVectors();
  }
  
  void renumberParticles() {
  	for (int i = 0; i < nParticles; i++) {
  		particles[i].index = i;
  	}
  }
  
  void resizeTempVectors() {
  	nVectorElements = 3*nParticles;
    zeros = new double[nVectorElements];
    x = new double[nVectorElements];
    v = new double[nVectorElements];
    b = new double[nVectorElements];
    tmpV = new double[nVectorElements];
    F0 = new double[nVectorElements];
    dv = new double[nVectorElements];
    r = new double[nVectorElements];
    d = new double[nVectorElements];
    q = new double[nVectorElements];
    p = new double[nVectorElements];
    s = new double[nVectorElements];
    t = new double[nVectorElements];
    rtilde = new double[nVectorElements];
  }

  void removeParticle(int i) {
    System.arraycopy(particles, i + 1, particles, i, (nParticles - i) - 1);
    particles[nParticles] = null;
    nParticles--;
    resizeTempVectors();
    renumberParticles();
  }

  void removeParticle(Particle p) {
    for (int i = 0; i < nParticles; i++) {
      if (particles[i] == p) {
        for (int f = 0; f < nForces; f++) {
          if (forces[f] instanceof Spring) {
            if (((Spring)forces[f]).a == p || ((Spring)forces[f]).b == p) {
              removeForce(f);
            }
          }
/*          else if (forces[f] instanceof Magnet) {
            if (((Magnet)forces[f]).a == p) {
              removeForce(f);
            }
          }
          else if (forces[f] instanceof CollisionForce) {
            if (((CollisionForce)forces[f]).a == p) {
              removeForce(f);
            }
          }
          */
        }
        removeParticle(i);
      }
    }
  }
/*  
  Magnet addMagnet(Particle a) {
    Magnet m = new Magnet(a);
    addForce(m);
    return m;
  }
  
  Magnet addMagnet(Particle a, float strength) {
    Magnet m = new Magnet(a);
    m.strength = strength;
    addForce(m);
    return m;
  }
  */
  Spring addSpring(Particle a, Particle b) {
    Spring s = new Spring(a, b);
    addForce(s);
    return s;
  }
  
  Spring addSpring(Particle a, Particle b, float strength, float damping, float length) {
    Spring s = new Spring(a, b);
    addForce(s);
    s.strength = strength;
    s.restLength = length;
    s.damping = damping;
    return s;
  }
  
  void addForce(Force f) {
    if (forces == null) {
      forces = new Force[START_FORCES];
    }

    if (nForces >= forces.length) {
      Force[] newForces = new Force[nForces + START_FORCES];
      System.arraycopy(forces, 0, newForces, 0, nForces);
      forces = newForces;
    }

    forces[nForces] = f;
    nForces++;
    f.init(this);
  }

  void removeForce(int i) {
    System.arraycopy(forces, i + 1, forces, i, (nForces - i) - 1);
    forces[nForces] = null;
    nForces--;
  }

  void removeForce(Force f) {
    for (int i = 0; i < nForces; i++) {
      if (forces[i] == f) {
        removeForce(i);
      }
    }
  }

  void zeroForces() {
    for (int i = 0; i < nParticles; i++) {
      particles[i].clearForce();
    }
  }

  void applyForces() {
    Force force = null;
    for (int i = 0; i < nForces; i++)  {
      force = forces[i];
      if (force.active)
        force.applyForce();
    }
    Particle p = null;
    for (int i = 0; i < nParticles; i++)  {
      p = particles[i];
      if (! p.fixed()) {
        p.addForce(-drag * p.velocity[0], -drag * p.velocity[1], -drag * p.velocity[2]);
        //System.out.println("Adding GY " + globals[GY]);
        p.addForce(gX * p.mass, gY * p.mass, gZ * p.mass);
      }
    }
  }

  void applyForces(Particle p) {
    if (p.fixed()) return;

    Force force = null;
    for (int i = 0; i < nForces; i++)  {
      force = forces[i];
      if (force.active)
        force.applyForce(p);
    }

    p.addForce(-drag * p.velocity[0], -drag * p.velocity[1], -drag * p.velocity[2]);
    p.addForce(gX, gY, gZ);
  }

  void calcNextState() {
    calcNextState(timeStep);
  }

  void calcNextState(float step) {
    Particle p = null;
    for (int i = 0; i < nParticles; i++)  {
      p = particles[i];
      if (! p.fixed())
        p.calculateNext(step);
    }
  }

  void updateState() {
    Particle p = null;
    for (int i = 0; i < nParticles; i++)  {
      p = particles[i];
      if (! p.fixed())
        p.updateNext();
    }
  }

  public BApplet getBApplet() {
    return bApplet;
  }

  private BApplet bApplet;

  public String getVersion() {
    return "0.2b";
  }

  public int getMinCompatibleProcessingVersion() {
    return 68;
  }

  public String getDescription() {
    return "Particle and spring-mass systems for Processing.";
  }

  public void initialize(BApplet bApplet) {
    this.bApplet = bApplet;

    // This is to prevent a nasty hang that occurs on Windows sometimes.
    // I don't know why it works or if it's the only solution
    // or if it always works.
    if (!bApplet.online()) {
      Frame frame = new Frame();
      frame.setVisible(true);
      frame.setVisible(false);
    }

    bApplet.registerCall(PRE_LOOP_HOOK, new Runnable() {
      public void run() {
        preloop();
      }
    });
    bApplet.registerCall(POST_LOOP_HOOK, new Runnable() {
      public void run() {
        postloop();
      }
    });

    nParticles = 0;
    nForces = 0;
    initGlobals();
  }
  
  public void setGravity(float gY) {
    this.gY = gY;
  }
  
  public void setGravity(float gX, float gY, float gZ) {
    this.gX = gX;
    this.gY = gY;
    this.gZ = gZ;
  }


  private void addSurface(Surface s) {
    s.pSystem = this;
    if (surfaces == null) {
      surfaces = new Surface[1];
    }
    else {
      Surface[] newSurfaces = new Surface[surfaces.length + 1];
      System.arraycopy(surfaces, 0, newSurfaces, 0, surfaces.length);
      surfaces = newSurfaces;
    }

    surfaces[surfaces.length - 1] = s;
  }

  public Surface loadSurface(String surfaceName) {
    Surface s = new Surface(bApplet, this, surfaceName);
    addSurface(s);
    return s;
  }
  
  
  
  
  
  
  
  
  private void clearForces() {
  	for (int i = 0; i < nForces; i++) {
  		((Spring)forces[i]).resetCalcs();
  	}
  	System.arraycopy(zeros, 0, F0, 0, nVectorElements);
  }
  
  private void calculateForces() {
  	for (int i = 0; i < nForces; i++) {
  		((Spring)forces[i]).calculateForce(F0);
  	}
  	int ai = 0;
  	for (int i = 0; i < nParticles; i++) {
  		
  		if (!particles[i].fixed()) F0[ai] += particles[i].mass * gX;
  		ai++;
  		if (!particles[i].fixed()) F0[ai] += particles[i].mass * gY;
  		ai++;
  		if (!particles[i].fixed()) F0[ai] += particles[i].mass * gZ;
  		ai++;
  	}
  }
  
  private void calculateJacobians() {
  	for (int i = 0; i < nForces; i++) {
  		((Spring)forces[i]).calculateJacobians(timeStep);
  	}
  }

  private void collectState() {
  	int allIndex = 0;
  	for (int i = 0; i < nParticles; i++) {
  		if (particles[i].fixed()) {
  			v[allIndex] = x[allIndex] - particles[i].pos[0];
  		}
  		else {
  			v[allIndex] = particles[i].velocity[0];
  		}
  		x[allIndex] = particles[i].pos[0];
  		allIndex++;
  		if (particles[i].fixed()) {
  			v[allIndex] = x[allIndex] - particles[i].pos[1];
  		}
  		else {
  			v[allIndex] = particles[i].velocity[1];
  		}
  		x[allIndex] = particles[i].pos[1];
  		allIndex++;
  		if (particles[i].fixed()) {
  			v[allIndex] = x[allIndex] - particles[i].pos[2];
  		}
  		else {
  			v[allIndex] = particles[i].velocity[2];
  		}
  		x[allIndex] = particles[i].pos[2];
  		allIndex++;
  	}
  }
  
  private void scatterState() {
  	int allIndex = 0;
  	for (int i = 0; i < nParticles; i++) {
  		if (!particles[i].fixed[0]) {
  			particles[i].pos[0] = (float)x[allIndex];
  			particles[i].velocity[0] = (float)v[allIndex];
  		}
  		allIndex++;
  		if (!particles[i].fixed[1]) {
  			particles[i].pos[1] = (float)x[allIndex];
  			particles[i].velocity[1] = (float)v[allIndex];
  		}
  		allIndex++;
  		if (!particles[i].fixed[2]) {
  			particles[i].pos[2] = (float)x[allIndex];
  			particles[i].velocity[2] = (float)v[allIndex];
  		}
  		allIndex++;
  	}
  }


public void impEulerStep(double h) {
	
	//	Vecd b = h*( fxv + h*(fdx*v) );
	multiplyDfDx(v, tmpV);
	for (int i = 0; i < nVectorElements; i++) {
		b[i] = h*(F0[i] + h*tmpV[i]);
	}
	
	conjGrad();
	//BiCGStab();

	for (int i = 0; i < nVectorElements; i++) {
		v[i] += dv[i];
		v[i] *= (1.0 - DEFAULT_DRAG);
		x[i] += h*v[i];
	}		
}

private double dot(double a[], double b[])
{
	double result = 0;
	for (int i = 0; i < a.length; i++) {
		result += a[i] * b[i];
	}
	return result;
}

final int IT_MAX = 100;
final double EPS = 0.00001;

private void conjGrad()
{
	System.arraycopy(zeros, 0, dv, 0, nVectorElements);
	int i = 0;
	multiplyA(dv, r);
	for (int vi = 0; vi < nVectorElements; vi++) {
		r[vi] = b[vi] - r[vi];
	}
	System.arraycopy(r, 0, d, 0, nVectorElements);
	double epsNew = dot(r, r);
	double eps0 = epsNew;
	while (i < IT_MAX && epsNew > EPS*eps0) {
		multiplyA(d, q);
		double alpha = epsNew / dot(d,q);
		for (int vi = 0; vi < nVectorElements; vi++) {
			dv[vi] += alpha * d[vi];
			r[vi] -= alpha * q[vi];
		}
		double epsOld = epsNew;
		epsNew = dot(r, r);
		double beta = epsNew/epsOld;
		for (int vi = 0; vi < nVectorElements; vi++) {
			d[vi] = r[vi] + beta*d[vi];
		}
		i++;
	}
}

void add(double alpha, double x[], double beta, double y[], double z[]) {
	for (int i = 0; i < x.length; i++) {
		z[i] = alpha*x[i] + beta*y[i];
	}
}


double initR = 0;
int maxIter = 100000;
double rtol = 1e-5;
double atol = 1e-50;
double dtol = 1e+5;

boolean converged(int iter, double rv[]) {
	double r = java.lang.Math.sqrt(dot(rv, rv));
	// Store initial residual
	if (iter == 0)
		initR = r;

	// Check for convergence
	if (r < Math.max(rtol * initR, atol))
		return true;

	// Check for divergence
	if (r > dtol * initR)
		return true;  // Not really. Diverged.
	
	if (iter >= maxIter)
		return true;  // Not really. Timed out.
	
	if (Double.isNaN(r))
		return true; // Not really. Diverged.

	// Neither convergence nor divergence
	return false;
}

private boolean BiCGStab()
{
	double phat[] = p;
	double shat[] = s;
	double rho_1 = 1., rho_2 = 1., alpha = 1., beta = 1., omega = 1.;
	boolean converged = false;
	// r = b - A*dv
	multiplyA(dv, q);
	add(1.0, b, -1.0, q, r);
	
	// rtilde = r
	System.arraycopy(r, 0, rtilde, 0, nVectorElements);

	int it = 0;
	//for (iter.setFirst(); !iter.converged(r, dv); iter.next()) {
	while (!converged(it, r)) {
		// rho_1 = rtilde . r
		rho_1 = dot(rtilde, r);
		if (rho_1 == 0.)
			// rho breakdown. Non-convergence
			return false;
		if (omega == 0.)
			// omega breakdown. Non-convergence
			return false;
			

		if (it == 0)
			// p = r
			System.arraycopy(r, 0, p, 0, nVectorElements);
		else {
			beta = (rho_1 / rho_2) * (alpha / omega);
			// temp = -omega * d + p;
			// p = beta * temp + r
			add(-omega, d, 1.0, p, tmpV);
			add(beta, tmpV, 1.0, r, p);
		}
		// Apply preconditioner: M.apply(p, phat);
		// d = A*phat
		multiplyA(phat, d);
		
		// alpha = rho_1 / (rtilde . d)
		alpha = rho_1 / dot(rtilde, d);
		
		// s = -alpha * d + r
		add(-alpha, d, 1.0, r, s);
		
		if (converged(it, s)) {
			// dv += alpha * phat
			add(alpha, phat, 1.0, dv, dv);
			return true;
		}
		// Apply preconditioner: M.apply(s, shat);
		// t = A * shat
		multiplyA(shat, t);
		
		// omega = (t . s) / (t . t)
		omega = dot(t, s) / dot(t, t);
		
		// dv += alpha * phat + omega * shat
		add(alpha, phat, 1.0, dv, dv);
		add(omega, shat, 1.0, dv, dv);
		// r = -omega * t + s
		add(-omega, t, 1.0, s, r);

		rho_2 = rho_1;
		it++;
	}
	return true;
}


private double ds[] = new double[3]; 
private double addJ[] = new double[3]; 

public void multiplyA(double src[], double dst[]) 
{
	int ai = 0;
	int bi;
	for (int i = 0; i < nParticles; i++) {
		if (!particles[i].fixed[0] ) {
			dst[ai] = src[ai] * particles[i].mass;
		}
		else {
			dst[ai] = src[ai] * FIXED_MASS;
		}
		ai++;
		if (!particles[i].fixed[1] ) {
			dst[ai] = src[ai] * particles[i].mass;
		}
		else {
			dst[ai] = src[ai] * FIXED_MASS;
		}
		ai++;
		if (!particles[i].fixed[2] ) {
			dst[ai] = src[ai] * particles[i].mass;
		}
		else {
			dst[ai] = src[ai] * FIXED_MASS;
		}
		ai++;
	}
	
	for (int i = 0; i < nForces; i++) {
		Spring s = (Spring)forces[i];
		ai = s.a.index * 3;
		bi = s.b.index * 3;
		ds[0] = src[ai] - src[bi];
		ds[1] = src[ai+1] - src[bi+1];
		ds[2] = src[ai+2] - src[bi+2];
		
		addJ[0] = s.B[0][0] * ds[0] + s.B[0][1] * ds[1] + s.B[0][2] * ds[2];
		addJ[1] = s.B[1][0] * ds[0] + s.B[1][1] * ds[1] + s.B[1][2] * ds[2];
		addJ[2] = s.B[2][0] * ds[0] + s.B[2][1] * ds[1] + s.B[2][2] * ds[2];
		
		dst[ai]   -= addJ[0];
		dst[ai+1] -= addJ[1];
		dst[ai+2] -= addJ[2];
		
		dst[bi]   += addJ[0];
		dst[bi+1] += addJ[1];
		dst[bi+2] += addJ[2];
	}
}

public void multiplyDfDx(double src[], double dst[])
{
	System.arraycopy(zeros, 0, dst, 0, nVectorElements);  // Zero dest
	int ai, bi;
	for (int i = 0; i < nForces; i++) {
		Spring s = (Spring)forces[i];
		ai = s.a.index * 3;
		bi = s.b.index * 3;
		ds[0] = src[ai] - src[bi];
		ds[1] = src[ai+1] - src[bi+1];
		ds[2] = src[ai+2] - src[bi+2];
		
		addJ[0] = s.Jx[0][0] * ds[0] + s.Jx[0][1] * ds[1] + s.Jx[0][2] * ds[2];
		addJ[1] = s.Jx[1][0] * ds[0] + s.Jx[1][1] * ds[1] + s.Jx[1][2] * ds[2];
		addJ[2] = s.Jx[2][0] * ds[0] + s.Jx[2][1] * ds[1] + s.Jx[2][2] * ds[2];
		
		dst[ai]   -= addJ[0];
		dst[ai+1] -= addJ[1];
		dst[ai+2] -= addJ[2];
		
		dst[bi]   += addJ[0];
		dst[bi+1] += addJ[1];
		dst[bi+2] += addJ[2];
	}
}

public void multiplyDfDv(double src[], double dst[])
{
	System.arraycopy(zeros, 0, dst, 0, nVectorElements);  // Zero dest
	int ai, bi;
	for (int i = 0; i < nForces; i++) {
		Spring s = (Spring)forces[i];
		ai = s.a.index * 3;
		bi = s.b.index * 3;
		ds[0] = src[ai] - src[bi];
		ds[1] = src[ai+1] - src[bi+1];
		ds[2] = src[ai+2] - src[bi+2];
		
		addJ[0] = s.Jv[0][0] * ds[0] + s.Jv[0][1] * ds[1] + s.Jv[0][2] * ds[2];
		addJ[1] = s.Jv[1][0] * ds[0] + s.Jv[1][1] * ds[1] + s.Jv[1][2] * ds[2];
		addJ[2] = s.Jv[2][0] * ds[0] + s.Jv[2][1] * ds[1] + s.Jv[2][2] * ds[2];
		
		dst[ai]   -= addJ[0];
		dst[ai+1] -= addJ[1];
		dst[ai+2] -= addJ[2];
		
		dst[bi]   += addJ[0];
		dst[bi+1] += addJ[1];
		dst[bi+2] += addJ[2];
	}
}

}
