/*
-----------------------------------------------------------------------------
This source file is part of OpenSpace3D
For the latest info, see http://www.openspace3d.com

Copyright (c) 2012 I-maginer

This program is free software; you can redistribute it and/or modify it under
the terms of the GNU Lesser General Public License as published by the Free Software
Foundation; either version 2 of the License, or (at your option) any later
version.

This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License along with
this program; if not, write to the Free Software Foundation, Inc., 59 Temple
Place - Suite 330, Boston, MA 02111-1307, USA, or go to
http://www.gnu.org/copyleft/lesser.txt

-----------------------------------------------------------------------------
*/

/*******************************************************************************
 Plugin Kinect 
 Client part
 Version: 1.0
 Author: I-maginer
 Date: 14/02/20011
 Last update: 14/03/2011
 
*******************************************************************************/

var SKL_USE_GLOBAL_COORDS = 0;;

var fConfidence = 0.2;;

var iPosTime = 800;;
var iLPosTime = 500;;
var POSHX = 0;;
var POSLCLICK = 1;;
var POSRCLICK = 2;;
var POSLSWIPE = 3;;
var POSRSWIPE = 4;;

// set to 1 to enable training
var bPoseTrain = 0;;
var lPoseLabel = "T pose"::"Cross"::"Hands up"::"Stand"::"Show left"::"Show right"::nil;;

typeof lBonesBindNames = [[I [S r1]] r1];;

struct PlugKinectUserStr = [
  KINU_Inst                      : PInstance,
  KINU_Device 	                 : ObjKinect,
  KINU_User                      : ObjKinectUser,
  KINU_Mesh                      : [SO3_OBJECT [F F F] [F F F F]],
  KINU_tScale                    : [F F F],
  KINU_bFlipX                    : I,
  KINU_tPmargin                  : [I I],
  KINU_lJointsList               : [[SO3_OBJECT [I [F F F] [F F F F] [F F F F] [F F F F]]] r1],
  KINU_InitPos                   : [F F F],
  KINU_iLeftHandFigure           : I,
  KINU_iRightHandFigure          : I,
  KINU_tRightHandFingers         : [I I I],
  KINU_tLeftHandFingers          : [I I I],
  KINU_tLeftHandPos              : [F F F],
  KINU_tRightHandPos             : [F F F],
  KINU_tLeftHandClickPos         : [F F F],
  KINU_tRightHandClickPos        : [F F F],
  KINU_tLastLeftHandTrans        : [F F F],
  KINU_tLastRightHandTrans       : [F F F],
  KINU_mlp                       : ObjMl,
  KINU_sLastdetectedPose         : S,
  KINU_sLastHeadTracking         : [[F F F] [F F F]],
  KINU_iTrainingPos              : I
  
  ] mkPlugKinectUserStr;;


fun resetBone(bone, kuserstr)=
  let SO3ObjectGetChildren bone -> lchild in
  let switch kuserstr.KINU_lJointsList bone -> [joint _ _ lquat _] in
  (
    SO3ObjectSetInheritOrientation bone 1;
    SO3ObjectSetOrientation bone lquat;
    
    while lchild != nil do
    (     
      resetBone (hd lchild) kuserstr;
      set lchild = tl lchild;
    );
  );
  0;;


fun resetSkeleton(kuserstr)=
  let kuserstr.KINU_Mesh -> [mesh ovec oquat] in
  let SO3GetRootBoneFromMesh mesh -> rootbone in
  (
    SO3ObjectSetOrientation mesh oquat;
    SO3ObjectSetPosition mesh ovec;
  
    let switch kuserstr.KINU_lJointsList rootbone -> [joint vec _ _ _] in
      SO3ObjectSetPosition rootbone vec;
    
    if (rootbone == nil) then nil else
      resetBone rootbone kuserstr;
  );
  
  set kuserstr.KINU_InitPos = nil;
  0;;


fun updateBone(l, kuserstr)=
  if l == nil then nil else
  let hd l -> [bone p] in
  let p -> [joint ovec oquat lquat lastquat] in
  (
    if (joint == USER_SKEL_L_HAND) || (joint == USER_SKEL_R_HAND) then nil else
    // local joint quaternion in world
    let _GETKinectUserJointOrientation kuserstr.KINU_User joint fConfidence -> jquat in
    if (jquat == nil) then nil else
    (
      if (SKL_USE_GLOBAL_COORDS) then
      (
        //using global
        let kuserstr.KINU_Mesh -> [mesh _ _] in
        let (SO3ObjectGetGlobalOrientation mesh) -> mquat in
        let SO3MathsQuatAdd mquat jquat -> jquat in
        let SO3MathsQuatAdd jquat SO3MathsQuatAdd mquat oquat -> fquat in
        //let if lastquat == nil then fquat else SO3MathsQuatInterpolate lastquat fquat 0.9 1 -> fquat in
        (
          SO3ObjectSetGlobalOrientation bone fquat;
          mutate p <- [_ _ _ _ fquat];
        );
      )
      else
      (
        //using local
        SO3ObjectSetInheritOrientation bone 0;
        SO3ObjectResetOrientation bone;
        let SO3GetBoneLocalOrientation bone jquat -> quat in
        let SO3MathsQuatAdd quat oquat -> fquat in
        //let if lastquat == nil then fquat else SO3MathsQuatInterpolate lastquat fquat 0.9 1 -> fquat in
        (
          SO3ObjectSetOrientation bone fquat;
          mutate p <- [_ _ _ _ fquat];
        );
      );
    );
    
    updateBone (tl l) kuserstr;
  );
  0;;


fun updateSkeleton(kuserstr)=
  let kuserstr.KINU_Mesh -> [mesh ovec oquat] in
  let SO3GetRootBoneFromMesh mesh -> rootbone in
  (
    if (rootbone == nil) then nil else
      updateBone kuserstr.KINU_lJointsList kuserstr;
  );
  0;;


