import processing.core.*; 
import processing.data.*; 

import processing.opengl.*; 
import processing.opengl.*; 
import oscP5.*; 
import netP5.*; 

import java.applet.*; 
import java.awt.Dimension; 
import java.awt.Frame; 
import java.awt.event.MouseEvent; 
import java.awt.event.KeyEvent; 
import java.awt.event.FocusEvent; 
import java.awt.Image; 
import java.io.*; 
import java.net.*; 
import java.text.*; 
import java.util.*; 
import java.util.zip.*; 
import java.util.regex.*; 

public class bufferDollMoCap extends PApplet {

 




OscP5 oscP5;

PaperDoll[] dolls;
int NUM_DOLLS = 4;

int [] jointTypes = new int[23];
float        zoomF =0.5f;
float        rotX = radians(180);  // by default rotate the whole scene 180deg around the x-axis, 
// the data from openni comes upside down
float        rotY = radians(0);
boolean drawDoll=false;
boolean drawMarkers=true;
int ballSize = 10;
Hashtable<Integer, Skeleton> skels = new Hashtable<Integer, Skeleton>();

ArrayList<Skeleton> skFrames[] = (ArrayList<Skeleton>[]) new ArrayList[NUM_DOLLS];
int rec_id=0;
boolean recording=false;

int xoffset[] = { 
  -500, -175, 175, 500
};

boolean showlive=true;
float speed = 0.4f;

public void setup() {
  size(800, 600, OPENGL); //Keep 4/3 aspect ratio, since it matches the kinect's.
  oscP5 = new OscP5(this, "127.0.0.1", 7110);
  //oscP5 = new OscP5(this, "192.168.0.231", 7110);

  textFont(createFont("Verdana", 24));

  //hint(ENABLE_OPENGL_4X_SMOOTH);
  noStroke();

  dolls = new PaperDoll[NUM_DOLLS];

  println(dolls.length);

  // cut paper animation
  for (int i=0;i<NUM_DOLLS;i++) {
    //dolls[i].loadBodyTextures();
    dolls[i] = new PaperDoll();
    dolls[i].sayHello();
    dolls[i].loadBodyTextures();
  }
}


/* incoming osc message are forwarded to the oscEvent method. */
// Here you can easily see the format of the OSC messages sent. For each user, the joints are named with 
// the joint named followed by user ID (head0, neck0 .... r_foot0; head1, neck1.....)
public void oscEvent(OscMessage msg) {

  //msg.print();

  if (msg.checkAddrPattern("/joint") && msg.checkTypetag("sifff")) {
    // We have received joint coordinates, let's find out which skeleton/joint and save the values ;)

    Integer id = msg.get(1).intValue();
    Skeleton s = skels.get(id);

    if (s == null) {
      s = new Skeleton(id);
      skels.put(id, s);
    }
    if (msg.get(0).stringValue().equals("head")) {
      s.headCoords[0] = msg.get(2).floatValue();
      s.headCoords[1] = msg.get(3).floatValue();
      s.headCoords[2] = msg.get(4).floatValue();
    }
    else if (msg.get(0).stringValue().equals("neck")) {
      s.neckCoords[0] = msg.get(2).floatValue();
      s.neckCoords[1] = msg.get(3).floatValue();
      s.neckCoords[2] = msg.get(4).floatValue();
    }
    else if (msg.get(0).stringValue().equals("r_collar")) {
      s.rCollarCoords[0] = msg.get(2).floatValue();
      s.rCollarCoords[1] = msg.get(3).floatValue();
      s.rCollarCoords[2] = msg.get(4).floatValue();
    }
    else if (msg.get(0).stringValue().equals("r_shoulder")) {
      s.rShoulderCoords[0] = msg.get(2).floatValue();
      s.rShoulderCoords[1] = msg.get(3).floatValue();
      s.rShoulderCoords[2] = msg.get(4).floatValue();
    }
    else if (msg.get(0).stringValue().equals("r_elbow")) {
      s.rElbowCoords[0] = msg.get(2).floatValue();
      s.rElbowCoords[1] = msg.get(3).floatValue();
      s.rElbowCoords[2] = msg.get(4).floatValue();
    }
    else if (msg.get(0).stringValue().equals("r_wrist")) {
      s.rWristCoords[0] = msg.get(2).floatValue();
      s.rWristCoords[1] = msg.get(3).floatValue();
      s.rWristCoords[2] = msg.get(4).floatValue();
    }
    else if (msg.get(0).stringValue().equals("r_hand")) {
      s.rHandCoords[0] = msg.get(2).floatValue();
      s.rHandCoords[1] = msg.get(3).floatValue();
      s.rHandCoords[2] = msg.get(4).floatValue();
    }
    else if (msg.get(0).stringValue().equals("r_finger")) {
      s.rFingerCoords[0] = msg.get(2).floatValue();
      s.rFingerCoords[1] = msg.get(3).floatValue();
      s.rFingerCoords[2] = msg.get(4).floatValue();
    }
    else if (msg.get(0).stringValue().equals("r_collar")) {
      s.lCollarCoords[0] = msg.get(2).floatValue();
      s.lCollarCoords[1] = msg.get(3).floatValue();
      s.lCollarCoords[2] = msg.get(4).floatValue();
    }  
    else if (msg.get(0).stringValue().equals("l_shoulder")) {
      s.lShoulderCoords[0] = msg.get(2).floatValue();
      s.lShoulderCoords[1] = msg.get(3).floatValue();
      s.lShoulderCoords[2] = msg.get(4).floatValue();
    }
    else if (msg.get(0).stringValue().equals("l_elbow")) {
      s.lElbowCoords[0] = msg.get(2).floatValue();
      s.lElbowCoords[1] = msg.get(3).floatValue();
      s.lElbowCoords[2] = msg.get(4).floatValue();
    }
    else if (msg.get(0).stringValue().equals("l_wrist")) {
      s.lWristCoords[0] = msg.get(2).floatValue();
      s.lWristCoords[1] = msg.get(3).floatValue();
      s.lWristCoords[2] = msg.get(4).floatValue();
    }
    else if (msg.get(0).stringValue().equals("l_hand")) {
      s.lHandCoords[0] = msg.get(2).floatValue();
      s.lHandCoords[1] = msg.get(3).floatValue();
      s.lHandCoords[2] = msg.get(4).floatValue();
    }
    else if (msg.get(0).stringValue().equals("l_finger")) {
      s.lFingerCoords[0] = msg.get(2).floatValue();
      s.lFingerCoords[1] = msg.get(3).floatValue();
      s.lFingerCoords[2] = msg.get(4).floatValue();
    }
    else if (msg.get(0).stringValue().equals("torso")) {
      s.torsoCoords[0] = msg.get(2).floatValue();
      s.torsoCoords[1] = msg.get(3).floatValue();
      s.torsoCoords[2] = msg.get(4).floatValue();
    }
    else if (msg.get(0).stringValue().equals("r_hip")) {
      s.rHipCoords[0] = msg.get(2).floatValue();
      s.rHipCoords[1] = msg.get(3).floatValue();
      s.rHipCoords[2] = msg.get(4).floatValue();
    } 
    else if (msg.get(0).stringValue().equals("r_knee")) {
      s.rKneeCoords[0] = msg.get(2).floatValue();
      s.rKneeCoords[1] = msg.get(3).floatValue();
      s.rKneeCoords[2] = msg.get(4).floatValue();
    } 
    else if (msg.get(0).stringValue().equals("r_ankle")) {
      s.rAnkleCoords[0] = msg.get(2).floatValue();
      s.rAnkleCoords[1] = msg.get(3).floatValue();
      s.rAnkleCoords[2] = msg.get(4).floatValue();
    } 
    else if (msg.get(0).stringValue().equals("r_foot")) {
      s.rFootCoords[0] = msg.get(2).floatValue();
      s.rFootCoords[1] = msg.get(3).floatValue();
      s.rFootCoords[2] = msg.get(4).floatValue();

      // right foot is always last, add to frames array
      if (recording) {
        Skeleton skF = new Skeleton(s);// s.clone();
        skFrames[rec_id].add(skF);
      }
    } 
    else if (msg.get(0).stringValue().equals("l_hip")) {
      s.lHipCoords[0] = msg.get(2).floatValue();
      s.lHipCoords[1] = msg.get(3).floatValue();
      s.lHipCoords[2] = msg.get(4).floatValue();
    } 
    else if (msg.get(0).stringValue().equals("l_knee")) {
      s.lKneeCoords[0] = msg.get(2).floatValue();
      s.lKneeCoords[1] = msg.get(3).floatValue();
      s.lKneeCoords[2] = msg.get(4).floatValue();
    } 
    else if (msg.get(0).stringValue().equals("l_ankle")) {
      s.lAnkleCoords[0] = msg.get(2).floatValue();
      s.lAnkleCoords[1] = msg.get(3).floatValue();
      s.lAnkleCoords[2] = msg.get(4).floatValue();
    } 
    else if (msg.get(0).stringValue().equals("l_foot")) {
      s.lFootCoords[0] = msg.get(2).floatValue();
      s.lFootCoords[1] = msg.get(3).floatValue();
      s.lFootCoords[2] = msg.get(4).floatValue();
    }
  }
  else if (msg.checkAddrPattern("/new_user") && msg.checkTypetag("i")) {
    // A new user is in front of the kinect... Tell him to do the calibration pose!
    println("New user with ID = " + msg.get(0).intValue());
  }
  else if (msg.checkAddrPattern("/new_skel") && msg.checkTypetag("i")) {
    //New skeleton calibrated! Lets create it!
    Integer id = msg.get(0).intValue();
    Skeleton s = new Skeleton(id);
    skels.put(id, s);
  }
  else if (msg.checkAddrPattern("/lost_user") && msg.checkTypetag("i")) {
    //Lost user/skeleton
    Integer id = msg.get(0).intValue();
    println("Lost user " + id);
    skels.remove(id);
  }
}



public void draw()
{
  background(0);  
  ambientLight(64, 64, 64);
  lightSpecular(255, 255, 255);
  directionalLight(224, 224, 224, .5f, 1, -1);

  if (showlive) {
    for (Skeleton s: skels.values()) {
      //fill(s.colors[0], s.colors[1], s.colors[2]);
      fill(255, 0, 0);
      for (float[] j: s.allCoords) {
        pushMatrix();
        translate(j[0]*width, j[1]*height, -j[2]*300);
        sphere(2 * ballSize/j[2]);
        popMatrix();
      }
    }
  }
  //if (!recording) {
  for (int i=0;i<skFrames.length; i++) {
    if ((i!=rec_id) || (!recording)) {
      if ((skFrames[i]!=null) && (skFrames[i].size()>0)) {
        int framenum=(PApplet.parseInt(frameCount*speed))%(skFrames[i].size());
        Skeleton s = (Skeleton) skFrames[i].get(framenum);

        fill(255, 255, 0);

        if (drawMarkers) {
          for (float[] j: s.allCoords) {
            pushMatrix();
            translate(j[0]*width+xoffset[i], j[1]*height, -j[2]*300);
            sphere(2 * ballSize/j[2]);
            popMatrix();
          }
        }

        if (drawDoll) {
          // draw doll
          pushMatrix();

          translate(xoffset[i], 0, 0);
          scale(width, height, -300.0f);

          dolls[rec_id].calcScreenCoords(s);
          //dolls[rec_id].printScreenCoords();
          popMatrix();

          dolls[rec_id].constrainCoords();
          //dolls[rec_id].printConstrainedCoords();

          pushMatrix();
          //translate(-xoffset[i]*0.3, 0, 0);
          dolls[rec_id].drawDoll();
          popMatrix();
        }
      }
    }
    else {
      fill(128);
      text("recording", 20, 120);
    }
  }

  fill(128);
  text("character "+(rec_id+1), 20, 30);
  if (skFrames[rec_id]!=null)
    text("frames "+skFrames[rec_id].size(), 20, 60);
  //text("speed "+String.format("%.2g%n", speed), 20, 90);
}

public void keyPressed() 
{
  switch(key) {
  case 'r':
    recording = !recording;
    if (recording) {
      if (skFrames[rec_id]==null)
        skFrames[rec_id] = new ArrayList();
      print("start recording to buffer ");
      println(rec_id);
    } 
    else {
      print("done recording. loop has ");
      print(skFrames[rec_id].size()-1);
      println(" frames.");
    }
    break;
  case 'c':
    skFrames[rec_id].clear();
    println("erased all frames");
    break;
  case 'n':
    rec_id=(rec_id+1)%skFrames.length;
    break;
  case 'l':
    showlive=!showlive;
    break;
  case 'd':
    drawDoll=!drawDoll;
    break;
  case 'm':
    drawMarkers=!drawMarkers;
    break;
  case '+':
    speed+=0.01f;
    break;
  case '-':
    speed-=0.01f;
    break;
  }

  switch(keyCode)
  {
  case LEFT:
    xoffset[rec_id]-=10;
    break;
  case RIGHT:
    // zoom out
    xoffset[rec_id]+=10;
    break;
  }
}


// segment names, my scheme
//static final int HEAD_NECK=0;
//static final int RIGHT_SHOULDER_RIGHT_ELBOW=1; 
//static final int RIGHT_ELBOW_RIGHT_HAND=2;
//static final int RIGHT_HIP_RIGHT_KNEE=3;
//static final int RIGHT_KNEE_RIGHT_FOOT=4;
//static final int LEFT_SHOULDER_LEFT_ELBOW=5; 
//static final int LEFT_ELBOW_LEFT_HAND=6; 
//static final int LEFT_HIP_LEFT_KNEE=7;
//static final int LEFT_KNEE_LEFT_FOOT=8;
//static final int SHOULDERS_HIPS=9;
//static final int LEFT_HIP_RIGHT_HIP=10;

static final int HEAD_NECK=0;
static final int LEFT_SHOULDER_LEFT_ELBOW=1; 
static final int LEFT_ELBOW_LEFT_HAND=2; 
static final int LEFT_HIP_LEFT_KNEE=3;
static final int LEFT_KNEE_LEFT_FOOT=4;
static final int RIGHT_SHOULDER_RIGHT_ELBOW=5; 
static final int RIGHT_ELBOW_RIGHT_HAND=6;
static final int RIGHT_HIP_RIGHT_KNEE=7;
static final int RIGHT_KNEE_RIGHT_FOOT=8;
static final int SHOULDERS_HIPS=9;
static final int LEFT_HIP_RIGHT_HIP=10;


int[] attachX = {
  -150, 
  // left arm
  0,//65, 
  0,//0,//65, 
  // left leg
  -100, 
  -200,//-120,//45, 
  // right arm
  0,//-65, 
  0,//-65, 
  // right leg
  100, 
  200,//120,//-45, 
  0, 
  0, 
  0
};

int[] attachY = {
  -25, 
  // left arm
  -10, 
  -10, 
  // left leg
  -120, 
  -170, 
  // right arm
  -10, 
  -5, 
  // right leg
  -120, 
  -170, 
  0, 
  0, 
  0
};

class PaperDoll { 

