/* Circle class */
class Circle implements GridObject {
  /* Which PointMass is the circle attached to? */
  boolean attachedToPointMass = false;
  int attachedPointMass;
  
  float radius;
  float radiusSquared;
  
  float damping = 0.95;
  /* Constructor */
  Circle (float r) {
    //position = pos.get();
    radius = r;
    radiusSquared = r*r;
  }
  /* Constraint solving algorithm */
  // Here we find out if the circle is overlapping the surfacem, and act accordingly.
  void solveConstraints (boolean preserveImpulse) {
    // set the wheel's position to it's attached PointMass's position
    float x = xPos[attachedPointMass];
    float y = yPos[attachedPointMass];
    
    float prevX = lastX[attachedPointMass];
    float prevY = lastY[attachedPointMass];
    
    // Make sure it isn't outside of the screen
    float vx = 0, vy = 0;
    if (preserveImpulse) {
      vx = (prevX - x) * damping;
      vy = (prevY - y) * damping;
    }
    
    if (x - radius < 0) {
      x = radius;
      if (preserveImpulse)
        prevX = x - vx;
    }
    if (x + radius > width) {
      x = 2 * (width - radius) - x;
      if (preserveImpulse)
        prevX = x - vx;
    }
    if (y - radius < 0) {
      y = 2*(radius) - y;
      if (preserveImpulse)
        prevY = y - vy;
    }
    if (y + radius > height) {
      y = 2 * (height - radius) - y;
      if (preserveImpulse)
        prevY = y - vy;
    }
    
    xPos[attachedPointMass] = x;
    yPos[attachedPointMass] = y;
    
    lastX[attachedPointMass] = prevX;
    lastY[attachedPointMass] = prevY;
    
    grid.update(this);
    List nearBy = grid.nearByObjects(x,y);
    for (int i = 0; i < nearBy.size(); i++) {
      Circle nextPM = (Circle) nearBy.get(i);
      avoid(nextPM, preserveImpulse);
    }
  }
  // Checks for a collision between two circles, and solves for it
  // Also creates a link if their velocities are similar.
  void avoid (Circle what, boolean preserveImpulse) {
    if (what != this) {
      float x = xPos[attachedPointMass];
      float y = yPos[attachedPointMass];
      
      float otherX = xPos[what.attachedPointMass];
      float otherY = yPos[what.attachedPointMass];

      float diffX = x - otherX;
      float diffY = y - otherY;

      float diffSquared = diffX * diffX + diffY * diffY;
      
      if (diffSquared <= sq(radius + what.radius)) { // first make sure they're intersecting
        
        // Previous velocity
        float v1x = x - lastX[attachedPointMass];
        float v1y = y - lastY[attachedPointMass];
        float v2x = otherX - lastX[what.attachedPointMass];
        float v2y = otherY - lastY[what.attachedPointMass];
        
        // distance between centers
        float d = sqrt(diffSquared);
        
        // minimum translation distance to push balls apart after intersecting
        float mtdX;
        float mtdY;
        if (d == 0) {
          d = radius + what.radius - 1;
          diffX = radius + what.radius;
          diffY = 0;
        } 
        
        float difference = ((radius + what.radius) - d) / d;
        
        mtdX = diffX * difference;
        mtdY = diffY * difference;
        
        // resolve intersection
        float im1 = 1f / 2f; // inverse mass quantities
        float im2 = 1f / 2f;
        
        // push-pull them based on mass
        xPos[attachedPointMass] += mtdX * (im1 / (im1 + im2));
        yPos[attachedPointMass] += mtdY * (im1 / (im1 + im2));
        
        xPos[what.attachedPointMass] -= mtdX * (im1 / (im1 + im2));
        yPos[what.attachedPointMass] -= mtdY * (im1 / (im1 + im2));
        
        if (preserveImpulse) { // preserve velocities/impulse based on http://codeflow.org/entries/2010/nov/29/verlet-collision-with-impulse-preservation/
          float f1 = (damping * (diffX * v1x + diffY * v1y)) / diffSquared;
          float f2 = (damping * (diffX * v2x + diffY * v2y)) / diffSquared;
        
          v1x += f2 * diffX - f1 * diffX;
          v2x += f1 * diffX - f2 * diffX;
          v1y += f2 * diffY - f1 * diffY;
          v2y += f1 * diffY - f2 * diffY;
          
          
          lastX[attachedPointMass] = xPos[attachedPointMass] - v1x;
          lastY[attachedPointMass] = yPos[attachedPointMass] - v1y;
          
          lastX[what.attachedPointMass] = xPos[what.attachedPointMass] - v2x;
          lastY[what.attachedPointMass] = yPos[what.attachedPointMass] - v2y;
        }
        
        // Now we see if they're moving at similar speeds
        float vX = xPos[attachedPointMass] - lastX[attachedPointMass];
        float vY = yPos[attachedPointMass] - lastY[attachedPointMass];
        float otherVX = xPos[what.attachedPointMass] - lastX[what.attachedPointMass];
        float otherVY = yPos[what.attachedPointMass] - lastY[what.attachedPointMass];
        
        float diffVX = vX - otherVX;
        float diffVY = vY - otherVY;
      
        if (diffVX * diffVX + diffVY * diffVY < 1) {
          
          // make sure we're not already attached
          if (linked(attachedPointMass, what.attachedPointMass))
            return;
          
          // find empty link
          int linkIndex = -1;
          for (int i = 0; i < links[attachedPointMass].length; i++) {
            if (linkStiff[attachedPointMass][i] == 0) {
              linkIndex = i;
              break;
            }
          }
          if (linkIndex == -1)
            return;
          
          // create link at linkIndex
          if (linkIndex >= linkCount[attachedPointMass])
            linkCount[attachedPointMass]++;
          links[attachedPointMass][linkIndex] = what.attachedPointMass;
          linkDist[attachedPointMass][linkIndex] = radius + what.radius + 1;
          linkStiff[attachedPointMass][linkIndex] = stiffness;
        }
      }
    }
  }
  