fun getBoneByJoint(kuserstr, joint)=
  let nil -> bone in
  (
    let sizelist kuserstr.KINU_lJointsList -> size in
    let 0 -> i in
    while (i < size) && (bone == nil) do
    (
      let nth_list kuserstr.KINU_lJointsList i -> [ebone [jnt _ _ _ _]] in
      if (joint != jnt) then nil else
        set bone = ebone;
        
      set i = i + 1;
    );
    bone;
  );;


fun getAngParam(kuserstr, quat, joint)=
  let kuserstr.KINU_tScale -> [xscale yscale zscale] in
  let if (joint == USER_SKEL_HEAD) || (joint == USER_SKEL_L_HAND) || (joint == USER_SKEL_R_HAND) then
        getBoneByJoint kuserstr joint
      else
        nil
  -> bone in
  let kuserstr.KINU_Mesh -> [mesh _ _] in
  if (bone != nil) then
  (
    let SO3MathsQuatToEulerXYZ SO3ObjectGetGlobalOrientation bone -> [x y z] in
      strcatn (ftoa SO3MathsRadianToDegree x)::" "::(ftoa SO3MathsRadianToDegree y)::" "::(ftoa SO3MathsRadianToDegree z)::nil;
  )
  else if (mesh != nil) then
  (
    let SO3ObjectGetGlobalOrientation mesh -> gquat in
    let SO3MathsQuatToEulerXYZ (SO3MathsQuatAdd gquat quat) -> [x y z] in
      strcatn (ftoa SO3MathsRadianToDegree x)::" "::(ftoa SO3MathsRadianToDegree y)::" "::(ftoa SO3MathsRadianToDegree z)::nil;
  )
  else
  (
    let SO3MathsQuatToEulerXYZ quat -> [x y z] in
      strcatn (ftoa SO3MathsRadianToDegree x)::" "::(ftoa SO3MathsRadianToDegree y)::" "::(ftoa SO3MathsRadianToDegree z)::nil; 
  );;


fun getVecParam(kuserstr, vec, joint)=
  let kuserstr.KINU_tScale -> [xscale yscale zscale] in
  let vec -> [x y z] in
  let if (joint == USER_SKEL_HEAD) || (joint == USER_SKEL_L_HAND) || (joint == USER_SKEL_R_HAND) then
        getBoneByJoint kuserstr joint
      else
        nil
  -> bone in
  let kuserstr.KINU_Mesh -> [mesh _ _] in
  if (bone != nil) then
  (
    let SO3ObjectGetGlobalPosition bone -> [x y z] in
      strcatn (ftoa x)::" "::(ftoa y)::" "::(ftoa z)::nil;
  )
  else if (mesh != nil) then
  (
    let [(-.x) y (-.z)] -> vec in
    let SO3ObjectGetGlobalPosition mesh -> gvec in
    let SO3MathsQuatGetDirection (SO3ObjectGetGlobalOrientation mesh) vec -> dir in
    let if (gvec == nil) then vec else addVectorF gvec (subVectorF dir gvec) -> [x y z] in
      strcatn (ftoa (xscale *. x))::" "::(ftoa (yscale *. y))::" "::(ftoa (zscale *. z))::nil;    
  )
  else
  (
    let [x y z] -> vec in
      strcatn (ftoa (xscale *. (if kuserstr.KINU_bFlipX then -.x else x)))::" "::(ftoa (yscale *. y))::" "::(ftoa (zscale *. z))::nil;   
  );;


/*
fun getOrientationParam(kuserstr, quat, joint)=
  let kuserstr.KINU_tScale -> [xscale yscale zscale] in
  let if (joint == USER_SKEL_HEAD) || (joint == USER_SKEL_L_HAND) || (joint == USER_SKEL_R_HAND) then
        getBoneByJoint kuserstr joint
      else
        nil
  -> bone in
  let kuserstr.KINU_Mesh -> [mesh _ _] in
  if (bone != nil) then
  (
    let SO3ObjectGetGlobalOrientation bone -> gquat in
    let SO3MathsQuatToEulerXYZ gquat -> [ax ay az] in
      strcatn (ftoa SO3MathsRadianToDegree ax)::" "::(ftoa SO3MathsRadianToDegree ay)::" "::(ftoa SO3MathsRadianToDegree az)::nil;
  )
  else if (mesh != nil) then
  (
    let [(-.x) y (-.z)] -> vec in
    let SO3MathsQuatAdd (SO3ObjectGetGlobalPosition mesh) quat -> gquat in
    let SO3MathsQuatToEulerXYZ gquat -> [ax ay az] in
      strcatn (ftoa SO3MathsRadianToDegree ax)::" "::(ftoa SO3MathsRadianToDegree ay)::" "::(ftoa SO3MathsRadianToDegree az)::nil;
  )
  else
  //use the current camera if mesh is not set
  (
    let V3DgetDefaultCamera c3dXsession -> cam in
    let SO3MathsQuatGetDirection (SO3ObjectGetGlobalOrientation cam) vec -> dir in
    let if (gvec == nil) || ((joint != USER_SKEL_L_HAND) && (joint != USER_SKEL_R_HAND)) then vec else addVectorF gvec dir -> [x y z] in
      strcatn (ftoa (xscale *. x))::" "::(ftoa (yscale *. y))::" "::(ftoa (zscale *. z))::nil;   
  );;
*/


