/* --------------------------------------------------------------------------
 * 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];

ArrayList frames;

PFont font;

int [] jointTypes = new int[14];


boolean recordFlag=false;

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

  font = loadFont("Courier-48.vlw");
  textFont(font);

  println(SimpleOpenNI.SKEL_HEAD);

  // disable mirror
  context.setMirror(false);

  if (recordFlag == false)
  {
    // playing, this works without the camera
    if ( context.openFileRecording("/Users/rtwomey/Movies/puppet/kinect_recordings/20120719_184947.oni") == false)
      //if ( context.openFileRecording("/Users/rtwomey/Movies/puppet/kinect_recordings/robert.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);
  context.start();

  // setup drawing and perspective
  stroke(255, 255, 255);
  smooth();  
  perspective(radians(45), float(width)/float(height), 10, 150000);
  noCursor();
  frameRate(30);

  // preload animation and skeleton tracking data
  frames = new ArrayList();
  preloadSkeletonPoints();

  // load paper doll graphics and data
  doll=new PaperDoll();
  doll.loadBodyTextures();
  assignJointTypes();
}

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

  int framenum=frameCount%(frames.size());

  background(0, 0, 0);

  fill(255);
  text(framenum, 10, 10);
  print(framenum);

  Frame currframe= (Frame) frames.get(framenum);

  // draw the skeleton if it's available
  if (currframe.isTracked()) {
    println("*");
    //drawSkeleton(userList[i]);
    getFrameCoords(currframe);
    doll.constrainCoords(sx, sy);

    strokeWeight(5);
    stroke(255, 0, 0);
    doll.drawDoll();
  } 
  else {
    println();
  }
  
    if ((context.nodes() & SimpleOpenNI.NODE_DEPTH) != 0)
  {
    if ((context.nodes() & SimpleOpenNI.NODE_IMAGE) != 0)
    {
      image(context.depthImage(), 0, 0);   
      image(context.rgbImage(), context.depthWidth() + 10, 0);
    }
    else
      image(context.depthImage(), 0, 0);
  }

}

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

//////// kinect stuff ////////

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;
}

//////// frame buffering ////////

void preloadSkeletonPoints() {

  print("Loading kinect tracking data... ");
  int tracked=0;

  //context.setPlaybackSpeedPlayer(0.0f);
  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
  
  context.seekPlayer(0, SimpleOpenNI.PLAYER_SEEK_SET);
  for (int f=0; f < context.framesPlayer(); f++) {
    context.seekPlayer(f, SimpleOpenNI.PLAYER_SEEK_SET);
    context.update();
    int[]   depthMap = context.depthMap();

    frames.add(new Frame());

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

        Frame currframe = (Frame) frames.get(frames.size()-1);

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

          PVector jointPos1 = new PVector();

          float confidence = context.getJointPositionSkeleton(userList[i], jointTypes[s], jointPos1);
          //print(f);

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

          currframe.storeCoords(s, x, y);
          if(s==13) {
            println(userList[i]);
            print(context.curFramePlayer());
            print(": ");
            print(jointPos1);
            print(" ");
            print(currframe.getX(0));
            print(",");
            println(currframe.getY(0));
          }            
        }


          //println(confidence);

        currframe.setTracked(true);
      }
    }
  }

  popMatrix();

  print("done loading. Loaded ");
  print(context.framesPlayer());
  print(" frames total, ");
  print(tracked);
  println(" frames with tracked skeleton");
}

// get screen coords from frame buffer
void getFrameCoords(Frame currframe) {

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

    sx[i]=currframe.getX(i);
    sy[i]=currframe.getY(i);
  }
}

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