class Body {
  /*
     O
    /|\
   / | \
    / \
   |   |
  */
  // each pointmass will be a joint to the body.
  PointMass head;
  PointMass shoulder;
  PointMass elbowLeft;
  PointMass elbowRight;
  PointMass handLeft;
  PointMass handRight;
  PointMass pelvis;
  PointMass kneeLeft;
  PointMass kneeRight;
  PointMass footLeft;
  PointMass footRight;
  Circle headCircle;
  
  float headLength;
  Body (PVector position, float bodyHeight) {
    headLength = bodyHeight / 7.5;

    // PointMasses
    // Here, they're initialized with random positions. 
    head = new PointMass(new PVector(position.x + random(-5,5),position.y + random(-5,5)));
    shoulder = new PointMass(new PVector(position.x + random(-5,5),position.y + random(-5,5)));
    elbowLeft = new PointMass(new PVector(position.x + random(-5,5),position.y + random(-5,5)));
    elbowRight = new PointMass(new PVector(position.x + random(-5,5),position.y + random(-5,5)));
    handLeft = new PointMass(new PVector(position.x + random(-5,5),position.y + random(-5,5)));
    handRight = new PointMass(new PVector(position.x + random(-5,5),position.y + random(-5,5)));
    pelvis = new PointMass(new PVector(position.x + random(-5,5),position.y + random(-5,5)));
    kneeLeft = new PointMass(new PVector(position.x + random(-5,5),position.y + random(-5,5)));
    kneeRight = new PointMass(new PVector(position.x + random(-5,5),position.y + random(-5,5)));
    footLeft = new PointMass(new PVector(position.x + random(-5,5),position.y + random(-5,5)));
    footRight = new PointMass(new PVector(position.x + random(-5,5),position.y + random(-5,5)));
    
    // Masses
    // Uses data from http://www.humanics-es.com/ADA304353.pdf
    head.mass = 4;
    shoulder.mass = 26; // shoulder to torso
    elbowLeft.mass = 2; // upper arm mass
    elbowRight.mass = 2; 
    handLeft.mass = 2;
    handRight.mass = 2;
    pelvis.mass = 15; // pelvis to lower torso
    kneeLeft.mass = 10;
    kneeRight.mass = 10;
    footLeft.mass = 5; // calf + foot
    footRight.mass = 5;
    
    // Limbs
    // PointMasses are attached to each other here.
    // Proportions are mainly used from http://www.idrawdigital.com/2009/01/tutorial-anatomy-and-proportion/
    head.attachTo(shoulder, 5/4 * headLength, 1, true);
    elbowLeft.attachTo(shoulder, headLength*3/2, 1, true);
    elbowRight.attachTo(shoulder, headLength*3/2, 1, true);
    handLeft.attachTo(elbowLeft, headLength*2, 1, true);
    handRight.attachTo(elbowRight, headLength*2, 1, true);
    pelvis.attachTo(shoulder,headLength*3.5,0.8,true);
    kneeLeft.attachTo(pelvis, headLength*2, 1, true);
    kneeRight.attachTo(pelvis, headLength*2, 1, true);
    footLeft.attachTo(kneeLeft, headLength*2, 1, true);
    footRight.attachTo(kneeRight, headLength*2, 1, true);
    
    // Head
    headCircle = new Circle(head.position, headLength*0.75);
    headCircle.attachToPointMass(head);
    
    // Invisible Constraints. These add resistance to some limbs from pointing in odd directions.
    // this keeps the head from tilting in extremely uncomfortable positions
    pelvis.attachTo(head, headLength*4.75, 0.02, false);
    // these constraints resist flexing the legs too far up towards the body
    footLeft.attachTo(shoulder, headLength*7.5, 0.001, false);
    footRight.attachTo(shoulder, headLength*7.5, 0.001, false);
    
    // The PointMasses (and circle!) is added to the world
    world.addCircle(headCircle);
    world.addPointMass(head);
    world.addPointMass(shoulder);
    world.addPointMass(pelvis);
    world.addPointMass(elbowLeft);
    world.addPointMass(elbowRight);
    world.addPointMass(handLeft);
    world.addPointMass(handRight);
    world.addPointMass(kneeLeft);
    world.addPointMass(kneeRight);
    world.addPointMass(footLeft);
    world.addPointMass(footRight);
  }
  // This must be used if the body is ever deleted
  void removeFromWorld () {
    world.removeCircle(headCircle);
    world.removePointMass(head);
    world.removePointMass(shoulder);
    world.removePointMass(pelvis);
    world.removePointMass(elbowLeft);
    world.removePointMass(elbowRight);
    world.removePointMass(handLeft);
    world.removePointMass(handRight);
    world.removePointMass(kneeLeft);
    world.removePointMass(kneeRight);
    world.removePointMass(footLeft);
    world.removePointMass(footRight);
  }
}