fun detectFingersFigure(kuserstr, lvec, rvec, mirror)=
  if (kuserstr.KINU_tLeftHandPos == nil) || (kuserstr.KINU_tRightHandPos == nil) then nil else
  let _GETKinectDeviceSize kuserstr.KINU_Device -> [kwidth kheight] in
  let lvec -> [glx gly glz] in
  let rvec -> [grx gry grz] in
  let kuserstr.KINU_tLeftHandPos -> [lx ly lz] in
  let kuserstr.KINU_tRightHandPos -> [rx ry rz] in
  let sqrt (sqr (lx -. glx) +. sqr (ly -. gly) +. sqr (lz -. glz)) -> lspeed in
  let sqrt (sqr (rx -. grx) +. sqr (ry -. gry) +. sqr (rz -. grz)) -> rspeed in
  let 1 -> nbframe in
  let _GETKinectUserFingersPixelPosition kuserstr.KINU_User if !mirror then USER_SKEL_R_HAND else USER_SKEL_L_HAND -> lfp in
  let _GETKinectUserFingersPixelPosition kuserstr.KINU_User if !mirror then USER_SKEL_L_HAND else USER_SKEL_R_HAND -> rfp in
  let _GETKinectUserJointPixelPosition kuserstr.KINU_User if !mirror then USER_SKEL_R_HAND else USER_SKEL_L_HAND 0.7 -> [lpx lpy lpz] in
  let _GETKinectUserJointPixelPosition kuserstr.KINU_User if !mirror then USER_SKEL_L_HAND else USER_SKEL_R_HAND 0.7 -> [rpx rpy rpz] in
  let V3DgetSessionView c3dXsession -> viewstr in
  let V3DgetDefaultViewport viewstr -> viewportstr in
  let V3DgetViewportSize viewstr viewportstr -> [_ _ w h] in
  (
    let kuserstr.KINU_tLeftHandFingers -> [nb mb interv] in
    if ((interv >= nbframe) && (nb != (mb / interv))) then
    (
      SendPluginEvent kuserstr.KINU_Inst "Left nb fingers" itoa (mb / interv) nil;
      set kuserstr.KINU_tLeftHandFingers = [(mb / interv) 0 0];

      if ((mb / interv) >= 2) || (!_GETKinectUserHandVisible kuserstr.KINU_User if !mirror then USER_SKEL_R_HAND else USER_SKEL_L_HAND) then 
      (
        set kuserstr.KINU_tLeftHandClickPos = nil;
        set kuserstr.KINU_tLastLeftHandTrans = [0.0 0.0 0.0];

        if (kuserstr.KINU_iLeftHandFigure != 6) then nil else
        (
          //addLogMessage "Left finger unclick";
          SendPluginEvent kuserstr.KINU_Inst "Left finger unclick" nil nil;
        );
        
        set kuserstr.KINU_iLeftHandFigure = 5;
      )
      else if ((mb / interv) <= 0) && ((absf lspeed) <=. 0.015) && (_GETKinectUserHandVisible kuserstr.KINU_User if !mirror then USER_SKEL_R_HAND else USER_SKEL_L_HAND) then 
      (
        if (kuserstr.KINU_iLeftHandFigure == 5) then
        (
          set kuserstr.KINU_iLeftHandFigure = 6;
          set kuserstr.KINU_tLeftHandClickPos = [glx gly glz];
          
          let ftoi ((itof lpx) *. (itof w) /. (itof kwidth)) -> px in
          let ftoi ((itof lpy) *. (itof h) /. (itof kheight)) -> py in
          (
            //addLogMessage "Left finger click";
            SendPluginEvent kuserstr.KINU_Inst "Left finger click" strcatn (itoa px)::" "::(itoa py)::nil nil;
          );
          0;
        )
        else nil;
      )
      /*else if (kuserstr.KINU_iLeftHandFigure != 6) then
      (
        set kuserstr.KINU_iLeftHandFigure = 0;
      )*/
      else nil;
      
      /*
        if lfp == nil then nil else
        addLogMessage strcat "Fingers on left : " itoa (sizelist lfp);
      */
      0;
    )
    else
    (
      if (interv >= nbframe) then
        mutate kuserstr.KINU_tLeftHandFingers <- [_ (if lfp == nil then 0 else (sizelist lfp)) 1]
      else
        mutate kuserstr.KINU_tLeftHandFingers <- [_ (mb + (if lfp == nil then 0 else (sizelist lfp))) /*(max mb (sizelist lfp))*/ (interv + 1)];
      0;
    );
    
    let kuserstr.KINU_tRightHandFingers -> [nb mb interv] in
    if ((interv >= nbframe) && (nb != (mb / interv))) then
    (
      SendPluginEvent kuserstr.KINU_Inst "Right nb fingers" itoa (mb / interv) nil;
      set kuserstr.KINU_tRightHandFingers = [(mb / interv) 0 0];
      
      if ((mb / interv) >= 2) || (!_GETKinectUserHandVisible kuserstr.KINU_User if !mirror then USER_SKEL_L_HAND else USER_SKEL_R_HAND) then
      (
        set kuserstr.KINU_tRightHandClickPos = nil;
        set kuserstr.KINU_tLastRightHandTrans = [0.0 0.0 0.0];
        
        if (kuserstr.KINU_iRightHandFigure != 6) then nil else
        (
          //addLogMessage "Right finger unclick";
          SendPluginEvent kuserstr.KINU_Inst "Right finger unclick" nil nil;
        );
        
        set kuserstr.KINU_iRightHandFigure = 5;
      )
      else if ((mb / interv) <= 0) && ((absf rspeed) <=. 0.015) && (_GETKinectUserHandVisible kuserstr.KINU_User if !mirror then USER_SKEL_L_HAND else USER_SKEL_R_HAND) then
      (
        if (kuserstr.KINU_iRightHandFigure == 5) then
        (
          set kuserstr.KINU_iRightHandFigure = 6;
  
          set kuserstr.KINU_tRightHandClickPos = [grx gry grz];
          
          let ftoi ((itof rpx) *. (itof w) /. (itof kwidth)) -> px in
          let ftoi ((itof rpy) *. (itof h) /. (itof kheight)) -> py in
          (
            //addLogMessage "Right finger click";
            //addLogMessage strcat "R speed : " (ftoa (absf rspeed));
            SendPluginEvent kuserstr.KINU_Inst "Right finger click" strcatn (itoa px)::" "::(itoa py)::nil nil;
          );
          0;
        )
        else nil;
      )
      /*else if (kuserstr.KINU_iRightHandFigure != 6) then
      (
        set kuserstr.KINU_iRightHandFigure = 0;
      )*/
      else nil;
      
      /*
      if rfp == nil then nil else
      addLogMessage strcat "Fingers on right : " itoa (sizelist rfp);
      */
      0;
    )
    else
    (
      if (interv >= nbframe) then
        mutate kuserstr.KINU_tRightHandFingers <- [_ (if rfp == nil then 0 else (sizelist rfp)) 1]
      else
        mutate kuserstr.KINU_tRightHandFingers <- [_ (mb + (if rfp == nil then 0 else (sizelist rfp))) /*(max mb (sizelist rfp))*/ (interv + 1)];
      0;
    );
  );
  0;;


