/*
-----------------------------------------------------------------------------
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

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


/*-------------------- Global variables --------------------*/
struct PlugMyo = [
  PMYO_inst                 : PInstance,
  PMYO_object               : ObjMyo,
  PMYO_father               : SO3_OBJECT,
  PMYO_iFatherMode          : I,
  PMYO_initpos              : [[F F F] [F F F F]],
  PMYO_tLastOrientation     : [F F F F],
  PMYO_tLastAccel           : [F F F],
  PMYO_tLastControl         : [F F F F],
  PMYO_tFirstRotation       : [I I I F F [F F F F]],
  PMYO_bLocal               : I,
  PMYO_iLastPose            : I,
  PMYO_iTick                : I,
  PMYO_bControl             : I,
  PMYO_bAccel               : I,
  PMYO_bOrientation         : I
          
] mkPlugMyo;;


/*------------------ Delete SpacePoint Fusion object -------------------*/
fun deleteOb(inst, myostr)=
  setPluginInstanceCbScenePreRender inst nil;
  
  // Destroy Fusion instance
	_DSmyoDevice myostr.PMYO_object;
	
  setPluginInstanceCbCameraChange inst nil;
  
  let myostr.PMYO_initpos -> [pos quat] in
  (
    SO3ObjectSetPosition myostr.PMYO_father pos;
    SO3ObjectSetOrientation myostr.PMYO_father quat;
  );
  0;;


fun computeZeroRollVector(forward, antigravity)=
  let crossVectorF forward antigravity -> m in
  let crossVectorF m forward -> roll in
    normalizeVectorF roll;;


fun rollFromZero(zeroroll, forward, up)=
  // The cosine of the angle between the up vector and the zero roll vector. Since both are
  // orthogonal to the forward vector, this tells us how far the Myo has been turned around the
  // forward axis relative to the zero roll vector, but we need to determine separately whether the
  // Myo has been rolled clockwise or counterclockwise.
  let dotVectorF up zeroroll -> cosine in

  // To determine the sign of the roll, we take the cross product of the up vector and the zero
  // roll vector. This cross product will either be the same or opposite direction as the forward
  // vector depending on whether up is clockwise or counter-clockwise from zero roll.
  // Thus the sign of the dot product of forward and it yields the sign of our roll value.
  let crossVectorF up zeroroll -> cp in
  let dotVectorF forward cp -> directionCosine in
  let if directionCosine <. 0.0 then -.1.0 else 1.0 -> sign in

    // Return the angle of roll (in degrees) from the cosine and the sign.
    SO3MathsRadianToDegree (sign *. acos cosine);;


fun normalizeAngle(a)=
  if a >. 180.0 then a -. 360.0 else if a <. (-.180.0) then a +. 360.0 else a;;


