import SimpleOpenNI.*;

int[] blackRect = {1, 1, 0, 1, 1, 1, 0, 1};

String[] blackName = {"ais", "cis", "dis", "eis", "fis", "gis", "tis", "lis"};

String[] blackNote = {"ais", "cis", "dis", "eis", "fis", "gis", "tis", "lis"};

String[] whiteName = {"a", "b", "c", "d", "e", "f", "g", "h", "i", "j"};

String[] whiteNote = {"a", "b", "c", "d", "e", "f", "g", "h", "i", "j"};


color[] couleur = {color(0), color(0), 0, color(0), color(0), color(0), 0, color(0)};

ArrayList<Key> keys = new ArrayList();

PGraphics internal;
boolean showImage=false;

SimpleOpenNI  myKinect;

PrintWriter output;

//... declaraciones de los vectores ...

    PVector rtKnee, leftKnee, rtShoulder, leftShoulder, rtElbow, leftElbow, rtHand,
leftHand, rtFoot, leftFoot;
    PVector rtHip, leftHip;

float x_coef;
float y_coef;


void setup() {

  myKinect = new SimpleOpenNI(this);

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

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

      myKinect.setMirror(false);

      //Enable the Depth Camera

      myKinect.enableDepth();
      int size_x = 1280;
      int size_y = 750;

      x_coef = (float) size_x / (float) myKinect.depthWidth();
      y_coef = (float) size_y / (float) myKinect.depthHeight();




      smooth();
      size(size_x, size_y, P3D);

  print("patience.");
  print(".");
  print(".");

  internal = createGraphics(1280, 750, P3D);

  print(".");

  internal.noSmooth();
  internal.beginDraw();

  print(".");
  internal.background(201);
  print("."); 

  internal.translate(width/2, height/2, 30);

  // defining all keys 


  //black Rect
  int largeur_black = 19; // width

  for (int x = 0; x < 8; x++) {

    // fill(couleur[x]);
    // -1 is for noStroke();

    if (blackRect[x] == 1)
      keys.add ( new Key (30-(2 * x * largeur_black), 110, largeur_black, 140,
        blackName[x], blackNote[x]+"-Note",
        couleur[x], -1,
        new PVector(94, 99, 255) ));
    keys.get(keys.size()-1).display_internal(  color(255, 0, 10*x)  );

    print(".");
  }//for

  // white Rect
  int largeur_white = 38;// width
  for (int i = 0; i < 9; i++) {
    keys.add ( new Key (30-(i*largeur_white), 110, largeur_white, 186,
      whiteName[i], whiteNote[i]+"-Note",
      color(128), color(0),
      new PVector(103, 100, 150)));
    keys.get(keys.size()-1).display_internal(  color(255, 255, 10*i)  );
    print(".");
  }//for

  internal.endDraw();
  println(".");

}// func

void mousePressed() {   // Cette partie ça doit être par la Kinect
  // all keys
  for (Key k : keys) {

    if (internal.get(mouseX, mouseY) == k.colKey) {
      //
      fill(255);

      println(k.name
        +" -> "
        +k.note);

    }//if
  }//for
}//func