  // joint names
  static final int HEAD=0;
  static final int NECK=1;
  static final int RIGHT_COLLAR=2;
  static final int RIGHT_SHOULDER=3; 
  static final int RIGHT_ELBOW=4;
  static final int RIGHT_WRIST=5; 
  static final int RIGHT_HAND=6; 
  static final int RIGHT_FINGER=7;
  static final int LEFT_COLLAR=8;
  static final int LEFT_SHOULDER=9; 
  static final int LEFT_ELBOW=10; 
  static final int LEFT_WRIST=11;
  static final int LEFT_HAND=12;
  static final int LEFT_FINGER=13;
  static final int TORSO=14;
  static final int RIGHT_HIP=15;
  static final int RIGHT_KNEE=16;
  static final int RIGHT_ANKLE=17;
  static final int RIGHT_FOOT=18;//18;  
  static final int LEFT_HIP=19;
  static final int LEFT_KNEE=20;
  static final int LEFT_ANKLE=21;
  static final int LEFT_FOOT=22;//22;

  
  //static final int numSegments = 2;
  //static final int numLimbs=5;
  static final int numSegments=23;//15;

  float SCALE_FACTOR=0.1f;

  float[] sx = new float[numSegments];
  float[] sy = new float[numSegments];

  float[] x = new float[numSegments];
  float[] y = new float[numSegments];
  float[] angle = new float[numSegments];
  float[] len = new float[numSegments];

