Segment[] segs;
float[] start={400,400,0};
int numSegs=20;
int segW=50;
int segH=15;
int segsDampMax=200;
int segsDampInc=segsDampMax/numSegs;
float[][][] segCoordsL; //segs, side (L,R), corner(top, bottom), x/y
float[][][] segCoordsR; //segs, side (L,R), corner(top, bottom), x/y


void setup(){
  size(800,800);
 background(50);
 buildSegs();
}

void buildSegs(){
  float[] start={400,400,0};
  segs=new Segment[numSegs];
  segCoordsL=new float[numSegs][2][2];
  segCoordsR=new float[numSegs][2][2];
  
  segs[0]=new Segment(segW,segH,100,true); 
  segs[0].setStart((int)start[0], (int)start[1], start[2]);
  start=segs[0].getEnd();
  for (int i=1; i<segs.length; i++){
    segs[i]=new Segment(segW-(segW/8*7/numSegs*i),segH-(segH/3*2/numSegs*i),100,false); 
    segs[i].setStart((int)start[0], (int)start[1], start[2]);
    start=segs[i].getEnd();
  }
}

void draw(){
  float[] start={400,300,0};
  float[][] ls={{0,0},{0,0}};
  float [] lc={0,0};
  float[][] rs={{0,0},{0,0}};
  float [] rc={0,0};
  fill(50);
  rect(0,0,width, height);
  //start={400,100,0};
/*  lc[0]=start[0];lc[1]=start[1];
  rc[0]=start[0];rc[1]=start[1];
  stroke(0,255,0);
  for (int i=0; i<segs.length; i++){
    segs[i].setStart((int)start[0], (int)start[1], start[2]);
    segs[i].drawS();
    start=segs[i].getEnd();
    ls=segs[i].getLeftSide();
    stroke(0,255,0);
    line(lc[0],lc[1],ls[1][0],ls[1][1]);
    lc=ls[1];
    rs=segs[i].getRightSide();
    line(rc[0],rc[1],rs[1][0],rs[1][1]);
    rc=rs[1];     
  }*/
  
 //try to render a curve rather than a seriese of lines.
 
   for (int i=0; i<segs.length; i++){
    segs[i].setStart((int)start[0], (int)start[1], start[2]);
    segs[i].drawS();
    start=segs[i].getEnd();
  }
  
  
  beginShape();
  stroke(255,0,0);
  noFill();
  curveVertex(segCoordsL[0][1][0],segCoordsL[0][1][1]);
  ellipse(segCoordsL[0][1][0],segCoordsL[0][1][1],5,5);
  for (int i=0; i<segs.length; i++){
    curveVertex(segCoordsL[i][1][0],segCoordsL[i][1][1]);
    ellipse(segCoordsL[i][1][0],segCoordsL[i][1][1],5,5);
  }
  endShape();
  
  beginShape();
  stroke(0,200,255);
  noFill();
  curveVertex(segCoordsR[0][1][0],segCoordsR[0][1][1]);
  ellipse(segCoordsR[0][1][0],segCoordsR[0][1][1],5,5);
  for (int i=0; i<segs.length; i++){
    curveVertex(segCoordsR[i][1][0],segCoordsR[i][1][1]);
    ellipse(segCoordsR[i][1][0],segCoordsR[i][1][1],5,5);
  }
  endShape();
  
}

void propegate(float a){
 float b;
 for (int i=segs.length-1; i>=0; i--){
   b=segs[i].rotateS(a);
   a=b;
 } 
}



void mouseDragged(){
  if (mouseX<pmouseX) propegate(PI/25);
  else if(mouseX>pmouseX) propegate(-PI/35);
}

/* This bit trakcs the current pressed mouse 
position calculates the angle from the current 
trunk end, and then calulats the difference from its own angle. 
It attempt to converge on this angle over time (dampened)
and passes that target up to other segments with 
steadily more dampening.
*************************************************/
void mouseMoved(){
  float[] last={0,0,0};
  float a;
  float b;
  last=segs[segs.length-1].getEnd();
  for (int i=segs.length-1; i>=0; i--){
   a=segs[i].getAngle((int)last[0], (int)last[1],mouseX, mouseY); 
   //if(isInsideTrunk()) a=a+PI; 
   b=segs[i].angleDiff(a);
   segs[i].rotateS((b-PI/2)/((segs.length-i)*segsDampInc+segsDampInc));
   segCoordsL[i]=segs[i].getLeftSide();
   segCoordsR[i]=segs[i].getRightSide();
  } 
}