void draw() {

  background(0);

//Get new data from the Kinect
      myKinect.update();

      //Draw camera image on the screenn
    //image(myKinect.rgbImage(),0,0);
       //image(myKinect.depthImage(),0,0);

      //Each new person is identified with a number, so draw up to 5 people
      //for(int userId=1; userId<=5; userId++){

        for (int userId=1; userId<=1;userId++){

          //Check to see if tracking

          if(myKinect.isTrackingSkeleton(userId)){

            //stroke(255,0,0);
            //strokeWeight(3);
            //drawSkeleton(userId);

            getBodyPoints(userId);

            //There are 24 possible joints that openNI tracks.  If we can get the point, draw it.   
            for(int bodyPart=1; bodyPart<=24; bodyPart++){

                //get the point as a vector
                PVector bodyPoint = getBodyPoint(userId, bodyPart);

                 //System.out.println("Body point: " + bodyPoint);

                fill(255);

                ellipse(bodyPoint.x, bodyPoint.y, 20, 20);
             }

             //draw the head bigger -- Demonstrates use of Constant SKEL_HEAD
             PVector headPoint = getBodyPoint(userId, SimpleOpenNI.SKEL_HEAD);

            ellipse(headPoint.x, headPoint.y, 50,50);

             PVector rtHand2D = new PVector(rtHand.x, rtHand.y);

             PVector rtElbow2D = new PVector(rtElbow.x, rtElbow.y);

             PVector rtShoulder2D = new PVector(rtShoulder.x, rtShoulder.y);

             PVector rtHip2D = new PVector(rtHip.x, rtHip.y);


             PVector torsoOrientation = PVector.sub(rtShoulder2D, rtHip2D);

             PVector upperArmOrientation = PVector.sub(rtElbow2D, rtShoulder2D);

             // calcula los ejes contra los cuales queremos medir nuestros ángulos
             float shoulderAngle = angleOf(rtElbow2D, rtShoulder2D,

torsoOrientation);
             float elbowAngle = angleOf(rtHand2D, rtElbow2D, upperArmOrientation);

             PVector ltHand2D = new PVector(leftHand.x, leftHand.y);

             PVector ltElbow2D = new PVector(rtElbow.x, leftElbow.y);

             PVector ltShoulder2D = new PVector(rtShoulder.x, leftShoulder.y);

             PVector ltHip2D = new PVector(leftHip.x, leftHip.y);


             PVector lttorsoOrientation = PVector.sub(ltShoulder2D, ltHip2D);

             float ltshoulderAngle = angleOf(ltElbow2D, ltShoulder2D, lttorsoOrientation);
             float ltelbowAngle = angleOf(rtHand2D, rtElbow2D, upperArmOrientation);

             rtShoulder.normalize();

             rtElbow.normalize();

             rtHand.normalize();

             leftShoulder.normalize();

             leftElbow.normalize();

             leftHand.normalize();



             println(
                     "Right Foot: " + (rtFoot.x) + "," + (rtFoot.y) + "," + (rtFoot.z) + "\n" +
                     "left Foot: " + (leftFoot.x) + "," + (leftFoot.y) + "," + (leftFoot.z) + "\n" +
                     "Right hand: " + (rtHand.x) + "," + (rtHand.y) + "," + (rtHand.z) + "\n" +
                     "Letf hand: " + (leftHand.x) + "," + (leftHand.y) + "," + (leftHand.z));


          }
          else
          {
            //print("Skeleton is not being tracked\n");
          }

  if (showImage)
  {     
    image(internal, 0, 0);
  } else
  {
    translate(width/2, height/2, 30);

    // box
    stroke(111);
    noFill();
    box(680, 400, 1000);

    // all keys
    for (Key k : keys) {
      k.display();
    }
  }
} 


//println(mouseX,mouseY);
      }
void keyPressed() {
  // toggle
  showImage=!showImage;
}

// ===========================================
// the class - a blueprint for one key

class Key {

  float x;

  float y;

  float w;

  float h;

  String name;

  String note;

  color colFill;

  color colStroke;

  color colKey;

  PVector translatePVector;

  //constr

  Key(float x_, float y_,
    float w_, float h_,
    String name_,
    String note_,
    color colFill_,
    color colStroke_,
    PVector translatePVector_) {

    //

    x=x_;

    y=y_;

    w=w_;

    h=h_;

    name=name_;

    note=note_;

    colFill = colFill_;

    colStroke = colStroke_;

    translatePVector = translatePVector_;
  }

//constr

  void display() {

    pushMatrix();

    translate(translatePVector.x,

      translatePVector.y,

      translatePVector.z);

    rotateX(PI/2);

    fill(colFill);

    if (colStroke==-1)

      noStroke();
    else
    stroke(colStroke);
    rect(x, y, w, h);

    // doesn't work : 
    //translate(x-w/2, 0);
    //box(w, h, 3);

    popMatrix();

  }//method

  void display_internal( color colKey_ ) {

    colKey=colKey_;

    internal.pushMatrix();

    internal.translate(translatePVector.x,

      translatePVector.y,

      translatePVector.z);

    internal.rotateX(PI/2);

    internal.fill(colKey);

    internal.noStroke();

    internal.stroke(0);

    internal.rect(x, y, w, h);

    internal.popMatrix();

  }

//method
  //
}