// Could be called "Head" if we wanted, since it's basically all it's used for.
class Circle {
  PVector position;
  float radius;
  
  // Most of the physics is done in the PointMass the Circle is attached to.
  boolean attachedToPointMass = false;
  PointMass attachedPointMass;
  
  Circle (PVector pos, float r) {
    position = pos.get();
    radius = r;
  }
  void solveConstraints () {
    // First move the circle to where its attached PointMass is.
    position = attachedPointMass.position.get();
    
    // Make sure it isn't outside of the screen
    if (position.y < radius)
      position.y = 2*(radius) - position.y;
    if (position.y > height-radius)
      position.y = 2 * (height - radius) - position.y;
    if (position.x > width-radius)
      position.x = 2 * (width - radius) - position.x;
    if (position.x < radius)
      position.x = 2*radius - position.x;
      
    // Move the PointMass to the corrected position.
    attachedPointMass.position = position.get();
  }
  void draw () {
    ellipse(position.x, position.y, radius*2, radius*2);
  }
  void attachToPointMass (PointMass p) {
    attachedPointMass = p;
  }
}

// The EnvCircle
// These are the static circles floating around that the bodies collide with.
class EnvCircle {
  PVector position;
  float radius;
  float radiusSquared;
  
  EnvCircle (PVector pos, float rad) {
    position = pos.get();
    radius = rad;
    radiusSquared = radius*radius;
  }
  
  // detect whether or not a point is colliding with circle, and correct it
  // The algorithm here is modified ball collision handling algorithm, which I wrote about here:
  // www.bluethen.com/wordpress/index.php/processing-app/do-you-like-balls/
  void solveCollision(PointMass pointM) {
    // first we see if the point is inside the circle.
    PVector delta = PVector.sub(pointM.position, position);  
    if (radiusSquared > (sq(delta.x) + sq(delta.y))) {
      float d = sqrt(delta.x * delta.x + delta.y * delta.y);
      
      // Instead of moving the point to the edge of the circle, 
      // we move it outside of the circle depending on how far inside it got.
      // This allows for a proper "bounce" to any collision with the circle.
      float difference = (radius - d) / d;
      pointM.position.add(PVector.mult(delta, difference));
      
      // We allow 3 frames for the bodies to position themselves in empty space before velocities are accounted for.
      if (frameCount < 3)
        pointM.lastPosition.set(pointM.position);
    }
  }
  void draw () {
    ellipse(position.x, position.y, radius * 2, radius * 2);  
  }
}

// The Link class is used for handling constraints between particles.
class Link {
  float restingDistance;
  float stiffness;
  
  PointMass p1;
  PointMass p2;
  
  // the scalars are how much "tug" the particles have on each other
  // this takes into account masses and stiffness, and are set in the Link constructor
  float scalarP1;
  float scalarP2;
  
  // if you want this link to be invisible, set this to false
  boolean drawThis;
  
  Link (PointMass which1, PointMass which2, float restingDist, float stiff, boolean drawMe) {
    p1 = which1; // when you set one object to another, it's pretty much a reference.
    p2 = which2; // Anything that'll happen to p1 or p2 in here will happen to the paticles in our array
    
    restingDistance = restingDist;
    stiffness = stiff;
    
    // Masses are accounted for. If you remember the cloth simulator,
    // http://www.openprocessing.org/visuals/?visualID=20140
    // we added this ability in anticipation of future applets that might use mass
    float im1 = 1 / p1.mass; 
    float im2 = 1 / p2.mass;
    scalarP1 = (im1 / (im1 + im2)) * stiffness;
    scalarP2 = (im2 / (im1 + im2)) * stiffness;
    
    drawThis = drawMe;
  }
  