  /* The circle's draw function */
  void draw () {
    noFill();
    stroke(255);
    ellipse(xPos[attachedPointMass], yPos[attachedPointMass], radius*2, radius*2);
  }
  
  /* Set the attached PointMass */
  void attachToPointMass (int p) {
    attachedPointMass = p;
  }
  
  float getX() {
    return xPos[attachedPointMass];
  }
  float getY() {
    return yPos[attachedPointMass];
  }
  float getLastX() {
    return lastX[attachedPointMass];
  }
  float getLastY() {
    return lastY[attachedPointMass];
  }
}

/* Constraints */
// includes links, boundary, and pinned (unused here) constraints
void solveConstraints () {
  for (int pmIndex = 0; pmIndex < pointMassCount; pmIndex++) { // loop through every PointMass
    /* Link Constraints */
    for (int linkIndex = 0; linkIndex < linkCount[pmIndex]; linkIndex++) { // loop through each PointMass's link
      if (linkStiff[pmIndex][linkIndex] != 0) {
        int otherPMIndex = links[pmIndex][linkIndex];
  
        float deltaX = xPos[pmIndex] - xPos[otherPMIndex];
        float deltaY = yPos[pmIndex] - yPos[otherPMIndex];
  
        float d = sqrt(deltaX * deltaX + deltaY * deltaY);
        
        // break the link and go on to the next one if it stretched too far.
        if (d > maxLinkDist) {
          linkStiff[pmIndex][linkIndex] = 0;
          continue;
        }
          
        float difference = (linkDist[pmIndex][linkIndex] - d) / d;
  
        float scalar = 0.5 * linkStiff[pmIndex][linkIndex] * difference;
        
        float deltaXScalar = deltaX * scalar;
        float deltaYScalar = deltaY * scalar;
        
        xPos[pmIndex] += deltaXScalar;
        yPos[pmIndex] += deltaYScalar;
  
        xPos[otherPMIndex] -= deltaXScalar;
        yPos[otherPMIndex] -= deltaYScalar;
      }
    }
    
    /* Boundary */
    if (yPos[pmIndex] < 1)
      yPos[pmIndex] = 2 * (1) - yPos[pmIndex];
    if (yPos[pmIndex] > height-1)
      yPos[pmIndex]= 2 * (height - 1) - yPos[pmIndex];
    if (xPos[pmIndex] > width-1)
      xPos[pmIndex] = 2 * (width - 1) - xPos[pmIndex];
    if (xPos[pmIndex] < 1)
      xPos[pmIndex] = 2 * (1) - xPos[pmIndex];
    
    /* Pinned */
    if (pinned[pmIndex]) {
      xPos[pmIndex] = pinnedX[pmIndex];
      yPos[pmIndex] = pinnedY[pmIndex];
    }

  }
}

