class List extends ArrayList{
  
  Object last(){
    if(size() == 0) return null;
    return get(size()-1);
  }
  
  Object first(){
    if(size()==0) return null;
    return get(0);
  }

}

// http://www.ryanjuckett.com/programming/animation/21-cyclic-coordinate-descent-in-2d?start=4

float simplifyAngle(float angle) {
  angle = angle % TWO_PI;
  if( angle < -PI )
    angle += (TWO_PI);
  else if( angle > PI )
    angle -= (TWO_PI);
  return angle;
}


class Bone {
  float x;
  float y;
  float angle;
  float cosAngle;
  float sinAngle;
  
  Bone(){
    
  }
  
  Bone(float x, float y, float angle){
    this.x = x;
    this.y = y;
    setAngle(angle);
  }
  
  Bone clone() {
    Bone result = new Bone();
    result.x = x;
    result.y = y;
    result.setAngle(angle);
    return result;
  }
  
  void setAngle(float angle) {
    this.angle = angle;
    cosAngle = cos(angle);
    sinAngle = sin(angle);
  }
  
}

float TRIVIAL_ARC_LENGTH = 0.00001;

int CCD_EXCEPTION = -1;
int CCD_SUCCESS = 0;
int CCD_PROCESSING = 1;
int CCD_FAILURE = 2;

class CCDResult{
  List bones;
  int status;
}

CCDResult iterateChain( List bones, PVector target, float arrivalDist) {
  
  CCDResult result = new CCDResult();
  if(bones.size() < 1) {
    println("Bone calculation requires a list with more than one bone");
    result.status = CCD_EXCEPTION;
    return result;
    
  }
  
  /* getWorldBones */
  List worldBones = new List();

  Bone root = ((Bone) bones.first()).clone();
  worldBones.add(root);
  for(int i = 1; i < bones.size(); i++) {
    Bone previous = (Bone) worldBones.get(i-1);
    Bone bone = (Bone) bones.get(i);
    Bone worldBone = new Bone();
    worldBone.x = previous.x + previous.cosAngle * bone.x
                             - previous.sinAngle * bone.y;
    worldBone.y = previous.y + previous.sinAngle * bone.x
                             + previous.cosAngle*bone.y;
    worldBone.setAngle(previous.angle + bone.angle);
    worldBones.add(worldBone);
  }
  /* --getWorldBones--*/
  
  float endX = ((Bone)worldBones.last()).x;
  float endY = ((Bone)worldBones.last()).y;
  
  boolean modifiedBones = false;
  for(int i = bones.size()-2; i >= 0; --i){
    Bone bone = (Bone)worldBones.get(i);
    PVector curToEnd = new PVector( endX - bone.x, endY - bone.y);
    PVector curToTarget = new PVector( target.x - bone.x, target.y - bone.y);
 
    float cosRotAng = 1;
    float sinRotAng = 0;
    float endTargetMag = (curToEnd.mag() * curToTarget.mag());
    if(endTargetMag > EPSILON){
      cosRotAng = (curToEnd.x * curToTarget.x + curToEnd.y * curToTarget.y) / endTargetMag;
      sinRotAng = (curToEnd.x * curToTarget.y - curToEnd.y * curToTarget.x) / endTargetMag;
    }
    // Clamp the cosine into range when computing the angle (might be out of range
    // due to floating point error).
    float rotAng = acos( max(-1, min(1,cosRotAng) ) );
    if( sinRotAng < 0.0 ) rotAng = -rotAng;
    //
    
    endX = bone.x + cosRotAng*curToEnd.x - sinRotAng*curToEnd.y;
    endY = bone.y + sinRotAng*curToEnd.x + cosRotAng*curToEnd.y;

    Bone resultBone = (Bone)bones.get(i);
    resultBone.setAngle( simplifyAngle( resultBone.angle + rotAng));
    
    // Check for termination
    PVector endToTarget = new PVector(target.x - endX, target.y - endY);
    if( endToTarget.mag() <= arrivalDist){
      // SHORT CIRCUIT LOOP (results in repetition because I'm not using references)
      result.bones = bones;
      result.status = CCD_SUCCESS;
    }
    if( !modifiedBones && abs(rotAng) * curToEnd.mag() > TRIVIAL_ARC_LENGTH)
      modifiedBones = true;
    
  }
  result.status = modifiedBones ? CCD_PROCESSING : CCD_FAILURE;
  // TODO see if it still works if I set this earlier
  result.bones = bones;
  return result;
}

List arm = new List();
int myLength = 20;
PImage aa,bb,cc;

void setup() {

  size(500, 700);
  arm.add(new Bone(width/2 ,  height/2 +300, 0));
  arm.add(new Bone(myLength,         0, 0));
  arm.add(new Bone(myLength,         0, 0));
  arm.add(new Bone(myLength,         0, 0));
  arm.add(new Bone(myLength,         0, 0));
  arm.add(new Bone(myLength,         0, 0));
  arm.add(new Bone(myLength,         0, 0));
  arm.add(new Bone(myLength,         0, 0));
  arm.add(new Bone(myLength,         0, 0));
  arm.add(new Bone(myLength,         0, 0));
  arm.add(new Bone(myLength,         0, 0));
  arm.add(new Bone(myLength,         0, 0));
  arm.add(new Bone(myLength,         0, 0));
  arm.add(new Bone(myLength,         0, 0));
  arm.add(new Bone(myLength,         0, 0));
  arm.add(new Bone(myLength,         0, 0));
  arm.add(new Bone(myLength,         0, 0));
  arm.add(new Bone(myLength,         0, 0));
  arm.add(new Bone(myLength,         0, 0));
  arm.add(new Bone(myLength,         0, 0));
  arm.add(new Bone(myLength,         0, 0));
  arm.add(new Bone(myLength,         0, 0));
  arm.add(new Bone(myLength,         0, 0));
  arm.add(new Bone(myLength,         0, 0));
  arm.add(new Bone(myLength,         0, 0));
  arm.add(new Bone(myLength,         0, 0));
  arm.add(new Bone(myLength,         0, 0));
  arm.add(new Bone(myLength,         0, 0));
  arm.add(new Bone(myLength,         0, 0));
  
  
  aa = loadImage("1.png");
  bb = loadImage("2.png");
  cc = loadImage("3.png");
  imageMode(CENTER);
}

void draw() {
  update();

  background(255);
  strokeWeight(5);
  stroke(0,0,0);
  drawBone(arm, 0);
  
}



void drawBone(List bones, int index) {
  
  Bone bone = (Bone) bones.get(index);

  pushMatrix();
  translate(bone.x,bone.y);
  rotate(bone.angle);
  
  if(index == 0){
    pushMatrix();
      rotate(-PI/2);
      image(aa, 0, -10);
    popMatrix();
  }else if(index == bones.size() -2){
    pushMatrix();
      rotate(-PI/2);
      image(cc, 0, 28); 
    popMatrix();
  }else{
    pushMatrix();
      rotate(-PI/2);
      image(bb, 0, 0); 
    popMatrix();
  }
  
  if(index < bones.size() - 2) {
    drawBone(bones,index + 1 );
  }
  popMatrix();
}

void update() {
  CCDResult result = iterateChain(arm, new PVector(mouseX, mouseY), 0.01);

  if(result.status != CCD_EXCEPTION && result.status != CCD_FAILURE) {
    arm = result.bones;
  }
}