  int[] texNums = new int[numSegments];

  float segLength = 100;

  float targetX, targetY;

  PImage bodyTextures[];// = new PImage[11];

  PaperDoll() {

    // assign texture indices
    texNums[0]=HEAD_NECK;
    texNums[1]=RIGHT_SHOULDER_RIGHT_ELBOW; 
    texNums[2]=RIGHT_ELBOW_RIGHT_HAND;
    texNums[3]=RIGHT_HIP_RIGHT_KNEE;
    texNums[4]=RIGHT_KNEE_RIGHT_FOOT;
    texNums[5]=LEFT_SHOULDER_LEFT_ELBOW;
    texNums[6]=LEFT_ELBOW_LEFT_HAND;
    texNums[7]=LEFT_HIP_LEFT_KNEE;
    texNums[8]=LEFT_KNEE_LEFT_FOOT;
    texNums[9]=SHOULDERS_HIPS;
    
    bodyTextures = new PImage[11];
    println("doll constructed");
    
  }


  
  
  public void calcScreenCoords(Skeleton skelly) {
  
    // convert from Kinect 3d skeleton coordinates
    // to 2d screen space coorddinates.
      
    for (int i=0;i<numSegments;i++) {
  
      PVector jointPos1 = new PVector();
      //println(userId);
      //println(jointTypes[i]);
  
      //float confidence = context.getJointPositionSkeleton(userId, jointTypes[i], jointPos1);
      jointPos1.x=skelly.allCoords[i][0];
      jointPos1.y=skelly.allCoords[i][1];
      jointPos1.z=skelly.allCoords[i][2];
      
      //println(jointPos1);
      
      float x=screenX(jointPos1.x, jointPos1.y, jointPos1.z);
      float y=screenY(jointPos1.x, jointPos1.y, jointPos1.z);
  
//      print(x); print(","); println(y);
  
      sx[i]=x;
      sy[i]=y;
      
    }
//    println("***");
  }