/* 
  Dough
  Written by Jared "BlueThen" C. on July 8, 2012
  
  http://bluethen.com
  http://twitter.com/bluethen
  http://openprocessing.org/portal/?userID=3044
  http://hawkee.com/profile/37047/
  
  Email me: bluethen (@) gmail . com
  
  Click and drag to interact
  'g' to toggle gravity
*/

int pointMassCount = 900; // how many pointmasses are there?
 
float stiffness = 0.1; // (0 .. 1) Stiffness between each link
int maxLinks = 4; // how many links is each pointmass allowed to?
int circleRadius = 6; // 1/2 minimum distance allowed between any two pointmasses 
float maxLinkDist = 40; // distance a link can stretch before breaking

float gravity = 980;

float mouseInfluenceSquared = sq(60);
float mouseTearInfluenceSquared = sq(10);

/* Timestep stuff */
long previousTime;
long currentTime;
final int fixedDeltaTime = 15;
float fixedDeltaTimeSeconds = (float)fixedDeltaTime / 1000.0;
float timestepSquared = sq(fixedDeltaTimeSeconds);
int leftOverDeltaTime = 0;

int constraintAccuracy = 1;

float mouseInfluenceScalar; // set during timestep calculation

/* Grid stuff */
// see Grid.pde
Grid grid;
List<GridObject> objects;

// performance tracking
float updateTime = 0;
float renderTime = 0;
int lastPrint = 0;

/* Setup */
// initialize all our stuff
void setup () {
  size(640,480, JAVA2D); // P2D and OPENGL are way faster, but lots of people can't run it
  
  // crate an objects
  objects = new ArrayList<GridObject>();
  
  for (int i = 0; i < pointMassCount; i++) {
    // create our pointmass
    initParticle(i, random(width), random(height));
    
    // make it a ball
    Circle circle = new Circle(circleRadius);    
    circle.attachToPointMass(i);
    
    // add it to our objects
    objects.add(circle);
  }
  
  // create our grid
  grid = new Grid(circleRadius*2, objects);
}
/* "Draw" */
// also "Physics"
void draw() {
  /******** Physics ********/
  /*
   Timestep inspired by Glenn Fiedler
     http://gafferongames.com/game-physics/fix-your-timestep/
   Velocity preservation of ball-to-ball collision inspired by Florian Boesch
     http://codeflow.org/entries/2010/nov/29/verlet-collision-with-impulse-preservation/
  */
  long updateStart = millis();
  currentTime = millis();
  long deltaTimeMS = currentTime - previousTime;
  previousTime = currentTime; // reset previousTime
  // Break up the elapsed time into constant-sized timesteps
  int timeStepAmt = (int)((float)(deltaTimeMS + leftOverDeltaTime) / (float)fixedDeltaTime);

  timeStepAmt = min(timeStepAmt, 5);
  
  // keep track of left over elapsed time for the next frame
  leftOverDeltaTime += (int)deltaTimeMS - (timeStepAmt * fixedDeltaTime);

  mouseInfluenceScalar = 1.0 / timeStepAmt;
  
  for (int iteration = 1; iteration <= timeStepAmt; iteration++) {
    
    // Solve links
    for (int c = 0; c < constraintAccuracy; c++)
      solveConstraints();
      
    // update mouse-to-pointmass interactions
    updateInteractions();
    
    // accelerations (gravity)
    accelerate(timestepSquared);
    
    // solve circle-to-circle collisions with no velocity preservation
    for (int c = 0; c < 2; c++)
      for (GridObject go : objects) {
        if (go instanceof Circle)
          ((Circle)go).solveConstraints(false); 
      }
    
    // apply inertia
    inertia();
    
    // solve circle-to-circle collisions with velocity preservation
    for (GridObject go : objects) {
      if (go instanceof Circle)
        ((Circle)go).solveConstraints(true); 
    }
    
  } // end timestep
  long updateEnd = millis();
  
  
  /* Rendering */
  long renderStart = millis();
  // clear background
  background(0);
  // use white for drawing
  stroke(255);
  
  // render points and links
  beginShape(LINES);
  for (int pmIndex = 0; pmIndex < pointMassCount; pmIndex++)
    renderPoint(pmIndex);  
  endShape();
  long renderEnd = millis();
  
  // perfromance
  // calculate performances of each part of the program
  // the times for this frame is factored in only as a small portion
  updateTime = (updateTime * 29 + (updateEnd - updateStart)) / 30;
  renderTime = (renderTime * 29 + (renderEnd - renderStart)) / 30;
  // Print performances every few seconds
  if (lastPrint != second() && second() % 4 == 0 && second() != 1) {
    lastPrint = second();
    float total = updateTime + renderTime;
    println("Update Time: " + (int)updateTime + "ms (" + int(100*updateTime/total) + "%)" + 
      " | Render Time: " + (int)renderTime + "ms (" + int(100*renderTime/total) + "%)");
    println("Total " + (int)total + "ms");
    println("Framerate: " + (int)frameRate);
  }
}
void toggleGravity () {
  if (gravity > 0)
    gravity = 0;
  else
    gravity = 392;  
}
void keyPressed() {
  if ((key == 'g') || (key == 'G'))
    toggleGravity();
}

