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

Copyright (c) 2020 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 ArSlamStr = [
  ARS_inst                 : PInstance,
  ARS_father               : SO3_OBJECT,
  ARS_refObj               : SO3_OBJECT,
  ARS_iFatherMode          : I,
  ARS_iRefSpace            : I,
  ARS_initpos              : [[F F F] [F F F F]],
  ARS_lastpos              : [[F F F] [F F F]],
  ARS_fsize                : F,
  ARS_iNbFeatures          : I,
  ARS_bVisible             : I,
  ARS_bState               : I,
  ARS_bTrackState          : I,
  ARS_bInvertAxis          : I,
  ARS_iBounceTimeCount     : I,  
  ARS_bETranslation        : I,
  ARS_bEMoved              : I,
  ARS_bERotated            : I,
  ARS_bEData               : I,
  ARS_bEPos                : I,
  ARS_bEOrient             : I,
  ARS_bEYaw                : I,
  ARS_bEPitch              : I,
  ARS_bERoll               : I

  ] mkArSlamStr;;
  

fun deleteOb(inst, objpstr)=
  _SetSlamDetectorState 0;
  
  setPluginInstanceCbScenePreRenderPhysic inst nil;
  setPluginInstanceCbCameraChange inst nil;
  
  let objpstr.ARS_initpos -> [pos quat] in
  (
    SO3ObjectSetPosition objpstr.ARS_father pos;
    SO3ObjectSetOrientation objpstr.ARS_father quat;
  );
  0;;


fun cbControlPreRender(inst, sessionstr, etime, objpstr)=  
  //_SetSlamDetectorImu [0.0 0.0 0.0] [0.0 0.0 0.0];

  //update slam IMU
  //if (!(_IsSensorAvailable SENSOR_TYPE_ACCELEROMETER) || !(_IsSensorAvailable SENSOR_TYPE_GYROSCOPE)) then nil else
  //(
  //  let _GetVectorData SENSOR_TYPE_GYROSCOPE -> gyro in
  //  let _GetVectorData SENSOR_TYPE_ACCELEROMETER -> accel in
  //    _SetSlamDetectorImu gyro accel (itof etime) /. 1000000.0;
  //);
  
  let if etime < 1000 then 1000 else etime -> etime in
  let _IsSlamFound -> visible in
  (
    if ((!objpstr.ARS_bState) || !visible || !objpstr.ARS_bTrackState) then nil else
    (  
      let multiplyVectorF [objpstr.ARS_fsize objpstr.ARS_fsize objpstr.ARS_fsize] _GetSlamCameraPosition -> pos in
      let _GetSlamCameraOrientation -> quat in
      (
        //force world pos if this is a camera
        if ((objpstr.ARS_iRefSpace != 2) || ((!objpstr.ARS_iFatherMode) && ((SO3ObjectGetType objpstr.ARS_father) != SO3_TYPE_CAMERA) && ((SO3ObjectGetType (hd SO3ObjectGetChildren objpstr.ARS_father)) != SO3_TYPE_CAMERA))) then nil else
            set objpstr.ARS_iRefSpace = 1;
        
        let pos -> [x y z] in
        let V3DgetSessionView sessionstr -> viewstr in
        let V3DgetDefaultViewport viewstr -> viewportstr in
        let V3DgetViewportSize viewstr viewportstr -> [_ _ w h] in
        let c3dxCameraSize -> [cw ch] in
        let if (objpstr.ARS_iRefSpace == 1) then SO3ObjectGetParent objpstr.ARS_father else if (objpstr.ARS_iRefSpace == 2) then SO3ViewportGetCamera viewportstr.V3D_viewport else if (objpstr.ARS_iRefSpace == 3) then objpstr.ARS_refObj else nil -> refobj in
        let if (refobj == nil) then [0.0 0.0 0.0] else SO3ObjectGetGlobalPosition refobj -> cpos  in
        let if (refobj == nil) then [0.0 0.0 0.0 1.0] else SO3ObjectGetGlobalOrientation refobj -> cquat in
        let SO3MathsQuatGetDirection cquat [x (-.y) z] -> dpos in
        let if objpstr.ARS_bInvertAxis then SO3MathsQuatAdd quat SO3MathsQuatAdd [0.0 0.5 0.0 0.5] [(-.0.5) 0.0 0.0 0.5] else quat -> nquat in
        (
          SO3ObjectSetGlobalPosition objpstr.ARS_father pos; //(addVectorF dpos cpos);          
          SO3ObjectSetGlobalOrientation objpstr.ARS_father quat; //SO3MathsQuatAdd cquat nquat;
          
          let SO3MathsQuatToEulerDegreePYR nquat -> [lpitch lyaw lroll] in
          (
            if (objpstr.ARS_lastpos == nil) then nil else
            let objpstr.ARS_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 (!objpstr.ARS_bETranslation) then nil else
                SendPluginEvent inst "Translation" (strcatnSep (ftoa nx)::(ftoa ny)::(ftoa nz)::nil " ") nil;

              if (!objpstr.ARS_bEMoved) then nil else
              if (((absf nx) <=. objpstr.ARS_fsize /. 20.0) && ((absf ny) <=. objpstr.ARS_fsize /. 20.0) && ((absf nz) <=. objpstr.ARS_fsize /. 20.0)) then nil else
              (
                SendPluginEvent inst "Moved" (strcatnSep (ftoa nx)::(ftoa ny)::(ftoa nz)::nil " ") nil;
              );
              
              if (!objpstr.ARS_bERotated) then nil else
              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;
              );
            );
            set objpstr.ARS_lastpos = [pos [lpitch lyaw lroll]];
          );
          
          let (addVectorF dpos cpos) -> [px py pz] in
          let SO3MathsQuatToEulerPYR SO3MathsQuatAdd cquat nquat -> [ax ay az] in
          let SO3MathsQuatGetDegreeYaw nquat 0 -> yaw in
          let SO3MathsQuatGetDegreePitch nquat 0 -> pitch in
          let SO3MathsQuatGetDegreeRoll nquat 0 -> roll in
          (
            if (!objpstr.ARS_bEData) then nil else
              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;
            
            if (!objpstr.ARS_bEPos) then nil else
              SendPluginEvent inst "Position" strcatnSep (ftoa px)::(ftoa py)::(ftoa pz)::nil " " nil;

            if (!objpstr.ARS_bEOrient) then nil else
              SendPluginEvent inst "Orientation" strcatnSep (ftoa (SO3MathsRadianToDegree ax))::(ftoa (SO3MathsRadianToDegree ay))::(ftoa (SO3MathsRadianToDegree az))::nil " " nil;
            
            if (!objpstr.ARS_bEYaw) then nil else
              SendPluginEvent inst "Yaw" ftoa yaw nil;
            
            if (!objpstr.ARS_bEPitch) then nil else
              SendPluginEvent inst "Pitch" ftoa pitch nil;
            
            if (!objpstr.ARS_bERoll) then nil else
              SendPluginEvent inst "Roll" ftoa roll nil;
          );
        );
      );
    );
        
    if (!objpstr.ARS_bState) then nil else
    ( 
      if (visible == 1) then
      (
        if (objpstr.ARS_bVisible != 0) then nil else
        (
          set objpstr.ARS_bVisible = 1;
          
          if (objpstr.ARS_iBounceTimeCount != nil) then
          (
            set objpstr.ARS_iBounceTimeCount = nil;
            0;
          )
          else
          (
            SendPluginEvent inst "Found" nil nil;
            0;
          );
        );
      )
      else
      ( 
        if (objpstr.ARS_bVisible == 0) then nil else
        (
          set objpstr.ARS_iBounceTimeCount = if (objpstr.ARS_iBounceTimeCount != nil) then objpstr.ARS_iBounceTimeCount + (etime / 1000) else 0;
          
          if (objpstr.ARS_iBounceTimeCount < iBounceTime) then nil else
          (
            set objpstr.ARS_iBounceTimeCount = nil;
            set objpstr.ARS_bVisible = 0;
            set objpstr.ARS_lastpos = nil;
            
            SendPluginEvent inst "Lost" nil nil;
          );
          0;
        );
      );
    );
  );
  0;;


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