  public void printScreenCoords() {
  
    // convert from Kinect 3d skeleton coordinates
    // to 2d screen space coorddinates.
    
    println("*** screen coords *** ");
      
    for (int i=0;i<numSegments;i++) {
     print(sx[i]); print(","); println(sy[i]);      
    }
    println("***");
  }
  
  
  public void printConstrainedCoords() {
  
    println("*** constrained coords *** ");
    for (int i=0;i<10;i++) {
     print(x[i]); print(","); println(y[i]);      
    }
    println("***");
  }
  
  public void constrainCoords() {
    // constrain screen coords to paper doll model
    
    float x1, y1, x2, y2;
    x1=sx[NECK];
    y1=sy[NECK];
    x2=sx[TORSO];
    y2=sy[TORSO];

    // calculate angle of torso from neck and torso control points
    angle[SHOULDERS_HIPS]=atan2(y2-y1, x2-x1);
    
    SCALE_FACTOR = dist(x1, y1, x2, y2)/len[SHOULDERS_HIPS]*3.5f;

//    print(dist(x1,y1,x2,y2)); print(" ");
//    print(len[SHOULDERS_HIPS]); print(" ");
//    println(SCALE_FACTOR);
    
    // make length invariate
    x2=x1+cos(angle[SHOULDERS_HIPS])*len[SHOULDERS_HIPS]*SCALE_FACTOR;
    y2=y1+sin(angle[SHOULDERS_HIPS])*len[SHOULDERS_HIPS]*SCALE_FACTOR;
    x1=x1+cos(angle[SHOULDERS_HIPS]-PI)*len[SHOULDERS_HIPS]*SCALE_FACTOR*0.07f;
    y1=y1+sin(angle[SHOULDERS_HIPS]-PI)*len[SHOULDERS_HIPS]*SCALE_FACTOR*0.07f;
    
    x[HEAD_NECK]=x1;
    y[HEAD_NECK]=y1;
    angle[HEAD_NECK]=angle[SHOULDERS_HIPS]-PI;

    x[SHOULDERS_HIPS]=x2;
    y[SHOULDERS_HIPS]=y2;
    
    // shoulders
    x[LEFT_SHOULDER_LEFT_ELBOW]=sx[LEFT_SHOULDER];
    y[LEFT_SHOULDER_LEFT_ELBOW]=sy[LEFT_SHOULDER];
    x[RIGHT_SHOULDER_RIGHT_ELBOW]=sx[RIGHT_SHOULDER];
    y[RIGHT_SHOULDER_RIGHT_ELBOW]=sy[RIGHT_SHOULDER];

    // hips
    x[LEFT_HIP_LEFT_KNEE]=sx[LEFT_HIP];
    y[LEFT_HIP_LEFT_KNEE]=sy[LEFT_HIP];
    x[RIGHT_HIP_RIGHT_KNEE]=sx[RIGHT_HIP];
    y[RIGHT_HIP_RIGHT_KNEE]=sy[RIGHT_HIP];

    // reachLimbs
    reachLimbs();
  }