// Grid is an algorithm I made about 2 years ago
// I revamped it, optimized it, and cleaned it significantly
// it takes objects, stores them in separate cells,
// and when called on, will provide a list of objects in a cell and cells adjacent to that cell
// it allows to save resources by only having collision checks between objects potentially close enough

class Grid {
  // The cells map keeps track of where each object is
  // The grid list keeps track of which objects are in each cell
  
  Map cells = new HashMap(); // Key: Object object, Value: grid position... (x/scale,y/scale)
                             // holds the position of the object in the grid. This makes sure that the object gets removed and replaced each time
                             // it changes position in the grid.
                             
                             
  List [][] grid; // grid[x/scale][y/scale] = List of objects
  int cellSize;
  Grid (int cellSize, List<GridObject> objects) {
    this.cellSize = cellSize;
    grid = new ArrayList[int(width/cellSize)+1][int(height/cellSize)+1];
    
    // construct a list for each cell
    for (int x = 0; x < grid.length; x++) {
      for (int y = 0; y < grid[x].length; y++) {
        grid[x][y] = new ArrayList(); // a list of objects for every cell
      } 
    }
    
    // add the objects to their cells
    // and keep track of which cell each object is in
    for (GridObject object : objects) {
      cells.put(object, new PVector(int(object.getX() / cellSize), int(object.getY() / cellSize)));
      grid[int(object.getX() / cellSize)][int(object.getY() / cellSize)].add(object); 
    }
  }
  void drawGrid () {
    stroke(50);
    for (int x = 0; x < grid.length; x++) {
      for (int y = 0; y < grid[x].length; y++) {
        noFill();
        rect(x*cellSize, y*cellSize,cellSize,cellSize);
      } 
    }
  }
  // Update needs to be called on by each object after they update their position
  // this makes sure that the object is still in the correct cell
  void update(GridObject object) {
    // first check if it needs to be moved
    
    // Find cell we stored it in
    PVector oldCellPos = (PVector) cells.get(object);
    int oldCellX = (int)oldCellPos.x;
    int oldCellY = (int)oldCellPos.y;

    // find cell it is in currently
    int cellX = (int)(object.getX() / cellSize);
    int cellY = (int)(object.getY() / cellSize);
  
    // if they match, stop there
    if (cellX == oldCellX && cellY == oldCellY)
      return;
    
    // safety check
    if (cellX >= 0 && cellX < grid.length && cellY >= 0 && cellY < grid[cellX].length) {
      // if the grid the object is in now doesn't contain that object...
      if (!grid[cellX][cellY].contains(object)) {
        // add the object and remove the object from its former cell
        grid[cellX][cellY].add(object); 
        grid[oldCellX][oldCellY].remove(object);
        // memorize which cell the object is in
        cells.put(object, new PVector(cellX, cellY));
      }
    }
  }
  
