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

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

// time value to prevent too quick lost / found
var iBounceTime = 300;;

struct ArMarkerStr = [
  ARM_inst                 : PInstance,
  ARM_father               : SO3_OBJECT,
  ARM_marker               : ObjArMarker,
  ARM_iFatherMode          : I,
  ARM_initpos              : [[F F F] [F F F F]],
  ARM_lastpos              : [[F F F] [F F F]],
  ARM_fsize                : F,
  ARM_bVisible             : I,
  ARM_bState               : I,
  ARM_bTrackState          : I,
  ARM_iBounceTimeCount     : I

  ] mkArMarkerStr;;
  

fun deleteOb(inst, objpstr)=
  /*if (objpstr.ARM_timerBounce == nil) then nil else
  (
    _deltimer objpstr.ARM_timerBounce;
    set objpstr.ARM_timerBounce = nil;
  );*/
    
  setPluginInstanceCbCameraChange inst nil;
  
  _DSarMarker objpstr.ARM_marker;
  set objpstr.ARM_marker = nil;
  
  let objpstr.ARM_initpos -> [pos quat] in
  (
    SO3ObjectSetPosition objpstr.ARM_father pos;
    SO3ObjectSetOrientation objpstr.ARM_father quat;
  );
  0;;

/*
fun cbBounceLost(trm, objpstr)=
  _deltimer objpstr.ARM_timerBounce;
  set objpstr.ARM_timerBounce = nil;

  SendPluginEvent objpstr.ARM_inst "Lost" itoa objpstr.ARM_iId nil;
  0;;
*/