fun getShortestRot(ang)=
  let ang -> [y p r] in
  let if y >. 180.0 then y -. 360.0 else if y <. (-.180.0) then y +. 360.0 else y -> y in
  let if p >. 180.0 then p -. 360.0 else if p <. (-.180.0) then p +. 360.0 else p -> p in
  let if r >. 180.0 then r -. 360.0 else if r <. (-.180.0) then r +. 360.0 else r -> r in
    [y p r];;


fun cbKinectData(inst, sessionstr, etime, kuserstr)=     
  updateSkeleton kuserstr;

  let _GETKinectDeviceMirror kuserstr.KINU_Device -> mirror in
  let _GETKinectUserJointPosition kuserstr.KINU_User USER_SKEL_HEAD fConfidence -> ghvec in
  let _GETKinectUserJointOrientation kuserstr.KINU_User USER_SKEL_HEAD fConfidence -> hquat in
  let _GETKinectUserJointPosition kuserstr.KINU_User USER_SKEL_TORSO fConfidence -> gwvec in
  let _GETKinectUserJointOrientation kuserstr.KINU_User USER_SKEL_TORSO fConfidence -> thquat in
  let _GETKinectUserJointPosition kuserstr.KINU_User if !mirror then USER_SKEL_R_HAND else USER_SKEL_L_HAND fConfidence -> glvec in
  let _GETKinectUserJointPosition kuserstr.KINU_User if !mirror then USER_SKEL_L_HAND else USER_SKEL_R_HAND fConfidence -> grvec in
  
  //convert to meter
  let multiplyVectorF ghvec [0.01 0.01 0.01] -> hvec in 
  let multiplyVectorF gwvec [0.01 0.01 0.01] -> wvec in
  let multiplyVectorF glvec [0.01 0.01 0.01] -> lvec in
  let multiplyVectorF grvec [0.01 0.01 0.01] -> rvec in
  let hvec -> [hx hy hz] in 
  let wvec -> [wx wy wz] in
  let lvec -> [lx ly lz] in
  let rvec -> [rx ry rz] in
  let subVectorF lvec wvec -> tlvec in
  let subVectorF rvec wvec -> trvec in
  
  let kuserstr.KINU_tScale -> [xscale yscale zscale] in
  
  let kuserstr.KINU_Mesh -> [mesh [_ oy _] _] in
  let SO3GetRootBoneFromMesh mesh -> rootbone in
  let [[0.0 0.0 0.0] [0.0 0.0 0.0]] -> hcontrol in
  (   
    if (gwvec == nil) then nil else
    (
      if (kuserstr.KINU_InitPos != nil) then nil else
        set kuserstr.KINU_InitPos = [(wx *. xscale) (wy *. yscale) (wz *. zscale)];
      
      let kuserstr.KINU_InitPos -> [ix iy iz] in
      let if iz == 0.0 then 0.0 else (iy /. iz) *. (wz *. zscale) -> cy in
      let if (((wz *. zscale) +. cy) == 0.0) then wy else ((wy *. yscale) /. (wz *. zscale) +. cy) -> nby in
      let switch kuserstr.KINU_lJointsList rootbone -> [joint [px py pz] boquat _ _] in

      let SO3MathsQuatGetYaw thquat 0 -> yaw in
      let SO3ObjectGetGlobalScale mesh -> [sx sy sz] in
      (
        SO3ObjectSetOrientation rootbone (SO3MathsQuatAdd thquat boquat);
        
        SO3ObjectSetPosition mesh addVectorF [(wx *. xscale) 0.0 (wz *. zscale)] [0.0 oy 0.0];
        SO3ObjectSetPosition rootbone [px (py +. (if (sy == 0.0) then 0.0 else (nby /. sy))) pz];
        
        let SO3MathsQuatToEulerXYZ thquat -> [ax ay az] in
        let strcatn (ftoa SO3MathsRadianToDegree ax)::" "::(ftoa SO3MathsRadianToDegree (if kuserstr.KINU_bFlipX then -.ay else ay))::" "::(ftoa SO3MathsRadianToDegree az)::nil -> la in
        let getVecParam kuserstr wvec USER_SKEL_TORSO -> lp in
          SendPluginEvent kuserstr.KINU_Inst "Torso" (strcatn lp::"\n"::la::nil) nil;
      );
    );
    
    if (ghvec == nil) || (kuserstr.KINU_InitPos == nil) then nil else
    let getAngParam kuserstr hquat USER_SKEL_HEAD -> la in
    let getVecParam kuserstr hvec USER_SKEL_HEAD -> lp in
    (
      SendPluginEvent kuserstr.KINU_Inst "Head" (strcatn lp::"\n"::la::nil) nil;
      
      if (kuserstr.KINU_sLastHeadTracking == nil) then nil else
      (
        let kuserstr.KINU_sLastHeadTracking -> [clvec clang] in
        let subVectorF hvec clvec -> [px py pz] in
        let SO3MathsQuatGetDirection thquat [px 0.0 pz] -> [ndx ndy ndz] in
        let [ndx py ndz] -> [px py pz] in
        let SO3MathsQuatToEulerDegreeYZX hquat -> nang in
        let getShortestRot (subVectorF nang clang) -> [roll yaw pitch] in
        //let [0.0 0.0 0.0] -> [roll yaw pitch] in
        // last "\n 1" define an accumulator control, note simple 0 to 2 for override orientation
        let strcatn (strcatnSep (ftoa px)::(ftoa py)::(ftoa pz)::nil " ")::"\n"::(strcatnSep (ftoa -.pitch)::(ftoa yaw)::(ftoa -.roll)::nil " ")::"\n"::"1"::nil -> spos in
          SendPluginEvent kuserstr.KINU_Inst "Head tracker control" spos kuserstr.KINU_Inst.INST_sName;
      );
      
      let SO3MathsQuatToEulerDegreeYZX hquat -> ang in
        set kuserstr.KINU_sLastHeadTracking = [hvec ang];
    );
    
    if (glvec == nil) then nil else
    let _GETKinectUserJointPosition kuserstr.KINU_User USER_SKEL_L_ELBOW fConfidence -> [_ _ lez] in
    (
      let _GETKinectUserJointOrientation kuserstr.KINU_User (if !mirror then USER_SKEL_R_ELBOW else USER_SKEL_L_ELBOW) fConfidence -> quat in
      let SO3MathsQuatToEulerXYZ quat -> [ax ay az] in
      let getVecParam kuserstr lvec (if !mirror then USER_SKEL_R_HAND else USER_SKEL_L_HAND) -> lp in
      let strcatn (ftoa SO3MathsRadianToDegree ax)::" "::(ftoa SO3MathsRadianToDegree (if kuserstr.KINU_bFlipX then -.ay else ay))::" "::(ftoa SO3MathsRadianToDegree az)::nil -> la in
      (
        if (kuserstr.KINU_tLeftHandClickPos == nil) then nil else
        (
          let subVectorF kuserstr.KINU_tLeftHandClickPos tlvec -> lhlv in
          let subVectorF lhlv kuserstr.KINU_tLastLeftHandTrans -> [nx ny nz] in
          let 180.0 *. -.nx -> fdx in
          let 180.0 *. -.ny -> fdy in
          let 180.0 *. nz -> fdz in
          (
            set kuserstr.KINU_tLastLeftHandTrans = lhlv;
            mutate hcontrol <- [[fdx fdy fdz] _];
          );
        );
        SendPluginEvent kuserstr.KINU_Inst "Left hand" (strcatn lp::"\n"::la::nil) nil;
      );
    );

    if (grvec == nil) then nil else
    let _GETKinectUserJointPosition kuserstr.KINU_User USER_SKEL_R_ELBOW fConfidence -> [_ _ rez] in
    (
      let _GETKinectUserJointOrientation kuserstr.KINU_User (if !mirror then USER_SKEL_L_ELBOW else USER_SKEL_R_ELBOW) fConfidence -> quat in
      let SO3MathsQuatToEulerXYZ quat -> [ax ay az] in
      let getVecParam kuserstr rvec (if !mirror then USER_SKEL_L_HAND else USER_SKEL_R_HAND) -> lp in
      let strcatn (ftoa SO3MathsRadianToDegree ax)::" "::(ftoa SO3MathsRadianToDegree (if kuserstr.KINU_bFlipX then -.ay else ay))::" "::(ftoa SO3MathsRadianToDegree az)::nil -> la in
      (
        if (kuserstr.KINU_tRightHandClickPos == nil) then nil else
        (
          let subVectorF kuserstr.KINU_tRightHandClickPos trvec -> lhlv in
          let subVectorF lhlv kuserstr.KINU_tLastRightHandTrans -> [nx ny nz] in
          let 180.0 *. nx -> fdx in
          let 180.0 *. ny -> fdy in
          let 180.0 *. nz -> fdz in
          (
            set kuserstr.KINU_tLastRightHandTrans = lhlv;
            mutate hcontrol <- [_ [fdx fdy fdz]];
          );
        );
        
        SendPluginEvent kuserstr.KINU_Inst "Right hand" (strcatn lp::"\n"::la::nil) nil;
      );
    );
    
    
    if ((glvec == nil) || (grvec == nil)) then nil else
    (
      let (absf(rx -. lx)) -> dx in
      let (absf(ry -. ly)) -> dy in
      let (absf(rz -. lz)) -> dz in
      let strcatn (ftoa dx)::" "::(ftoa dy)::" "::(ftoa dz)::nil -> lv in
      let 90.0 /. 0.40 -> coef in
      let 0.0 -> ax in
      let ((rz -. lz) *. coef) -> ay in
      let ((ry -. ly) *. coef) -> az in
      let strcatn (ftoa ax)::" "::(ftoa ay)::" "::(ftoa (-.az))::nil -> la in
      (
        SendPluginEvent kuserstr.KINU_Inst "Hands distance" lv nil;
        SendPluginEvent kuserstr.KINU_Inst "Hands rotation" la nil;
      );
    );
    
    detectFingersFigure kuserstr tlvec trvec mirror;

    let hcontrol -> [[vx vy vz] [ax ay az]] in 
      SendPluginEvent inst "Hands control" (strbuild ((ftoa (if !mirror then -.vx else vx))::(ftoa vy)::(ftoa vz)::nil)::((ftoa -.ay)::(ftoa (if !mirror then -.ax else ax))::(ftoa az)::nil)::nil) inst.INST_sName;
    
    set kuserstr.KINU_tLeftHandPos = tlvec;
    set kuserstr.KINU_tRightHandPos = trvec;
  );
  
  // pose detection
  if (bPoseTrain) then nil else
  let _GETKinectUserJointOrientation kuserstr.KINU_User USER_SKEL_TORSO 0.1 -> tquat in
  let USER_SKEL_L_SHOULDER::USER_SKEL_R_SHOULDER::USER_SKEL_L_ELBOW::USER_SKEL_R_ELBOW::nil -> lj in
  let nil -> lout in
  (
    let sizelist lj -> size in
    let 0 -> i in
    while (lj != nil) do
    (
      let (_GETKinectUserJointOrientation kuserstr.KINU_User (hd lj) 0.1) -> quat in
      let if (quat == nil) then [0.0 0.0 0.0] else SO3MathsQuatToEulerXYZ SO3MathsQuatSubstract quat tquat -> [x y z] in
        set lout = [(SO3MathsRadianToDegree x) (SO3MathsRadianToDegree y) (SO3MathsRadianToDegree z)]::lout;
      
      set lj = tl lj;
    );
    
    //if ((sizelist lout) != 4) then nil else
      _MlAddDetectionData kuserstr.KINU_mlp revertlist lout;
  );  
  0;;

  
