/*
 * Copyright (c) 2017 Yoshihisa Nitta
 * Released under the MIT license
 * http://opensource.org/licenses/mit-license.php
 */

/*
 * NtUnity.cs version 1.0.2 2017/08/25
 *  http://nw.tsuda.ac.jp/NtKinectDLL/
 *
 * requires:
 *   NtKinectDLL version 1.2.1 or later
 */


using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using System;
using System.Runtime.InteropServices;

namespace NtUnity {
  public class Kinect {
    public const int
      // Number
      bodyCount = 6,
      jointCount = 25,
      rgbCols = 1920,
      rgbRows = 1080,
      depthCols = 512,
      depthRows = 424,
      // JointType
      JointType_SpineBase= 0,
      JointType_SpineMid= 1,
      JointType_Neck= 2,
      JointType_Head= 3,
      JointType_ShoulderLeft= 4,
      JointType_ElbowLeft= 5,
      JointType_WristLeft= 6,
      JointType_HandLeft= 7,
      JointType_ShoulderRight= 8,
      JointType_ElbowRight= 9,
      JointType_WristRight= 10,
      JointType_HandRight= 11,
      JointType_HipLeft= 12,
      JointType_KneeLeft= 13,
      JointType_AnkleLeft= 14,
      JointType_FootLeft= 15,
      JointType_HipRight= 16,
      JointType_KneeRight= 17,
      JointType_AnkleRight= 18,
      JointType_FootRight= 19,
      JointType_SpineShoulder= 20,
      JointType_HandTipLeft= 21,
      JointType_ThumbLeft= 22,
      JointType_HandTipRight= 23,
      JointType_ThumbRight= 24,
      // TrackingState
      TrackingState_NotTracked= 0,
      TrackingState_Inferred= 1,
      TrackingState_Tracked= 2,
      // FacePoint
      FacePointType_None= -1,
      FacePointType_EyeLeft= 0,
      FacePointType_EyeRight= 1,
      FacePointType_Nose= 2,
      FacePointType_MouthCornerLeft= 3,
      FacePointType_MouthCornerRight= 4,
      FacePointType_Count= ( FacePointType_MouthCornerRight + 1 ) ,
      // a_FaceProperty
      FaceProperty_Happy= 0,
      FaceProperty_Engaged= 1,
      FaceProperty_WearingGlasses= 2,
      FaceProperty_LeftEyeClosed= 3,
      FaceProperty_RightEyeClosed= 4,
      FaceProperty_MouthOpen= 5,
      FaceProperty_MouthMoved= 6,
      FaceProperty_LookingAway= 7,
      FaceProperty_Count= ( FaceProperty_LookingAway + 1 ) ,
      // FaceDetectionResult
      DetectionResult_Unknown= 0,
      DetectionResult_No= 1,
      DetectionResult_Maybe= 2,
      DetectionResult_Yes= 3,
      // HDFace
      HDFaceVerticesSize = 1347,
      // dummy
      NtKinectdummy = 0;

    [DllImport ("NtKinectDLL")] private static extern IntPtr getKinect();
    [DllImport ("NtKinectDLL")] private static extern void stopKinect(IntPtr ptr);

    // OpenCV
    [DllImport ("NtKinectDLL")] private static extern void imshow(IntPtr ptr);
    [DllImport ("NtKinectDLL")] private static extern void imshowBlack(IntPtr ptr);

    // CoordinateMapper
    [DllImport ("NtKinectDLL")] private static extern void mapCameraPointToColorSpace(IntPtr ptr,IntPtr sv,IntPtr cv,int n);
    [DllImport ("NtKinectDLL")] private static extern void mapCameraPointToDepthSpace(IntPtr ptr,IntPtr sv,IntPtr dv,int n);
    [DllImport ("NtKinectDLL")] private static extern void mapDepthPointToColorSpace(IntPtr ptr,IntPtr dv,IntPtr dth,IntPtr cv,int n);
    [DllImport ("NtKinectDLL")] private static extern void mapDepthPointToCameraSpace(IntPtr ptr,IntPtr dv,IntPtr dth,IntPtr sv,int n);

    // Multi Thread
    [DllImport ("NtKinectDLL")] private static extern void acquire(IntPtr ptr);
    [DllImport ("NtKinectDLL")] private static extern void release(IntPtr ptr);

    // Audio
    [DllImport ("NtKinectDLL")] private static extern void setAudio(IntPtr ptr, bool flag);
    [DllImport ("NtKinectDLL")] private static extern float getBeamAngle(IntPtr ptr);
    [DllImport ("NtKinectDLL")] private static extern float getBeamAngleConfidence(IntPtr ptr);
    [DllImport ("NtKinectDLL")] private static extern ulong getAudioTrackingId(IntPtr ptr);
    [DllImport ("NtKinectDLL")] private static extern void openAudio(IntPtr ptr, IntPtr filename);
    [DllImport ("NtKinectDLL")] private static extern void closeAudio(IntPtr ptr);
    [DllImport ("NtKinectDLL")] private static extern bool isOpenedAudio(IntPtr ptr);

