Segment[] segs;
float[] start={400,400,0};
int numSegs=20;
int segW=50;
int segH=15;
float maxSegFlex=PI/6;
int segsDampMax=300;
int segsDampInc=segsDampMax/numSegs;
boolean showSegs=false;
boolean showJoins=false;
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();
 mouseMoved();
}

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/10*9/numSegs*i),segH-(segH/4*3/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);
  noStroke();
  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]);
    if (showSegs) segs[i].drawS();
    start=segs[i].getEnd();
  }
  
  
  beginShape();
  stroke(100,200,255);
  if(!showSegs) fill(100,200,255); else noFill();
  //noFill();
  curveVertex(segCoordsL[0][1][0],segCoordsL[0][1][1]);
  if (showJoins) 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]);
    if (showJoins) ellipse(segCoordsL[i][1][0],segCoordsL[i][1][1],5,5);
  }
  curveVertex(segCoordsL[segs.length-1][1][0],segCoordsL[segs.length-1][1][1]);
  //endShape();
  /*
  beginShape();
  stroke(0,200,255);
  noFill();
  curveVertex(segCoordsR[0][1][0],segCoordsR[0][1][1]);
  if (showJoins) 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]);
    if (showJoins) ellipse(segCoordsR[i][1][0],segCoordsR[i][1][1],5,5);
  }
  curveVertex(segCoordsL[segs.length-1][1][0],segCoordsL[segs.length-1][1][1]);
  endShape();
  */
  //beginShape();
  //stroke(0,200,255);
  //noFill();
  curveVertex(segCoordsR[segs.length-1][1][0],segCoordsR[segs.length-1][1][1]);
  if (showJoins) ellipse(segCoordsR[0][1][0],segCoordsR[0][1][1],5,5);
  for (int i=segs.length-1; i>=0; i--){
    curveVertex(segCoordsR[i][1][0],segCoordsR[i][1][1]);
    if (showJoins) ellipse(segCoordsR[i][1][0],segCoordsR[i][1][1],5,5);
  }
  curveVertex(segCoordsL[0][1][0],segCoordsL[0][1][1]);
    curveVertex(segCoordsL[0][1][0],segCoordsL[0][1][1]);

  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 inside=isInsideTrunk();
  if(inside>0)
    alignToMouse();
  else
    pushBack(inside);  
}

void alignToMouse(){
  float[] last={0,0,0};
  float a;
  float b;
  last=segs[segs.length-1].getStart();
  for (int i=segs.length-1; i>=0; i--){
   a=segs[i].getAngle((int)last[0], (int)last[1],mouseX, mouseY); 
   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();
  } 
}

void pushBack(float diff){
  float[] xya=segs[0].getStart();
  float mouseA=segs[0].getAngle((int)xya[0], (int)xya[1], (int)mouseX, (int)mouseY);
  for (int i=segs.length-1; i>=0; i--){
     if(mouseA>=segs[i].getAbsAngle())
       segs[i].rotateS(-PI/500*i);
     else
       segs[i].rotateS(PI/500*i);
  }
}

float 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)/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=maxSegFlex;
  } 
  
  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;
  }
  
  void contract(float a){
    
  }
  
  float getAngle(int sx, int sy, int ex, int ey){
   return atan2(ey-sy,ex-sx); 
  }
  
  float angleDiff(float ang){
    return ang-(startA+flexA);
  }
  
  float getAbsAngle(){
    float[] xya=segs[0].getStart();
    float[] end=this.getEnd();
    float AA=this.getAngle((int)xya[0], (int)xya[1],(int)end[0], (int)end[1]);
    return AA;
  }
  
}