/*! \brief Callback on "Calibration Started" dms action
*
*  Get event when Kinect Calibration has started
*
*  <b>Prototype:</b> fun [PInstance DMI S S I [F F F I]] I
*
*  \param OBJKinect : KInect Instance
*  \param PInstance : plugIT instance
* 
*  \return I : 0
**/
fun cbCalibrationStart(kuser, kuserstr)=
  SendPluginEvent kuserstr.KINU_Inst "Calibration started" nil nil;
  0;;
  
  
/*! \brief Callback on "Calibration Ended" dms action
*
*  Get event when Kinect Calibration has ended
*
*  <b>Prototype:</b> fun [PInstance DMI S S I [F F F I]] I
*
*  \param OBJKinect : KInect Instance
*  \param PInstance : plugIT instance
* 
*  \return I : 0
**/
fun cbCalibrationEnd(kuser, kuserstr)=
  SendPluginEvent kuserstr.KINU_Inst "Calibration ended" nil nil;
  0;;


/*! \brief Callback on "Pose Detected" dms action
*
*  Get event when Kinect Pose detection has found
*
*  <b>Prototype:</b> fun [PInstance DMI S S I [F F F I]] I
*
*  \param OBJKinect : KInect Instance
*  \param PInstance : plugIT instance
* 
*  \return I : 0
**/
fun cbCalibrationPoseDetected(kuser, kuserstr)=
  SendPluginEvent kuserstr.KINU_Inst "Pose detected" nil nil;
  0;;