    // RGB
    [DllImport ("NtKinectDLL")] private static extern void setRGB(IntPtr ptr);
    [DllImport ("NtKinectDLL")] private static extern int getRGB(IntPtr ptr, IntPtr data);

    // Depth
    [DllImport ("NtKinectDLL")] private static extern void setDepth(IntPtr ptr);
    [DllImport ("NtKinectDLL")] private static extern int getDepth(IntPtr ptr, IntPtr data);

    // Infrared
    [DllImport ("NtKinectDLL")] private static extern void setInfrared(IntPtr ptr);
    [DllImport ("NtKinectDLL")] private static extern int getInfrared(IntPtr ptr, IntPtr data);

    // BodyIndex
    [DllImport ("NtKinectDLL")] private static extern void setBodyIndex(IntPtr ptr);
    [DllImport ("NtKinectDLL")] private static extern int getBodyIndex(IntPtr ptr, IntPtr data);
    
    // Skeleton
    [DllImport ("NtKinectDLL")] private static extern void setSkeleton(IntPtr ptr);
    [DllImport ("NtKinectDLL")] private static extern int getSkeleton(IntPtr ptr, IntPtr skelton, IntPtr state, IntPtr id, IntPtr tid);
    [DllImport ("NtKinectDLL")] private static extern int handState(IntPtr ptr,int id,bool isLeft);
    
    // Face
    [DllImport ("NtKinectDLL")] private static extern void setFace(IntPtr ptr, bool isColorSpace);
    [DllImport ("NtKinectDLL")] private static extern int getFace(IntPtr ptr, IntPtr point,IntPtr rect,IntPtr direction,IntPtr property,IntPtr tid);

    // HDFace
    [DllImport ("NtKinectDLL")] private static extern void setHDFace(IntPtr ptr);
    [DllImport ("NtKinectDLL")] private static extern int getHDFace(IntPtr ptr, IntPtr point, IntPtr tid, IntPtr status);
    
    // Gesture
    [DllImport ("NtKinectDLL")] private static extern void setGestureFile(IntPtr ptr, IntPtr filename);
    [DllImport ("NtKinectDLL")] private static extern int setGestureId(IntPtr ptr, IntPtr name, int id); // id: non-zero
    [DllImport ("NtKinectDLL")] private static extern void setGesture(IntPtr ptr);
    [DllImport ("NtKinectDLL")] private static extern int getDiscreteGesture(IntPtr ptr, IntPtr gid, IntPtr confidence, IntPtr tid);
    [DllImport ("NtKinectDLL")] private static extern int getContinuousGesture(IntPtr ptr, IntPtr gid, IntPtr progress, IntPtr tid);
    [DllImport ("NtKinectDLL")] private static extern int getGidMapSize();

    private IntPtr kinect;
    //public Vector3[] joint = new Vector3[jointCount];
    //public int[] jointState = new int[jointCount];

    // audio
    public float beamAngle;
    public float beamAngleConfidence;
    public ulong audioTrackingId;
    // images
    public Color32[] rgbImage;
    public ushort[] depthImage;
    public ushort[] infraredImage;
    public byte[] bodyIndexImage;
    // skeleton
    public List<List<Vector3>> skeleton;
    public List<List<int>> skeletonState;
    public List<int> skeletonId;
    public List<ulong> skeletonTrackingId;
    // skeleton (internal)
    private float[] skel;
    private int[] skelState;
    private int[] skelId;
    private ulong[] skelTrackingId;
    // face
    public List<List<Vector2>> facePoint;
    public List<Vector4> faceRect;
    public List<Vector3> faceDirection;
    public List<List<int>> faceProperty;
    public List<ulong> faceTrackingId;
    // face (internal)
    private float[] fcPoint;
    private float[] fcRect;
    private float[] fcDirection;
    private int[] fcProperty;
    private ulong[] fcTrackingId;
    // hdface
    public List<List<Vector3>> hdfacePoint;
    public List<ulong> hdfaceTrackingId;
    public List<int> hdfaceStatus;
    // hdface (internal)
    private float[] hdfcPoint;
    private ulong[] hdfcTrackingId;
    private int[] hdfcStatus;
    // gesture
    public List<int> discreteGesture;
    public List<float> gestureConfidence;
    public List<ulong> discreteGestureTrackingId;
    public List<int> continuousGesture;
    public List<float> gestureProgress;
    public List<ulong> continuousGestureTrackingId;
    // gesture (internal)
    private int[] gstId;
    private float[] gstFloat;
    private ulong[] gstTrackingId;
      