  public void reachLimbs() {
    // left arm
    reachSegment(LEFT_ELBOW_LEFT_HAND, sx[LEFT_HAND], sy[LEFT_HAND]);
    reachSegment(LEFT_SHOULDER_LEFT_ELBOW, targetX, targetY);
    positionSegment(LEFT_SHOULDER_LEFT_ELBOW, LEFT_ELBOW_LEFT_HAND);

    // right arm
    reachSegment(RIGHT_ELBOW_RIGHT_HAND, sx[RIGHT_HAND], sy[RIGHT_HAND]);
    reachSegment(RIGHT_SHOULDER_RIGHT_ELBOW, targetX, targetY);
    positionSegment(RIGHT_SHOULDER_RIGHT_ELBOW, RIGHT_ELBOW_RIGHT_HAND);

    // left leg
    reachSegment(LEFT_KNEE_LEFT_FOOT, sx[LEFT_FOOT], sy[LEFT_FOOT]);
    reachSegment(LEFT_HIP_LEFT_KNEE, targetX, targetY);
    positionSegment(LEFT_HIP_LEFT_KNEE, LEFT_KNEE_LEFT_FOOT);

    // right leg
    reachSegment(RIGHT_KNEE_RIGHT_FOOT, sx[RIGHT_FOOT], sy[RIGHT_FOOT]);
    reachSegment(RIGHT_HIP_RIGHT_KNEE, targetX, targetY);
    positionSegment(RIGHT_HIP_RIGHT_KNEE, RIGHT_KNEE_RIGHT_FOOT);
  }

  public void reachSegment(int seg, float xin, float yin) {
    float dx = xin - x[seg];
    float dy = yin - y[seg];
    angle[seg] = atan2(dy, dx);  
    
//    targetX = xin - (cos(angle[seg]) * len[seg] + attachX[seg])*SCALE_FACTOR;
//    targetY = yin - (sin(angle[seg]) * len[seg] + attachY[seg])*SCALE_FACTOR;
    targetX = xin - (cos(angle[seg]) * len[seg])*SCALE_FACTOR;
    targetY = yin - (sin(angle[seg]) * len[seg])*SCALE_FACTOR;
  }