fun cbNavPreRender(inst, sessionstr, etime, myostr)=  
  if ((_tickcount - myostr.PMYO_iTick) < 2) then nil else
  if (myostr.PMYO_tLastOrientation == nil) then nil else
  let myostr.PMYO_tLastOrientation -> quat in
  let myostr.PMYO_tLastAccel -> [accX accY accZ] in
  (
    set myostr.PMYO_iTick = _tickcount;
    if(myostr.PMYO_tLastControl == nil) then nil else
    let SO3MathsQuatToEulerDegreePYR myostr.PMYO_tLastControl -> [ox oy oz] in
    let SO3MathsQuatToEulerDegreePYR quat -> [ax yaw az] in
    let yaw -. oy -> ryaw in
    let if ryaw >. 180.0 then ryaw -. 360.0 else if ryaw <. (-.180.0) then ryaw +. 360.0 else ryaw -> ryaw in
    if (!myostr.PMYO_bControl) then nil else
    (
      let ftoa ax -> sax in
      let ftoa ryaw -> say in
      let ftoa az -> saz in
        SendPluginEvent myostr.PMYO_inst "Control" (strcatn "0 0 0\n"::sax::" "::say::" "::saz::"\n2"::nil) inst.INST_sName;
    );
    
    set myostr.PMYO_tLastControl = quat;
  
    if (!myostr.PMYO_bAccel) then nil else
    let SO3MathsQuatGetDirection quat [accX accY accZ] -> [laccX laccY laccZ] in
      SendPluginEvent myostr.PMYO_inst "Accel" strcatnSep (ftoa accX)::(ftoa accY)::(ftoa accZ)::nil " " nil;
    
    if (myostr.PMYO_iFatherMode == 2) then
    (
    	//TODO manage physic for navigation get relativ Yaw pitch roll
      let if (myostr.PMYO_iFatherMode == 1) then SO3MathsQuatAdd quat [0.0 1.0 0.0 0.0] else quat -> rquat in
      if (myostr.PMYO_bLocal) then
        SO3ObjectSetOrientation myostr.PMYO_father rquat
      else
        SO3ObjectSetGlobalOrientation myostr.PMYO_father rquat;  
    )
    else
    (
      let if (myostr.PMYO_iFatherMode == 1) then SO3MathsQuatAdd quat [0.0 1.0 0.0 0.0] else quat -> rquat in
      if (myostr.PMYO_bLocal) then
        SO3ObjectSetOrientation myostr.PMYO_father rquat
      else
        SO3ObjectSetGlobalOrientation myostr.PMYO_father rquat;  
    );

    let myostr.PMYO_tFirstRotation -> [state lstate levent referenceRoll lastv fquat] in
    
    // Current zero roll vector and roll value.
    let SO3MathsQuatToEulerPYR quat -> [x y z] in
    let SO3MathsQuatGetDirection quat [0.0 0.0 itof (_GetMyoArmDirection myostr.PMYO_object)] -> forward in
    let computeZeroRollVector forward [0.0 1.0 0.0] -> zeroRoll in
    let rollFromZero zeroRoll forward SO3MathsQuatGetDirection quat [0.0 1.0 0.0] -> roll in
    let normalizeAngle (roll -. referenceRoll) -> relativeRoll in
    let if ((abs (ftoi relativeRoll)) > 5) then if ((ftoi relativeRoll) > 0) then 1 else -1 else 0 -> dir in
    let normalizeAngle roll -> nroll in
    if (!state) then 
    (
      if (!levent) then nil else
        SendPluginEvent myostr.PMYO_inst "Fist rotation end" (ftoa relativeRoll) nil;
      0;
    )
    else
    (
      if (!lstate) then
      (
        mutate myostr.PMYO_tFirstRotation <- [_ 1 0 nroll 0.0 quat]
      )
      else
      (
        if (!levent) then
        (
          SendPluginEvent myostr.PMYO_inst "Fist rotation start" (ftoa relativeRoll) nil;
          mutate myostr.PMYO_tFirstRotation <- [_ _ 1 _ relativeRoll _];
        )
        else
        (
          SendPluginEvent myostr.PMYO_inst "Fist rotation" strcatn (ftoa relativeRoll)::" "::(ftoa (relativeRoll -. lastv))::" "::(itoa dir)::nil nil;
          mutate myostr.PMYO_tFirstRotation <- [_ _ _ _ relativeRoll _];
        );
      );
      0;
    );

    if (!myostr.PMYO_bOrientation) then nil else
    let SO3MathsQuatToEulerPYR quat -> [x z y] in
      SendPluginEvent myostr.PMYO_inst "Orientation" strcatnSep (ftoa (SO3MathsRadianToDegree x))::(ftoa (SO3MathsRadianToDegree y))::(ftoa (SO3MathsRadianToDegree z))::nil " " nil;
  );  
  0;;


/*----------------------- Callbacks ------------------------*/
/* New orientation data */
fun cbNewOrientationData(myoobj, myostr, quat)=
  set myostr.PMYO_tLastOrientation = quat;
  0;;


/* New raw data */
fun cbAccel(myoobj, myostr, vel)=
  set myostr.PMYO_tLastAccel = vel;
  0;;


fun cbPose(myoobj, myostr, pose)=
  if (pose == myostr.PMYO_iLastPose) then nil else
  let if pose == 0 then "Rest" else
      if pose == 1 then "Fist" else
      if pose == 2 then "Wave in" else
      if pose == 3 then "Wave out" else
      if pose == 4 then "Finger spread" else
      //if pose == 5 then "Reserved" else
      if pose == 5 then "Double tap" else
      "Unknown"
  -> spose in
  (
    let myostr.PMYO_tFirstRotation -> [state lstate _ _ _ _] in
    if (pose == 1) then
      if (state == 1) then nil else
      (
        mutate myostr.PMYO_tFirstRotation <- [1 0 0 0.0 0.0 [0.0 0.0 0.0 1.0]]
      )
    else
      if (state == 0) then nil else
      (
        mutate myostr.PMYO_tFirstRotation <- [0 0 _ _ _ _];
      );
    
    set myostr.PMYO_iLastPose = pose;
    SendPluginEvent myostr.PMYO_inst spose nil nil;
  );
  0;;
  