  void constraintSolve () {
    // calculate the distance between the two particles
    PVector delta = PVector.sub(p1.position, p2.position);  
    float d = sqrt(delta.x * delta.x + delta.y * delta.y);
    float difference = (restingDistance - d) / d;
    
    // Uncomment this if you want the ragdolls to be able to tear. 
    //if (d > 30) 
    //  p1.removeLink(this);

    // P1.position += delta * scalarP1 * difference
    // P2.position -= delta * scalarP2 * difference
    p1.position.add(PVector.mult(delta, scalarP1 * difference));
    p2.position.sub(PVector.mult(delta, scalarP2 * difference));
  }
}

// PointMass
// This is pretty much the Particle class used in Curtain
// http://www.openprocessing.org/visuals/?visualID=20140
class PointMass {
  PVector lastPosition; // for calculating position change (velocity)
  PVector position;

  PVector acceleration; 

  float mass = 1;
  float damping = 20; // friction

  // An ArrayList for links, so we can have as many links as we want to this PointMass
  ArrayList links = new ArrayList();

  boolean pinned = false;
  PVector pinLocation = new PVector(0,0);

  // PointMass constructor
  PointMass (PVector pos) {
    position = pos.get();
    lastPosition = pos.get();
    acceleration = new PVector(0,0);
  }
  
  // The update function is used to update the physics of the particle.
  // motion is applied, and links are drawn here
  void updatePhysics (float timeStep) { 
    // gravity:
    // f(gravity) = m * g
    PVector fg = new PVector(0, mass * world.gravity, 0);
    this.applyForce(fg);

    /*
       We use Verlet Integration to simulate the physics
       In Verlet Integration, the rule is simple: any change in position will result in a change of velocity
       Therefore, things in motion will stay in motion. If you want to push a PointMass towards a direction,
       just move its position and it'll continue going that way.   
    */
    // velocity = position - lastPosition
    PVector velocity = PVector.sub(position, lastPosition);
    // apply damping: acceleration -= velocity * (damping/mass)
    acceleration.sub(PVector.mult(velocity,damping/mass)); 
    // newPosition = position + velocity + 0.5 * acceleration * deltaTime * deltaTime
    PVector nextPos = PVector.add(PVector.add(position, velocity), PVector.mult(PVector.mult(acceleration, 0.5), timeStep * timeStep));
    // reset variables
    lastPosition.set(position);
    position.set(nextPos);
    acceleration.set(0,0,0);

    // make sure the particle stays in its place if it's pinned
    // (This isn't used for this simulation, but it's there anyways.)
    if (pinned)
      position.set(pinLocation);
  } 
  void updateInteractions () {
    // this is where our interaction comes in.
    if (mousePressed) {
      float distanceSquared = sq(mouseX - position.x) + sq(mouseY - position.y);
      if (mouseButton == LEFT) {
        if (distanceSquared < mouseInfluenceSize) { // remember mouseInfluenceSize was squared in setup()
          // move particles towards where the mouse is moving
          // amount to add onto the particle position:
          position.x += (mouseX - pmouseX) * 0.1 * (sqrt(mouseInfluenceSize) - sqrt(distanceSquared)) / sqrt(mouseInfluenceSize);
          position.y += (mouseY - pmouseY) * 0.1 * (sqrt(mouseInfluenceSize) - sqrt(distanceSquared)) / sqrt(mouseInfluenceSize);
        }
      }
      else { // if the right mouse button is clicking, we tear the cloth by removing links
        if (distanceSquared < mouseTearSize) 
          links.clear();
      }
    }
  }

