Spin3d spin = new Spin3d();
PFont font;
Figure fig;
FPS fps = new FPS();

void setup() {
  size(400,400);
  framerate(30);
  depth();
  font = loadFont("font.vlw");
  textFont(font,10);
  angleMode(DEGREES);
  hint(DISABLE_SMOOTH_HACK);
  fig = new Figure(1.0f);
  updateActive(0);
  //fig.updateUp();
  ellipseMode(CENTER);
//  smooth();
}

void text(String s, float x, float y) {
  for (int i=0; i<s.length(); i++) {
    text(s.charAt(i),x,y);
    x+=font.width(s.charAt(i))*10;
  }
}
boolean edit = false;
boolean direction = false;
void keyPressed() {
  if (key=='e' || key=='E') edit = !edit;
  if (keyCode==KeyEvent.VK_ESCAPE) edit = false;
  if (key==' ') direction = !direction;
  if (key=='j') moveup = !moveup;
  if (!edit)
    switch (keyCode) {
      case UP:
        updateActive(-1);
        break;
      case DOWN:
        updateActive(1);
        break;
    }
  else
    switch (keyCode) {
      case UP:
        moveJoint(0,0,-1);
        break;
      case DOWN:
        moveJoint(0,0,1);
        break;
      case LEFT:
        moveJoint(direction?-1:0,direction?0:-1,0);
        break;
      case RIGHT:
        moveJoint(direction?1:0,direction?0:1,0);
        break;
    }
}
boolean moveup = false;
void moveJoint(float dx, float dy, float dz) {
  float factor = 0.02f;
  Joint j = fig.joints[active];
  if (j.locked) return;
  if (moveup) {
    j.ux+=dx*factor;
    j.uy+=dy*factor;
    j.uz+=dz*factor;
  } else {
    j.lx+=dx*factor;
    j.ly+=dy*factor;
    j.lz+=dz*factor;
  }
}
int active = 0;
void updateActive(int d) {
  fig.joints[active].active = false;
  active+=d;
  if (active<0) active = fig.joints.length-1;
  if (active>=fig.joints.length) active = 0;
  fig.joints[active].active = true;
}
void draw() {
  background(240);
  noStroke();
  push();
    translate(width/2,height/2);
    scale(80);
    spin.update();
    spin.apply();
    fig.draw();
  pop();
  push();
    fig.draw2d();
    translate(5,10);
    fig.drawTitles();
  pop();
  fps.frame();
  text(fps.fps+"[30]fps",width-45,10);
}
void mouseDragged() {
  spin.mouseMoved();
}
class Figure extends Joint {
  public Joint joints[];
  public Figure(float scale) {
    super("Root",true,0,1,0,0,scale,
      new Joint[]{
        new Joint("Stomach",true,0,0,-1, 0.8f, 1.0f, new Joint[]{
          new Joint("Chest",false,0,0,-1, 0.5f, 1.0f, new Joint[]{
            new Joint("Neck",false,0,0,-1, 0.3f),
            new Joint("Right Shoulder",true,1,0,0, 0.4f, 1.0f, new Joint[]{
              new Joint("Right Upper Arm",false,1,0,0,0.7f, 1.0f, new Joint[]{
                new Joint("Right Lower Arm",false,1,0,0,0.7f)
              })
            }),
            new Joint("Left Shoulder",true,-1,0,0, 0.4f, 1.0f, new Joint[]{
              new Joint("Left Upper Arm",false,-1,0,0,0.7f, 1.0f, new Joint[]{
                new Joint("Left Lower Arm",false,-1,0,0,0.7f)
              })
            })
          })
        }),
        new Joint("Right Hip",true,1,0,0.2f, 0.3f, 1.0f, new Joint[]{
          new Joint("Right Upper Leg",false,0.2f,0.2f,0.5f,1.0f, 1.0f, new Joint[]{
            new Joint("Right Lower Leg",false,0,0,1,1.0f,1.0f, new Joint[]{
              new Joint("Right Foot",false,0,1,0.0001f,0.3f)
            })
          })
        }),
        new Joint("Left Hip",true,-1,0,0.2f, 0.3f, 1.0f, new Joint[]{
          new Joint("Left Upper Leg",false,0,0,1,1.0f, 1.0f, new Joint[]{
            new Joint("Left Lower Leg",false,0,0,1,1.0f,1.0f, new Joint[]{
              new Joint("Left Foot",false,0,1,0.0001f,0.3f)
            })
          })
        }),
      }
    );
    joints = getJointList();
  }
}

class Joint {
  String label;
  float lx, ly, lz, scale, length;
  float ux=0,uy=1,uz=0;
  float x,y,z;
  