/*! \brief Callback on "disconnected" dms action
*
*  Get event when Kinect pose detection has lost
*
*  <b>Prototype:</b> fun [PInstance DMI S S I [F F F I]] I
*
*  \param OBJKinect : KInect Instance
*  \param PInstance : plugIT instance
* 
*  \return I : 0
**/
fun cbCalibrationPoseLost(kuser, kuserstr)=
  SendPluginEvent kuserstr.KINU_Inst "Pose lost" nil nil;
  0;;


fun cbHandFound(kuser, kuserstr, hand)=
  let _GETKinectDeviceMirror kuserstr.KINU_Device -> mirror in
  if ((hand == USER_SKEL_R_HAND && !mirror) || (hand == USER_SKEL_L_HAND && mirror)) then
    SendPluginEvent kuserstr.KINU_Inst "Left hand found" nil nil
  else if ((hand == USER_SKEL_L_HAND && !mirror) || (hand == USER_SKEL_R_HAND && mirror)) then
    SendPluginEvent kuserstr.KINU_Inst "Right hand found" nil nil
  else nil;
  0;;


fun cbHandLost(kuser, kuserstr, hand)=
  let _GETKinectDeviceMirror kuserstr.KINU_Device -> mirror in
  if ((hand == USER_SKEL_R_HAND && !mirror) || (hand == USER_SKEL_L_HAND && mirror)) then
  (
    if (kuserstr.KINU_iLeftHandFigure != 6) then nil else
    (
       SendPluginEvent kuserstr.KINU_Inst "Left finger unclick" nil nil; 
       set kuserstr.KINU_iLeftHandFigure = 5;
    );
    
    set kuserstr.KINU_tLeftHandClickPos = nil;
    set kuserstr.KINU_tLastLeftHandTrans = [0.0 0.0 0.0];
        
    SendPluginEvent kuserstr.KINU_Inst "Left hand lost" nil nil;
  )
  else if ((hand == USER_SKEL_L_HAND && !mirror) || (hand == USER_SKEL_R_HAND && mirror)) then
  (
    if (kuserstr.KINU_iRightHandFigure != 6) then nil else
    (
       SendPluginEvent kuserstr.KINU_Inst "Right finger unclick" nil nil; 
       set kuserstr.KINU_iRightHandFigure = 5;
    );
    
    set kuserstr.KINU_tRightHandClickPos = nil;
    set kuserstr.KINU_tLastRightHandTrans = [0.0 0.0 0.0];

    SendPluginEvent kuserstr.KINU_Inst "Right hand lost" nil nil;
  )
  else nil;
  0;;


fun cbHandMove(kuser, kuserstr, hand, trans)=
  let trans -> [x y z] in
  let _GETKinectUserJointPixelPosition kuserstr.KINU_User hand 0.7 -> [px py pz] in
  //let _GETKinectUserJointPixelPosition kuserstr.KINU_User USER_SKEL_TORSO 0.9 -> [tx ty tz] in
  let V3DgetSessionView c3dXsession -> viewstr in
  let V3DgetDefaultViewport viewstr -> viewportstr in
  let V3DgetViewportSize viewstr viewportstr -> [_ _ w h] in
  let _GETKinectDeviceMirror kuserstr.KINU_Device -> mirror in
  let _GETKinectDeviceSize kuserstr.KINU_Device -> [kwidth kheight] in
  let kuserstr.KINU_tPmargin -> [xm ym] in
  let ftoi ((itof (px - (xm / 2))) *. (itof w) /. (itof (kwidth - xm))) -> px in
  let ftoi ((itof (py - (ym / 2))) *. (itof h) /. (itof (kheight - ym))) -> py in
  (
    //addLogMessage strcatn "Hand move : "::(itoa x)::" "::(itoa y)::" "::(itoa z)::nil;
    if ((hand == USER_SKEL_R_HAND && !mirror) || (hand == USER_SKEL_L_HAND && mirror)) then
      SendPluginEvent kuserstr.KINU_Inst "Left hand move" strcatn (itoa px)::" "::(itoa py)::" "::(itoa pz)::nil nil
    else if ((hand == USER_SKEL_L_HAND && !mirror) || (hand == USER_SKEL_R_HAND && mirror)) then
      SendPluginEvent kuserstr.KINU_Inst "Right hand move" strcatn (itoa px)::" "::(itoa py)::" "::(itoa pz)::nil nil
    else nil;
  );
  0;;


fun foundBoneType(name)=
  let -1 -> type in
  (
    let sizelist lBonesBindNames -> size in
    let 0 -> i in
    while (i < size) && (type == -1) do
    (
      let nth_list lBonesBindNames i -> [ftype lnames] in
      if (strfindiList lnames name) == nil then nil else
        set type = ftype;
      
      set i = i + 1;
    );
    type;
  );;


