import java.applet.*; 
import java.awt.*; 
import java.awt.image.*; 
import java.awt.event.*; 
import java.io.*; 
import java.net.*; 
import java.text.*; 
import java.util.*; 
import java.util.zip.*; 

public class threestringsdist extends BApplet {
int pcount = 30;
ParticleSystem system;
Particle p[] = new Particle[100];
Spring s[] = new Spring[99];
Particle p2[] = new Particle[100];
Spring s2[] = new Spring[99];
Particle p3[] = new Particle[100];
Spring s3[] = new Spring[99];
Weight w;

void setup() {
  size(800, 800);
  background(0);
  colorMode(RGB, 1.0f);

  system = new ParticleSystem();
  // create forces and particles here
  for (int i = 0; i < pcount; i++) {
    p[i] = new Particle();
    p[i].setPosition(i*20+50, 100, 0);
    system.addParticle(p[i]);
    

    if (i != 0) {
      s[i-1] = new Spring(p[i-1], p[i],0.5f);
      s[i-1].setRestLength(20.8f);
      s[i-1].setStrength(1.0f);
      s[i-1].setDamping(0.00f);
      //s[i- ].a   p[i- ] 
      //s[i- ].b   p[i] 
      system.addForce(s[i-1]);
      
    }
  }
  
  for (int i = 0; i < pcount; i++) {
    p2[i] = new Particle();
    p2[i].setPosition(300+i*4, 100, i*20+90);
    system.addParticle(p2[i]);
    
    if (i != 0) {
      if (i==1) {
        s2[i-1] = new Spring(p[15],p2[i], 0.5f);
        s2[i-1].setRestLength(20.8f);
        s2[i-1].setStrength(0.80f);
        s2[i-1].setDamping(0.3f);
        system.addForce(s2[i-1]);
      }
      else {
        s2[i-1] = new Spring(p2[i-1],p2[i],0.5f);
        s2[i-1].setRestLength(20.8f);
        s2[i-1].setStrength(0.80f);
        s2[i-1].setDamping(0.3f);
        system.addForce(s2[i-1]);
      }
    }
  }
  
  s2[0].a = p[15]; 
  
  for (int i = 0; i < pcount; i++) {
    p3[i] = new Particle();
    p3[i].setPosition(320+i*-4, 100, i*20+90);
    system.addParticle(p3[i]);
    
    if (i != 0) {
      if (i==1) {
        s3[i-1] = new Spring(p[12],p3[i], 0.5f);
        s3[i-1].setRestLength(20.8f);
        s3[i-1].setStrength(0.80f);
        s3[i-1].setDamping(0.3f);
        system.addForce(s3[i-1]);
      }
      else {
        s3[i-1] = new Spring(p3[i-1],p3[i],0.5f);
        s3[i-1].setRestLength(20.8f);
        s3[i-1].setStrength(0.80f);
        s3[i-1].setDamping(0.3f);
        system.addForce(s3[i-1]);
      }
    }
  }
  s3[0].a = p[12]; 
  
  p[0].fixed = true;
  p[pcount-1].fixed = true;
  p2[pcount-1].fixed = true;
  p3[pcount-1].fixed = true;
  
  // mess with this
  system.gy = 1;
}

void loop() {
  // loop over p
  fill(0.4f, 0.6f, 0.0f);
  //ellipse(   ,   ,   ,   ) 
  for (int i = 0; i < pcount; i++) {
    
    ellipse(p[i].x, p[i].y, 4, 4);
    if (i!=0) {
        ellipse(p2[i].x, p2[i].y, 4, 4);
        ellipse(p3[i].x, p3[i].y, 4, 4);
        }
    
    
    if (i != 0) {
      float stress = sqrt(s[i-1].Fx*s[i-1].Fx + s[i-1].Fy*s[i-1].Fy + s[i-1].Fz*s[i-1].Fz);
      stroke(stress/40.0f, stress/40.0f, stress/40.0f);
      line(p[i-1].x+2, p[i-1].y+2,p[i].x+2, p[i].y+2);
      
      float stress2 = sqrt(s2[i-1].Fx*s2[i-1].Fx + s2[i-1].Fy*s2[i-1].Fy + s2[i-1].Fz*s2[i-1].Fz);
      stroke(stress2/40.0f, stress2/40.0f, stress2/40.0f);
      if (i==1) {
        line(p[15].x+2, p[15].y+2,p2[i].x+2, p2[i].y+2);
      }
      else {line(p2[i-1].x+2, p2[i-1].y+2,p2[i].x+2, p2[i].y+2);}
      
      float stress3 = sqrt(s3[i-1].Fx*s3[i-1].Fx + s3[i-1].Fy*s3[i-1].Fy + s3[i-1].Fz*s3[i-1].Fz);
      stroke(stress3/40.0f, stress3/40.0f, stress3/40.0f);
      if (i==1) {
        line(p[12].x+2, p[12].y+2,p3[i].x+2, p3[i].y+2);
      }
      else {line(p3[i-1].x+2, p3[i-1].y+2,p3[i].x+2, p3[i].y+2);}
    }
  }
  system.calc(1f);
  system.update();
}



//******************************

/**
* You can implement a spring this way 
* Let the "distance force" Fm be computed by
*
*      Fmx   km * (Ax - Bx)
*      Fmy   km * (Ay - By)
*      Fmz   km * (Az - Bz)
*
* with (km    ). In order for you simulation not to get out of
* control, you'll want to add a damping force Fd of the form
*
*      Fdx   -kd * Vx
*      Fdy   -kd * Vy
*      Fdz   -kd * Vz
*
* where (kd    ), so that the total force of F is
*
*      F   Fm + Fd
*/

class Spring extends Force {
  Particle a, b;
  float Fx, Fy, Fz;
  