  boolean active = false;
  boolean locked = false;
  
  Joint parent = null;
  
  public Joint(String label, boolean locked, float lx, float ly, float lz, float length) {
    this(label,locked,lx,ly,lz,length,1.0f,null);
  }
  public Joint(String label,boolean locked, float lx, float ly, float lz, float length, float scale, Joint children[]) {
    this.label = label;
    this.locked = locked;
    this.length = length;
    this.scale = scale;
    this.lx = lx;
    this.ly = ly;
    this.lz = lz;
    if (children==null) children = new Joint[]{};
    this.children = children;
    for (int i =0; i<children.length; i++)
      children[i].parent = this;
  }
  
  Joint children[];

  void node(float r) {
    box(active?r*2:r);
  }
  
  float cos = cos(90);
  float sin = sin(90);
  void updateUp() {
  /*
    if (parent!=null && !locked) {
      ux = (lx+parent.x)/2;
      uy = (lx+parent.y)/2;
      uz = (lx+parent.z)/2;
    }
    for (int i=0; i<children.length; i++)
      children[i].updateUp();
//    uz = lz*cos - ly*sin;
//    uy = lz*sin + ly*cos;
*/
  }

  void drawCrosshair() {
    if (locked) return;
    stroke(0);
    line(0,0,-2,0,0,2);
    if (direction) line(2,0,0,-2,0,0);
    else line(0,2,0,0,-2,0);
    noStroke();
  }
  float tx,ty,tz;
  void draw() {
    push();
      scale(scale);
      fill(active?200:0,active?0:200,0);
      push();
        stroke(active?200:0,active?0:200,0);
        line(0,0,0,lx,ly,lz);
        noStroke();
        translate(lx,ly,lz);
        node(0.025f);
        if (edit&&active&&!moveup)
          drawCrosshair();
      pop();
      push();
        stroke(active?200:150,active?200:150,0);
        line(0,0,0,ux,uy,uz);
        noStroke();
        translate(ux,uy,uz);
        fill(active?200:150,active?200:150,0);
        node(0.03f);
        if (edit&&active&&moveup)
          drawCrosshair();
      pop();
      PGraphics3 g2 = (PGraphics3)g;
      push();
        lookat(0,0,0,-lx,ly,lz,ux,uy,uz);

        if (active)
          fill(180,0,0);
        else
          fill(80);
        beginShape(TRIANGLES);
          vertex(0,-0.05f,0);
          vertex(0,0.05f,0);
          vertex(0,0,-length);
          
          vertex(-0.1f,0,0);
          vertex(0.1f,0,0);
          vertex(0,0,-length);
        endShape();
        push();    
          translate(0,0,-length/2);
          tx = screenX(0,0,0)+8;
          ty = screenY(0,0,0)+4;
        pop();
        fill(active?200:0,0,active?0:200);
        node(0.05f);
        translate(0,0,-length);
        float m03 = g2.m03;
        float m13 = g2.m13;
        float m23 = g2.m23;
      pop();
      
      x = m03-g2.m03;
      y = m13-g2.m13;
      z = m23-g2.m23;

      g2.m03 = m03;
      g2.m13 = m13;
      g2.m23 = m23;
        
        drawChildren();
    pop();
  }
  int size() {
    int size = 1;
    for (int i=0; i<children.length; i++)
      size += children[i].size();
    return size;
  }
  Joint[] getJointList() {
    Joint list[] = new Joint[size()];
    getJointList(list,0);
    return list;
  }
  int getJointList(Joint list[], int offset) {
    list[offset++] = this;
    for (int i=0; i<children.length; i++)
      offset = children[i].getJointList(list,offset);
    return offset;
  }
  void drawChildren() {
    for (int i=0; i<children.length; i++)
      children[i].draw();
  }
  void draw2d() {
    fill(active?200:0,0,active?0:200);
    text(label,(int)tx,(int)ty);
    for (int i=0; i<children.length; i++)
      children[i].draw2d();
  }
  void drawTitles() {
    fill(active?200:0,0,0);
    if (active&&!locked)
      text(label+(edit?"[editing "+(moveup?"up":"aim")+"] - press j to switch":" - press e to edit"),0,0);
    else
      text(label,0,0);
    float y = screenY(0,0,0);
    translate(9,13,0);
    for (int i=0; i<children.length; i++) {
      stroke(0);
      float yy = y - screenY(0,0,0);
      line(-7,-2,-7,yy+5);
      line(-7,-2,-2,-2);
      children[i].drawTitles();
    }
    translate(-9,0,0);
  }
}