    public Kinect() {
      kinect = getKinect();
      //rgbImage = new byte[rgbRows * rgbCols * 4];
      rgbImage = new Color32[rgbRows * rgbCols];
      depthImage = new ushort[depthRows * depthCols];
      infraredImage = new ushort[depthRows * depthCols];
      bodyIndexImage = new byte[depthRows * depthCols];
      skeleton = new List<List<Vector3>>();
      skeletonState = new List<List<int>>();
      skeletonId = new List<int>();
      skeletonTrackingId = new List<ulong>();
      skel = new float[bodyCount * jointCount * 3];
      skelState = new int[bodyCount * jointCount];
      skelId = new int[bodyCount];
      skelTrackingId = new ulong[bodyCount];
      facePoint = new List<List<Vector2>>();
      faceRect = new List<Vector4>();
      faceDirection = new List<Vector3>();
      faceProperty = new List<List<int>>();
      faceTrackingId = new List<ulong>();
      fcPoint = new float[bodyCount * FacePointType_Count * 3];
      fcRect = new float[bodyCount * 4];
      fcDirection = new float[bodyCount * 3];
      fcProperty = new int[bodyCount * FaceProperty_Count * 2];
      fcTrackingId = new ulong[bodyCount];
      hdfacePoint = new List<List<Vector3>>();
      hdfaceTrackingId = new List<ulong>();
      hdfaceStatus = new List<int>();
      hdfcPoint = new float[bodyCount * HDFaceVerticesSize * 3];
      hdfcTrackingId = new ulong[bodyCount];
      hdfcStatus = new int[bodyCount * 2];
      discreteGesture = new List<int>();
      gestureConfidence = new List<float>();
      discreteGestureTrackingId = new List<ulong>();
      continuousGesture = new List<int>();
      gestureProgress = new List<float>();
      continuousGestureTrackingId = new List<ulong>();
      gstId = new int[bodyCount * 100];
      gstFloat = new float[bodyCount * 100];
      gstTrackingId = new ulong[bodyCount * 100];
    }
    public void stopKinect() { stopKinect(kinect); }

    // OpenCV
    public void imshow() { imshow(kinect); }
    public void imshowBlack() { imshowBlack(kinect); }
    
    // coordinateMapper
    public void mapCameraPointToColorSpace(List<Vector3> skel,ref List<Vector2> color,int n) {
      float[] sv = new float[n * 3];
      float[] cv = new float[n * 2];
      for (int i=0; i<n; i++) {
	sv[3*i] = skel[i].x; sv[3*i+1] = skel[i].y; sv[3*i+2] = skel[i].z;
      }
      GCHandle gch = GCHandle.Alloc(sv,GCHandleType.Pinned);
      GCHandle gch2 = GCHandle.Alloc(cv,GCHandleType.Pinned);
      mapCameraPointToColorSpace(kinect,gch.AddrOfPinnedObject(),gch2.AddrOfPinnedObject(),n);
      gch.Free();
      gch2.Free();
      color.Clear();
      for (int i=0; i<n; i++) {
	color.Add(new Vector2(cv[2*i],cv[2*i+1]));
      }
    }
    public void mapCameraPointToDepthSpace(List<Vector3> skel, ref List<Vector2> depth, int n) {
      float[] sv = new float[n * 3];
      float[] dv = new float[n * 2];
      for (int i=0; i<n; i++) {
	sv[3*i] = skel[i].x; sv[3*i+1] = skel[i].y; sv[3*i+2] = skel[i].z;
      }
      GCHandle gch = GCHandle.Alloc(sv,GCHandleType.Pinned);
      GCHandle gch2 = GCHandle.Alloc(dv,GCHandleType.Pinned);
      mapCameraPointToDepthSpace(kinect,gch.AddrOfPinnedObject(),gch2.AddrOfPinnedObject(), n);
      gch.Free();
      gch2.Free();
      depth.Clear();
      for (int i=0; i<n; i++) {
	depth.Add(new Vector2(dv[2*i],dv[2*i+1]));
      }
    }
    public void mapDepthPointToColorSpace(List<Vector2> depth,ushort[] dth,ref List<Vector2> color,int n) {
      float[] dv = new float[n * 2];
      float[] cv = new float[n * 2];
      for (int i=0; i<n; i++) {
	dv[2*i] = depth[i].x; dv[2*i+1] = depth[i].y;
      }
      GCHandle gch = GCHandle.Alloc(dv,GCHandleType.Pinned);
      GCHandle gch2 = GCHandle.Alloc(dth,GCHandleType.Pinned);
      GCHandle gch3 = GCHandle.Alloc(cv,GCHandleType.Pinned);
      mapDepthPointToColorSpace(kinect,gch.AddrOfPinnedObject(),gch2.AddrOfPinnedObject(),gch3.AddrOfPinnedObject(), n);
      gch.Free();
      gch2.Free();
      gch3.Free();
      color.Clear();
      for (int i=0; i<n; i++) {
	color.Add(new Vector2(cv[2*i],dv[2*i+1]));
      }
    }
    void mapDepthPointToCameraSpace(List<Vector2> depth,ushort[] dth,ref List<Vector3> skel,int n) {
      float[] dv = new float[n * 2];
      float[] sv = new float[n * 3];
      for (int i=0; i<n; i++) {
	dv[2*i] = depth[i].x; dv[2*i+1] = depth[i].y;
      }
      GCHandle gch = GCHandle.Alloc(dv,GCHandleType.Pinned);
      GCHandle gch2 = GCHandle.Alloc(dth,GCHandleType.Pinned);
      GCHandle gch3 = GCHandle.Alloc(sv,GCHandleType.Pinned);
      mapDepthPointToCameraSpace(kinect,gch.AddrOfPinnedObject(),gch2.AddrOfPinnedObject(),gch3.AddrOfPinnedObject(),n);
      gch.Free();
      gch2.Free();
      gch3.Free();
      skel.Clear();
      for (int i=0; i<n; i++) {
	skel.Add(new Vector3(sv[3*i],sv[3*i+1],sv[3*i+2]));
      }
    }