  float r = 1;  // Rest length
  float km = 0.005f; // Spring constant
  float kd = 0.01f; // Damping constant

  //Spring()  
    //km    .   f   // current mouse damping
    //kd    .  f    // damping

    //r     . f     // rest length of  
  // 

  /*
  Spring(float restLength, float springConstant, float dampingConstant)  
    r   restLength 
    km   springConstant 
    kd   dampingConstant 
   
  */

  Spring(Particle a, Particle b) {
    this.a = a;
    this.b = b;
  }

  Spring(Particle a, Particle b, float r) {
    this.a = a;
    this.b = b;
    this.r = r;
  }

  void setRestLength(float r) {
    this.r = r;
  }

  void setStrength(float km) {
    this.km = km;
  }

  void setDamping(float kd) {
    this.kd = kd;
  }

  void applyForce() {
    //float dist   distance(a.pos, b.pos) 
    //float dist   (float) Math.sqrt((a.x - b.x) * (a.x - b.x) +
    //			   (a.y - b.y) * (a.y - b.y) +
    //			   (a.z - b.z) * (a.z - b.z)) 

    float Lx = a.x - b.x;
    float Ly = a.y - b.y;
    float Lz = a.z - b.z;
    float dist = (float) Math.sqrt(Lx*Lx + Ly*Ly + Lz*Lz);
    if (dist == 0) dist = 0.000001f;

     Fx = - (km * (dist - r) +
    kd * (a.vx - b.vx) * Lx / dist) * Lx / dist;
     Fy = - (km * (dist - r) +
    kd * (a.vy - b.vy) * Ly / dist) * Ly / dist;
     Fz = - (km * (dist - r) +
    kd * (a.vz - b.vz) * Lz / dist) * Lz / dist;

    a.addForce( Fx,  Fy,  Fz);
    b.addForce(-Fx, -Fy, -Fz);
  }

  void applyForce(Particle p) {
    if (a != p && b != p) return;

    //float dist   distance(a.pos, b.pos) 
    //float dist   (float) Math.sqrt((a.x - b.x) * (a.x - b.x) +
    //			   (a.y - b.y) * (a.y - b.y) +
    //			   (a.z - b.z) * (a.z - b.z)) 

    float Lx = a.x - b.x;
    float Ly = a.y - b.y;
    float Lz = a.z - b.z;
    float dist = (float) Math.sqrt(Lx*Lx + Ly*Ly + Lz*Lz);
    if (dist == 0) dist = 0.000001f;

     Fx = - (km * (dist - r) +
    kd * (a.vx - b.vx) * Lx / dist) * Lx / dist;
     Fy = - (km * (dist - r) +
    kd * (a.vy - b.vy) * Ly / dist) * Ly / dist;
     Fz = - (km * (dist - r) +
    kd * (a.vz - b.vz) * Lz / dist) * Lz / dist;
    //System.out.println("f is " + Fx + " " + Fy + " " + Fz) 

    if (a == p) {
      a.addForce( Fx,  Fy,  Fz);
    } else {
      b.addForce(-Fx, -Fy, -Fz);
    }
  }
}

//********************************* weight

class Weight extends Force {
  Particle a;
  Particle w;
  float Fx, Fy, Fz;
  float mass;
  
    
  float r = 1;  // Rest length
  float km = 0.005f; // Spring constant
  float kd = 0.01f; // Damping constant

  //Spring()  
    //km    .   f   // current mouse damping
    //kd    .  f    // damping

