/* --------------------------------------------------------------------------
 * SimpleOpenNI User3d Test
 * --------------------------------------------------------------------------
 * Processing Wrapper for the OpenNI/Kinect library
 * http://code.google.com/p/simple-openni
 * --------------------------------------------------------------------------
 * prog:  Max Rheiner / Interaction Design / zhdk / http://iad.zhdk.ch/
 * date:  02/16/2011 (m/d/y)
 * ----------------------------------------------------------------------------
 * this demos is at the moment only for 1 user, will be implemented later
 * ----------------------------------------------------------------------------
 */

import SimpleOpenNI.*;

SimpleOpenNI context;

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      autoCalib=true;

PVector      bodyCenter = new PVector();
PVector      bodyDir = new PVector();

PImage bodyTextures[] = new PImage[11];

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;

void setup()
{
  size(1024, 768, P3D);  // strange, get drawing error in the cameraFrustum if i use P3D, in opengl there is no problem
  context = new SimpleOpenNI(this);

  println(SimpleOpenNI.SKEL_HEAD);

  // disable mirror
  context.setMirror(false);

  // enable depthMap generation 
  if (context.enableDepth() == false)
  {
    println("Can't open the depthMap, maybe the camera is not connected!"); 
    exit();
    return;
  }

  // enable skeleton generation for all joints
  context.enableUser(SimpleOpenNI.SKEL_PROFILE_ALL);

  stroke(255, 255, 255);
  smooth();  
  perspective(radians(45), 
  float(width)/float(height), 
  10, 150000);

  noCursor();
  
  loadBodyTextures();
}

void loadBodyTextures() {
  String basepath = "/Users/rtwomey/Pictures/kpuppet/rauschenberg_";

  bodyTextures[HEAD_NECK]=loadImage(basepath+"HEAD_NECK.jpg");
  bodyTextures[LEFT_SHOULDER_LEFT_ELBOW]=loadImage(basepath+"LEFT_SHOULDER_LEFT_ELBOW.jpg"); 
  bodyTextures[LEFT_ELBOW_LEFT_HAND]=loadImage(basepath+"LEFT_ELBOW_LEFT_HAND.jpg");
  bodyTextures[LEFT_HIP_LEFT_KNEE]=loadImage(basepath+"LEFT_HIP_LEFT_KNEE.jpg");
  bodyTextures[LEFT_KNEE_LEFT_FOOT]=loadImage(basepath+"LEFT_KNEE_LEFT_FOOT.jpg");
  bodyTextures[RIGHT_SHOULDER_RIGHT_ELBOW]=loadImage(basepath+"RIGHT_SHOULDER_RIGHT_ELBOW.jpg"); 
  bodyTextures[RIGHT_ELBOW_RIGHT_HAND]=loadImage(basepath+"RIGHT_ELBOW_RIGHT_HAND.jpg");
  bodyTextures[RIGHT_HIP_RIGHT_KNEE]=loadImage(basepath+"RIGHT_HIP_RIGHT_KNEE.jpg");
  bodyTextures[RIGHT_KNEE_RIGHT_FOOT]=loadImage(basepath+"RIGHT_KNEE_RIGHT_FOOT.jpg");
  bodyTextures[SHOULDERS_HIPS]=loadImage(basepath+"SHOULDERS_HIPS.jpg");
  bodyTextures[LEFT_HIP_RIGHT_HIP]=loadImage(basepath+"LEFT_HIP_RIGHT_HIP.jpg");
  
  println("done loading images.");
}

void draw()
{
  // update the cam
  context.update();

  background(0, 0, 0);

  // set the scene pos
  translate(width/2, height/2, 0);
  rotateX(rotX);
  rotateY(rotY);
  scale(zoomF);

  int[]   depthMap = context.depthMap();
  int     steps   = 3;  // to speed up the drawing, draw every third point
  int     index;
  PVector realWorldPoint;

  translate(0, 0, -1000);  // set the rotation center of the scene 1000 infront of the camera

  //  stroke(100); 
  //  for(int y=0;y < context.depthHeight();y+=steps)
  //  {
  //    for(int x=0;x < context.depthWidth();x+=steps)
  //    {
  //      index = x + y * context.depthWidth();
  //      if(depthMap[index] > 0)
  //      { 
  //        // draw the projected point
  //        realWorldPoint = context.depthMapRealWorld()[index];
  //        point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);
  //      }
  //    } 
  //  } 

  // draw the skeleton if it's available
  int[] userList = context.getUsers();
  for (int i=0;i<userList.length;i++)
  {
    if (context.isTrackingSkeleton(userList[i]))
      drawSkeleton(userList[i]);
  }    

  // draw the kinect cam
  //context.drawCamFrustum();
}