    // Multi Thread
    public void acquire() { acquire(kinect); }
    public void release() { release(kinect); }
    // Audio
    public void setAudio(bool flag) {
      setAudio(kinect,flag);
      beamAngle = getBeamAngle(kinect);
      beamAngleConfidence = getBeamAngleConfidence(kinect);
      audioTrackingId = getAudioTrackingId(kinect);
    }
    /*
    public float getBeamAngle() { return getBeamAngle(kinect); }
    public float getBeamAngleConfidence() { return getBeamAngleConfidence(kinect); }
    */
    public void openAudio(string filename) {
      System.IntPtr fname = Marshal.StringToHGlobalUni(filename);
      openAudio(kinect,fname);
      Marshal.FreeHGlobal(fname);
    }
    public void closeAudio() { closeAudio(kinect); }
    public bool isOpenedAudio() { return isOpenedAudio(kinect); }
    // RGB
    public void setRGB() { setRGB(kinect); getRGB2(); }
    public int getRGB2() {
      GCHandle gch = GCHandle.Alloc(rgbImage,GCHandleType.Pinned);
      int n = getRGB(kinect,gch.AddrOfPinnedObject());
      gch.Free();
      return n;
    }

    // Depth
    public void setDepth() { setDepth(kinect); getDepth(); }
    public int getDepth() {
      GCHandle gch = GCHandle.Alloc(depthImage,GCHandleType.Pinned);
      int n = getDepth(kinect,gch.AddrOfPinnedObject());
      gch.Free();
      return n;
    }

    // Infrared
    public void setInfrared() { setInfrared(kinect); getInfrared(); }
    public int getInfrared() {
      GCHandle gch = GCHandle.Alloc(infraredImage,GCHandleType.Pinned);
      int n = getInfrared(kinect,gch.AddrOfPinnedObject());
      gch.Free();
      return n;
    }

    // BodyIndex
    public void setBodyIndex() {setBodyIndex(kinect); getBodyIndex(); }
    public int getBodyIndex() {
      GCHandle gch = GCHandle.Alloc(bodyIndexImage,GCHandleType.Pinned);
      int n = getBodyIndex(kinect,gch.AddrOfPinnedObject());
      gch.Free();
      return n;
    }

    // Skeleton
    public void setSkeleton() { setSkeleton(kinect); getSkeleton(); }
    public int getSkeleton() {
      GCHandle gch = GCHandle.Alloc(skel,GCHandleType.Pinned);
      GCHandle gch2 = GCHandle.Alloc(skelState,GCHandleType.Pinned);
      GCHandle gch3 = GCHandle.Alloc(skelId,GCHandleType.Pinned);
      GCHandle gch4 = GCHandle.Alloc(skelTrackingId,GCHandleType.Pinned);
      int n = getSkeleton(kinect,gch.AddrOfPinnedObject(),gch2.AddrOfPinnedObject(),gch3.AddrOfPinnedObject(),gch4.AddrOfPinnedObject());
      gch.Free();
      gch2.Free();
      gch3.Free();
      gch4.Free();
      skeleton.Clear(); skeletonState.Clear(); skeletonId.Clear(); skeletonTrackingId.Clear();
      int idx = 0, st=0;
      for (int i=0; i<n; i++) {
	skeleton.Add(new List<Vector3>());
	skeletonState.Add(new List<int>());
	for (int j=0; j<jointCount; j++) {
	  skeleton[i].Add(new Vector3(skel[idx++], skel[idx++], skel[idx++]));
	  skeletonState[i].Add(skelState[st++]);
	}
	skeletonId.Add(skelId[i]);
	skeletonTrackingId.Add(skelTrackingId[i]);
      }
      return n;
    }