    //r     . f     // rest length of  
  // 

 

  Weight(Particle a, float mass) {
    this.a = a;
    
    this.mass = mass;
  }

  void setPosition(float x,float y,float z) {
    w.setPosition(x,y,z);
    w.mass = this.mass;
  }

  void setRestLength(float r) {
    this.r = r;
  }

  void setStrength(float km) {
    this.km = km;
  }

  void setDamping(float kd) {
    this.kd = kd;
  }

  void setMass(float mass) {
    w.mass = mass;
  }

  void applyForce() {
    //float dist   distance(a.pos, b.pos) 
    //float dist   (float) Math.sqrt((a.x - b.x) * (a.x - b.x) +
    //			   (a.y - b.y) * (a.y - b.y) +
    //			   (a.z - b.z) * (a.z - b.z)) 

    float Lx = a.x - w.x;
    float Ly = a.y - w.y;
    float Lz = a.z - w.z;
    float dist = (float) Math.sqrt(Lx*Lx + Ly*Ly + Lz*Lz);
    if (dist == 0) dist = 0.000001f;

     Fx = - (km * (dist - r) +
    kd * (a.vx - w.vx) * Lx / dist) * Lx / dist;
     Fy = - (km * (dist - r) +
    kd * (a.vy - w.vy) * Ly / dist) * Ly / dist;
     Fz = - (km * (dist - r) +
    kd * (a.vz - w.vz) * Lz / dist) * Lz / dist;

    a.addForce( Fx,  Fy,  Fz);
    w.addForce(-Fx, -Fy, -Fz);
  }

  void applyForce(Particle p) {
    if (a != p && w != p) return;

    //float dist   distance(a.pos, b.pos) 
    //float dist   (float) Math.sqrt((a.x - b.x) * (a.x - b.x) +
    //			   (a.y - b.y) * (a.y - b.y) +
    //			   (a.z - b.z) * (a.z - b.z)) 

    float Lx = a.x - w.x;
    float Ly = a.y - w.y;
    float Lz = a.z - w.z;
    float dist = (float) Math.sqrt(Lx*Lx + Ly*Ly + Lz*Lz);
    if (dist == 0) dist = 0.000001f;

     Fx = - (km * (dist - r) +
    kd * (a.vx - w.vx) * Lx / dist) * Lx / dist;
     Fy = - (km * (dist - r) +
    kd * (a.vy - w.vy) * Ly / dist) * Ly / dist;
     Fz = - (km * (dist - r) +
    kd * (a.vz - w.vz) * Lz / dist) * Lz / dist;
    //System.out.println("f is " + Fx + " " + Fy + " " + Fz) 

    if (a == p) {
      a.addForce( Fx,  Fy,  Fz);
    } else {
      w.addForce(-Fx, -Fy, -Fz);
    }
  }
}



//*******************************

// TODO figure out what typeForce is inside applyForces

class ParticleSystem {
  boolean useBuckets = false;
  static final int X_BUCKETS = 100;
  static final int Y_BUCKETS = 100;
  static final int Z_BUCKETS = 100;
  static final int BUCKET_SIZE = 20;

  static final int X_START = -1000;
  static final int Y_START = -1000;
  static final int Z_START = -1000;

  static final int X_MAX = (X_START + BUCKET_SIZE * X_BUCKETS);
  static final int Y_MAX = (Y_START + BUCKET_SIZE * Y_BUCKETS);
  static final int Z_MAX = (Z_START + BUCKET_SIZE * Z_BUCKETS);

  float gx; // gravity
  float gy;
  float gz;

  float kDrag; // general viscous drag
  ParticleEntry particles;
  ParticleBucket buckets[][][] =
  new ParticleBucket[X_BUCKETS][Y_BUCKETS][Z_BUCKETS];

  ForceEntry forces;

  ParticleSystem() {
    //particles   null 
    //forces   null 

    //gx    . f 
    //gy    . f 
    //gz    . f 

    gx = 0;
    gy = 0;
    gz = 0;

    kDrag = 0.4f; // .  f  // general viscous drag

    if (useBuckets) {
      for (int x = 0; x < X_BUCKETS; x++) {
        for (int y = 0; y < Y_BUCKETS; y++) {
          for (int z = 0; z < Z_BUCKETS; z++) {
            buckets[x][y][z] =
            new ParticleBucket(x, y, z,
            X_START + x * BUCKET_SIZE,
            X_START + (x+1) * BUCKET_SIZE,
            Y_START + y * BUCKET_SIZE,
            Y_START + (y+1) * BUCKET_SIZE,
            Z_START + z * BUCKET_SIZE,
            Z_START + (z+1) * BUCKET_SIZE);
          }
        }
      }
    }
  }

