Limb l1,l2,l3;
Limb[] limbs;
float[] start={0,0,0};
int maxLimb=20;
int wLimb=20;
int lLimb=20;

void setup(){
  frameRate(30);
 size(800,600);
 background(50);
 buildLimbs();
}

void buildLimbs(){
 //float[] start={0,0,0};
 start[0]=400;
 start[1]=300;
 start[2]=0;
 limbs=new Limb[maxLimb];
 for (int i=0; i<limbs.length; i++){
   limbs[i]=new Limb((int)start[0], (int)start[1],0,start[2],wLimb,lLimb,200);
   start=limbs[i].getEnd();
 } 
}

void draw(){
  //background(100,10);
  fill(50);
  rect(0,0,width, height);
  start[0]=400;
  start[1]=300;
  start[2]=0;
  for (int i=0; i<limbs.length; i++){
   limbs[i].setStart((int)start[0], (int)start[1], start[2]);
   limbs[i].drawL();
   start=limbs[i].getEnd();
  } 
  float lastAA;
  float lastA=limbs[0].getAngle();
  limbs[0].shiftAngle(PI/50);
  for (int i=1; i<limbs.length; i++){
    lastAA=lastA;
    lastA=limbs[i].getAngle();
    limbs[i].setAngle(lastAA);
  }
 }

void mouseDragged(){
  
}



class Limb{
  int x, y, w, l, c;
  float startA, flexA;
  float maxFlexA=PI/5;
  float flexDir=1;
  int gap=5;
  Limb(int x_, int y_,float startA_,float flexA_,int w_,int l_, int c_){
    x=x_;
    y=y_;
    startA=startA_;
    flexA=flexA_;
    w=w_;
    l=l_;
    c=c_ ;   
  }
  
  void drawL(){
    fill(0,255,0);
    //ellipse(x,y,5,5);
     pushMatrix();
     rectMode(CORNER);
     translate(x,y);
     rotate(startA+flexA);
     stroke(c);
     noFill();
     //fill(c);
     //noStroke();
     rect(-w/2,0,w,l);
     translate(0,l);
     fill(0,0,255);
     //ellipse(0,0,5,5);
     popMatrix();
  }
  
  float[] getEnd(){
    float[] xya={0,0,0};
    xya[0]=x+(cos(startA+flexA+PI/2)*(l+gap));
    xya[1]=y+(sin(startA+flexA+PI/2)*(l+gap));
    xya[2]=startA+flexA;
    fill(255,0,0);
    //ellipse((int)xya[0],(int)xya[1],5,5);
    return xya;
  }
  
  void setStart(int nx, int ny, float na){
    this.x=nx;
    this.y=ny;
    this.startA=na;
    
  }
  
  void shiftAngle(float pa){
    this.flexA+=pa*flexDir;
    if(flexA>maxFlexA||flexA<-maxFlexA) flexDir=-flexDir; 
  }
  
  void setAngle(float a){
    flexA=a; 
  }
  
  void setPos(int tx,int ty){
     x=tx; y=ty;
  }
  
  boolean contains(int tx,int ty){
    return (tx>x-wLimb/2&&
      tx<x+wLimb/2&&
      ty>y-lLimb/2&&
      ty<y+lLimb/2);
  }
  
  float getAngle(){
   return(flexA); 
  }
}