// draw the skeleton with the selected joints
void drawSkeleton(int userId)
{
  //strokeWeight(3);
  noStroke();
  
  drawTexturedTorso(userId, SimpleOpenNI.SKEL_LEFT_SHOULDER, SimpleOpenNI.SKEL_RIGHT_SHOULDER,
                            SimpleOpenNI.SKEL_RIGHT_HIP, SimpleOpenNI.SKEL_LEFT_HIP, SHOULDERS_HIPS);  

  drawTexturedLimb(userId, SimpleOpenNI.SKEL_HEAD, SimpleOpenNI.SKEL_NECK, HEAD_NECK);

  //drawTexturedLimb(userId, SimpleOpenNI.SKEL_NECK, SimpleOpenNI.SKEL_LEFT_SHOULDER);
  drawTexturedLimb(userId, SimpleOpenNI.SKEL_LEFT_SHOULDER, SimpleOpenNI.SKEL_LEFT_ELBOW, LEFT_SHOULDER_LEFT_ELBOW);
  drawTexturedLimb(userId, SimpleOpenNI.SKEL_LEFT_ELBOW, SimpleOpenNI.SKEL_LEFT_HAND, LEFT_ELBOW_LEFT_HAND);

  //drawLimb(userId, SimpleOpenNI.SKEL_NECK, SimpleOpenNI.SKEL_RIGHT_SHOULDER);
  drawTexturedLimb(userId, SimpleOpenNI.SKEL_RIGHT_SHOULDER, SimpleOpenNI.SKEL_RIGHT_ELBOW, RIGHT_SHOULDER_RIGHT_ELBOW);
  drawTexturedLimb(userId, SimpleOpenNI.SKEL_RIGHT_ELBOW, SimpleOpenNI.SKEL_RIGHT_HAND, RIGHT_ELBOW_RIGHT_HAND);

  //drawTexturedLimb(userId, SimpleOpenNI.SKEL_LEFT_SHOULDER, SimpleOpenNI.SKEL_TORSO);
  //drawTexturedLimb(userId, SimpleOpenNI.SKEL_RIGHT_SHOULDER, SimpleOpenNI.SKEL_TORSO);

  //drawLimb(userId, SimpleOpenNI.SKEL_TORSO, SimpleOpenNI.SKEL_LEFT_HIP);
  drawTexturedLimb(userId, SimpleOpenNI.SKEL_LEFT_HIP, SimpleOpenNI.SKEL_LEFT_KNEE, LEFT_HIP_LEFT_KNEE);
  drawTexturedLimb(userId, SimpleOpenNI.SKEL_LEFT_KNEE, SimpleOpenNI.SKEL_LEFT_FOOT, LEFT_KNEE_LEFT_FOOT);

  //drawLimb(userId, SimpleOpenNI.SKEL_TORSO, SimpleOpenNI.SKEL_RIGHT_HIP);
  drawTexturedLimb(userId, SimpleOpenNI.SKEL_RIGHT_HIP, SimpleOpenNI.SKEL_RIGHT_KNEE, RIGHT_HIP_RIGHT_KNEE);
  drawTexturedLimb(userId, SimpleOpenNI.SKEL_RIGHT_KNEE, SimpleOpenNI.SKEL_RIGHT_FOOT, RIGHT_KNEE_RIGHT_FOOT);  
    
  drawTexturedHips(userId, SimpleOpenNI.SKEL_LEFT_HIP, SimpleOpenNI.SKEL_RIGHT_HIP, LEFT_HIP_RIGHT_HIP);  

  
  strokeWeight(1);
}

void drawTexturedLimb(int userId, int jointType1, int jointType2, int currtex) {
  PVector jointPos1 = new PVector();
  PVector jointPos2 = new PVector();
  float  confidence;

  // draw the joint position
  confidence = context.getJointPositionSkeleton(userId, jointType1, jointPos1);
  confidence = context.getJointPositionSkeleton(userId, jointType2, jointPos2);

  float sx=screenX(jointPos1.x, jointPos1.y, jointPos1.z);
  float sy=screenY(jointPos1.x, jointPos1.y, jointPos1.z);
  
  println(sx);
  println(sy);
  
  float tw=(float)bodyTextures[currtex].width;
  float th=-50.0;
  
  
  beginShape();
  texture(bodyTextures[currtex]);
  vertex(jointPos1.x-tw, jointPos1.y-th, jointPos1.z, 0.0, 0.0);
  vertex(jointPos1.x+tw, jointPos1.y-th, jointPos1.z, (float)bodyTextures[currtex].width, 0.0);
  vertex(jointPos2.x+tw, jointPos2.y+th, jointPos2.z, (float)bodyTextures[currtex].width, (float)bodyTextures[currtex].height);
  vertex(jointPos2.x-tw, jointPos2.y+th, jointPos2.z, 0.0, (float) bodyTextures[currtex].height);
  endShape();

}