  public void positionSegment(int seg1, int seg2) {
    x[seg2] = x[seg1] + (cos(angle[seg1]) * len[seg1] + attachX[seg1])*SCALE_FACTOR;
    y[seg2] = y[seg1] + (sin(angle[seg1]) * len[seg1] + attachY[seg1])*SCALE_FACTOR;
  }


  // drawing textured doll to screen
  
  public void drawDoll() {
    drawTorso();
    drawHead();
    drawLimbs();
    
    fill(255);
    
    //text("sf "+SCALE_FACTOR, 20, 120);
  }

  public void drawTorso() {

    int tex=SHOULDERS_HIPS;

    float dx1=SCALE_FACTOR*(float)bodyTextures[SHOULDERS_HIPS].width*cos((angle[SHOULDERS_HIPS]+HALF_PI));
    float dy1=SCALE_FACTOR*(float)bodyTextures[SHOULDERS_HIPS].width*sin((angle[SHOULDERS_HIPS]+HALF_PI));

//    float dx2=SCALE_FACTOR*(float)bodyTextures[SHOULDERS_HIPS].width*cos((angle[SHOULDERS_HIPS]-HALF_PI));
//    float dy2=SCALE_FACTOR*(float)bodyTextures[SHOULDERS_HIPS].width*sin((angle[SHOULDERS_HIPS]-HALF_PI));

    // calculate four corners of torso
    float x1, y1, x2, y2, x3, y3, x4, y4;
    
    // shoulders
    x1=x[HEAD_NECK]+dx1;
    y1=y[HEAD_NECK]+dy1;
    x4=x[HEAD_NECK]-dx1;
    y4=y[HEAD_NECK]+dy1;

    // hips
    x2=x[SHOULDERS_HIPS]+dx1;
    y2=y[SHOULDERS_HIPS]-dy1;
    x3=x[SHOULDERS_HIPS]-dx1;
    y3=y[SHOULDERS_HIPS]-dy1;

//    x1=x[HEAD_NECK]-dx1;
//    y1=y[HEAD_NECK]+dy1;
//    x4=x[HEAD_NECK]-dx2;
//    y4=y[HEAD_NECK]+dy1;
//
//    // hips
//    x2=x[SHOULDERS_HIPS]-dx1;
//    y2=y[SHOULDERS_HIPS]+dy2;
//    x3=x[SHOULDERS_HIPS]-dx2;
//    y3=y[SHOULDERS_HIPS]+dy2;


    noStroke();
    beginShape();
    texture(bodyTextures[tex]);
    vertex(x1, y1, 0.0f, 0.0f, 0.0f);
    vertex(x2, y2, 0.0f, 0.0f, (float)bodyTextures[tex].height);
    vertex(x3, y3, 0.0f, (float)bodyTextures[tex].width, (float)bodyTextures[tex].height);
    vertex(x4, y4, (float) bodyTextures[tex].width, 0.0f);
    endShape();
    
    //text("dx1 "+dx1+" \ndy1 "+dy1, 20, 150);
    //text("dx2 "+dx2+" \ndy2 "+dy2, 20, 210);

  }

  public void drawHead() {

    // calc head coords
    // from neck position and head angle
    int tex=HEAD_NECK;

    float tw=SCALE_FACTOR*2.0f*(float)bodyTextures[tex].width*cos((angle[HEAD_NECK]-HALF_PI));//bodyTextures[currtex].width;
    float th=SCALE_FACTOR*2.0f*(float)bodyTextures[tex].width*sin((angle[HEAD_NECK]-HALF_PI));

    float x2=x[HEAD_NECK]-attachX[HEAD_NECK]*SCALE_FACTOR;
    float y2=y[HEAD_NECK]-attachY[HEAD_NECK]*SCALE_FACTOR;

    float x1=x2+len[HEAD_NECK]*SCALE_FACTOR*cos(angle[HEAD_NECK]);
    float y1=y2+len[HEAD_NECK]*SCALE_FACTOR*sin(angle[HEAD_NECK]);

    // draw head
    noStroke();
    beginShape();
    texture(bodyTextures[tex]);
    vertex(x1, y1, 0.0f, 0.0f, 0.0f);
    vertex(x2, y2, 0.0f, 0.0f, (float)bodyTextures[tex].height);
    vertex(x2+tw, y2+th, 0.0f, (float)bodyTextures[tex].width, (float)bodyTextures[tex].height);
    vertex(x1+tw, y1+th, 0.0f, (float) bodyTextures[tex].width, 0.0f);    
    endShape();
  }