  void draw () {
    // draw the links and points
    stroke(0);
    if (links.size() > 0) {
      for (int i = 0; i < links.size(); i++) {
        Link currentLink = (Link) links.get(i);
        if (currentLink.drawThis) // some links are invisible
          line(position.x, position.y, currentLink.p2.position.x, currentLink.p2.position.y);
      }
    }
    else
      point(position.x, position.y);
  }
  // here we tell each Link to solve constraints
  void solveConstraints () {
    for (int i = 0; i < links.size(); i++) {
      Link currentLink = (Link) links.get(i);
      currentLink.constraintSolve();
    }
    for (int i = 0; i < circles.size(); i++) {
      EnvCircle circle = (EnvCircle) circles.get(i);
      circle.solveCollision(this);
    }
    
    // These if statements keep the particles within the screen
    if (position.y < 1)
      position.y = 2 - position.y;
    if (position.y > height-1)
      position.y = 2 * (height - 1) - position.y;
    if (position.x > width-1)
      position.x = 2 * (width - 1) - position.x;
    if (position.x < 1)
      position.x = 2 - position.x;
  }

  // attachTo can be used to create links between this particle and other particles
  void attachTo (PointMass P, float restingDist, float stiff, boolean drawThis) {
    Link lnk = new Link(this, P, restingDist, stiff, drawThis);
    links.add(lnk);
  }
  void removeLink (Link lnk) {
    links.remove(lnk);
  }  
  void removeLink (PointMass P) {
    for (int i = 0; i < links.size(); i++) {
      Link lnk = (Link) links.get(i);
      if ((lnk.p1 == P) || (lnk.p2 == P))
        links.remove(i);
    }
  }

  void applyForce (PVector f) {
    // acceleration = (1/mass) * force
    // or
    // acceleration = force / mass
    acceleration.add(PVector.div(PVector.mult(f,1), mass));
  }

  void pinTo (PVector location) {
    pinned = true;
    pinLocation.set(location);
  }
}

// World
// All physics and objects, as well as the time step stuff, are handled here.
class World {
  // All of our objects
  ArrayList pointMasses = new ArrayList(); 
  ArrayList circles = new ArrayList();
  
  // Psh. Who needs gravity!
  float gravity = 0;
  
  // These variables are used to keep track of how much time is elapsed between each frame
  // they're used in the physics to maintain a certain level of accuracy and consistency
  // this program should run the at the same rate whether it's running at 30 FPS or 300,000 FPS
  long previousTime;
  long currentTime;
  // Delta means change. It's actually a triangular symbol, to label variables in equations
  // some programmers like to call it elapsedTime, or changeInTime. It's all a matter of preference
  // To keep the simulation accurate, we use a fixed time step
  int fixedDeltaTime;
  float fixedDeltaTimeSeconds;
  // the leftOverDeltaTime carries over change in time that isn't accounted for over to the next frame
  int leftOverDeltaTime;

  // How many times constraints are solved each frame
  int constraintAccuracy;
  
  World (int deltaTimeLength, int constraintAcc) {
    fixedDeltaTime = deltaTimeLength;
    fixedDeltaTimeSeconds = (float)fixedDeltaTime / 1000;
    previousTime = millis();
    currentTime = previousTime;  
    constraintAccuracy = constraintAcc;
  }
  
  void update () {
    /* Time related stuff */
    currentTime = millis();
    // deltaTimeMS: change in time in milliseconds since last frame
    long deltaTimeMS = currentTime - previousTime;
    // Reset previousTime.
    previousTime = currentTime;
    // timeStepAmt will be how many of our fixedDeltaTime's can fit in the physics for this frame.
    int timeStepAmt = (int)((float)(deltaTimeMS + leftOverDeltaTime) / (float)fixedDeltaTime);
    // reset leftOverDeltaTime.
    leftOverDeltaTime = (int)deltaTimeMS - (timeStepAmt * fixedDeltaTime); 
    float fixedDeltaTimeSeconds = (float)fixedDeltaTime / 1000;
    
    /* Physics */
    for (int iteration = 1; iteration <= timeStepAmt; iteration++) {
      // update each PointMass's position
      for (int i = 0; i < pointMasses.size(); i++) {
        PointMass p = (PointMass) pointMasses.get(i);
        p.updatePhysics(fixedDeltaTimeSeconds);
      }
      // solve the constraints
      for (int x = 0; x < constraintAccuracy; x++) {
        for (int i = 0; i < pointMasses.size(); i++) {
          PointMass p = (PointMass) pointMasses.get(i);
          p.solveConstraints();
        }
        for (int i = 0; i < circles.size(); i++) {
          Circle c = (Circle) circles.get(i);
          c.solveConstraints();  
        }
      }
    }
    
    // we use a separate loop for drawing so points and their links don't get drawn more than once
    // (rendering can be a major resource hog if not done efficiently)
    // also, interactions (mouse dragging) is applied
    for (int i = 0; i < pointMasses.size(); i++) {
      PointMass p = (PointMass) pointMasses.get(i);
      p.updateInteractions();
      p.draw();
    }
    for (int i = 0; i < circles.size(); i++) {
      Circle c = (Circle) circles.get(i);
      c.draw();  
    }
  }
  // Functions for adding PointMasses and Circles.
  void addCircle (Circle c) {
    circles.add(c);  
  }
  void removeCircle (Circle c) {
    circles.remove(c);  
  }
  void addPointMass (PointMass p) {
    pointMasses.add(p);
  }
  void removePointMass (PointMass p) {
    pointMasses.remove(p);
  }
}