fun setSlamParam(objpstr)=
  let (max 10 objpstr.ARS_iNbFeatures) -> mfeatures in
  let (max 5 ftoi ((itof objpstr.ARS_iNbFeatures) *. 0.76)) -> minfeatures in
  let if (objpstr.ARS_iNbFeatures > 1000) then 4 else 2 -> minframes in
  let minframes * 2 -> maxframes in    
    _SetSlamDetectorParams mfeatures minfeatures minframes maxframes;
  0;;


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


fun cbDisableMarker(inst, from, action, param, reply, objpstr)=
  set objpstr.ARS_bState = 0;
  set objpstr.ARS_bVisible = 0;
  set objpstr.ARS_iBounceTimeCount = nil;
  _SetSlamDetectorState 0;
  0;;


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


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


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


fun newOb(inst)=
  let (getPluginInstanceParam inst "object") -> objname in
  let atof (getPluginInstanceParam inst "msize") -> msize in
  let if (msize == nil) then 1.0 else msize -> msize in
  
  let atoi(getPluginInstanceParam inst "imode") -> imode in
  let if imode == nil then 2 else imode -> imode in
  let (getPluginInstanceParam inst "objRef") -> objref in
  let V3DgetObjectByName c3dXsession objref -> refobj in
  
  let atoi (getPluginInstanceParam inst "maxfeatures") -> maxfeatures in
  let if maxfeatures == nil then 650 else maxfeatures -> maxfeatures in
    
  let atoi (getPluginInstanceParam inst "enable") -> enable in
  let if enable == nil then 1 else enable -> enable in
  
  let atoi (getPluginInstanceParam inst "track") -> track in
  let if track == nil then 1 else track -> track in
  
  let atoi (getPluginInstanceParam inst "invertaxis") -> invertaxis in
  let if invertaxis == nil then 0 else invertaxis -> invertaxis in
  
  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 "Translation" -> btranslation in  
  let (IsInEditor inst) || IsEventLinked inst "Moved" -> bemoved in  
  let (IsInEditor inst) || IsEventLinked inst "Rotated" -> berot in
  let (IsInEditor inst) || IsEventLinked inst "Data changed" -> bedata in
  let (IsInEditor inst) || IsEventLinked inst "Position" -> bepos in
  let (IsInEditor inst) || IsEventLinked inst "Orientation" -> beorient in
  let (IsInEditor inst) || IsEventLinked inst "Yaw" -> beyaw in
  let (IsInEditor inst) || IsEventLinked inst "Pitch" -> bepitch in
  let (IsInEditor inst) || IsEventLinked inst "Roll" -> beroll in

  let mkArSlamStr [inst father refobj iobjmode imode [cpos cquat] nil msize maxfeatures 0 enable track invertaxis nil btranslation bemoved berot bedata bepos beorient beyaw bepitch beroll] -> objpstr in
  (
    if (!enable) then nil else
      cbEnableMarker inst nil nil nil nil objpstr;
    
    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;
    
    setPluginInstanceCbScenePreRenderPhysic inst mkfun4 @cbControlPreRender objpstr;
    setPluginInstanceCbDel inst mkfun2 @deleteOb objpstr;
  );
  0;;


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