  public void drawLimbs() {
    // left arm
    drawTexturedSegment(LEFT_SHOULDER_LEFT_ELBOW, x[LEFT_SHOULDER_LEFT_ELBOW], y[LEFT_SHOULDER_LEFT_ELBOW], angle[LEFT_SHOULDER_LEFT_ELBOW]);
    drawTexturedSegment(LEFT_ELBOW_LEFT_HAND, x[LEFT_ELBOW_LEFT_HAND], y[LEFT_ELBOW_LEFT_HAND], angle[LEFT_ELBOW_LEFT_HAND]);

    // right arm
    drawTexturedSegment(RIGHT_SHOULDER_RIGHT_ELBOW, x[RIGHT_SHOULDER_RIGHT_ELBOW], y[RIGHT_SHOULDER_RIGHT_ELBOW], angle[RIGHT_SHOULDER_RIGHT_ELBOW]);
    drawTexturedSegment(RIGHT_ELBOW_RIGHT_HAND, x[RIGHT_ELBOW_RIGHT_HAND], y[RIGHT_ELBOW_RIGHT_HAND], angle[RIGHT_ELBOW_RIGHT_HAND]);

    // left leg
    drawTexturedSegment(LEFT_HIP_LEFT_KNEE, x[LEFT_HIP_LEFT_KNEE], y[LEFT_HIP_LEFT_KNEE], angle[LEFT_HIP_LEFT_KNEE]);
    drawTexturedSegment(LEFT_KNEE_LEFT_FOOT, x[LEFT_KNEE_LEFT_FOOT], y[LEFT_KNEE_LEFT_FOOT], angle[LEFT_KNEE_LEFT_FOOT]);

    // right leg
    drawTexturedSegment(RIGHT_HIP_RIGHT_KNEE, x[RIGHT_HIP_RIGHT_KNEE], y[RIGHT_HIP_RIGHT_KNEE], angle[RIGHT_HIP_RIGHT_KNEE]);
    drawTexturedSegment(RIGHT_KNEE_RIGHT_FOOT, x[RIGHT_KNEE_RIGHT_FOOT], y[RIGHT_KNEE_RIGHT_FOOT], angle[RIGHT_KNEE_RIGHT_FOOT]);
  }


  public void drawTexturedSegment(int seg, float x, float y, float a) {

    int currtex = texNums[seg];
    x=x-attachX[seg]*SCALE_FACTOR;
    y=y-attachY[seg]*SCALE_FACTOR;
    float tw=SCALE_FACTOR*(float)bodyTextures[currtex].width*cos((a-HALF_PI));//bodyTextures[currtex].width;
    float th=SCALE_FACTOR*(float)bodyTextures[currtex].width*sin((a-HALF_PI));

    float x2=x+len[seg]*cos(a)*SCALE_FACTOR;
    float y2=y+len[seg]*sin(a)*SCALE_FACTOR;

    noStroke();
    beginShape();
    texture(bodyTextures[currtex]);

    vertex(x-tw, y-th, 0.0f, 0.0f, 0.0f);
    vertex(x2-tw, y2-th, 0.0f, 0.0f, (float)bodyTextures[currtex].height);
    vertex(x2+tw, y2+th, 0.0f, (float)bodyTextures[currtex].width, (float)bodyTextures[currtex].height);
    vertex(x+tw, y+th, 0.0f, (float) bodyTextures[currtex].width, 0.0f);
    endShape();
  }


