import oscP5.*;
import netP5.*;
import toxi.geom.Quaternion;
import java.util.Map;
import java.util.HashMap;
import processing.core.*;
import java.lang.reflect.*;



class Chordata {

  OscP5 oscP5;

  Method displayArm;
  PApplet parent;

  HashMap bones = new HashMap<String, Quaternion>();
  Chordata(PApplet parent_, int port) {
    parent = parent_;
    /* start oscP5, listening for incoming messages at port 12000 */
    oscP5 = new OscP5(this, port);

    /* myRemoteLocation is a NetAddress. a NetAddress takes 2 parameters,
     * an ip address and a port number. myRemoteLocation is used as parameter in
     * oscP5.send() when sending osc packets to another computer, device,
     * application. usage see below. for testing purposes the listening port
     * and the port of the remote location address are the same, hence you will
     * send messages back to this sketch.
     */


    try {
      displayArm = parent.getClass().getMethod("displayBone",
      new Class[] {
        String.class
      }
      );
    }
    catch (Exception e) {
      // no such method, or an error.. which is fine, just ignore

        PApplet.println("Missing... " + e.getMessage());
    }
  }

  public synchronized void display() {
    parent.pushMatrix();
    for (Object me: bones.entrySet()) {
      String boneName = (String) ((Map.Entry) me).getKey();
      Quaternion boneOrientation = (Quaternion) ((Map.Entry) me).getValue();
      //println(boneName + "," + boneOrientation);
      displayEvent(boneName, boneOrientation);
    }
    parent.popMatrix();

  }

  private void displayEvent(String boneName, Quaternion quat) {
      if (displayArm != null) {
        try {
          //displayMethod.invoke(parent, i);
          parent.pushMatrix();
          if (quat != null) {
          parent.applyMatrix(getMatrix(quat));

          displayArm.invoke(parent, boneName);
          parent.popMatrix();
          }
        }
        catch (Exception e) {
          PApplet.println("Disabling displayMethod() because of an error.");
          // e.printStackTrace();
          displayArm = null;
        }
      }
    }


  PMatrix getMatrix(Quaternion q) {
    float[] m = new float[16];
    q.toMatrix4x4().toFloatArray(m);
    PMatrix result = new PMatrix3D(
      m[0], m[1], m[2], m[3],
      m[4], m[5], m[6], m[7],
      m[8], m[9], m[10], m[11],
      m[12], m[13], m[14], m[15]);
    return result;
  }


  /* incoming osc message are forwarded to the oscEvent method. */
   synchronized void oscEvent(OscMessage theOscMessage) {
    /* print the address pattern and the typetag of the received OscMessage */
    //print("### received an osc message.");
    bones.put(theOscMessage.addrPattern(), new Quaternion(theOscMessage.get(1).floatValue(), theOscMessage.get(2).floatValue(), theOscMessage.get(3).floatValue(), theOscMessage.get(0).floatValue()));
    //PApplet.print(" addrpattern: "+theOscMessage.addrPattern() + "\n");
    //for (int i = 0; i < 4; i++) {
    //  println(" typetag: "+theOscMessage.get(i).floatValue());
    //}
  }
}