let vec = p5.Vector
class IKJoint {
  constructor(numSegments, pointDist) {
    this.origin = new vec(0, 0);
    this.points = [];
    for (let i = 0; i < numSegments + 1; i++) {
      this.points.push(new ConstraintPoint(0, 0, pointDist, 1.3));
    }
    this.pointDist = pointDist;
    this.iterations = 20;
  }

  moveStart(targetX, targetY) {
    let numPts = this.points.length;
    this.points[0].size = 1;
    this.points[0].follow(targetX, targetY);

    for (let i = 1; i < numPts; i++) {
      let prevPoint = this.points[i - 1];
      this.points[i].follow(prevPoint.pos.x, prevPoint.pos.y, prevPoint.forward);
    }
  }

  moveEnd(targetX, targetY) {
    let numPts = this.points.length;
    this.points[numPts - 1].pos.x = targetX;
    this.points[numPts - 1].pos.y = targetY;

    for (let i = numPts - 2; i >= 0; i--) {
      let prevPoint = this.points[i + 1];
      this.points[i].follow(prevPoint.pos.x, prevPoint.pos.y, null);
    }
  }

  moveTo(origX, origY, targetX, targetY) {
    for (let i = 0; i < this.iterations; i++) {
      this.moveEnd(targetX, targetY);
      this.moveStart(origX, origY);
    }
  }
}

class ConstraintPoint {
  constructor(x, y, size, angleConstraint = 0.2) {
    this.size = size;
    this.pos = new vec(x, y);
    this.forward = new vec(0, 0);
    this.angleConstraint = angleConstraint;
  }

  getRel(angleOffset, dist) {
    let dir = vec.rotate(this.forward, angleOffset);
    dir.setMag(dist);
    return vec.add(this.pos, dir);
  }

  follow(targetX, targetY, parentForward = null) {
    let target = new vec(targetX, targetY);
    let targetForward = vec.sub(target, this.pos);
    let targetAngle = targetForward.heading();

    if (parentForward) {
      let parentAngle = parentForward.heading();
      let diff = targetAngle - parentAngle;

      while (diff < -PI) diff += TWO_PI;
      while (diff > PI) diff -= TWO_PI;

      let maxAngle = this.angleConstraint;
      diff = constrain(diff, -maxAngle, maxAngle);

      targetAngle = parentAngle + diff;
    }

    this.forward = p5.Vector.fromAngle(targetAngle);

    let dir = p5.Vector.mult(this.forward, -this.size);
    this.pos = vec.add(target, dir);
  }
}

let ik = new IKJoint(8, 20);
function setup() {
	createCanvas(400, 400);
}

function draw() {
  background(220);
	ik.moveTo(200, 200, mouseX, mouseY);

	for (let i = 0;i < 8;i++) {
		let p = ik.points[i].pos
		noFill();
		circle(p.x, p.y, 20);
	}

}