/**
 * Reach 2  
 * based on code from Keith Peters.
 * 
 * The arm follows the position of the mouse by
 * calculating the angles with atan2(). 
 */

int numSegments = 2;
int numLimbs=4;
float[][] x = new float[numLimbs][numSegments];
float[][] y = new float[numLimbs][numSegments];
float[][] angle = new float[numLimbs][numSegments];
float[][] len = new float[numLimbs][numSegments];

float segLength = 100;
float targetX, targetY;

void setup() {
  size(1280, 720);
  strokeWeight(20.0);
  stroke(255, 100);
  
  // set base coords
  x[0][1]=300;
  y[0][1]=300;
  x[1][1]=400;
  y[1][1]=300;
  x[2][1]=380;
  y[2][1]=400;
  x[3][1]=320;
  y[3][1]=400;
  
  len[0][1]=len[1][1]=190;
  len[0][0]=len[1][0]=90;
  len[2][1]=len[3][1]=140;
  len[2][0]=len[3][0]=120;
  
  
//  x[x.length-1] = width/2;     // Set base x-coordinate
//  y[x.length-1] = height;  // Set base y-coordinate
}

void draw() {
  background(0);
  
  reachSegment(0, 0, mouseX, mouseY);
  reachSegment(1, 0, mouseX, mouseY);
  reachSegment(2, 0, mouseX, mouseY);
  reachSegment(3, 0, mouseX, mouseY);
  
  for(int j=0; j<numLimbs; j++)
  for(int i=1; i<numSegments; i++) {
    reachSegment(j, i, targetX, targetY);
  }
  
  for(int j=0; j<numLimbs; j++)
  for(int i=x[j].length-1; i>=1; i--) {
    positionSegment(j, i, i-1);  
  } 
  for(int j=0; j<numLimbs; j++)
  for(int i=0; i<x[j].length; i++) {
    segment(j, i, x[j][i], y[j][i], angle[j][i], (i+1)*2); 
  }
}

void positionSegment(int l, int a, int b) {
  x[l][b] = x[l][a] + cos(angle[l][a]) * len[l][a];//segLength;
  y[l][b] = y[l][a] + sin(angle[l][a]) * len[l][a];//segLength; 
}

void reachSegment(int l, int i, float xin, float yin) {
  float dx = xin - x[l][i];
  float dy = yin - y[l][i];
  angle[l][i] = atan2(dy, dx);  
  targetX = xin - cos(angle[l][i]) * len[l][i];//segLength;
  targetY = yin - sin(angle[l][i]) * len[l][i];//segLength;
}

void segment(int j, int i, float x, float y, float a, float sw) {
  strokeWeight(sw);
  pushMatrix();
  translate(x, y);
  rotate(a);
  line(0, 0, len[j][i], 0);
  //line(0, 0, segLength, 0);
  popMatrix();
}