fun cbControlPreRender(inst, sessionstr, etime, objpstr)=
  let if etime < 1000 then 1000 else etime -> etime in
  let _ISarMarkerVisible objpstr.ARM_marker -> visible in
  (
    if ((!objpstr.ARM_bState) || !visible || !objpstr.ARM_bTrackState) then nil else
    (
      let _GETarMarkerPosition objpstr.ARM_marker -> pos in
      let _GETarMarkerPixelPosition objpstr.ARM_marker -> pixelpos in
      let _GETarMarkerOrientation objpstr.ARM_marker -> quat in
      (
        let pos -> [x y z] in
        let pixelpos -> [pixx pixy pixz] in
        let V3DgetSessionView sessionstr -> viewstr in
        let V3DgetDefaultViewport viewstr -> viewportstr in
        let V3DgetViewportSize viewstr viewportstr -> [_ _ w h] in
        let c3dxCameraSize -> [cw ch] in
        let ftoi ((itof pixx) *. (itof w) /. (itof cw)) -> pixx in
        let if c3dxCameraFlip then w - pixx else pixx -> pixx in
        let ftoi ((itof pixy) *. (itof h) /. (itof ch)) -> pixy in
        let SO3ViewportGetCamera viewportstr.V3D_viewport -> camera in
        let SO3ObjectGetGlobalPosition camera -> cpos  in
        let SO3ObjectGetGlobalOrientation camera -> cquat in
        let SO3MathsQuatGetDirection cquat [x (-.y) z] -> dpos in
        (
          if (!objpstr.ARM_iFatherMode) then
          (
            SO3ObjectSetGlobalPosition objpstr.ARM_father (addVectorF dpos cpos);
            SO3ObjectSetGlobalOrientation objpstr.ARM_father SO3MathsQuatAdd cquat quat;
          )
          else nil;
          
          let SO3MathsQuatToEulerDegreePYR quat -> [lpitch lyaw lroll] in
          (
            if (objpstr.ARM_lastpos == nil) then nil else
            let objpstr.ARM_lastpos -> [opos orot] in
            let subVectorF pos opos -> [nx ny nz] in
            let subVectorF [lpitch lyaw lroll] orot -> [npitch nyaw nroll] in
            let if npitch >. 180.0 then npitch -. 360.0 else if npitch <. (-.180.0) then npitch +. 360.0 else npitch -> npitch in
            let if nyaw >. 180.0 then nyaw -. 360.0 else if nyaw <. (-.180.0) then nyaw +. 360.0 else nyaw -> nyaw in
            let if nroll >. 180.0 then nroll -. 360.0 else if nroll <. (-.180.0) then nroll +. 360.0 else nroll -> nroll in
            (
              if (((absf nx) <=. objpstr.ARM_fsize /. 20.0) && ((absf ny) <=. objpstr.ARM_fsize /. 20.0) && ((absf nz) <=. objpstr.ARM_fsize /. 20.0)) then nil else
              (
                SendPluginEvent inst "Moved" (strcatnSep (ftoa nx)::(ftoa ny)::(ftoa nz)::nil " ") nil;
              );
              
              if ((absf npitch) <=. 0.01 && (absf nyaw) <=. 0.01 && (absf nroll) <=. 0.01) then nil else
              (
                SendPluginEvent inst "Rotated" (strcatnSep (ftoa npitch)::(ftoa nyaw)::(ftoa nroll)::nil " ") nil;
              );
              
              let ftoa (-.lpitch) -> sax in
              let ftoa nyaw -> say in
              let ftoa (-.lroll) -> saz in
                SendPluginEvent inst "Control" (strcatn (ftoa (-.nx))::" "::(ftoa (-.ny))::" "::(ftoa (-.nz))::"\n"::sax::" "::say::" "::saz::"\n2"::nil) inst.INST_sName;
            );  
            set objpstr.ARM_lastpos = [pos [lpitch lyaw lroll]];
          );
          
          let (addVectorF dpos cpos) -> [px py pz] in
          let SO3MathsQuatToEulerXYZ SO3MathsQuatAdd cquat quat -> [ax ay az] in
          let SO3MathsQuatGetDegreeYaw quat 0 -> yaw in
          let SO3MathsQuatGetDegreePitch quat 0 -> pitch in
          let SO3MathsQuatGetDegreeRoll quat 0 -> roll in
          (
            SendPluginEvent inst "Data changed" strcatn (strcatnSep (ftoa px)::(ftoa py)::(ftoa pz)::nil " ")::"\n"::(strcatnSep (ftoa (SO3MathsRadianToDegree ay))::(ftoa (SO3MathsRadianToDegree ax))::(ftoa (SO3MathsRadianToDegree az))::nil " ")::nil nil;
            SendPluginEvent inst "Position" strcatnSep (ftoa px)::(ftoa py)::(ftoa pz)::nil " " nil;
            SendPluginEvent inst "Pixel position" strcatnSep (itoa pixx)::(itoa pixy)::(itoa pixz)::nil " " nil;
            SendPluginEvent inst "Orientation" strcatnSep (ftoa (SO3MathsRadianToDegree ay))::(ftoa (SO3MathsRadianToDegree ax))::(ftoa (SO3MathsRadianToDegree az))::nil " " nil;
            SendPluginEvent inst "Yaw" ftoa yaw nil;
            SendPluginEvent inst "Pitch" ftoa pitch nil;
            SendPluginEvent inst "Roll" ftoa roll nil;
          );
        );
      );
    );
  
    if (!objpstr.ARM_bState) then nil else
    ( 
      if (visible == 1) then
      (
        if (objpstr.ARM_bVisible != 0) then nil else
        (
          set objpstr.ARM_bVisible = 1;
          
          if (objpstr.ARM_iBounceTimeCount != nil) then
          (
            set objpstr.ARM_iBounceTimeCount = nil;
            0;
          )
          else
          (
            SendPluginEvent inst "Found" nil nil;
            0;
          );
        );
      )
      else
      ( 
        if (objpstr.ARM_bVisible == 0) then nil else
        (
          set objpstr.ARM_iBounceTimeCount = if (objpstr.ARM_iBounceTimeCount != nil) then objpstr.ARM_iBounceTimeCount + (etime / 1000) else 0;
          
          if (objpstr.ARM_iBounceTimeCount < iBounceTime) then nil else
          (
            set objpstr.ARM_iBounceTimeCount = nil;
            set objpstr.ARM_bVisible = 0;
            set objpstr.ARM_lastpos = nil;
            SendPluginEvent inst "Lost" nil nil;
          );
          0;
        );
      );
    );
  );
  0;;