    private int faceFailCount = 0;
    // Face
    public void setFace() { setFace(kinect,true); getFace(); }
    public int getFace() {
      GCHandle gch = GCHandle.Alloc(fcPoint,GCHandleType.Pinned);
      GCHandle gch2 = GCHandle.Alloc(fcRect,GCHandleType.Pinned);
      GCHandle gch3 = GCHandle.Alloc(fcDirection,GCHandleType.Pinned);
      GCHandle gch4 = GCHandle.Alloc(fcProperty,GCHandleType.Pinned);
      GCHandle gch5 = GCHandle.Alloc(fcTrackingId,GCHandleType.Pinned);
      int n = getFace(kinect,gch.AddrOfPinnedObject(),gch2.AddrOfPinnedObject(),gch3.AddrOfPinnedObject(),gch4.AddrOfPinnedObject(),gch5.AddrOfPinnedObject());
      gch.Free();
      gch2.Free();
      gch3.Free();
      gch4.Free();
      gch5.Free();
      if (n == 0) {
	faceFailCount++;
	if (faceFailCount < 10) {
	  return 0;
	} else {
	  faceFailCount = 0;
	}
      }
      facePoint.Clear(); faceRect.Clear(); faceDirection.Clear(); faceProperty.Clear(); faceTrackingId.Clear();
      int idx=0, ridx=0, didx=0, pidx = 0;
      for (int i=0; i<n; i++) {
	facePoint.Add(new List<Vector2>());
	for (int j=0; j<FacePointType_Count; j++) {
	  facePoint[i].Add(new Vector2(fcPoint[idx++],fcPoint[idx++]));
	}
	faceRect.Add(new Vector4(fcRect[ridx++],fcRect[ridx++],fcRect[ridx++],fcRect[ridx++]));
	faceDirection.Add(new Vector3(fcDirection[didx++],fcDirection[didx++],fcDirection[didx++]));
	faceProperty.Add(new List<int>());
	for (int j=0; j<FaceProperty_Count; j++) {
	  faceProperty[i].Add(fcProperty[pidx++]);
	}
	faceTrackingId.Add(fcTrackingId[i]);
      }
      return n;
    }
    public Vector3 getFaceDirectionByTrackingId(ulong tid) {
      for (int i=0; i<faceTrackingId.Count; i++) {
	if (faceTrackingId[i] == tid) {
	  return faceDirection[i];
	}
      }
      return Vector3.zero;
    }

    // HDFace
    public void setHDFace() { setHDFace(kinect); getHDFace(); }
    public int getHDFace() {
      GCHandle gch = GCHandle.Alloc(hdfcPoint,GCHandleType.Pinned);
      GCHandle gch2 = GCHandle.Alloc(hdfcTrackingId,GCHandleType.Pinned);
      GCHandle gch3 = GCHandle.Alloc(hdfcStatus,GCHandleType.Pinned);
      int n = getHDFace(kinect,gch.AddrOfPinnedObject(),gch2.AddrOfPinnedObject(),gch3.AddrOfPinnedObject());
      gch.Free();
      gch2.Free();
      gch3.Free();
      hdfacePoint.Clear(); hdfaceTrackingId.Clear(); hdfaceStatus.Clear();
      int idx = 0;
      for (int i=0; i<n; i++) {
	hdfacePoint.Add(new List<Vector3>());
	for (int j=0; j < HDFaceVerticesSize; j++) {
	  hdfacePoint[i].Add(new Vector3(hdfcPoint[idx++],hdfcPoint[idx++],hdfcPoint[idx++]));
	}
	hdfaceTrackingId.Add(hdfcTrackingId[i]);
	hdfaceStatus.Add(hdfcStatus[i]);
      }
      return n;
    }

