/* --------------------------------------------------------------------------
 * 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;
PaperDoll doll;

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();

//PVector[] sc = new PVector[14];
float []sx = new float[14];
float []sy = new float[14];

int [] jointTypes = new int[14];


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;

boolean recordFlag=false;

void setup()
{
  size(1400, 800, 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;
//  }

  if(recordFlag == false)
  {
    // playing, this works without the camera
    if ( context.openFileRecording("test.oni") == false)
    {
      println("can't find recording !!!!");
      exit();
    }

    // it's possible to run the sceneAnalyzer over the recorded data strea   
    if ( context.enableScene() == false)
    {
      println("can't setup scene!!!!");
      exit();
      return;
    }
    
    println("This file has " + context.framesPlayer() + " frames.");

  } else {
      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();

  
  doll=new PaperDoll();
  doll.loadBodyTextures();
  assignJointTypes();
}

void assignJointTypes() {

  jointTypes[PaperDoll.HEAD]=SimpleOpenNI.SKEL_HEAD;
  jointTypes[PaperDoll.NECK]=SimpleOpenNI.SKEL_NECK;
  jointTypes[PaperDoll.LEFT_SHOULDER]=SimpleOpenNI.SKEL_LEFT_SHOULDER;
  jointTypes[PaperDoll.LEFT_ELBOW]=SimpleOpenNI.SKEL_LEFT_ELBOW;
  jointTypes[PaperDoll.LEFT_HAND]=SimpleOpenNI.SKEL_LEFT_HAND;
  jointTypes[PaperDoll.RIGHT_SHOULDER]=SimpleOpenNI.SKEL_RIGHT_SHOULDER;
  jointTypes[PaperDoll.RIGHT_ELBOW]=SimpleOpenNI.SKEL_RIGHT_ELBOW;
  jointTypes[PaperDoll.RIGHT_HAND]=SimpleOpenNI.SKEL_RIGHT_HAND;
  jointTypes[PaperDoll.LEFT_HIP]=SimpleOpenNI.SKEL_LEFT_HIP;
  jointTypes[PaperDoll.LEFT_KNEE]=SimpleOpenNI.SKEL_LEFT_KNEE;
  jointTypes[PaperDoll.LEFT_FOOT]=SimpleOpenNI.SKEL_LEFT_FOOT;
  jointTypes[PaperDoll.RIGHT_HIP]=SimpleOpenNI.SKEL_RIGHT_HIP;
  jointTypes[PaperDoll.RIGHT_KNEE]=SimpleOpenNI.SKEL_RIGHT_KNEE;
  jointTypes[PaperDoll.RIGHT_FOOT]=SimpleOpenNI.SKEL_RIGHT_FOOT;
}

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

  background(0, 0, 0);

  // set the scene pos

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

  
  //  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])) {
      pushMatrix();
      translate(width/2, height/2, 0);
      rotateX(rotX);
      rotateY(rotY);
      scale(zoomF);
      translate(0, 0, -1000);  // set the rotation center of the scene 1000 infront of the camera
      //drawSkeleton(userList[i]);
      calcScreenCoords(userList[i]);
      doll.constrainCoords(sx, sy);
      popMatrix();
      strokeWeight(5);
      stroke(255, 0, 0);
      doll.drawDoll();
    }
  }    

if(recordFlag == false)
    drawTimeline();
  
  // draw the kinect cam
  //context.drawCamFrustum();
}

void drawTimeline()
{
  pushStyle();

  stroke(255,255,0);
  line(10, height - 20, 
       width -10 ,height - 20);  

  stroke(0);
  rectMode(CENTER);
  fill(255,255,0);
  
  int pos = (int)((width - 2 * 10) * (float)context.curFramePlayer() / (float)context.framesPlayer());
  rect(pos, height - 20, 
       7,17);  

  popStyle();
}

// calc screen coords
void calcScreenCoords(int userId) {

  for (int i=0;i<14;i++) {

    //int i=SimpleOpenNI.SKEL_HEAD;

    PVector jointPos1 = new PVector();
    //println(userId);
    //println(jointTypes[i]);

    float confidence = context.getJointPositionSkeleton(userId, jointTypes[i], jointPos1);
    //println(jointPos1);
    //println(confidence);

    float x=screenX(jointPos1.x, jointPos1.y, jointPos1.z);
    float y=screenY(jointPos1.x, jointPos1.y, jointPos1.z);

    //println(x);
    //println(y);

    sx[i]=x;
    sy[i]=y;

    //    sc[i].x=screenX(jointPos.x, jointPos.y, jointPos.z); 
    //    sc[i].y=screenY(jointPos.x, jointPos.y, jointPos.z); 
    //    sc[i].z=0.0;

    //println(sc[i]);
  }
}

// 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 tw=(float)doll.bodyTextures[currtex].width;
  float th=-50.0;

  beginShape();
  texture(doll.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)doll.bodyTextures[currtex].width, 0.0);
  vertex(jointPos2.x+tw, jointPos2.y+th, jointPos2.z, (float)doll.bodyTextures[currtex].width, (float)doll.bodyTextures[currtex].height);
  vertex(jointPos2.x-tw, jointPos2.y+th, jointPos2.z, 0.0, (float) doll.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)doll.bodyTextures[currtex].width;
  float th=-50.0;
  beginShape();
  texture(doll.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)doll.bodyTextures[currtex].width, 0.0);
  vertex(jp3.x+tw, jp3.y+th, jp3.z, (float)doll.bodyTextures[currtex].width, (float)doll.bodyTextures[currtex].height);
  vertex(jp4.x-tw, jp4.y-th, jp4.z, 0.0, (float) doll.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)doll.bodyTextures[currtex].width;
  float th=-50.0-(float)doll.bodyTextures[currtex].width;

  beginShape();
  texture(doll.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)doll.bodyTextures[currtex].width, 0.0);
  vertex(jp2.x+tw, jp2.y+th, jp2.z, (float)doll.bodyTextures[currtex].width, (float)doll.bodyTextures[currtex].height);
  vertex(jp1.x+tw, jp1.y-th, jp1.z, 0.0, (float) doll.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();
}