fun cbChangeCamera(inst, viewstr, sessionstr, camera, objpstr)=
  let V3DgetCameraByType sessionstr camera objpstr.ARM_iFatherMode -> nfather in
  let SO3ObjectGetPosition nfather -> cpos in
  let SO3ObjectGetOrientation nfather -> cquat in
  (
    let objpstr.ARM_initpos -> [pos quat] in
    (
      SO3ObjectSetPosition objpstr.ARM_father pos;
      SO3ObjectSetOrientation objpstr.ARM_father quat;
    );
   
    set objpstr.ARM_father = nfather; 
    set objpstr.ARM_initpos = [cpos cquat];
  );
  0;;


fun cbEnableMarker(inst, from, action, param, reply, objpstr)=
  set objpstr.ARM_bState = 1;
  0;;


fun cbDisableMarker(inst, from, action, param, reply, objpstr)=
  set objpstr.ARM_bState = 0;
  set objpstr.ARM_bVisible = 0;
  set objpstr.ARM_iBounceTimeCount = nil;
  0;;


fun cbEnableMarkerTracking(inst, from, action, param, reply, objpstr)=
  set objpstr.ARM_bTrackState = 1;
  0;;


fun cbDisableMarkerTracking(inst, from, action, param, reply, objpstr)=
  set objpstr.ARM_bTrackState = 0;
  0;;


fun cbSetMarkerSize(inst, from, action, param, reply, objpstr)=
  if ((atof param) == nil) then nil else
  let atof param -> fparam in
  (
    _SETarMarkerSize objpstr.ARM_marker fparam;
    set objpstr.ARM_fsize = fparam;
  );
  0;;


fun cbRegisterFrame(inst, from, action, param, reply, objpstr)=
  let c3dxCameraSize -> [cx cy] in
  let strextr param -> lp in
  let (nth_list (hd lp) 0) -> sx in
  let if sx == nil then 0 else atoi sx -> x in
  let (nth_list (hd lp) 1) -> sy in
  let if sy == nil then 0 else atoi sy -> y in
  let (nth_list (hd lp) 2) -> sw in
  let if sw == nil then cx else atoi sw -> w in
  let (nth_list (hd lp) 3) -> sh in
  let if sh == nil then cy else atoi sh -> h in
	  _SETarMarkerFromPictureZone objpstr.ARM_marker x y w h;
	0;;


fun newOb(inst)=
  let (getPluginInstanceParam inst "object") -> objname in
  let atoi (getPluginInstanceParam inst "enable") -> enable in
  let if enable == nil then 1 else enable -> enable in
  
  let atof (getPluginInstanceParam inst "msize") -> msize in
  let if (msize == nil) then 0.08 else msize -> msize in
  
  let atoi (getPluginInstanceParam inst "track") -> track in
  let if track == nil then 1 else track -> track in
  
  let V3DgetObjectByName c3dXsession objname -> father in
  let V3DgetObjectTypeByName objname -> iobjmode in
	
  let SO3ObjectGetPosition father -> cpos in
  let SO3ObjectGetOrientation father -> cquat in
  let _CRarMarkerFromFace _channel msize -> marker in
  //_CRarMarkerFromObject _channel (_checkpack strcat (getPluginDirectory (getInstancePlugin inst)) "/lbpcascade_frontalface.xml") msize maxfeatures
  let mkArMarkerStr [inst father marker iobjmode [cpos cquat] nil msize 0 enable track nil] -> objpstr in
  (
    if !iobjmode then nil else
      setPluginInstanceCbCameraChange inst mkfun5 @cbChangeCamera objpstr;
    
    PluginRegisterAction inst "Set scale" mkfun6 @cbSetMarkerSize objpstr;
    PluginRegisterAction inst "Enable" mkfun6 @cbEnableMarker objpstr;
    PluginRegisterAction inst "Disable" mkfun6 @cbDisableMarker objpstr;
    PluginRegisterAction inst "Enable tracking" mkfun6 @cbEnableMarkerTracking objpstr;
    PluginRegisterAction inst "Disable tracking" mkfun6 @cbDisableMarkerTracking objpstr;
    PluginRegisterAction inst "Register current frame" mkfun6 @cbRegisterFrame objpstr;
    
    setPluginInstanceCbScenePreRender inst mkfun4 @cbControlPreRender objpstr;
    setPluginInstanceCbDel inst mkfun2 @deleteOb objpstr;
  );
  0;;


fun IniPlug(file)=
  PlugRegister @newOb nil;
  setPluginEditor @dynamicedit;
  0;;