fun cbVibrate(inst, from, action, param, reply, myostr)=
  _VibrateMyoDevice myostr.PMYO_object (atoi param);
  0;;


/* Update camera control */
fun cbChangeCamera(inst, viewstr, sessionstr, camera, myostr)=
  let V3DgetCameraByType sessionstr camera myostr.PMYO_iFatherMode -> nfather in
  let SO3ObjectGetPosition nfather -> cpos in
  let SO3ObjectGetOrientation nfather -> cquat in
  (
    let myostr.PMYO_initpos -> [pos quat] in
    (
      SO3ObjectSetPosition myostr.PMYO_father pos;
      SO3ObjectSetOrientation myostr.PMYO_father quat;
    );
    
    set myostr.PMYO_father = nfather; 
    set myostr.PMYO_initpos = [cpos cquat];
  );
  0;;
  
  
/* Device connected */
fun cbConnected(myoobj, myostr)=
  SendPluginEvent myostr.PMYO_inst "Connected" nil nil;
  setPluginInstanceCbScenePreRender myostr.PMYO_inst mkfun4 @cbNavPreRender myostr;
  0;;


/* Device disconnected */
fun cbDisconnected(myoobj, myostr)=
  setPluginInstanceCbScenePreRender myostr.PMYO_inst nil;
  set myostr.PMYO_tLastOrientation = nil;
  
  SendPluginEvent myostr.PMYO_inst "Disconnected" nil nil;
  0;;


fun cbArmRecognized(myoobj, myostr, arm)=
  SendPluginEvent myostr.PMYO_inst "Arm recognized" itoa arm nil;
  0;;


fun cbArmLost(myoobj, myostr)=
  SendPluginEvent myostr.PMYO_inst "Arm lost" nil nil;
  0;;


/*--------------- Create new SpacePoint Fusion instance ----------------*/
fun newOb(inst)=
  let (getPluginInstanceParam inst "object") -> objname in
  let atoi (getPluginInstanceParam inst "local") -> local in
  let if (local == nil) then 1 else local -> local in
  
  // Set the SpacePoint Fusion global structure
  let V3DgetObjectByName c3dXsession objname -> father in
  let V3DgetObjectTypeByName objname -> iobjmode in
  let SO3ObjectGetPosition father -> cpos in
  let SO3ObjectGetOrientation father -> cquat in
  
  let (IsInEditor inst) || IsEventLinked inst "Control" -> bcontrol in  
  let (IsInEditor inst) || IsEventLinked inst "Accel" -> borientation in
  let (IsInEditor inst) || IsEventLinked inst "Orientation" -> borientation in
  let mkPlugMyo [inst nil father iobjmode [cpos cquat] nil nil nil [0 0 0 0.0 0.0 [0.0 0.0 0.0 1.0]] local 0 0 bcontrol borientation borientation] -> myostr in
  (
    set myostr.PMYO_object = _CRmyoDevice _channel @cbConnected myostr @cbDisconnected myostr;
    
    if !iobjmode then nil else
      setPluginInstanceCbCameraChange inst mkfun5 @cbChangeCamera myostr;
    
    PluginRegisterAction inst "Vibrate" mkfun6 @cbVibrate myostr;
      
    // Define the callback to call when the plugin instance is deleted
    setPluginInstanceCbDel inst mkfun2 @deleteOb myostr;

    _CBMyoOrientation myostr.PMYO_object @cbNewOrientationData myostr;
    _CBMyoAccel myostr.PMYO_object @cbAccel myostr;
    _CBMyoPose myostr.PMYO_object @cbPose myostr;
    _CBMyoArmRecognized myostr.PMYO_object @cbArmRecognized myostr;
    _CBMyoArmLost myostr.PMYO_object @cbArmLost myostr;
  );
  0;;


/*------------------- Initialize plugIT --------------------*/
fun IniPlug(file)=
  PlugRegister @newOb nil;
  setPluginEditor @dynamicedit;
  0;;