fun linkBones(bone, kuserstr)=
  let SO3ObjectGetName bone -> bonename in
  let SO3ObjectGetChildren bone -> lchild in
  (
    let foundBoneType bonename -> type in
    if (type == -1) then nil else
    let SO3ObjectGetGlobalOrientation bone -> gquat in
    let SO3ObjectGetOrientation bone -> lquat in
    let SO3ObjectGetPosition bone -> vec in
    let kuserstr.KINU_Mesh -> [mesh _ _] in
    (
      if (SKL_USE_GLOBAL_COORDS) then
        set kuserstr.KINU_lJointsList = [bone [type vec gquat lquat nil]]::kuserstr.KINU_lJointsList
      else
      (
        //for local use
        SO3ObjectSetInheritOrientation bone 0;
        SO3ObjectResetOrientation bone;
        SO3ObjectSetOrientation bone SO3MathsQuatAdd (SO3ObjectGetGlobalOrientation mesh) gquat;
        let SO3ObjectGetOrientation bone -> sklquat in
        (
          SO3ObjectSetInheritOrientation bone 1;  
          SO3ObjectSetOrientation bone lquat;
          set kuserstr.KINU_lJointsList = [bone [type vec sklquat lquat nil]]::kuserstr.KINU_lJointsList;
        );
      );
    );
    
    while lchild != nil do
    (     
      linkBones (hd lchild) kuserstr;
      set lchild = tl lchild;
    );
  );
  0;;


fun cbUserFound(kuser, kuserstr)=
  SendPluginEvent kuserstr.KINU_Inst "User found" nil nil;
  
  // force an update to be sure the model is correct in case of a reset or stop animation on user found
  let V3DgetSessionView c3dXsession -> viewstr in
      SO3BufferUpdate viewstr.V3D_buffer;
  
  let kuserstr.KINU_Mesh -> [mesh _ _] in
  let SO3GetRootBoneFromMesh mesh -> rootbone in
  if (rootbone == nil) then nil else
  (
    SO3ObjectResetOrientation mesh;
    SO3ObjectRotateYaw mesh (if (!_GETKinectDeviceMirror kuserstr.KINU_Device) then 0.0 else (SO3MathsDegreeToRadian 180.0)) SO3_LOCAL_TS;
    
    let SO3ObjectGetOrientation rootbone -> lquat in
    let SO3ObjectGetPosition rootbone -> vec in
     set kuserstr.KINU_lJointsList = [rootbone [USER_SKEL_WAIST vec lquat lquat nil]]::kuserstr.KINU_lJointsList;
    linkBones rootbone kuserstr;
    
    set kuserstr.KINU_lJointsList = revertlist kuserstr.KINU_lJointsList;
  );
  0;;


fun cbUserLost(kuser, kuserstr)=
  resetSkeleton kuserstr;
  set kuserstr.KINU_sLastHeadTracking = nil;
  SendPluginEvent kuserstr.KINU_Inst "User lost" nil nil;
  0;;


fun cbPoseDetect(ml, kuserstr, catname)=
  if (!strcmp catname kuserstr.KINU_sLastdetectedPose) then nil else
  (
    SendPluginEvent kuserstr.KINU_Inst catname nil nil;
    SendPluginEvent kuserstr.KINU_Inst "Pose detected" catname nil;
    set kuserstr.KINU_sLastdetectedPose = catname;
  );
  0;;


fun cbAddTrainData(inst, from, action, param, reply, kuserstr)=
  let nth_list lPoseLabel kuserstr.KINU_iTrainingPos -> label in
  (
    addLogMessage strcat "KinectUser Train : " label;
    let _GETKinectUserJointOrientation kuserstr.KINU_User USER_SKEL_TORSO 0.1 -> tquat in
    let USER_SKEL_L_SHOULDER::USER_SKEL_R_SHOULDER::USER_SKEL_L_ELBOW::USER_SKEL_R_ELBOW::nil -> lj in
    let nil -> lout in
    (
      let sizelist lj -> size in
      let 0 -> i in
      while (lj != nil) do
      (
        let (_GETKinectUserJointOrientation kuserstr.KINU_User (hd lj) 0.1) -> quat in
        if (quat == nil) then nil else
          let SO3MathsQuatToEulerXYZ SO3MathsQuatSubstract quat tquat -> [x y z] in
            set lout = [(SO3MathsRadianToDegree x) (SO3MathsRadianToDegree y) (SO3MathsRadianToDegree z)]::lout;
        
        set lj = tl lj;
      );
      
      if ((sizelist lout) != 4) then nil else
      (
        _MlAddTrainingData kuserstr.KINU_mlp revertlist lout label;
        _MlSaveData kuserstr.KINU_mlp _getmodifypack strcat (getPluginDirectory (getInstancePlugin kuserstr.KINU_Inst)) "/res/kinposes.xlm";
        addLogMessage strcatn "KinectUser save "::label::" to "::(strcat (getPluginDirectory (getInstancePlugin kuserstr.KINU_Inst)) "/res/kinposes.xlm")::nil;
        set kuserstr.KINU_iTrainingPos = kuserstr.KINU_iTrainingPos + 1;
      );
    );
  );
  0;;


// Create Kinect Callback
fun Create(inst, from, action, param, reply, kuserstr)=
  if (kuserstr.KINU_User != nil) then nil else
  let if (atoi param) == nil then 0 else atoi param -> id in
  let _GETKinectDeviceById id -> mKinect in
  let _CRKinectUser _channel mKinect -> kuser in
	(
	  set kuserstr.KINU_Device = mKinect;
	  set kuserstr.KINU_User = kuser;
	  
	  _CBKinectUserFound kuser @cbUserFound kuserstr;
	  _CBKinectUserLost kuser @cbUserLost kuserstr;
  	_CBKinectUserCalibrationStart kuser @cbCalibrationStart kuserstr;
		_CBKinectUserCalibrationEnd kuser @cbCalibrationEnd kuserstr;
		_CBKinectUserPoseDetected kuser @cbCalibrationPoseDetected kuserstr;
		_CBKinectUserPoseLost kuser @cbCalibrationPoseLost kuserstr;
		_CBKinectUserHandFound kuser @cbHandFound kuserstr;
		_CBKinectUserHandLost kuser @cbHandLost kuserstr;
	  _CBKinectUserHandMove kuser @cbHandMove kuserstr;
	  
    setPluginInstanceCbScenePreRender kuserstr.KINU_Inst mkfun4 @cbKinectData kuserstr;
	);
  0;;