    // Gesture
    public void setGestureFile(string filename) {
      IntPtr gbd = Marshal.StringToHGlobalUni(filename);
      setGestureFile(kinect,gbd);
      Marshal.FreeHGlobal(gbd);
    }
    public int setGestureId(string name, int id) {
      System.IntPtr g = Marshal.StringToHGlobalUni(name); // discrete
      int n = setGestureId(kinect,g,id);
      Marshal.FreeHGlobal(g);
      return n;
    }
    public void setGesture() { setGesture(kinect); }
    public int getDiscreteGesture() {
      int size = bodyCount * getGidMapSize();
      if (gstId.Length < size) {
	gstId = new int[size];
	gstFloat = new float[size];
	gstTrackingId = new ulong[size];
      }
      GCHandle gch = GCHandle.Alloc(gstId,GCHandleType.Pinned);
      GCHandle gch2 = GCHandle.Alloc(gstFloat,GCHandleType.Pinned);
      GCHandle gch3 = GCHandle.Alloc(gstTrackingId,GCHandleType.Pinned);
      int n = getDiscreteGesture(kinect,gch.AddrOfPinnedObject(),gch2.AddrOfPinnedObject(),gch3.AddrOfPinnedObject());
      gch.Free();
      gch2.Free();
      gch3.Free();
      discreteGesture.Clear(); gestureConfidence.Clear(); discreteGestureTrackingId.Clear();
      for (int i=0; i<n; i++) {
	discreteGesture.Add(gstId[i]);
	gestureConfidence.Add(gstFloat[i]);
	discreteGestureTrackingId.Add(gstTrackingId[i]);
      }
      return n;
    }
    public int getContinuousGesture() {
      int size = bodyCount * getGidMapSize();
      if (gstId.Length < size) {
	gstId = new int[size];
	gstFloat = new float[size];
	gstTrackingId = new ulong[size];
      }
      GCHandle gch = GCHandle.Alloc(gstId,GCHandleType.Pinned);
      GCHandle gch2 = GCHandle.Alloc(gstFloat,GCHandleType.Pinned);
      GCHandle gch3 = GCHandle.Alloc(gstTrackingId,GCHandleType.Pinned);
      int n = getContinuousGesture(kinect,gch.AddrOfPinnedObject(),gch2.AddrOfPinnedObject(),gch3.AddrOfPinnedObject());
      gch.Free();
      gch2.Free();
      gch3.Free();
      continuousGesture.Clear(); gestureProgress.Clear(); continuousGestureTrackingId.Clear();
      for (int i=0; i<n; i++) {
	continuousGesture.Add(gstId[i]);
	gestureProgress.Add(gstFloat[i]);
	continuousGestureTrackingId.Add(gstTrackingId[i]);
      }
      return n;
    }
  }
  public class RigBone {
    public GameObject gameObject;
    public HumanBodyBones bone;
    public bool isValid;
    public Transform transform {
      get { return animator.GetBoneTransform(bone); }
    }
    Animator animator;
    Quaternion savedValue;
    public RigBone(GameObject g, HumanBodyBones b) {
      gameObject = g;
      bone = b;
      isValid = false;
      animator = gameObject.GetComponent<Animator>();
      if (animator == null) {
	Debug.Log("no Animator Component");
	return;
      }
      Avatar avatar = animator.avatar;
      if (avatar == null || !avatar.isHuman || !avatar.isValid) {
	Debug.Log("Avatar is not Humanoid or it is not valid");
	return;
      }
      isValid = true;
      savedValue = animator.GetBoneTransform(bone).localRotation;
    }
    public void set(float a, float x, float y, float z) {
      set(Quaternion.AngleAxis(a, new Vector3(x,y,z)));
    }
    public void set(Quaternion q) {
      animator.GetBoneTransform(bone).localRotation = q;
      savedValue = q;
    }
    public void mul(float a, float x, float y, float z) {
      mul(Quaternion.AngleAxis(a, new Vector3(x,y,z)));
    }
    public void mul(Quaternion q) {
      Transform tr = animator.GetBoneTransform(bone);
      tr.localRotation = q * tr.localRotation;
    }
    public void offset(float a, float x, float y, float z) {
      offset(Quaternion.AngleAxis(a, new Vector3(x,y,z)));
    }
    public void offset(Quaternion q) {
      animator.GetBoneTransform(bone).localRotation = q * savedValue;
    }
    public void changeBone(HumanBodyBones b) {
      bone = b;
      savedValue = animator.GetBoneTransform(bone).localRotation;
    }
  }