  // Find a list of nearby objects
  // goes through each cell adjacent to the current cell, and the current cell,
  // and generates a list of objects contained in those cells
  List nearByObjects (float x, float y) {
    List nearBy = new ArrayList();
    int cellX = (int)(x / cellSize); 
    int cellY = (int)(y / cellSize);
    
    if (cellX >= 0 && cellX < grid.length) {
      // center column
      if (cellY >= 0 && cellY < grid[cellX].length) {
        // middle
        for (Object object : grid[cellX][cellY])
          nearBy.add(object); 
        // top
        // cellY+1 >= 0 can be assumed since cellY >= 0 is checked in this block
        if (cellY+1 < grid[cellX].length) {
          for (Object object : grid[cellX][cellY + 1])
            nearBy.add(object); 
        }
      }
      // bottom
      if (cellY-1 >= 0 && cellY-1 < grid[cellX].length) {
        for (Object object : grid[cellX][cellY - 1])
          nearBy.add(object); 
      }
      
      // right column
      if (cellX+1 < grid.length) {
        if (cellY >= 0 && cellY < grid[cellX + 1].length) {
          // middle right
          for (Object object : grid[cellX + 1][cellY]) 
            nearBy.add(object); 
          if (cellY+1 < grid[cellX + 1].length) {
            // top right
            for (Object object : grid[cellX + 1][cellY + 1])
              nearBy.add(object); 
          }
        }
        if (cellY-1 >= 0 && cellY-1 < grid[cellX + 1].length) {
          // bottom right
          for (Object object : grid[cellX + 1][cellY - 1])
            nearBy.add(object); 
        }
      }
    }
    
    // left column
    if (cellX-1 >= 0 && cellX-1 < grid.length) {
      if (cellY >= 0 && cellY < grid[cellX - 1].length) {
        // center left
        for (Object object : grid[cellX - 1][cellY])
          nearBy.add(object); 
        // top left
        if (cellY+1 < grid[cellX - 1].length) {
          for (Object object : grid[cellX - 1][cellY + 1])
            nearBy.add(object); 
        }
      }
      if (cellY-1 >= 0 && cellY-1 < grid[cellX - 1].length) {
        // bottom left
        for (Object object : grid[cellX - 1][cellY - 1])
          nearBy.add(object); 
      }
    }
    return nearBy;
  }
}

// Used by Grid so we can hook it up with any other program
// To use, type "implements GridObject" after "class Object," like in Bird
// and add float getX() and float getY() methods to return its position
// create a Grid with a list of GridObjects in the constructor, and a cell size with the largest distance between two objects
// (for a ball simulator, that would be the radius of the largest ball x 2
// store a Grid object somewhere, and have each object use the grid's update() method to store its location
public interface GridObject {
  float getX();
  float getY(); 
}

/* Physics */
// Here we apply velocity integration

