class Part
{
  float x;
  float y;
  float r = 15;
  float tx;
  float ty;
  float ox;
  float oy;
  float sx;
  float sy;
  float friction;
  boolean pinned;
  public Part(float xv, float yv)
  {
    x = xv;
    y = yv;
    ox = x;
    oy = y;
  }
  void run()
  {
    y+=.5;
    verlet();
    borders();
  }
  void borders()
  {
    if(x<0) x = 0;
    //if(y<r) y = r/2;
    if(x>width) x = width;
    if(y>height)
    {
      y = height;
      friction = 0.5;
    }
    else
    {
      friction = 0;
    }
  }
  void verlet()
  {
    tx = x;
    ty = y;
    x += (x-ox)-(x-ox)*friction;
    y += (y-oy)-(y-oy)*friction;
    ox = tx;
    oy = ty;
  }
}

ArrayList parts;
ArrayList cons;
PFont nums;
Part grabbed;
Part temp;
Const tempC;
Part selecting = null;
Boolean grabbingPart = false;
int loopAmount = 100;

void setup()
{
  size(700,700);
  parts = new ArrayList();
  cons = new ArrayList();
  for(int i = 0;i<20;i++)
  {
    parts.add(new Part(width/2+100*cos(i/20.0*TWO_PI),height/2+100*sin(i/20.0*TWO_PI)));
  }
  println(dist(width/2+100*cos(1/20.0*TWO_PI),height/2+100*sin(1/20.0*TWO_PI),width/2+100*cos(2/20.0*TWO_PI),height/2+100*sin(2/20.0*TWO_PI)));
  int ps = parts.size();
  for(int i = 0;i<19;i++)
  {
    cons.add(new Const(parts.get(i),parts.get(i+1),31.28,.7));
  }
  cons.add(new Const(parts.get(19),parts.get(0),31.28,.7));
  for(int i = 1;i<18;i++)
  {
    int n = (ps-i<0) ? -i : ps-i;
    int n2 = (ps-i-3<0) ? -i-3 : ps-i-3;
    cons.add(new Const(parts.get(n),parts.get(n2),90.8,.5));
  }
  cons.add(new Const(parts.get(19),parts.get(2),90.8,.5));
    cons.add(new Const(parts.get(18),parts.get(1),90.8,.5));
      cons.add(new Const(parts.get(17),parts.get(0),90.8,.5));
  nums = loadFont("ArialMT-10.vlw");
  textAlign(CENTER);
}

void draw()
{
  background(205);
  textFont(nums);
  fill(0);
  textAlign(LEFT,TOP);
  text("Left click to select closest particle\nPress P to make a particle\nRight click to pin particle\nHold spacebar & click a pinned particle to unpin\nHold C & click two particles to make set-length constraint\nHold V & click to make a constraint that keeps current distance between particles\nPress 'b' to make a box\n"+parts.size()+" particles & "+cons.size()+" constraints satisfied "+loopAmount+" times per frame at "+floor(frameRate)+" FPS\nUse UP and DOWN keys to change how many times constraints are settled. Lower = faster",5,5);
  for(int j=0;j<loopAmount;j++)
  {
    for(int i = 0;i<cons.size();i++)
    {
      tempC = (Const)cons.get(i);
      tempC.satisfy();
    }
  }
  for(int i = 0;i<cons.size();i++)
  {
    tempC = (Const)cons.get(i);
    line(tempC.p1.x,tempC.p1.y,tempC.p2.x,tempC.p2.y);
  }
  for(int i = 0;i<parts.size();i++)
  {
    temp = (Part)parts.get(i);
    fill(255);
    if(temp.pinned)
      fill(255,0,0);
    ellipse(temp.x,temp.y,temp.r,temp.r);
    textFont(nums);
    fill(0);
    textAlign(CENTER,CENTER);
    text(i,temp.x,temp.y);
    temp.run();
    if(temp.pinned)
    {
      temp.x = temp.sx;
      temp.y = temp.sy;
    }
  }
  if(mousePressed)
  {
    if(grabbingPart == false)
      grabbed = findClosest(mouseX,mouseY);
    grabbingPart = true;
    noFill();
    ellipse(grabbed.x,grabbed.y,grabbed.r+6,grabbed.r+6);
    grabbed.x = mouseX;
    grabbed.y = mouseY;
    if(mouseButton == RIGHT)
    {
      grabbed.pinned = true;
      grabbed.sx = grabbed.x;
      grabbed.sy = grabbed.y;
    }
    if(keyPressed)
    {
      if(key == ' ')
        grabbed.pinned = false;
      if(key == 'c')
      {
        if(selecting != null && selecting != grabbed)
          cons.add(new Const(selecting,grabbed,50,.9));
        selecting = grabbed;
      }
      if(key == 'v')
      {
        if(selecting != null && selecting != grabbed)
          cons.add(new Const(selecting,grabbed,dist(selecting.x,selecting.y,grabbed.x,grabbed.y),.9));
        selecting = grabbed;
      }
    }
  }
  //only use this if you want the last contstraint made to be adjustable
  if(keyPressed)
  {
    if(key == 'w')
      ((Const)cons.get(cons.size()-1)).distance = 10;
    if(key == 's')
      ((Const)cons.get(cons.size()-1)).distance = 50;
  }
}

