//
//   to start osceleton streaming from command line
//  ./osceleton -w -a 192.168.0.45 -i simpleloop.oni

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

OscP5 oscP5, oscP5Send;
NetAddress myRemoteLocation;

PaperDoll[] dolls;
int NUM_DOLLS = 4;
float MILLIS_PER_FRAME = 33.3333333;

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);
int framenum =0;
        
boolean drawDoll=true;
boolean drawDollTexture=false;
boolean drawDollOutline=true;
boolean drawSticks=true;
boolean drawMarkers=true;
boolean drawTextCoords=false;
boolean sendOSC=true;
boolean printOSC=false;
boolean printCoords=false;

int lasttime=0, thistime;
final int CLEAR_DELAY=200;

boolean heartOn=false;
boolean paused=false;

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[] = { 
  -400, -100, 100, 400
};

boolean showlive=true;
float speed = 0.4;

void setup() {
  
  //size(800, 600, OPENGL); //Keep 4/3 aspect ratio, since it matches the kinect's.
  size(1280, 800, OPENGL);

  //noCursor();
  
  oscP5 = new OscP5(this, "127.0.0.1", 7110);
  //oscP5 = new OscP5(this, "192.168.0.231", 7110);
  
  oscP5Send = new OscP5(this, 12000);
  // max/MSP on leo's computer
  //myRemoteLocation = new NetAddress("127.0.0.1", 7400 );
  //myRemoteLocation = new NetAddress("192.168.0.127", 7400 );
  myRemoteLocation = new NetAddress("169.254.102.159", 7400 );

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

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



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

  strokeWeight(10);
  stroke(128, 128, 0);
  noFill();
  float qw=width/4.0;
  rect(rec_id*qw, 0, qw, height);
  noStroke();

  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 (true) { //(i!=rec_id)) || (!recording)) {
      if ((skFrames[i]!=null) && (skFrames[i].size()>0)) {
        if((i==rec_id) && recording) {
          framenum=skFrames[i].size()-1;
        } else {
          if(!paused)
            framenum=(int(millis()/MILLIS_PER_FRAME))%(skFrames[i].size());
        }
        
        Skeleton s = (Skeleton) skFrames[i].get(framenum);


//        if (drawMarkers) {
//          //s.drawSticks(xoffset[i]);
//          pushMatrix();
//          //translate(xoffset[i], 0, 0);
//          //scale(width, height, -300.0);
//          fill(255, 255, 0);
//          strokeWeight(10);
//          stroke(128, 128, 0);
//          s.drawDots(xoffset[i]);
//          //s.drawDots(0);
//          popMatrix();
//        }

        if (drawDoll || drawSticks) {
          // draw doll
          pushMatrix();
          scale(width, height, -300.0);
          dolls[rec_id].calcScreenCoords(s);
          //dolls[rec_id].printScreenCoords();
          popMatrix();

          if (drawSticks) {
            pushMatrix();
            translate(xoffset[i], 0, 0);
            dolls[rec_id].drawOSCeleton();
            popMatrix();
          }

          dolls[rec_id].constrainCoords();
          
          if(sendOSC)
            dolls[rec_id].sendCoords(rec_id);
            
          if (drawDoll) {
            pushMatrix();
            translate(xoffset[i], 0, 0);
            dolls[rec_id].drawDoll();
            if(printCoords)
              dolls[rec_id].printConstrainedCoords();
            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);
  
    // heartlight
  if(sendOSC)
    sendHeartBeat();
}

void sendHeartBeat() {

      OscMessage myMessage = new OscMessage("/heartlight");
      //myMessage.add(i);
      if(heartOn) {
        myMessage.add(1.0); /* add an int to the osc message */
      } else {
        myMessage.add(0.0);
      }
      heartOn=!heartOn;
      
      /* send the message */
      oscP5Send.send(myMessage, myRemoteLocation);
}

void keyPressed() 
{
  switch(key) {
  case 'r':
    recording = !recording;
//    thistime=millis();
//    if(thistime-lasttime<CLEAR_DELAY) {
//      if(skFrames[rec_id].size()>0)
//        skFrames[rec_id].clear();
//      println("erased all frames");
//    }
//    lasttime=thistime;

    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':
    if(skFrames[rec_id].size()>0)
      skFrames[rec_id].clear();
    println("erased all frames");
    break;
  case 'n':
    recording=false;
    rec_id=(rec_id+1)%skFrames.length;
    break;
  case 'l':
    showlive=!showlive;
    break;
  case 'd':
    drawDoll=!drawDoll;
    break;
  case 'm':
    drawMarkers=!drawMarkers;
    break;
  case 't':
    drawTextCoords=!drawTextCoords;
    break;
  case ' ':
    paused=!paused;
    break;
  case 'p':
    printCoords=!printCoords;
    break;
//  case '+':
//    speed+=0.01;
//    break;
//  case '-':
//    speed-=0.01;
//    break;
  }

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