  class HumanoidSkeleton {
    private static int[] jointSegment = new int[] {
      Kinect.JointType_SpineBase, Kinect.JointType_SpineMid,             // Spine
      Kinect.JointType_Neck, Kinect.JointType_Head,                      // Neck
      // left
      Kinect.JointType_ShoulderLeft, Kinect.JointType_ElbowLeft,         // LeftUpperArm
      Kinect.JointType_ElbowLeft, Kinect.JointType_WristLeft,            // LeftLowerArm
      Kinect.JointType_WristLeft, Kinect.JointType_HandLeft,             // LeftHand
      Kinect.JointType_HipLeft, Kinect.JointType_KneeLeft,               // LeftUpperLeg
      Kinect.JointType_KneeLeft, Kinect.JointType_AnkleLeft,             // LeftLowerLeg6
      Kinect.JointType_AnkleLeft, Kinect.JointType_FootLeft,             // LeftFoot
      // right
      Kinect.JointType_ShoulderRight, Kinect.JointType_ElbowRight,       // RightUpperArm
      Kinect.JointType_ElbowRight, Kinect.JointType_WristRight,          // RightLowerArm
      Kinect.JointType_WristRight, Kinect.JointType_HandRight,           // RightHand
      Kinect.JointType_HipRight, Kinect.JointType_KneeRight,             // RightUpperLeg
      Kinect.JointType_KneeRight, Kinect.JointType_AnkleRight,           // RightLowerLeg
      Kinect.JointType_AnkleRight, Kinect.JointType_FootRight,           // RightFoot
    };
    public Vector3[] joint = new Vector3[Kinect.jointCount];
    public int[] jointState = new int[Kinect.jointCount];

    Dictionary<HumanBodyBones,Vector3> trackingSegment = null;
    Dictionary<HumanBodyBones, int>  trackingState = null;

    private static HumanBodyBones[] humanBone = new HumanBodyBones[] {
      HumanBodyBones.Hips,
      HumanBodyBones.Spine,
      HumanBodyBones.UpperChest,
      HumanBodyBones.Neck,
      HumanBodyBones.Head,
      HumanBodyBones.LeftUpperArm,
      HumanBodyBones.LeftLowerArm,
      HumanBodyBones.LeftHand,
      HumanBodyBones.LeftUpperLeg,
      HumanBodyBones.LeftLowerLeg,
      HumanBodyBones.LeftFoot,
      HumanBodyBones.RightUpperArm,
      HumanBodyBones.RightLowerArm,
      HumanBodyBones.RightHand,
      HumanBodyBones.RightUpperLeg,
      HumanBodyBones.RightLowerLeg,
      HumanBodyBones.RightFoot,
    };

    private static HumanBodyBones[] targetBone = new HumanBodyBones[] {
      HumanBodyBones.Spine,
      HumanBodyBones.Neck,
      HumanBodyBones.LeftUpperArm,
      HumanBodyBones.LeftLowerArm,
      HumanBodyBones.LeftHand,
      HumanBodyBones.LeftUpperLeg,
      HumanBodyBones.LeftLowerLeg,
      HumanBodyBones.LeftFoot,
      HumanBodyBones.RightUpperArm,
      HumanBodyBones.RightLowerArm,
      HumanBodyBones.RightHand,
      HumanBodyBones.RightUpperLeg,
      HumanBodyBones.RightLowerLeg,
      HumanBodyBones.RightFoot,
    };

    public GameObject humanoid;
    private Dictionary<HumanBodyBones, RigBone> rigBone = null;
    private bool isSavedPosition = false;
    private Vector3 savedPosition;
    //private Quaternion savedHumanoidRotation;