  public void loadBodyTextures() {
    
      println("hello");

    String basepath = "warrior_princess_screen/";

    bodyTextures[HEAD_NECK]=loadImage(basepath+"HEAD_NECK.png");
    bodyTextures[LEFT_SHOULDER_LEFT_ELBOW]=loadImage(basepath+"LEFT_SHOULDER_LEFT_ELBOW.png"); 
    bodyTextures[LEFT_ELBOW_LEFT_HAND]=loadImage(basepath+"LEFT_ELBOW_LEFT_HAND.png");
    bodyTextures[LEFT_HIP_LEFT_KNEE]=loadImage(basepath+"LEFT_HIP_LEFT_KNEE.png");
    bodyTextures[LEFT_KNEE_LEFT_FOOT]=loadImage(basepath+"LEFT_KNEE_LEFT_FOOT.png");
    bodyTextures[RIGHT_SHOULDER_RIGHT_ELBOW]=loadImage(basepath+"RIGHT_SHOULDER_RIGHT_ELBOW.png"); 
    bodyTextures[RIGHT_ELBOW_RIGHT_HAND]=loadImage(basepath+"RIGHT_ELBOW_RIGHT_HAND.png");
    bodyTextures[RIGHT_HIP_RIGHT_KNEE]=loadImage(basepath+"RIGHT_HIP_RIGHT_KNEE.png");
    bodyTextures[RIGHT_KNEE_RIGHT_FOOT]=loadImage(basepath+"RIGHT_KNEE_RIGHT_FOOT.png");
    bodyTextures[SHOULDERS_HIPS]=loadImage(basepath+"SHOULDERS_HIPS.png");
    //bodyTextures[LEFT_HIP_RIGHT_HIP]=loadImage(basepath+"LEFT_HIP_RIGHT_HIP.jpg");

    println("done loading body textures.");

    len[HEAD_NECK]=bodyTextures[HEAD_NECK].height;
    len[LEFT_SHOULDER_LEFT_ELBOW]=bodyTextures[LEFT_SHOULDER_LEFT_ELBOW].height;
    len[RIGHT_SHOULDER_RIGHT_ELBOW]=bodyTextures[RIGHT_SHOULDER_RIGHT_ELBOW].height;
    len[LEFT_ELBOW_LEFT_HAND]=bodyTextures[LEFT_ELBOW_LEFT_HAND].height;
    len[RIGHT_ELBOW_RIGHT_HAND]=bodyTextures[RIGHT_ELBOW_RIGHT_HAND].height;
    len[LEFT_HIP_LEFT_KNEE]=bodyTextures[LEFT_HIP_LEFT_KNEE].height;
    len[RIGHT_HIP_RIGHT_KNEE]=bodyTextures[RIGHT_HIP_RIGHT_KNEE].height;
    len[LEFT_KNEE_LEFT_FOOT]=bodyTextures[LEFT_KNEE_LEFT_FOOT].height;
    len[RIGHT_KNEE_RIGHT_FOOT]=bodyTextures[RIGHT_KNEE_RIGHT_FOOT].height;
    len[SHOULDERS_HIPS]=bodyTextures[SHOULDERS_HIPS].height;
  }
  
  public void sayHello() {
    println("stupid message.");
  }
}

class Skeleton {
  // We just use this class as a structure to store the joint coordinates sent by OSC.
  // The format is {x, y, z}, where x and y are in the [0.0, 1.0] interval, 
  // and z is in the [0.0, 7.0] interval.
  float headCoords[] = new float[3];
  float neckCoords[] = new float[3];
  float rCollarCoords[] = new float[3];
  float rShoulderCoords[] = new float[3];
  float rElbowCoords[] = new float[3];
  float rWristCoords[] = new float[3];
  float rHandCoords[] = new float[3];
  float rFingerCoords[] = new float[3];
  float lCollarCoords[] = new float[3];
  float lShoulderCoords[] = new float[3];
  float lElbowCoords[] = new float[3];
  float lWristCoords[] = new float[3];
  float lHandCoords[] = new float[3];
  float lFingerCoords[] = new float[3];
  float torsoCoords[] = new float[3];
  float rHipCoords[] = new float[3];
  float rKneeCoords[] = new float[3];
  float rAnkleCoords[] = new float[3];
  float rFootCoords[] = new float[3];
  float lHipCoords[] = new float[3];
  float lKneeCoords[] = new float[3];
  float lAnkleCoords[] = new float[3];
  float lFootCoords[] = new float[3];
  float[] allCoords[] = {headCoords, neckCoords, rCollarCoords, rShoulderCoords, rElbowCoords, rWristCoords,
                       rHandCoords, rFingerCoords, lCollarCoords, lShoulderCoords, lElbowCoords, lWristCoords,
                       lHandCoords, lFingerCoords, torsoCoords, rHipCoords, rKneeCoords, rAnkleCoords,
                       rFootCoords, lHipCoords, lKneeCoords, lAnkleCoords, lFootCoords};
                    
  int id; //here we store the skeleton's ID as assigned by OpenNI and sent through OSC.
  float colors[] = {255, 255, 255};// The color of this skeleton

  Skeleton(int id) {
    this.id = id;
    colors[0] = random(128, 255);
    colors[1] = random(128, 255);
    colors[2] = random(128, 255);
  }
  
  public Skeleton(Skeleton mom) {
    // copy all data in source array
      for (int i=0; i < mom.allCoords.length; i++) {
        allCoords[i][0]=mom.allCoords[i][0];
        allCoords[i][1]=mom.allCoords[i][1];
        allCoords[i][2]=mom.allCoords[i][2];
      }  
        // Copy all the fields of Dog.
    }
}
  public int sketchWidth() { return 800; }
  public int sketchHeight() { return 600; }
  public String sketchRenderer() { return OPENGL; }
  static public void main(String args[]) {
    PApplet.main(new String[] { "bufferDollMoCap" });
  }
}