void keyPressed()
{
  if(key == 'b')
    makeBox(40);
  if(key == 'p')
    parts.add(new Part(mouseX,mouseY));
  if(keyCode == UP)
    loopAmount++; 
  if(keyCode == DOWN && loopAmount>1)
    loopAmount--;
}

void keyReleased()
{
  if(key == 'c')
    selecting = null;
  if(key == 'v')
    selecting = null;
}

void mouseReleased()
{
  grabbingPart = false;
}

//find closest particle to given x and y
Part findClosest(float x,float y)
{
  float curBest = 9999;
  Part best = (Part)parts.get(0);
  Part t; //temporary particle placeholder
  for(int i = 0;i<parts.size();i++)
  {
    t = (Part)parts.get(i);
    if(dist(x,y,t.x,t.y)<curBest)
    {
      curBest = dist(x,y,t.x,t.y);
      best = t;
    }
  }
  return best;
}

//=====================================================SHAPES==================================================================
void makeBox(float side)
{
  for(int i = 0;i<4;i++)
    parts.add(new Part(mouseX+random(20),mouseY+random(20)));
  int ps = parts.size();
  cons.add(new Const(parts.get(ps-4),parts.get(ps-1),side,.9));
  cons.add(new Const(parts.get(ps-1),parts.get(ps-2),side,.9));
  cons.add(new Const(parts.get(ps-2),parts.get(ps-3),side,.9));
  cons.add(new Const(parts.get(ps-3),parts.get(ps-4),side,.9));
  cons.add(new Const(parts.get(ps-4),parts.get(ps-2),side*sqrt(2),.9));
  cons.add(new Const(parts.get(ps-1),parts.get(ps-3),side*sqrt(2),.9));
}

class Const
{
  float distance;
  float dampening;
  Part p1;
  Part p2;
  public Const(Object input1,Object input2,float dist,float damp)
  {
    dampening = damp;
    distance = dist;
    p1 = (Part)input1;
    p2 = (Part)input2;
  }
  public float moveX(float angle,float distance)
  {
    return cos(angle)*distance;
  }
  public float moveY(float angle,float distance)
  {
    return sin(angle)*distance;
  }
  void borders(float x,float y, float r)
  {
    if(x<r) x = r;
    if(y<r) y = r;
    if(x>width-r) x = width-r;
    if(y>height-r) y = height-r;
  }
  public void satisfy()
  {
    float x1 = p1.x;
    float x2 = p2.x;
    float y1 = p1.y;
    float y2 = p2.y;
    float diff = dist(x1,y1,x2,y2) - distance;
    float angle = atan2(y2-y1,x2-x1);
    if(!p1.pinned && !p2.pinned)
    {
      p1.x+=moveX(angle,diff/2)*dampening;
      p1.y+=moveY(angle,diff/2)*dampening;
      p2.x-=moveX(angle,diff/2)*dampening;
      p2.y-=moveY(angle,diff/2)*dampening;
    }
    if(p1.pinned && !p2.pinned)
    {
      //p1.x+=moveX(angle,diff/2)*dampening;
      //p1.y+=moveY(angle,diff/2)*dampening;
      p2.x-=moveX(angle,diff)*dampening;
      p2.y-=moveY(angle,diff)*dampening;
    }
    if(!p1.pinned && p2.pinned)
    {
      p1.x+=moveX(angle,diff)*dampening;
      p1.y+=moveY(angle,diff)*dampening;
      //p2.x-=moveX(angle,diff)*dampening;
      //p2.y-=moveY(angle,diff)*dampening;
    }
    borders(p1.x,p1.y,p1.r);
    borders(p2.x,p2.y,p2.r);
  }
}