  // called from Particle. init 
  ParticleBucket findBucket(Particle p) {
    int x = (int) ((p.x - X_START) / BUCKET_SIZE);
    if (x < 0 || x >= X_BUCKETS) return null;

    int y = (int) ((p.y - Y_START) / BUCKET_SIZE);
    if (y < 0 || y >= Y_BUCKETS) return null;

    int z = (int) ((p.z - Z_START) / BUCKET_SIZE);
    if (z < 0 || z >= Z_BUCKETS) return null;

    return buckets[x][y][z];
  }

  void addParticle(Particle p) {
    ParticleEntry newbie = new ParticleEntry();
    newbie.p = p;
    p.system = this;
    newbie.next = particles;
    particles = newbie;
    // This forces the addition of the particle to the space bucket
    p.setPosition(p.x, p.y, p.z);
  }

  void removeParticle(Particle p) {
    if (p.bucket != null) {
      p.bucket.remove(p);
    }

    ParticleEntry prev = particles;
    if (prev == null) return;

    if (prev.p == p) {
      particles = prev.next;
      //delete prev 
      return;
    }

    ParticleEntry cur = prev.next;
    while (cur != null) {
      if (cur.p == p) {
        prev.next = cur.next;
        //delete cur 
        return;
      }
      prev = cur;
      cur = cur.next;
    }
  }

  void addForce(Force f) {
    ForceEntry newbie = new ForceEntry();
    newbie.f = f;
    newbie.next = forces;
    forces = newbie;
  }

  void removeForce(Force f) {
    ForceEntry prev = forces;
    if (prev == null) return;

    if (prev.f == f) {
      forces = prev.next;
      //delete prev 
      return;
    }

    ForceEntry cur = prev.next;
    while (cur != null) {
      if (cur.f == f) {
        prev.next = cur.next;
        //delete cur 
        return;
      }
      prev = cur;
      cur = cur.next;
    }
  }

  /*
  void zeroForces()  
    for (ParticleEntry entry   particles  entry !  null  entry   entry.next)  
      entry.p.clearForce() 
     
   
  */

  /* // doesn't appear to be in use
  void applyForces()  
    for (ForceEntry entry   forces  entry !  null  entry   entry.next)  
      if (entry.f.enabled)  
        entry.f.applyForce() 
       
     
    for (ParticleEntry entry   particles  entry !  null  entry   entry.next)  
      Particle p   entry.p 
      if (!p.fixed)  
        p.addForce(-kDrag * p.vx, -kDrag * p.vy, -kDrag * p.vz) 
        p.addForce(gx, gy, gz) 
       
     
   
  */

  // called by Particle for runge-kutta
  void applyForces(Particle p) {
    if (p.fixed) return;

    for (ForceEntry entry = forces; entry != null; entry = entry.next) {
      if (entry.f.enabled) entry.f.applyForce(p);
    }

    p.addForce(-kDrag * p.vx, -kDrag * p.vy, -kDrag * p.vz);
    p.addForce(gx, gy, gz);
  }

  void calc(float step) {
    for (ParticleEntry entry = particles; entry != null; entry = entry.next) {
      if (!entry.p.fixed) entry.p.calc(step);
    }
  }

  void update() {
    for (ParticleEntry entry = particles; entry != null; entry = entry.next) {
      if (!entry.p.fixed) entry.p.update();
    }
  }
}

//*******************************

class ParticleEntry {
  Particle p;
  ParticleEntry next;
}

//*******************************

class ParticleBucket {
  float x1, x2, y1, y2, z1, z2;
  int x, y, z;
  ParticleEntry particles;

  ParticleBucket(int x, int y, int z,
  float x1, float x2,
  float y1, float y2,
  float z1, float z2) {
    particles = null;

    this.x = x;
    this.y = y;
    this.z = z;

    this.x1 = x1;
    this.x2 = x2;
    this.y1 = y1;
    this.y2 = y2;
    this.z1 = z1;
    this.z2 = z2;
  }

  boolean contains(Particle p) {
    if ((p.x > x2) || (p.x < x1) ||
    (p.y > y2) || (p.y < y1) ||
    (p.z > z2) || (p.z < z1)) return false;

    return true;
  }

  void add(Particle p) {
    ParticleEntry nextp = particles;
    while (nextp != null) {
      if (nextp.p == p) return;
      nextp = nextp.next;
    }
    nextp = new ParticleEntry();
    nextp.p = p;
    nextp.next = particles;
    particles = nextp;
  }