/* Accelerate */ 
// (mainly used for gravity)
void accelerate(float timestepSquared) {
  for (int pmIndex = 0; pmIndex < pointMassCount; pmIndex++) {
    accY[pmIndex] += gravity;
    
    float nextX = xPos[pmIndex] + 0.5 * accX[pmIndex] * timestepSquared;
    float nextY = yPos[pmIndex] + 0.5 * accY[pmIndex] * timestepSquared;
    
    xPos[pmIndex] = nextX;
    yPos[pmIndex] = nextY;
    
    accX[pmIndex] = 0;
    accY[pmIndex] = 0;
  }
}
/* Intertia */
// Objects in motion will stay in motion
void inertia() {
  for (int pmIndex = 0; pmIndex < pointMassCount; pmIndex++) {
    // find velocity
    float velX = xPos[pmIndex] - lastX[pmIndex];
    float velY = yPos[pmIndex] - lastY[pmIndex];
    
    // add the velocity to the position
    float nextX = xPos[pmIndex] + velX;
    float nextY = yPos[pmIndex] + velY;
    
    // reset last position
    lastX[pmIndex] = xPos[pmIndex];
    lastY[pmIndex] = yPos[pmIndex];
    
    xPos[pmIndex] = nextX;
    yPos[pmIndex] = nextY;
  }
}
/* Interactions */
// allow the user to turn and knead the dough
void updateInteractions() {
  for (int pmIndex = 0; pmIndex < pointMassCount; pmIndex++) {
    if (!pinned[pmIndex]) {
      if (mousePressed) {
        // we use distPointToSegmentSquared to find out how close the pointmass is to the segment mouse to last mouse position
        // this makes the interactions consistent and not spotty (esp. at low framerates)
        float distanceSquared = distPointToSegmentSquared(pmouseX, pmouseY, mouseX, mouseY, xPos[pmIndex], yPos[pmIndex]);
        if (mouseButton == LEFT) {
          if (distanceSquared < mouseInfluenceSquared) {
            // Set the pointmass velocity to the direction the mouse is moving
            lastX[pmIndex] = xPos[pmIndex] - (mouseX-pmouseX) * mouseInfluenceScalar;
            lastY[pmIndex] = yPos[pmIndex] - (mouseY-pmouseY) * mouseInfluenceScalar;
          }
        }
        else {
          if (distanceSquared < mouseTearInfluenceSquared) {
            // remove links if the user is right clicking
            for (int lnk = 0; lnk < linkStiff[pmIndex].length; lnk++)
              linkStiff[pmIndex][lnk] = 0;
          }
        }
      }
    }
  }
}
// Credit to: http://www.codeguru.com/forum/showpost.php?p=1913101&postcount=16
float distPointToSegmentSquared (float lineX1, float lineY1, float lineX2, float lineY2, float pointX, float pointY) {
  if (lineX1 == lineX2 && lineY1 == lineY2)
    return sq(pointX - lineX1) + sq(pointY - lineY1);
    
  float vx = lineX1 - pointX;
  float vy = lineY1 - pointY;
  float ux = lineX2 - lineX1;
  float uy = lineY2 - lineY1;
  
  float len = ux*ux + uy*uy;
  float det = (-vx * ux) + (-vy * uy);
  if ((det < 0) || (det > len)) {
    ux = lineX2 - pointX;
    uy = lineY2 - pointY;
    return min(vx*vx+vy*vy, ux*ux+uy*uy);
  }
  
  det = ux*vy - uy*vx;
  return (det*det) / len;
}

/* PointMass Variables */
float [] xPos = new float [pointMassCount];
float [] yPos = new float [pointMassCount];
float [] lastX = new float [pointMassCount];
float [] lastY = new float [pointMassCount];
float [] accX = new float [pointMassCount];
float [] accY = new float [pointMassCount];

int [] linkCount = new int [pointMassCount];                // linkCount[PointMassIndex] = How many links
int [][] links = new int [pointMassCount][maxLinks];          // links[PointMassIndex][LinkIndex] = Other PointMass's Index
float [][] linkDist = new float[pointMassCount][maxLinks];    // linkDist[PointMassIndex][LinkIndex] = Resting Distance
float [][] linkStiff = new float[pointMassCount][maxLinks];   // linkStiff[PointMassIndex][LinkIndex] = Link's stiffness

boolean [] pinned = new boolean [pointMassCount];
float [] pinnedX = new float [pointMassCount];
float [] pinnedY = new float [pointMassCount];

/* Create a Point Mass */
// Create a new point mass
void initParticle (int index, float x, float y) { 
  xPos[index] = x;
  yPos[index] = y;
  
  lastX[index] = x;
  lastY[index] = y;
  
  accX[index] = 0;
  accY[index] = 0;
}
/* Create a link */
// attach two pointmasses
void attach (int index1, int index2, float restingDist, float stiffness) {
  linkCount[index1]++;
  links[index1][linkCount[index1]-1] = index2;
  linkDist[index1][linkCount[index1]-1] = restingDist;
  linkStiff[index1][linkCount[index1]-1] = stiffness;
}
/* Render */
// render a point's links (or the point, if there are no points)
void renderPoint (int index) {
  int linkC = 0;
  for (int linkIndex = 0; linkIndex < linkCount[index]; linkIndex++) {
    if (linkStiff[index][linkIndex] != 0) {
      vertex(xPos[index], yPos[index]);
      vertex(xPos[links[index][linkIndex]], yPos[links[index][linkIndex]]);
      linkC++;
    }
  }
  if (linkC == 0) {
    vertex(xPos[index], yPos[index]);
    vertex(xPos[index], yPos[index]);
  }
}
/* Link Check */
// check if a link exists between two point masses
boolean linked(int index1, int index2) {
  for (int i = 0; i < links[index1].length; i++) {
    if (links[index1][i] == index2)
      return true;
    if (links[index2][i] == index1)
      return true;
  }  
  return false;
}