// n

  PVector getBodyPoint(int user, int bodyPart) {

      PVector jointPos=new PVector(), jointPos_Proj=new PVector();

      myKinect.getJointPositionSkeleton(user, bodyPart, jointPos);

      myKinect.convertRealWorldToProjective(jointPos, jointPos_Proj);

      //System.out.println("Parte del cuerpo: " + bodyPart + " Posición " + jointPos);

      jointPos_Proj.x *= x_coef;

      jointPos_Proj.y *= y_coef;

      return jointPos_Proj;

    }


    void getBodyPoints(int userId) {

      rtKnee = getBodyPoint(userId, SimpleOpenNI.SKEL_RIGHT_KNEE);

      leftKnee = getBodyPoint(userId, SimpleOpenNI.SKEL_LEFT_KNEE);

      rtShoulder = getBodyPoint(userId, SimpleOpenNI.SKEL_RIGHT_SHOULDER);

      leftShoulder = getBodyPoint(userId, SimpleOpenNI.SKEL_LEFT_SHOULDER);

      rtElbow = getBodyPoint(userId, SimpleOpenNI.SKEL_LEFT_ELBOW);

      leftElbow = getBodyPoint(userId, SimpleOpenNI.SKEL_LEFT_ELBOW);

      rtHand = getBodyPoint(userId, SimpleOpenNI.SKEL_RIGHT_HAND);

      leftHand = getBodyPoint(userId, SimpleOpenNI.SKEL_LEFT_HAND);

      rtFoot = getBodyPoint(userId, SimpleOpenNI.SKEL_RIGHT_FOOT);

      leftFoot = getBodyPoint(userId, SimpleOpenNI.SKEL_LEFT_FOOT);

      rtHip = getBodyPoint(userId, SimpleOpenNI.SKEL_RIGHT_HIP);

      leftHip = getBodyPoint(userId, SimpleOpenNI.SKEL_LEFT_HIP);
    }
    ///////////////////


    // draw the skeleton with the selected joints
    //Simple OpenNI has a method called drawLimb, which simply draws a line
    // between two body points
    /*
    void drawSkeleton(int userId)
    {


      myKinect.drawLimb(userId, SimpleOpenNI.SKEL_HEAD, SimpleOpenNI.SKEL_NECK);

      myKinect.drawLimb(userId, SimpleOpenNI.SKEL_NECK, SimpleOpenNI.SKEL_LEFT_SHOULDER);
      myKinect.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_SHOULDER, SimpleOpenNI.SKEL_LEFT_ELBOW);
      myKinect.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_ELBOW, SimpleOpenNI.SKEL_LEFT_HAND);

      myKinect.drawLimb(userId, SimpleOpenNI.SKEL_NECK, SimpleOpenNI.SKEL_RIGHT_SHOULDER);
      myKinect.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_SHOULDER, SimpleOpenNI.SKEL_RIGHT_ELBOW);
      myKinect.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_ELBOW, SimpleOpenNI.SKEL_RIGHT_HAND);

      myKinect.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_SHOULDER, SimpleOpenNI.SKEL_TORSO);
  myKinect.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_SHOULDER, SimpleOpenNI.SKEL_TORSO);

      myKinect.drawLimb(userId, SimpleOpenNI.SKEL_TORSO, SimpleOpenNI.SKEL_LEFT_HIP);
      myKinect.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_HIP, SimpleOpenNI.SKEL_LEFT_KNEE);
      myKinect.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_KNEE, SimpleOpenNI.SKEL_LEFT_FOOT);

      myKinect.drawLimb(userId, SimpleOpenNI.SKEL_TORSO, SimpleOpenNI.SKEL_RIGHT_HIP);
      myKinect.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_HIP, SimpleOpenNI.SKEL_RIGHT_KNEE);
      myKinect.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_KNEE, SimpleOpenNI.SKEL_RIGHT_FOOT);


    }
*/
    // -----------------------------------------------------------------
    // SimpleOpenNI has a number of event handlers, that are triggered when
    // "User events" occur.

    void onNewUser(SimpleOpenNI curContext, int userId)
    {
      println("onNewUser - userId: " + userId);

      println("  start pose detection");

      myKinect.startTrackingSkeleton(userId);
    }

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


    float angleOf(PVector one, PVector two, PVector axis) {

      PVector limb = PVector.sub(two, one);

      return degrees(PVector.angleBetween(limb, axis));
    }