void drawTexturedTorso(int userId, int jt1, int jt2, int jt3, int jt4, int currtex) {
  PVector jp1 = new PVector();
  PVector jp2 = new PVector();
  PVector jp3 = new PVector();
  PVector jp4 = new PVector();

  float  confidence;

  // draw the joint position
  confidence = context.getJointPositionSkeleton(userId, jt1, jp1);
  confidence = context.getJointPositionSkeleton(userId, jt2, jp2);
  confidence = context.getJointPositionSkeleton(userId, jt3, jp3);
  confidence = context.getJointPositionSkeleton(userId, jt4, jp4);
  
  float tw=50.0;//(float)bodyTextures[currtex].width;
  float th=-50.0;
  beginShape();
  texture(bodyTextures[currtex]);
  vertex(jp1.x-tw, jp1.y-th, jp1.z, 0.0, 0.0);
  vertex(jp2.x+tw, jp2.y-th, jp2.z, (float)bodyTextures[currtex].width, 0.0);
  vertex(jp3.x+tw, jp3.y+th, jp3.z, (float)bodyTextures[currtex].width, (float)bodyTextures[currtex].height);
  vertex(jp4.x-tw, jp4.y-th, jp4.z, 0.0, (float) bodyTextures[currtex].height);
  endShape();
}

void drawTexturedHips(int userId, int jt1, int jt2, int currtex) {
  PVector jp1 = new PVector();
  PVector jp2 = new PVector();
  
  float  confidence;

  // draw the joint position
  confidence = context.getJointPositionSkeleton(userId, jt1, jp1);
  confidence = context.getJointPositionSkeleton(userId, jt2, jp2);
  
  float tw=50.0;//(float)bodyTextures[currtex].width;
  float th=-50.0-(float)bodyTextures[currtex].width;
  
  beginShape();
  texture(bodyTextures[currtex]);
  vertex(jp1.x-tw, jp1.y-th, jp1.z, 0.0, 0.0);
  vertex(jp2.x+tw, jp2.y-th, jp2.z, (float)bodyTextures[currtex].width, 0.0);
  vertex(jp2.x+tw, jp2.y+th, jp2.z, (float)bodyTextures[currtex].width, (float)bodyTextures[currtex].height);
  vertex(jp1.x+tw, jp1.y-th, jp1.z, 0.0, (float) bodyTextures[currtex].height);
  endShape();
}
// -----------------------------------------------------------------
// SimpleOpenNI user events

void onNewUser(int userId)
{
  println("onNewUser - userId: " + userId);
  println("  start pose detection");

  if (autoCalib)
    context.requestCalibrationSkeleton(userId, true);
  else    
    context.startPoseDetection("Psi", userId);
}

void onLostUser(int userId)
{
  println("onLostUser - userId: " + userId);
}

void onExitUser(int userId)
{
  println("onExitUser - userId: " + userId);
}

void onReEnterUser(int userId)
{
  println("onReEnterUser - userId: " + userId);
}


void onStartCalibration(int userId)
{
  println("onStartCalibration - userId: " + userId);
}

void onEndCalibration(int userId, boolean successfull)
{
  println("onEndCalibration - userId: " + userId + ", successfull: " + successfull);

  if (successfull) 
  { 
    println("  User calibrated !!!");
    context.startTrackingSkeleton(userId);
  } 
  else 
  { 
    println("  Failed to calibrate user !!!");
    println("  Start pose detection");
    context.startPoseDetection("Psi", userId);
  }
}

void onStartPose(String pose, int userId)
{
  println("onStartdPose - userId: " + userId + ", pose: " + pose);
  println(" stop pose detection");

  context.stopPoseDetection(userId); 
  context.requestCalibrationSkeleton(userId, true);
}

void onEndPose(String pose, int userId)
{
  println("onEndPose - userId: " + userId + ", pose: " + pose);
}

// -----------------------------------------------------------------
// Keyboard events

void keyPressed()
{
  switch(key)
  {
  case ' ':
    context.setMirror(!context.mirror());
    break;
  }

  switch(keyCode)
  {
  case LEFT:
    rotY += 0.1f;
    break;
  case RIGHT:
    // zoom out
    rotY -= 0.1f;
    break;
  case UP:
    if (keyEvent.isShiftDown())
      zoomF += 0.01f;
    else
      rotX += 0.1f;
    break;
  case DOWN:
    if (keyEvent.isShiftDown())
    {
      zoomF -= 0.01f;
      if (zoomF < 0.01)
        zoomF = 0.01;
    }
    else
      rotX -= 0.1f;
    break;
  }
}

void getBodyDirection(int userId, PVector centerPoint, PVector dir)
{
  PVector jointL = new PVector();
  PVector jointH = new PVector();
  PVector jointR = new PVector();
  float  confidence;

  // draw the joint position
  confidence = context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_LEFT_SHOULDER, jointL);
  confidence = context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_HEAD, jointH);
  confidence = context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_RIGHT_SHOULDER, jointR);

  // take the neck as the center point
  confidence = context.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_NECK, centerPoint);

  /*  // manually calc the centerPoint
   PVector shoulderDist = PVector.sub(jointL,jointR);
   centerPoint.set(PVector.mult(shoulderDist,.5));
   centerPoint.add(jointR);
   */

  PVector up = new PVector();
  PVector left = new PVector();

  up.set(PVector.sub(jointH, centerPoint));
  left.set(PVector.sub(jointR, centerPoint));

  dir.set(up.cross(left));
  dir.normalize();
}