boolean isInsideTrunk(){
  float[] xya=segs[0].getStart();
  float[] xya2=segs[segs.length-1].getEnd();
  float hypTrunk=sqrt(pow(xya2[0]-xya[0],2)+pow(xya2[1]-xya[1],2));
  float hypMouse=sqrt(pow(mouseX-xya[0],2)+pow(mouseY-xya[1],2));
  return hypMouse<hypTrunk;
}
//***********************************************************
//***********************************************************

class Segment{
 int x,y,w,l;
 color c;
 float startA, flexA;
 float maxFlexA;//=PI/8;
 int gap=5;
 float[] buffer=new float[10];
 boolean anchor;

 Segment(int w_,int l_, int c_, boolean anchor_){
    w=w_;
    l=l_;
    c=color(c_,150,150);
    anchor=anchor_;
    if (anchor) maxFlexA=2*PI;
    else maxFlexA=PI/6;
  } 
  
  void setStart(int nx, int ny, float na){
    this.x=nx;
    this.y=ny;
    this.startA=na;
  }
  
  float[] getStart(){
   float[] xya={this.x,this.y,this.startA+this.flexA};
   return xya;
  }
  
  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;
    //stroke(255,0,0);
    //noFill();
    //ellipse((int)xya[0],(int)xya[1],5,5);
    return xya;
  }
  
  float[][] getLeftSide(){
    float[][] leftSide={{0,0},{0,0}};
    leftSide[0][0]=x-(cos(startA+flexA)*(w/2));
    leftSide[0][1]=y-(sin(startA+flexA)*(w/2));
    leftSide[1][0]=leftSide[0][0]+(cos(startA+flexA+PI/2)*(l));
    leftSide[1][1]=leftSide[0][1]+(sin(startA+flexA+PI/2)*(l));    
    //stroke(255,0,0);
    //line (leftSide[0][0],leftSide[0][1],leftSide[1][0],leftSide[1][1]);
    return leftSide;
  }
  
  float[][] getRightSide(){
    float[][] rightSide={{0,0},{0,0}};
    rightSide[0][0]=x+(cos(startA+flexA)*(w/2));
    rightSide[0][1]=y+(sin(startA+flexA)*(w/2));
    rightSide[1][0]=rightSide[0][0]+(cos(startA+flexA+PI/2)*(l));
    rightSide[1][1]=rightSide[0][1]+(sin(startA+flexA+PI/2)*(l));    
    //stroke(0,0,255);
    //line (rightSide[0][0],rightSide[0][1],rightSide[1][0],rightSide[1][1]);
    return rightSide;
  }
  
  void drawS(){
    fill(0,255,0);
    //ellipse(x,y,5,5);
     pushMatrix();
     rectMode(CORNER);
     translate(x,y);
     rotate(startA+flexA);
     //stroke(0);
     //noFill();
     fill(c);
     noStroke();
     rect(-w/2,0,w,l);
     translate(0,l);
     //fill(0,0,255);
     //ellipse(0,0,5,5);
     popMatrix();
  }
  
  float rotateS(float a){
    float excess=0.0;
    this.flexA+=a;
    if(!this.anchor){
      if(this.flexA>this.maxFlexA){
        excess=this.flexA-this.maxFlexA;
        this.flexA=maxFlexA;
      }
      if(this.flexA<-this.maxFlexA){
        excess=this.flexA+this.maxFlexA;
        this.flexA=-maxFlexA;
      } 
    }
    return excess;
  }
  
  float getAngle(int sx, int sy, int ex, int ey){
   return atan2(ey-sy,ex-sx); 
  }
  
  float angleDiff(float ang){
    return ang-(startA+flexA);
  }
  
}