  void remove(Particle p) {
    ParticleEntry currentp = particles;
    ParticleEntry previousp = null;

    boolean found = false;
    while (currentp != null) {
      if (currentp.p == p) {
        found = true;
        break;
      }
      previousp = currentp;
      currentp = currentp.next;
    }
    if (!found) return;

    if (previousp != null) {
      particles = currentp.next;
    } else {
      previousp.next = currentp.next;
    }
  }
}

//***********************

class Particle {
  ParticleSystem system;
  boolean fixed = false;

  float mass = 1;
  float x, y, z;
  float vx, vy, vz;
  float fx, fy, fz;

  float calcx, calcy, calcz;
  float calcvx, calcvy, calcvz;

  ParticleBucket bucket;
  Object data;	// any class that is to be associated with this particle

  void setPosition(float x, float y, float z) {
    this.x = x;
    this.y = y;
    this.z = z;

    if (system != null) {
      x = Math.max(Math.min(x, ParticleSystem.X_MAX - 1),
      ParticleSystem.X_START);
      y = Math.max(Math.min(y, ParticleSystem.Y_MAX - 1),
      ParticleSystem.Y_START);
      z = Math.max(Math.min(z, ParticleSystem.Z_MAX - 1),
      ParticleSystem.Z_START);

      if (system.useBuckets) {
        ParticleBucket newBucket = system.findBucket(this);
        if (newBucket != bucket) {
          if (bucket != null) bucket.remove(this);
          if (newBucket != null) newBucket.add(this);
          bucket = newBucket;
        }
      }
    }
  }

  void setVelocity(float dx, float dy, float dz) {
    vx = dx;
    vy = dy;
    vz = dz;
  }

  void clearForce() {
    fx = 0;
    fy = 0;
    fz = 0;
  }

  void setForce(float fx, float fy, float fz) {
    this.fx = fx;
    this.fy = fy;
    this.fz = fz;
  }

  void addForce(float fx, float fy, float fz) {
    this.fx += fx;
    this.fy += fy;
    this.fz += fz;
  }

  void calc(float step) {
    // EULER
    /*
    // would need to clearForce and apply forces here too
    calcx   x + step * vx 
    calcy   y + step * vy 
    calcz   z + step * vz 
    calcvx   vx + step * fx 
    calcvy   vy + step * fy 
    calcvz   vz + step * fz 
    */

    float savedvx = vx;
    float savedvy = vy;
    float savedvz = vz;

    // why wasn't this here in simon's code 
    clearForce();
    system.applyForces(this);

    //if (fx !   ) System.out.println(fx) 

    float k1x = step * fx / mass;
    float k1y = step * fy / mass;
    float k1z = step * fz / mass;

    vx += k1x / 2f;
    vy += k1y / 2f;
    vz += k1z / 2f;

    clearForce();
    system.applyForces(this);

    float k2x = step * fx / mass;
    float k2y = step * fy / mass;
    float k2z = step * fz / mass;

    vx = savedvx + k2x / 2f;
    vy = savedvy + k2y / 2f;
    vz = savedvz + k2z / 2f;

    clearForce();
    system.applyForces(this);

    float k3x = step * fx / mass;
    float k3y = step * fy / mass;
    float k3z = step * fz / mass;

    vx = savedvx + k3x;
    vy = savedvy + k3y;
    vz = savedvz + k3z;

    clearForce();
    system.applyForces(this);

    float k4x = step * fx / mass;
    float k4y = step * fy / mass;
    float k4z = step * fz / mass;

    vx = savedvx;
    vy = savedvy;
    vz = savedvz;

    calcvx = vx + (k1x + 2*k2x + 2*k3x + k4x) / 6f;
    calcvy = vy + (k1y + 2*k2y + 2*k3y + k4y) / 6f;
    calcvz = vz + (k1z + 2*k2z + 2*k3z + k4z) / 6f;
    //System.out.println("calcvx   " + calcvx) 

    calcx = x + step * calcvx;
    calcy = y + step * calcvy;
    calcz = z + step * calcvz;
  }

  void update() {
    //if (x !  calcx)
    //System.out.println("moving " + x + " to " + calcx) 

    setPosition(calcx, calcy, calcz);
    vx = calcvx;
    vy = calcvy;
    vz = calcvz;
  }

  void trace() {
    //printf("Pos  %f, %f, %f\n", x, y, z) 
    //printf("Vel  %f, %f, %f\n", vx, vy, vz) 
  }
}

//*****************************

class ForceEntry {
  Force f;
  ForceEntry next;
}

abstract class Force {
  boolean enabled;

  Force() {
    enabled = true;
  }

  abstract void applyForce();

  abstract void applyForce(Particle p);
}

}