    public HumanoidSkeleton(GameObject h) {
      humanoid = h;
      rigBone = new Dictionary<HumanBodyBones, RigBone>();
      foreach (HumanBodyBones bone in humanBone) {
	rigBone[bone] = new RigBone(humanoid,bone);
      }
      //savedHumanoidRotation = humanoid.transform.rotation;
      trackingSegment = new Dictionary<HumanBodyBones,Vector3>(targetBone.Length);
      trackingState = new Dictionary<HumanBodyBones, int>(targetBone.Length);
    }
    private void swapJoint(int a, int b) {
      Vector3 tmp = joint[a]; joint[a] = joint[b]; joint[b] = tmp;
      int t = jointState[a]; jointState[a] = jointState[b]; jointState[b] = t;
    }
    public void set(Kinect kinect, int n, bool mirrored, bool move) {
      Vector3 faceDir = kinect.getFaceDirectionByTrackingId(kinect.skeletonTrackingId[n]);
      if (isSavedPosition == false
	  && kinect.skeletonState[n][Kinect.JointType_SpineBase] != Kinect.TrackingState_NotTracked) {
	isSavedPosition = true;
	savedPosition = kinect.skeleton[n][Kinect.JointType_SpineBase];
      }
      for (int i=0; i<kinect.skeleton[n].Count; i++) {
	Vector3 jt = kinect.skeleton[n][i];
	if (mirrored) {
	  joint[i] = new Vector3(-jt.x, jt.y, -jt.z);
	} else {
	  joint[i] = new Vector3(jt.x, jt.y, savedPosition.z*2 - jt.z);
	}
	jointState[i] = kinect.skeletonState[n][i];
      }
      if (mirrored) {
	swapJoint(Kinect.JointType_ShoulderLeft, Kinect.JointType_ShoulderRight);
	swapJoint(Kinect.JointType_ElbowLeft, Kinect.JointType_ElbowRight);
	swapJoint(Kinect.JointType_WristLeft, Kinect.JointType_WristRight);
	swapJoint(Kinect.JointType_HandLeft, Kinect.JointType_HandRight);
	swapJoint(Kinect.JointType_HipLeft, Kinect.JointType_HipRight);
	swapJoint(Kinect.JointType_KneeLeft, Kinect.JointType_KneeRight);
	swapJoint(Kinect.JointType_AnkleLeft, Kinect.JointType_AnkleRight);
	swapJoint(Kinect.JointType_FootLeft, Kinect.JointType_FootRight);
	swapJoint(Kinect.JointType_HandTipLeft, Kinect.JointType_HandTipRight);
	swapJoint(Kinect.JointType_ThumbLeft, Kinect.JointType_ThumbRight);
      }
      for (int i=0; i<targetBone.Length; i++) {
	int s = jointSegment[2*i], e = jointSegment[2*i+1];
	trackingSegment[targetBone[i]] = joint[e] - joint[s];
	trackingState[targetBone[i]] = System.Math.Min(jointState[e],jointState[s]);
      }

      Vector3 waist = joint[Kinect.JointType_HipRight] - joint[Kinect.JointType_HipLeft];
      waist = new Vector3(waist.x, 0, waist.z);
      Quaternion rot = Quaternion.FromToRotation(Vector3.right,waist);
      Quaternion rotInv = Quaternion.Inverse(rot);
 
      Vector3 shoulder = joint[Kinect.JointType_ShoulderRight] - joint[Kinect.JointType_ShoulderLeft];
      shoulder = new Vector3(shoulder.x, 0, shoulder.z);
      Quaternion srot = Quaternion.FromToRotation(Vector3.right,shoulder);
      //Quaternion srotInv = Quaternion.Inverse(srot);

      humanoid.transform.rotation = Quaternion.identity;
      foreach (HumanBodyBones bone in targetBone) {
	if (trackingState[bone] != Kinect.TrackingState_NotTracked) {
	  rigBone[bone].transform.rotation = rotInv * Quaternion.FromToRotation(Vector3.up,trackingSegment[bone]);
	}
      }
      rigBone[HumanBodyBones.UpperChest].offset(srot);
      float pitch = faceDir.x, yaw = faceDir.y, roll = faceDir.z;
      if (mirrored) {
	pitch = -pitch;
	roll = -roll;
      } else {
	pitch = -pitch;
	yaw = -yaw;
      }
      rigBone[HumanBodyBones.Head].transform.rotation = toQ(pitch, yaw, roll);
      Quaternion bodyRot = rot;
      if (mirrored) {
	bodyRot = Quaternion.AngleAxis(180,Vector3.up) * bodyRot;
      }
      humanoid.transform.rotation = bodyRot;
      if (move == true) {
	Vector3 m = joint[Kinect.JointType_SpineBase];
	if (mirrored) m = new Vector3(-m.x, m.y, -m.z);
	humanoid.transform.position = m;
      }
    }
    public static Quaternion toQ (float pitch, float yaw, float roll) {
      yaw *= Mathf.Deg2Rad;
      pitch *= Mathf.Deg2Rad;
      roll *= Mathf.Deg2Rad;
      float rollOver2 = roll * 0.5f;
      float sinRollOver2 = (float)System.Math.Sin ((double)rollOver2);
      float cosRollOver2 = (float)System.Math.Cos ((double)rollOver2);
      float pitchOver2 = pitch * 0.5f;
      float sinPitchOver2 = (float)System.Math.Sin ((double)pitchOver2);
      float cosPitchOver2 = (float)System.Math.Cos ((double)pitchOver2);
      float yawOver2 = yaw * 0.5f;
      float sinYawOver2 = (float)System.Math.Sin ((double)yawOver2);
      float cosYawOver2 = (float)System.Math.Cos ((double)yawOver2);
      Quaternion result;
      result.w = cosYawOver2 * cosPitchOver2 * cosRollOver2 + sinYawOver2 * sinPitchOver2 * sinRollOver2;
      result.x = cosYawOver2 * sinPitchOver2 * cosRollOver2 + sinYawOver2 * cosPitchOver2 * sinRollOver2;
      result.y = sinYawOver2 * cosPitchOver2 * cosRollOver2 - cosYawOver2 * sinPitchOver2 * sinRollOver2;
      result.z = cosYawOver2 * cosPitchOver2 * sinRollOver2 - sinYawOver2 * sinPitchOver2 * cosRollOver2;
      return result;
    }
  }
}