fun deleteOb(inst, kuserstr) =
  setPluginInstanceCbScenePreRender kuserstr.KINU_Inst nil;
  
  resetSkeleton kuserstr;
  
  _DSKinectUser kuserstr.KINU_User;
  set kuserstr.KINU_User = nil;
  _DSml kuserstr.KINU_mlp;
  0;;


// Destroy Kinect Callback
fun Destroy(inst, from, action, param, reply, kuserstr)=
  setPluginInstanceCbScenePreRender kuserstr.KINU_Inst nil;
  
  resetSkeleton kuserstr;
  
  _DSKinectUser kuserstr.KINU_User;
  set kuserstr.KINU_User = nil;
  set kuserstr.KINU_sLastdetectedPose = nil;
  0;;


fun cbNewOb(inst)=
  let (getPluginInstanceParam inst "object") -> objname in
  let atof (getPluginInstanceParam inst "scaleX") -> ax in
  let atof (getPluginInstanceParam inst "scaleY") -> ay in
  let atof (getPluginInstanceParam inst "scaleZ") -> az in
  
  let if ax == nil then 1.0 else ax -> ax in
  let if ay == nil then 1.0 else ay -> ay in
  let if az == nil then 1.0 else az -> az in
  let atoi (getPluginInstanceParam inst "flipX") -> flipx in
  let if flipx == nil then 0 else flipx -> flipx in
  let atoi (getPluginInstanceParam inst "handXmargin") -> handxmargin in
  let if handxmargin == nil then 0 else handxmargin -> handxmargin in
  let atoi (getPluginInstanceParam inst "handYmargin") -> handymargin in
  let if handymargin == nil then 0 else handymargin -> handymargin in
  
  let SO3SceneGetObject (V3DgetSession c3dXsession) objname -> obj in
  let SO3ObjectGetOrientation obj -> quat in
  let SO3ObjectGetPosition obj -> vec in
  
  let _CRml _channel 0 1 0.61 -> mlp in
  let mkPlugKinectUserStr [inst nil nil [obj vec quat] [ax ay az] flipx [handxmargin handymargin] nil nil 0 0 [0 0 0] [0 0 0] nil nil nil nil [0.0 0.0 0.0] [0.0 0.0 0.0] mlp nil nil 0] -> kuserstr in
  (
    if (bPoseTrain) then nil else
    (
      _MlLoadData mlp (_checkpack strcat (getPluginDirectory (getInstancePlugin inst)) "/res/kinposes.xlm");
      _CBMlDetect mlp @cbPoseDetect kuserstr;
    );
    
  	PluginRegisterAction inst "Create" mkfun6 @Create kuserstr;
    PluginRegisterAction inst "Destroy" mkfun6 @Destroy kuserstr;
    
    if (!bPoseTrain) then nil else
      PluginRegisterAction inst "Train pose" mkfun6 @cbAddTrainData kuserstr;
    
    setPluginInstanceCbDel inst mkfun2 @deleteOb kuserstr;
  );
  0;;


fun IniPlug(file)=
  set lBonesBindNames = 
                  [USER_SKEL_HEAD "head"::"tête"::"tete"::nil]::
                  [USER_SKEL_NECK "neck"::"cou"::nil]::
                  [USER_SKEL_TORSO "chest"::"stomach"::"spine2"::"spine3"::"colonne1"::"colonne2"::nil]::
                  [USER_SKEL_WAIST "waist"::"pelvis"::"bassin"::"hips"::nil]::
                  
                  [USER_SKEL_L_COLLAR nil]::
                  [USER_SKEL_R_COLLAR nil]::
                  
                  [USER_SKEL_L_SHOULDER "humerus.l"::"l upperarm"::"l_upperarm"::"g membre supérieur du bras"::"leftarm"::nil]::
                  [USER_SKEL_R_SHOULDER "humerus.r"::"r upperarm"::"r_upperarm"::"d membre supérieur du bras"::"rightarm"::nil]::
                  
                  [USER_SKEL_L_HAND "hand.l"::"l hand"::"l_hand"::"g main"::"lefthand"::nil]::
                  [USER_SKEL_R_HAND "hand.r"::"r hand"::"r_hand"::"d main"::"righthand"::nil]::
                  
                  [USER_SKEL_L_FINGERTIP nil]::
                  [USER_SKEL_R_FINGERTIP nil]::
                  
                  [USER_SKEL_L_WRIST nil]::
                  [USER_SKEL_R_WRIST nil]::
                  
                  [USER_SKEL_L_ELBOW "ulna.l"::"l forearm"::"l_forearm"::"g avant bras"::"leftforearm"::nil]::
                  [USER_SKEL_R_ELBOW "ulna.r"::"r forearm"::"r_forearm"::"d avant bras"::"rightforearm"::nil]::
                  
                  [USER_SKEL_L_HIP "thigh.l"::"l thigh"::"l_thigh"::"g cuisse"::"leftupleg"::nil]::
                  [USER_SKEL_R_HIP "thigh.r"::"r thigh"::"r_thigh"::"d cuisse"::"rightupleg"::nil]::
                  
                  [USER_SKEL_L_KNEE "calf.l"::"l calf"::"l_calf"::"g mollet"::"leftleg"::nil]::
                  [USER_SKEL_R_KNEE "calf.r"::"r calf"::"r_calf"::"d mollet"::"rightleg"::nil]::
                  
                  //[USER_SKEL_L_FOOT "foot.l"::"l foot"::"g pied"::"leftfoot"::nil]::
                  //[USER_SKEL_R_FOOT "foot.r"::"r foot"::"d pied"::"rightfoot"::nil]::
                  
                  [USER_SKEL_L_ANKLE nil]::
                  [USER_SKEL_R_ANKLE nil]::        
                  nil;

  PlugRegister @cbNewOb nil;
  setPluginEditor @dynamicedit;
  0;;