/* 
  Ragdoll Aquarium
  Made by BlueThen on February 21, 2011
  Optimized, refined, etc on March 6, 2011.
  www.bluethen.com
  
  'R' to reset.
  Click to interact
*/

// This arraylist will keep track of the circles all of the bodies will collide with.
ArrayList circles = new ArrayList();
// See: World class
// All of the physics and objects are handled inside the "World"
World world;

// Distance from each PointMass where interaction is done from the cursor.
// We keep tear size from the cloth simulator for the fun of it.
// The cloth simulator can be found at http://www.openprocessing.org/visuals/?visualID=20140
float mouseInfluenceSize = 100; 
float mouseTearSize = 20;

int bodyCount = 300;
int circleCount = 3;
// How tall is everyone? (Pixels)
int bodyHeights = 20;
// How big are the circles? (Radius)
int circleMin = 50;
int circleMax = 100;

// Setup is called whenever the program starts
void setup () {
  // 640x480.
  // P2D is the renderer used.
  size(640,480, P2D);
  
  // The "sizes" are squared here, so they don't need to be when compared in the PointMass class
  mouseInfluenceSize *= mouseInfluenceSize;
  mouseTearSize *= mouseTearSize;
  
  // Reset function initializes World, the bodies, and the circles in our environment.
  reset();
}
// Draw () is called over and over. It's our "frame" of the animation.
void draw () {
  // Color everything white so we can draw on top of it again.
  background(255);
  
  // Update physics, draw bodies, etc.
  world.update();
  
  // Draw the environment
  for (int i = 0; i < circles.size(); i++) {
    EnvCircle circle = (EnvCircle) circles.get(i);
    circle.draw();
  }
  
  if (frameCount % 60 == 0)
    println("Frame rate is " + frameRate);
}

// The reset function randomly places bodies and circles around the screen.
void reset () {
  // World class is constructed with 2 parameters: (int deltaTimeLength, int constraintAcc)
  // deltaTimeLength is the timestep used. Smaller would be more accurate.
  // constraintAcc is how many times constraints are solved per timestep.
  world = new World(15, 5);
  
  // Clear the cirlces just in case reset() has already been called
  circles.clear();
  
  // Create the Bodies. in the Body constructor, everything is added to the World automatically.
  for (int i = 0; i < bodyCount; i++) {
    Body body = new Body(new PVector(random(width), random(height)), bodyHeights);
  }  
  // Add circles to the environment
  for (int i = 0; i < circleCount; i++) {
    EnvCircle circle = new EnvCircle(new PVector(random(width), random(height)), random(circleMin,circleMax));
    circles.add(circle);
  }  
  // set frameCount to 0
  // The first constraint solve can cause ragdolls to go a little crazy
  // (bodies spawned inside circles will fly out)
  // So in EnvCircle, when bodies are pushed away, their velocities are kept to 0 if the frameCount is too low
  frameCount = 0;
}

// Allow the user to reset everything when they press 'R'.
void keyPressed() {
  if ((key == 'r') || (key == 'R'))
    reset();
}