/*
-----------------------------------------------------------------------------
This source file is part of OpenSpace3D
For the latest info, see https://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
https://www.gnu.org/copyleft/lesser.txt

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

/* Authors: Bastien Bourineau */
/* Last update: 02/19/2024 */
/* Version: 1.0 */

struct PNavigationM = [
                    OBNM_inst       : PInstance,
                    OBNM_h3d        : SO3_OBJECT,
                    OBNM_iSrcMode   : I,
                    OBNM_bSons      : I
                    
                  ] mkPNavigationM;;



fun getConnList(l, radius, ln)=
  if (l == nil) then ln else
  (
    let hd l -> obj in
    let SO3ObjectGetName obj -> name in
    if (!strfindi "connection" name 0) then nil else
    let if (!strfindi "oneway" name 0) then 1 else 0 -> bdir in
      set ln = [(SO3ObjectGetGlobalPosition obj) (SO3ObjectGetGlobalPosition hd (SO3ObjectGetChildren obj)) radius bdir]::ln;
    
    getConnList (tl l) radius ln
  );;
 

fun setObjectNavigationState(obstr, l, flag, sons)=
  if l == nil then nil else
  let hd l -> obj in
  (
    let SO3SceneNodeGetBody obj -> cbody in
    let SO3BodyGetShapeDefinition cbody -> [btype _ _ _ _ _] in
    let SO3BodyGetMassMatrix cbody -> [cmass _] in
    let if (flag == nil) then nil else if (btype == SO3_TYPE_BODY_TREE) then SO3_NAVIGATION_MESH else if (cmass == 0.0) then SO3_NAVIGATION_CONVEX_OBSTACLE else flag -> nflag in
    (
      SO3ObjectSetNavigationType obj nflag;
      //addLogMessage strcatn "Set obj: "::(SO3ObjectGetName obj)::" to "::(itoa nflag)::nil;
    );
    
    if (!sons) then nil else
      setObjectNavigationState obstr (SO3ObjectGetChildren obj) (if (flag != nil) then SO3_NAVIGATION_BOX_OBSTACLE else nil) sons;
    
    setObjectNavigationState obstr (tl l) flag sons;
  );
  0;;


fun deleteOb(inst, obstr)=
  setObjectNavigationState obstr obstr.OBNM_h3d::nil nil obstr.OBNM_bSons;
  0;;


fun cbGetPosRadius(inst, from, action, param, reply, obstr)=
  let strextr param -> lp in
  if (lp == nil) then nil else
  let [(atof (nth_list (hd lp) 0)) (atof (nth_list (hd lp) 1)) (atof (nth_list (hd lp) 2)) (atof (nth_list (hd lp) 3))] -> [x y z r] in
  (
    let if (r == nil) || (r == 0.0) then
          SO3SceneGetNavigationPosition (V3DgetSession c3dXsession) [x y z]
        else
          SO3SceneGetNavigationRandomPositionInCircle (V3DgetSession c3dXsession) [x y z] r
    -> [nx ny nz] in
    SendPluginEvent inst "Position in radius" strcatnSep (ftoa nx)::(ftoa ny)::(ftoa nz)::nil " " nil;
  );
  0;;


fun cbGetRandomPos(inst, from, action, param, reply, obstr)=
  let SO3SceneGetNavigationRandomPosition (V3DgetSession c3dXsession) -> [nx ny nz] in
    SendPluginEvent inst "Random position" strcatnSep (ftoa nx)::(ftoa ny)::(ftoa nz)::nil " " nil;
  0;;


fun newOb(inst)=
  let (getPluginInstanceParam inst "sourceobject") -> sourcename in
  
  let atoi (getPluginInstanceParam inst "sons") -> sons in
  let if (sons == nil) then 0 else sons -> sons in
  
  let atof (getPluginInstanceParam inst "aradius") -> aradius in
  let if (aradius == nil) then 0.25 else aradius -> aradius in
  let atof (getPluginInstanceParam inst "aheight") -> aheight in
  let if (aheight == nil) then 1.75 else aheight -> aheight in
  let atof (getPluginInstanceParam inst "maxslope") -> maxslope in
  let if (maxslope == nil) then 30.0 else maxslope -> maxslope in
  let atof (getPluginInstanceParam inst "maxclimb") -> maxclimb in
  let if (maxclimb == nil) then 0.25 else maxclimb -> maxclimb in
  let atof (getPluginInstanceParam inst "cellsize") -> cellsize in
  let if (cellsize == nil) then 0.1 else cellsize -> cellsize in
  let atof (getPluginInstanceParam inst "cellheight") -> cellheight in
  let if (cellheight == nil) then 0.1 else cellheight -> cellheight in
  let atoi (getPluginInstanceParam inst "tilesize") -> tilesize in
  let if (tilesize == nil) then 16 else tilesize -> tilesize in
  /*
  let atof (getPluginInstanceParam inst "minregion") -> minregion in
  let if (minregion == nil) then 20.0 else minregion -> minregion in
  let atof (getPluginInstanceParam inst "mergeregion") -> mergeregion in
  let if (mergeregion == nil) then 15.0 else mergeregion -> mergeregion in
  */
  
  let V3DgetObjectByName c3dXsession sourcename -> source in
  let V3DgetObjectTypeByName sourcename -> isourcemode in

  let mkPNavigationM [inst source isourcemode sons] -> obstr in
  (
    SO3SceneSetNavigationMeshParams (V3DgetSession c3dXsession) aradius aheight maxslope maxclimb cellsize cellheight tilesize nil nil;
    
    let getConnList SO3ObjectGetChildren source aradius nil -> lc in
      SO3SceneSetNavigationMeshConnections (V3DgetSession c3dXsession) lc;
    
    setObjectNavigationState obstr source::nil SO3_NAVIGATION_MESH sons;    
    SO3SceneUpdateNavigationMesh (V3DgetSession c3dXsession);
    
    PluginRegisterAction inst "Get random position" mkfun6 @cbGetRandomPos obstr;
    PluginRegisterAction inst "Get position in radius" mkfun6 @cbGetPosRadius obstr;
    
    setPluginInstanceCbDel inst mkfun2 @deleteOb obstr;
    SendPluginEvent inst "Loaded" nil nil;
  );
  0;;


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