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

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

struct BasicNavigation = [
  BNAV_shell                : SO3_OBJECT,
  BNAV_camera               : SO3_OBJECT,
  BNAV_neckShell            : SO3_OBJECT,
  BNAV_camShell             : SO3_OBJECT,  
  BNAV_body                 : SO3_PHYSICBODY,
  BNAV_pos                  : [[F F F] [F F F]],
  BNAV_move                 : [[F F F] [F F F]],
  BNAV_qLastOrientation     : [F F F F],
  BNAV_lControls            : [[S [[F F F] [F F F] F I I]] r1],
  BNAV_bHasMoved            : I,
  BNAV_fFov                 : F,
  BNAV_fSpeed               : F,
  BNAV_fMass                : F,
  BNAV_fJumpForce           : F,
  BNAV_bJumping             : I,
  BNAV_fCamHeight           : F,
  BNAV_fLastYPos            : F,
  BNAV_lPressedKeys         : [[I I] r1],
  BNAV_bDisableDefControl   : I,
  BNAV_bMouse               : I,
  BNAV_leftPoint            : [I I I],
  BNAV_rightPoint           : [I I I],
  BNAV_tCamLimit            : [F F],
  BNAV_bFlyMode             : I,
  BNAV_bState               : I,
  BNAV_iTick                : I

  ]mkBasicNavigation;;


fun computeControls(sessionstr, bnavstr, vnbsec, fspeed)=
  let [fspeed fspeed fspeed] -> vspeed in
  // analogic data
  let [0.0 0.0 0.0] -> avec in
  let [0.0 0.0 0.0] -> aang in
  // real data
  let [0.0 0.0 0.0] -> vec in
  let [0.0 0.0 0.0] -> ang in
  
  let 0.0 -> ycam in
  let 0 -> angOverride in
  let 0 -> addCamHeight in
  let 0 -> havedata in
  (
    let sizelist bnavstr.BNAV_lControls -> size in
    let 0 -> i in
    while i < size do
    (
      let nth_list bnavstr.BNAV_lControls i -> [_ tcontrol] in
      let tcontrol -> [nvec nang nycam mode newdata] in
      if (!newdata && mode != 0) then nil else
      (
        if ((mode == 1) || (mode == 4)) then // accumulator
        (
          set vec = addVectorF vec nvec;
          
          if (angOverride) then nil else
            set ang = addVectorF ang nang;
          
          set ycam = ycam +. nycam;
          if (mode != 4) then nil else // add cam height
            set addCamHeight = 1;
          
          //reset control
          mutate tcontrol <- [[0.0 0.0 0.0] [0.0 0.0 0.0] 0.0 mode 0];
          0;
        )
        else if (mode == 2) || (mode == 3) then // override angle
        (
          set angOverride = 1;
          set vec = addVectorF vec nvec;
          set ang = nang;
          
          if (mode == 2) then
            set ycam = ycam +. nycam
          else
          (
            set addCamHeight = 0;
            set ycam = nycam;
          );
          
          //reset control
          mutate tcontrol <- [[0.0 0.0 0.0] [0.0 0.0 0.0] 0.0 mode 0];
          0;
        )
        else
        (
          set avec = addVectorF avec nvec;
          set aang = addVectorF aang nang;
          mutate tcontrol <- [_ _ _ _ 0];
          0;
        );
        set havedata = 1;
      );
      
      set i = i + 1;
    );
    
    // compute speed and time
    let if bnavstr.BNAV_bDisableDefControl then [[0.0 0.0 0.0] [0.0 0.0 0.0]] else bnavstr.BNAV_move -> [kvec kang] in
    (
      set avec = multiplyVectorF multiplyVectorF (addVectorF avec kvec) vnbsec [2.0 2.0 2.0]; //2 meter per second ?
      set aang = multiplyVectorF multiplyVectorF (addVectorF aang kang) vnbsec [90.0 90.0 90.0]; //45 degree per second ?
      set avec = multiplyVectorF avec vspeed;
      
      let aang -> [pitch yaw roll] in
        set aang = [(SO3MathsDegreeToRadian pitch) (SO3MathsDegreeToRadian yaw) (SO3MathsDegreeToRadian roll)];
    );
    
    let ang -> [ax ay az] in
      set ang = [(SO3MathsDegreeToRadian ax) (SO3MathsDegreeToRadian ay) (SO3MathsDegreeToRadian az)];

    let ang -> [ax ay az] in
    let aang -> [pitch yaw roll] in
    let if (angOverride) then [ax (ay +. yaw) az] else (addVectorF aang ang) -> fang in
      [(addVectorF avec vec) fang ycam angOverride addCamHeight havedata];
  );;


fun pitchCamera(bnavstr, ang, absolute)=
  let SO3MathsQuatGetPitch SO3ObjectGetOrientation bnavstr.BNAV_camShell 0 -> ax in
  let bnavstr.BNAV_tCamLimit -> [lx lx2] in
  let if (absolute) then ang else (ax +. ang) -> tang in
  (
    if (tang <. lx) then
    (
      V3DsetObjectOrientationDeg bnavstr.BNAV_camShell [0.0 (SO3MathsRadianToDegree lx) 0.0];
      0;
    )
    else if (tang >. lx2) then
    (
      V3DsetObjectOrientationDeg bnavstr.BNAV_camShell [0.0 (SO3MathsRadianToDegree lx2) 0.0];
      0;
    )
    else
    (
      if (absolute) then
      (
        V3DsetObjectOrientationDeg bnavstr.BNAV_camShell [0.0 (SO3MathsRadianToDegree ang) 0.0];
        0;
      )
      else
      (
        SO3ObjectRotatePitch bnavstr.BNAV_camShell ang SO3_LOCAL_TS;
        0;
      );
    );
  );
  0;;


fun rollCamera(bnavstr, ang, absolute)=
  if (absolute) then
  (
    V3DsetObjectOrientationDeg bnavstr.BNAV_camera [0.0 0.0 (SO3MathsRadianToDegree ang)];
    0;
  )
  else
  (
    SO3ObjectRotateRoll bnavstr.BNAV_camera ang SO3_LOCAL_TS;
    0;
  );
  0;;


// ignore avatar bodies and disabled bodies
fun getValidRay(lray)=
  let nil -> body in
  let 0.0 -> dist in
  let [0.0 1.0 0.0] -> normal in
  (
    while ((lray != nil) && (body == nil)) do
    (
      let hd lray -> [tbody tdist tnormal] in
      (
        if (((SO3BodyGetType tbody) & 1) || (SO3BodyGetIgnoreCollision tbody) || (SO3BodyGetFluid tbody)) then nil else
        (
          set body = tbody;
          set dist = tdist;
          set normal = tnormal;
        );
      );
      set lray = tl lray;
    );    
    [body dist normal]
  );;


fun cbNavKeyDown(inst, viewstr, key, ascode, bnavstr)=
  if VUIisFocusedInput then nil else
  (
    let _keybdstate -> ctrlstate in
    let bnavstr.BNAV_move -> [vec ang] in
    if (switch bnavstr.BNAV_lPressedKeys key) != nil then nil else
    (
      let if (key == (KEYBgetKeyNumber "UPARROW")) then // UP
            -.1.0
          else if (key == (KEYBgetKeyNumber "DOWNARROW")) then // DOWN
            1.0
          else
            nil
      -> z in
      let if (key == (KEYBgetKeyNumber "LEFTARROW")) then // LEFT
            1.0
          else if (key == (KEYBgetKeyNumber "RIGHTARROW")) then // RIGHT
            -.1.0
          else
            nil
      -> yaw in
      (
        if z == nil then nil else
        if ctrlstate & 2 then
          mutate vec <- [_ (-.z) _]
        else if ctrlstate & 1 then
          mutate ang <- [(-.z) _ _]
        else
          mutate vec <- [_ _ z];
        
        if yaw == nil then nil else
        if ctrlstate & 2 then
          mutate vec <- [-.yaw _ _]
        else
          mutate ang <- [_ yaw _];
      );
      
      set bnavstr.BNAV_lPressedKeys = [key (if ctrlstate == nil then 0 else ctrlstate)]::bnavstr.BNAV_lPressedKeys;
    );
  
    if ((key == (KEYBgetKeyNumber "SPACE")) && (!bnavstr.BNAV_bJumping)) then // SPACE
    (
      let SO3ObjectGetGlobalPosition bnavstr.BNAV_shell -> [gx gy gz] in
      let getValidRay SO3PhysicsRayCastExt (V3DgetSession c3dXsession) [gx (gy +. 0.1) gz] [0.0 (-.1.0) 0.0] 0.15 -> [body dist normal] in
      if (body == nil) then nil else
      (
        set bnavstr.BNAV_bJumping = 1;
        SendPluginEvent inst "Jumped" nil nil;
        SO3BodyAddImpulse bnavstr.BNAV_body [0.0 bnavstr.BNAV_fJumpForce 0.0] [gx gy gz];
      );
    )
    else nil;
  );
  0;;


fun cbNavKeyUp(inst, viewstr, key, bnavstr)=
  if VUIisFocusedInput then nil else
  (
    let bnavstr.BNAV_move -> [vec ang] in
    let switch bnavstr.BNAV_lPressedKeys key -> ctrlstate in
    (
      set bnavstr.BNAV_lPressedKeys = (remove_idx_from_list bnavstr.BNAV_lPressedKeys key);
      
      // UP & DOWN
      if ((key != (KEYBgetKeyNumber "UPARROW")) && (key != (KEYBgetKeyNumber "DOWNARROW"))) then nil else
      (
        let if (isT1InList bnavstr.BNAV_lPressedKeys (KEYBgetKeyNumber "UPARROW")) then // UP
              if ctrlstate & 1 || ctrlstate & 2 then 1.0 else -.1.0
            else if (isT1InList bnavstr.BNAV_lPressedKeys (KEYBgetKeyNumber "DOWNARROW")) then // DOWN
              if ctrlstate & 1 || ctrlstate & 2 then -.1.0 else 1.0
            else
              0.0
        -> nv in
        if ctrlstate & 1 then
          mutate ang <- [nv _ _]
        else if ctrlstate & 2 then
          mutate vec <- [_ nv _]
        else
          mutate vec <- [_ _ nv];
      );
      
      // UP & DOWN
      if ((key != (KEYBgetKeyNumber "LEFTARROW")) && (key != (KEYBgetKeyNumber "RIGHTARROW"))) then nil else
      (
        let if (isT1InList bnavstr.BNAV_lPressedKeys (KEYBgetKeyNumber "LEFTARROW")) then // LEFT
              if ctrlstate & 2 then -.1.0 else 1.0
            else if (isT1InList bnavstr.BNAV_lPressedKeys (KEYBgetKeyNumber "RIGHTARROW")) then // RIGHT
              if ctrlstate & 2 then 1.0 else -.1.0
            else
              0.0
        -> nv in
        if ctrlstate & 2 then
          mutate vec <- [nv _ _]
        else
          mutate ang <- [_ nv _];
      );
      
      if (key != (KEYBgetKeyNumber "SPACE")) then nil else // SPACE
        set bnavstr.BNAV_bJumping = 0;
    );
  );
  0;;


fun cbNavPreRender2(inst, sessionstr, etime, bnavstr)=
  //update Y rotation on camera independently of physics
  if (!V3DphysGetState c3dXsession) then nil else
  (
    let sessionstr.V3D_sessionView -> viewstr in
    let (V3DWidgetCtrlHasFocus viewstr) -> widgetctrl in  
    let if (widgetctrl != nil) && (SO3WidgetGetMouseEnable widgetctrl) && ((SO3WidgetGetMode widgetctrl) != 3) && (SO3WidgetIsMouseOver widgetctrl) && bnavstr.BNAV_bMouse then 1 else 0 -> disablenavig in 
    if (disablenavig) then nil else
      SO3ObjectSetGlobalOrientation bnavstr.BNAV_neckShell bnavstr.BNAV_qLastOrientation;
  );
  0;;


fun cbNavPreRender(inst, sessionstr, etime, bnavstr)=
  set bnavstr.BNAV_iTick = bnavstr.BNAV_iTick + etime;
  
  let (1000000 / SO3WorldGetFPS sessionstr.V3D_session) -> phystime in
  if ((V3DphysGetState c3dXsession) && (bnavstr.BNAV_iTick < phystime)) then nil else
  (
    let maxf 0.000001 (itof (bnavstr.BNAV_iTick / 1000)) -> ftime in //milliseconds
    let ftime /. 1000.0 -> nbsec in
    let [nbsec nbsec nbsec] -> vnbsec in
  
    let sessionstr.V3D_sessionView -> viewstr in
    let (V3DWidgetCtrlHasFocus viewstr) -> widgetctrl in  
    let if (widgetctrl != nil) && (SO3WidgetGetMouseEnable widgetctrl) && ((SO3WidgetGetMode widgetctrl) != 3) && (SO3WidgetIsMouseOver widgetctrl) && bnavstr.BNAV_bMouse then 1 else 0 -> disablenavig in 
    // manage the case that a widget get the focus
    /*if (disablenavig) then
    (
      set bnavstr.BNAV_lPressedKeys = nil;
      set bnavstr.BNAV_move = [[0.0 0.0 0.0] [0.0 0.0 0.0]];
      let SO3BodyGetVelocity bnavstr.BNAV_body -> [velx vely velz] in
        SO3BodySetVelocity bnavstr.BNAV_body [0.0 vely 0.0];
      SO3BodySetOmega bnavstr.BNAV_body [0.0 0.0 0.0];
      0;
    )
    else*/
    (
      let bnavstr.BNAV_fSpeed -> speed in
      let computeControls sessionstr bnavstr vnbsec speed -> [vec [pitch yaw roll] ycam angOverride addCamHeight havedata] in
      let [speed speed speed] -> vspeed in
      let bnavstr.BNAV_bHasMoved -> hasmoved in
      (
        // allow other objects to push the avatar, but this make the avatar slide on ground
        //set bnavstr.BNAV_bHasMoved = 0;
        
        if (V3DphysGetState c3dXsession) then
        (
          let if (bnavstr.BNAV_fMass >. 0.0) && (!bnavstr.BNAV_bFlyMode) then [1.0 0.0 1.0] else [1.0 1.0 1.0] -> vmul in
          let SO3ObjectGetDerivedDirectionAxis (if bnavstr.BNAV_bFlyMode then bnavstr.BNAV_camShell else bnavstr.BNAV_neckShell) multiplyVectorF vec vmul -> mvec in
          
          let divideVectorF mvec vnbsec -> mvel in
          let SO3BodyGetVelocity bnavstr.BNAV_body -> [velx vely velz] in
          let if (bnavstr.BNAV_fMass >. 0.0) && (!bnavstr.BNAV_bFlyMode) then vely else 0.0 -> vely in
          if (!hasmoved) && (zeroVectorF vec) then nil else
          (
            SO3BodySetVelocity bnavstr.BNAV_body addVectorF mvel [0.0 vely 0.0];
            if (zeroVectorF vec) then nil else
              set bnavstr.BNAV_bHasMoved = 1;
            
            //stairs
            let SO3ObjectGetGlobalPosition bnavstr.BNAV_shell -> gpos in
            let SO3ObjectGetGlobalOrientation (if bnavstr.BNAV_bFlyMode then bnavstr.BNAV_camShell else bnavstr.BNAV_neckShell) -> gquat in
            let SO3MathsQuatGetDirection gquat (addVectorF vec (multiplyVectorF (normalizeVectorF vec) [0.20 0.0 0.20])) -> mtvec in
            let addVectorF gpos mtvec -> [gx gy gz] in
            let getValidRay SO3PhysicsRayCastExt (V3DgetSession c3dXsession) [gx (gy +. 1.0) gz] [0.0 (-.1.0) 0.0] 1.0 -> [ntbody ntdist [_ nty _]] in
            let if ((nty != nil) && (nty <. 0.85)) then 0.0 else ntdist -> ntdist in
            let (1.0 -. ntdist) -> nydist in
            let if (nydist >. 0.01) && (nydist <=. 0.30) then nydist else 0.0 -> nydist in
            if (nydist == 0.0) then nil else
              SO3ObjectTranslate bnavstr.BNAV_shell [0.0 nydist 0.0] SO3_WORLD_TS;
          );
          
          if (!hasmoved) && (yaw == 0.0) then nil else
          (
            SO3BodySetOmega bnavstr.BNAV_body [0.0 0.0 0.0];        
            set bnavstr.BNAV_qLastOrientation = SO3MathsQuatAdd bnavstr.BNAV_qLastOrientation (SO3MathsEulerXYZToQuat [yaw 0.0 0.0]);
            
            //SO3ObjectSetGlobalOrientation bnavstr.BNAV_neckShell bnavstr.BNAV_qLastOrientation;
            SO3ObjectRotateYaw bnavstr.BNAV_neckShell yaw SO3_LOCAL_TS;
            //let yaw /. nbsec -> yaw in
              //SO3BodySetOmega bnavstr.BNAV_body [0.0 yaw 0.0];
            
            if (yaw == 0.0) then nil else
              set bnavstr.BNAV_bHasMoved = 1;
          );
          0;
        )
        else
        (
          if (bnavstr.BNAV_bFlyMode) then
            let SO3ObjectGetDerivedDirectionAxis bnavstr.BNAV_camShell vec -> mvec in
              SO3ObjectTranslate bnavstr.BNAV_shell mvec SO3_WORLD_TS
          else
            let SO3ObjectGetDerivedDirectionAxis bnavstr.BNAV_neckShell vec -> mvec in
              SO3ObjectTranslate bnavstr.BNAV_shell mvec SO3_WORLD_TS;
                  
          set bnavstr.BNAV_qLastOrientation = SO3MathsQuatAdd bnavstr.BNAV_qLastOrientation (SO3MathsEulerXYZToQuat [yaw 0.0 0.0]);
          SO3ObjectRotateYaw bnavstr.BNAV_neckShell yaw SO3_LOCAL_TS;
          0;
        );
  
        pitchCamera bnavstr pitch angOverride;
        rollCamera bnavstr roll angOverride;
    
        // move the camera shell on Y to simulate the head position
        if (!havedata) then nil else
        let if (!addCamHeight) then
            (
              if (ycam == 0.0) then
                bnavstr.BNAV_fCamHeight
              else
                ycam
            )
            else
            (
              if (bnavstr.BNAV_fLastYPos == 0.0) then
                bnavstr.BNAV_fCamHeight +. ycam
              else
                bnavstr.BNAV_fLastYPos +. ycam
            )
        -> ycam in
        (
          let SO3ObjectGetPosition bnavstr.BNAV_neckShell -> [_ oy _] in
            SO3ObjectTranslate bnavstr.BNAV_neckShell [0.0 (ycam -. oy) 0.0] SO3_WORLD_TS;
          
          set bnavstr.BNAV_fLastYPos = ycam;
        );
        
        if (disablenavig) || (viewstr.V3D_bVRpointer) then nil else
        (
          let V3DgetCursorTrans viewstr -> curstrans in
          // avoid object axis handler to move the camera
          let sessionstr.V3D_selectedAxis -> [axisstr _] in
          if !bnavstr.BNAV_bMouse || bnavstr.BNAV_bDisableDefControl || (curstrans == nil) || (viewstr.V3D_iClickStatus != V3DCLICK_LEFT) || ((axisstr != nil) && axisstr.V3D_iAxisMode) then nil else
          let curstrans -> [dx dy] in
          let V3DgetViewSize viewstr -> [iWinW iWinH] in
          let (1.0 /. ((itof iWinW) *. 0.5) *. (itof dx)) -> dxnorm in
          let (1.0 /. ((itof iWinH) *. 0.5) *. -.(itof dy)) -> dynorm in    
          let if dxnorm >. 1.0 then 1.0 else if dxnorm <. -.1.0 then -.1.0 else dxnorm -> dxone in
          let if dynorm >. 1.0 then 1.0 else if dynorm <. -.1.0 then -.1.0 else dynorm -> dyone in
          let dxone *. nbsec -> dxone in
          let dyone *. nbsec -> dyone in
          (
            if (V3DphysGetState c3dXsession) then
            (
              if ((!((V3DisMoveClicked viewstr) & V3DCLICK_SHIFT)) || angOverride) then nil else
                pitchCamera bnavstr (SO3MathsDegreeToRadian -.dyone *. 45.0) 0;
              
              if ((V3DisMoveClicked viewstr) & V3DCLICK_CTRL) then nil else
              (
                set bnavstr.BNAV_qLastOrientation = SO3MathsQuatAdd bnavstr.BNAV_qLastOrientation (SO3MathsEulerXYZToQuat [(-.(SO3MathsDegreeToRadian dxone *. 90.0)) 0.0 0.0]);
                SO3ObjectRotateYaw bnavstr.BNAV_neckShell (-.(SO3MathsDegreeToRadian dxone *. 90.0)) SO3_LOCAL_TS;
                //SO3BodySetOmega bnavstr.BNAV_body [0.0 (-.(SO3MathsDegreeToRadian (dxone *. 90.0 /. nbsec))) 0.0];
                set bnavstr.BNAV_bHasMoved = 1;
              );
      
              let if ((V3DisMoveClicked viewstr) & V3DCLICK_SHIFT) then
                    [0.0 0.0 0.0]
                  else if ((V3DisMoveClicked viewstr) & V3DCLICK_CTRL) then
                    [dxone (-.dyone) 0.0]
                  else
                    [0.0 0.0 dyone]
              -> mvec in
              let if ((bnavstr.BNAV_fMass >. 0.0) && (!bnavstr.BNAV_bFlyMode)) then [2.0 0.0 2.0] else [2.0 2.0 2.0] -> vmul in
              let SO3ObjectGetDerivedDirectionAxis (if bnavstr.BNAV_bFlyMode then bnavstr.BNAV_camShell else bnavstr.BNAV_neckShell) multiplyVectorF (multiplyVectorF mvec vspeed) vmul -> mvec in
              let SO3BodyGetVelocity bnavstr.BNAV_body -> [velx vely velz] in
              let if (bnavstr.BNAV_fMass >. 0.0) && (!bnavstr.BNAV_bFlyMode) then vely else 0.0 -> vely in
              (
                SO3BodySetVelocity bnavstr.BNAV_body addVectorF (divideVectorF mvec vnbsec) [0.0 vely 0.0];
                if (zeroVectorF mvec) then nil else
                (                                                  
                  set bnavstr.BNAV_bHasMoved = 1;
                  
                  //stairs
                  let SO3ObjectGetGlobalPosition bnavstr.BNAV_shell -> gpos in
                  let SO3ObjectGetGlobalOrientation (if bnavstr.BNAV_bFlyMode then bnavstr.BNAV_camShell else bnavstr.BNAV_neckShell) -> gquat in
                  let SO3MathsQuatGetDirection gquat (addVectorF mvec (multiplyVectorF (normalizeVectorF mvec) [0.20 0.0 0.20])) -> mtvec in
                  let addVectorF gpos mtvec -> [gx gy gz] in
                  let getValidRay SO3PhysicsRayCastExt (V3DgetSession c3dXsession) [gx (gy +. 1.0) gz] [0.0 (-.1.0) 0.0] 1.0 -> [ntbody ntdist [_ nty _]] in
                  let if ((nty != nil) && (nty <. 0.85)) then 0.0 else ntdist -> ntdist in
                  let (1.0 -. ntdist) -> nydist in
                  let if (nydist >. 0.01) && (nydist <=. 0.30) then nydist else 0.0 -> nydist in
                  if (nydist == 0.0) then nil else
                    SO3ObjectTranslate bnavstr.BNAV_shell [0.0 nydist 0.0] SO3_WORLD_TS;
                );
              );
              0;
            )
            else
            (
              if ((!((V3DisMoveClicked viewstr) & V3DCLICK_SHIFT)) || angOverride) then nil else
                pitchCamera bnavstr (SO3MathsDegreeToRadian -.dyone *. 45.0) 0;
              
              if ((V3DisMoveClicked viewstr) & V3DCLICK_CTRL) then nil else
              (
                set bnavstr.BNAV_qLastOrientation = SO3MathsQuatAdd bnavstr.BNAV_qLastOrientation (SO3MathsEulerXYZToQuat [(-.(SO3MathsDegreeToRadian dxone *. 90.0)) 0.0 0.0]);
                SO3ObjectRotateYaw bnavstr.BNAV_neckShell (-.(SO3MathsDegreeToRadian dxone *. 90.0)) SO3_LOCAL_TS;
    
                set bnavstr.BNAV_bHasMoved = 1;
              );
              
              let if ((V3DisMoveClicked viewstr) & V3DCLICK_SHIFT) then
                    [0.0 0.0 0.0]
                  else if ((V3DisMoveClicked viewstr) & V3DCLICK_CTRL) then
                    [dxone (-.dyone) 0.0]
                  else
                    [0.0 0.0 dyone]
              -> mvec in
              (
                if (bnavstr.BNAV_bFlyMode) then
                  let SO3ObjectGetDerivedDirectionAxis bnavstr.BNAV_camShell multiplyVectorF (multiplyVectorF mvec vspeed) [2.0 2.0 2.0] -> mvec in
                    SO3ObjectTranslate bnavstr.BNAV_shell mvec SO3_WORLD_TS
                else
                let SO3ObjectGetDerivedDirectionAxis bnavstr.BNAV_neckShell multiplyVectorF (multiplyVectorF mvec vspeed) [2.0 2.0 2.0] -> mvec in
                  SO3ObjectTranslate bnavstr.BNAV_shell mvec SO3_WORLD_TS;
                  
                if (zeroVectorF mvec) then nil else
                  set bnavstr.BNAV_bHasMoved = 1;
              );
            );
          );
        );
      );
    );
    set bnavstr.BNAV_iTick = 0;
  );
  0;;


fun deleteOb(inst, bnavstr)=
  let SO3ObjectGetChildren bnavstr.BNAV_camera -> lchild in
  let sizelist lchild -> size in
  let 0 -> i in
  while i < size do
  (
    let nth_list lchild i -> child in
      SO3ObjectUnlink child;
    
    set i = i + 1;
  );
  
  setPluginInstanceCbScenePreRenderPhysic inst nil;
  setPluginInstanceCbScenePreRender2 inst nil;
  
  if (bnavstr.BNAV_bDisableDefControl) then nil else
  (
    setPluginInstanceCbKeyDown inst nil;
    setPluginInstanceCbKeyUp inst nil;
    setPluginInstanceCbTouchPointAdd inst nil;
    setPluginInstanceCbTouchPointRemove inst nil;
    setPluginInstanceCbTouchPointUpdate inst nil;
  );
  
  V3DdelCamera c3dXsession bnavstr.BNAV_camera;
  SO3ObjectDestroy bnavstr.BNAV_shell;
  SO3ObjectDestroy bnavstr.BNAV_camShell;
  V3DenableNavigate c3dXsession 1;
  0;;


fun cbJump(inst, from, action, param, reply, bnavstr)=
  let SO3ObjectGetGlobalPosition bnavstr.BNAV_shell -> gvec in
  let getValidRay SO3PhysicsRayCastExt (V3DgetSession c3dXsession) gvec [0.0 (-.1.0) 0.0] bnavstr.BNAV_fCamHeight -> [body dist normal] in
  if (bnavstr.BNAV_bJumping || (body == nil) || (dist >. ((bnavstr.BNAV_fCamHeight *. 0.5) +. 0.05))) then nil else
  (
    SendPluginEvent inst "Jumped" nil nil;
    SO3BodyAddImpulse bnavstr.BNAV_body [0.0 bnavstr.BNAV_fJumpForce 0.0] gvec;
  );
  0;;


// multi touch
fun cbAddCursor(inst, viewstr, id, x, y, bnavstr)=  
  // right of the screen for rotations and left for translations
  let (V3DgetDefaultViewport viewstr) -> viewportstr in
  let SO3ViewportGetPixelPositionSize viewportstr.V3D_viewport -> [_ _ vw vh] in
  if (x >= (vw / 2)) then
    set bnavstr.BNAV_rightPoint = [id x y]
  else
    set bnavstr.BNAV_leftPoint = [id x y];
  
  set bnavstr.BNAV_bMouse = 0;
  0;;


fun cbUpdateCursor(inst, viewstr, id, x, y, vx, vy, bnavstr)=
  //do not use touch for navigation if a widget is used, it can be used as a control
  let c3dXsession -> sessionstr in
  let sessionstr.V3D_sessionView -> viewstr in
  let (V3DWidgetCtrlHasFocus viewstr) -> widgetctrl in  
  if (widgetctrl != nil) && (SO3WidgetGetMouseEnable widgetctrl) && ((SO3WidgetGetMode widgetctrl) != 3) && (SO3WidgetIsMouseOver widgetctrl) then nil else
  (
    let bnavstr.BNAV_leftPoint -> [lid _ _] in
    let bnavstr.BNAV_rightPoint -> [rid _ _] in
    let bnavstr.BNAV_move -> [vec ang] in
    let (V3DgetDefaultViewport viewstr) -> viewportstr in
    let SO3ViewportGetPixelPositionSize viewportstr.V3D_viewport -> [_ _ vw vh] in
  
    let (itof (vw / 4)) -> xscratio in
    let (itof vh / 4) -> yscratio in
    if (id == lid) then
    (
      let bnavstr.BNAV_leftPoint -> [_ ox oy] in
      let ((itof (ox - x)) /. xscratio) *. (-.1.0) -> nx in 
      let if nx >. 1.0 then 1.0 else if nx <. (-.1.0) then (-.1.0) else nx -> nx in
      let ((itof (oy - y)) /. yscratio) *. (-.1.0) -> ny in 
      let if ny >. 1.0 then 1.0 else if ny <. (-.1.0) then (-.1.0) else ny -> ny in
      let if ((absf nx) <. 0.05) then 0.0 else nx -> nx in
      let if ((absf ny) <. 0.05) then 0.0 else ny -> ny in
        mutate vec <- [nx _ ny];
    )
    else if (id == rid) then
    (
      let bnavstr.BNAV_rightPoint -> [_ ox oy] in
      let ((itof (ox - x)) /. xscratio) -> nx in
      let if nx >. 1.0 then 1.0 else if nx <. (-.1.0) then (-.1.0) else nx -> nx in
      let ((itof (oy - y)) /. yscratio) -> ny in
      let if ny >. 1.0 then 1.0 else if ny <. (-.1.0) then (-.1.0) else ny -> ny in
      let if ((absf nx) <. 0.05) then 0.0 else nx -> nx in
      let if ((absf ny) <. 0.05) then 0.0 else ny -> ny in
        mutate ang <- [ny nx _];
    )
    else nil;
  );
  0;;


fun cbRemoveCursor(inst, viewstr, id, bnavstr)=
  let bnavstr.BNAV_leftPoint -> [lid _ _] in
  let bnavstr.BNAV_rightPoint -> [rid _ _] in
  let bnavstr.BNAV_move -> [vec ang] in
  if (id == lid) then
  (
    mutate vec <- [0.0 _ 0.0];
    set bnavstr.BNAV_leftPoint = nil;
  )
  else if (id == rid) then
  (
    mutate ang <- [0.0 0.0 _];
    set bnavstr.BNAV_rightPoint = nil;
  )
  else nil;
  
  if (bnavstr.BNAV_leftPoint != nil || bnavstr.BNAV_rightPoint != nil) then nil else
  (
    set bnavstr.BNAV_bMouse = 1;
  );
  0;;


fun cbControl(inst, from, action, param, reply, bnavstr)=
  //get the input control from the link
  let if reply == nil then "control" else reply -> reply in
  let switchstr bnavstr.BNAV_lControls reply -> icontrol in
  (
    // if not already int the list we add it
    if (icontrol != nil) then nil else
    (
      set bnavstr.BNAV_lControls = [reply [[0.0 0.0 0.0] [0.0 0.0 0.0] 0.0 0 1]]::bnavstr.BNAV_lControls;
      set icontrol = switchstr bnavstr.BNAV_lControls reply;
    );
    
    let strextr param -> l in
    let hd l -> sv in
    let hd tl l -> sa in
    let hd (hd tl tl l) -> mode in
    let icontrol -> [ovec oang ycam _ _] in
    let ovec -> [ovx ovy ovz] in
    let oang -> [oax oay oaz] in
    let if mode == nil then 0 else atoi mode -> imode in
    let [(nth_list sv 0) (nth_list sv 1) (nth_list sv 2)] -> [svx svy svz] in
    let [(if (!strcmp "_" svx) || (svx == nil) then ovx else (atof svx)) (if (!strcmp "_" svy) || (svy == nil) then ovy else (atof svy)) (if (!strcmp "_" svz) || (svz == nil) then ovz else (atof svz))] -> [vx vy vz] in
    let [(nth_list sa 0) (nth_list sa 1) (nth_list sa 2)] -> [sax say saz] in
    let [(if (!strcmp "_" sax) || (sax == nil) then oax else (atof sax)) (if (!strcmp "_" say) || (say == nil) then oay else (atof say)) (if (!strcmp "_" saz) || (saz == nil) then oaz else (atof saz))] -> [ax ay az] in
    (
      if ((imode == 1) || (imode == 4)) then
        // accumulator
        mutate icontrol <- [(addVectorF ovec [vx 0.0 vz]) (addVectorF oang [ax ay az]) (ycam +. vy) imode 1]
      else if (imode == 2) || (imode == 3) then
        // override orientation
        mutate icontrol <- [(addVectorF ovec [vx 0.0 vz]) [ax ay az] (if (imode == 2) then (ycam +. vy) else vy) imode 1]
      else
        mutate icontrol <- [[vx vy vz] [ax ay az] 0.0 imode 1];
    );
  );
  0;;


fun cbSetPosition(inst, from, action, param, reply, bnavstr)=
  let strextr param -> l in
  let hd l -> sv in
  let hd tl l -> sa in
  let [atof(nth_list sv 0) atof(nth_list sv 1) atof(nth_list sv 2)] -> vec in
  let [atof(nth_list sa 0) atof(nth_list sa 1) atof(nth_list sa 2)] -> ang in
  (
    SO3ObjectSetPosition bnavstr.BNAV_shell vec;
    let ang -> [ax ay az] in
    let if ax == nil then 0.0 else ax -> ax in
    let if ay == nil then 0.0 else ay -> ay in
    let if az == nil then 0.0 else az -> az in
    let SO3MathsEulerXYZToQuat [(SO3MathsDegreeToRadian ay) 0.0 0.0] -> quatinit in
    (
      set bnavstr.BNAV_qLastOrientation = quatinit;
      SO3ObjectSetGlobalOrientation bnavstr.BNAV_neckShell quatinit;
      SO3ObjectSetOrientation bnavstr.BNAV_camShell (SO3MathsEulerXYZToQuat [0.0 (SO3MathsDegreeToRadian ax) (SO3MathsDegreeToRadian az)]);
      SO3BodySetInitialState bnavstr.BNAV_body;
      V3DcameraTeleport (V3DgetSessionView c3dXsession) bnavstr.BNAV_camera;
    );
  );
  0;;


fun cbGetPosition(inst, from, action, param, reply, bnavstr)=
  let SO3ObjectGetPosition bnavstr.BNAV_shell -> [px py pz] in
  let if (V3DphysGetState c3dXsession) then py -. (bnavstr.BNAV_fCamHeight *. 0.5) else py -> py in
  let SO3MathsQuatToEulerXYZ (SO3ObjectGetGlobalOrientation bnavstr.BNAV_neckShell) -> [ay _ _] in
  let SO3MathsQuatToEulerXYZ (SO3ObjectGetOrientation bnavstr.BNAV_camShell) -> [_ ax az] in
  (
    SendPluginEvent inst "Position" strcatn (strcatnSep (ftoa px)::(ftoa py)::(ftoa pz)::nil " ")::"\n"::(strcatnSep (ftoa (SO3MathsRadianToDegree ax))::(ftoa (SO3MathsRadianToDegree ay))::(ftoa (SO3MathsRadianToDegree az))::nil " ")::nil nil;
  );
  0;;


fun cbSetAngleLimit(inst, from, action, param, reply, bnavstr)=
  if param == nil then nil else
  let bnavstr.BNAV_tCamLimit -> [_ otlimit] in
  let strextr param -> l in
  let SO3MathsDegreeToRadian (atof (nth_list (hd l) 0)) -> blimit in
  let SO3MathsDegreeToRadian (atof (nth_list (hd l) 1)) -> tlimit in
  if blimit == nil && tlimit == nil then nil else
  let if tlimit == nil then otlimit else tlimit -> tlimit in
  (
    set bnavstr.BNAV_tCamLimit = [blimit tlimit];
  );
  0;;


fun cbEnable(inst, from, action, param, reply, bnavstr)=
  let c3dXsession -> sessionstr in
  if (!(V3DgetNavigateControlState sessionstr)) then nil else
  (
    setPluginInstanceCbScenePreRenderPhysic inst mkfun4 @cbNavPreRender bnavstr;
    setPluginInstanceCbScenePreRender2 inst mkfun4 @cbNavPreRender2 bnavstr;
    
    set bnavstr.BNAV_lControls = nil;
    
    if (bnavstr.BNAV_bDisableDefControl) then nil else
    (
      setPluginInstanceCbKeyDown inst mkfun5 @cbNavKeyDown bnavstr;
      setPluginInstanceCbKeyUp inst mkfun4 @cbNavKeyUp bnavstr;
      setPluginInstanceCbTouchPointAdd inst mkfun6 @cbAddCursor bnavstr;
      setPluginInstanceCbTouchPointRemove inst mkfun4 @cbRemoveCursor bnavstr;
      setPluginInstanceCbTouchPointUpdate inst mkfun8 @cbUpdateCursor bnavstr;
    );
    
    SO3BodySetForce bnavstr.BNAV_body [0.0 0.0 0.0];
    SO3BodySetOmega bnavstr.BNAV_body [0.0 0.0 0.0];
    SO3BodySetVelocity bnavstr.BNAV_body [0.0 0.0 0.0];
    
    V3DphysEnableBody bnavstr.BNAV_body 1;
    
    SO3BodySetGravityEnable bnavstr.BNAV_body !bnavstr.BNAV_bFlyMode;
    SO3BodySetIgnoreCollision bnavstr.BNAV_body 0;
    
    set bnavstr.BNAV_iTick = 0;
  );
  set bnavstr.BNAV_bState = 1;
  0;;


fun cbDisable(inst, from, action, param, reply, bnavstr)=
  let c3dXsession -> sessionstr in
  (
    setPluginInstanceCbScenePreRenderPhysic inst nil;
    setPluginInstanceCbScenePreRender2 inst nil;
    
    if (bnavstr.BNAV_bDisableDefControl) then nil else
    (
      setPluginInstanceCbKeyDown inst nil;
      setPluginInstanceCbKeyUp inst nil;
      setPluginInstanceCbTouchPointAdd inst nil;
      setPluginInstanceCbTouchPointRemove inst nil;
      setPluginInstanceCbTouchPointUpdate inst nil;
    );
    
    set bnavstr.BNAV_lPressedKeys = nil;
    set bnavstr.BNAV_move = [[0.0 0.0 0.0] [0.0 0.0 0.0]];
    set bnavstr.BNAV_bState = 0;
    set bnavstr.BNAV_iTick = 0;
    
    SO3BodySetForce bnavstr.BNAV_body [0.0 0.0 0.0];
    SO3BodySetOmega bnavstr.BNAV_body [0.0 0.0 0.0];
    SO3BodySetVelocity bnavstr.BNAV_body [0.0 0.0 0.0];
    
    V3DphysEnableBody bnavstr.BNAV_body 0;
    
    SO3BodySetGravityEnable bnavstr.BNAV_body 0;
    SO3BodySetIgnoreCollision bnavstr.BNAV_body 1;
  );
  0;;


fun cbNavigationControlState(inst, sessionstr, state, bnavstr)=
  if (!state) then
  (
    let bnavstr.BNAV_bState -> prevstate in
    (
      cbDisable inst nil nil nil nil bnavstr;
      set bnavstr.BNAV_bState = prevstate;      
    );
  )
  else
  (
    if !bnavstr.BNAV_bState then nil else
      cbEnable inst nil nil nil nil bnavstr;
  );
  0;;


fun cbSetSpeed(inst, from, action, param, reply, bnavstr)=
  if (atof param) == nil then nil else
    set bnavstr.BNAV_fSpeed = atof param;
  0;;


fun cbActiveCamera(inst, from, action, param, reply, bnavstr)=
  V3DsetDefaultCamera c3dXsession bnavstr.BNAV_camera;
  V3DcameraTeleport (V3DgetSessionView c3dXsession) bnavstr.BNAV_camera;
  if (!bnavstr.BNAV_bState) then nil else
    cbEnable inst from action param reply bnavstr;
  0;;


fun cbDeactivateCamera(inst, from, action, param, reply, bnavstr)=
  V3DremoveDefaultCamera c3dXsession bnavstr.BNAV_camera;
  let bnavstr.BNAV_bState -> prevstate in
  (
    cbDisable inst from action param reply bnavstr;
    set bnavstr.BNAV_bState = prevstate;
  );
  0;;


fun cbCameraFocalLength(inst, from, action, param, reply, bnavstr)=
  if ((atof param) == nil) then nil else
    SO3CameraSetFocalLength bnavstr.BNAV_camera (atof param);    
  0;;


fun cbCameraFov(inst, from, action, param, reply, bnavstr)=
  if ((atof param) == nil) then nil else
  (
    SO3CameraSetFOVy bnavstr.BNAV_camera SO3MathsDegreeToRadian (atof param);
    set bnavstr.BNAV_fFov = (atof param);
  );
  0;;


fun cbCameraClipping(inst, from, action, param, reply, bnavstr)=
  if ((atof param) == nil) then nil else
  let hd strextr param -> l in
  let atof (hd l) -> near in
  let atof (hd tl l) -> far in
  (
    SO3CameraSetNearClipDistance bnavstr.BNAV_camera near;
    SO3CameraSetFarClipDistance bnavstr.BNAV_camera far;
  );
  0;;


fun cbEnableFlyMode(inst, from, action, param, reply, bnavstr)=
  set bnavstr.BNAV_bFlyMode = 1;
  SO3BodySetGravityEnable bnavstr.BNAV_body 0;
  0;;


fun cbDisableFlyMode(inst, from, action, param, reply, bnavstr)=
  set bnavstr.BNAV_bFlyMode = 0;
  SO3BodySetGravityEnable bnavstr.BNAV_body 1;
  0;;


fun cbSetCameraHeight(inst, from, action, param, reply, bnavstr)=
  let atof param -> cameraheight in
  if (cameraheight == nil) || (bnavstr.BNAV_fCamHeight == cameraheight) then nil else
  (
    SO3BodyDestroy bnavstr.BNAV_body;
    
    SO3ObjectSetPosition bnavstr.BNAV_neckShell [0.0 cameraheight 0.0];
    set bnavstr.BNAV_body = SO3BodyCreateCapsuleExt bnavstr.BNAV_shell 0.20 (cameraheight +. 0.25) [0.0 ((cameraheight +. 0.25) *. 0.5) 0.0] [0.0 0.0 0.0 1.0];
    
    V3DphysSetBodyMaterial c3dXsession bnavstr.BNAV_body "avatar";
    SO3BodySetLinearDamping bnavstr.BNAV_body 0.0;
    SO3BodySetAngularDamping bnavstr.BNAV_body [0.0 0.0 0.0];
    SO3BodyCreateBasicJointUpVector bnavstr.BNAV_body [0.0 1.0 0.0];
    SO3BodySetContiniousCollisionMode bnavstr.BNAV_body 1;
    SO3BodySetType bnavstr.BNAV_body 1;
    SO3BodySetGravityEnable bnavstr.BNAV_body !bnavstr.BNAV_bFlyMode;
    SO3BodySetInitialState bnavstr.BNAV_body;
    SO3BodySetMass bnavstr.BNAV_body if bnavstr.BNAV_fMass <=. 0.0 then 1.0 else bnavstr.BNAV_fMass;
    SO3BodySetAutoSleep bnavstr.BNAV_body 0;
    SO3BodySetFreeze bnavstr.BNAV_body 0;
    V3DphysEnableBody bnavstr.BNAV_body bnavstr.BNAV_bState;
    
    set bnavstr.BNAV_fCamHeight = cameraheight;
  );
  0;;


fun newOb(inst)=
  V3DenableNavigate c3dXsession 0;
  let atof (getPluginInstanceParam inst "speed") -> speed in
  let if speed == nil then 1.0 else speed -> speed in
  let atof (getPluginInstanceParam inst "mass") -> mass in
  let if mass == nil then 10.0 else mass -> mass in
  let atof (getPluginInstanceParam inst "jumpforce") -> jumpforce in
  let if jumpforce == nil then 5.0 else jumpforce -> jumpforce in
  let atof (getPluginInstanceParam inst "cameraheight") -> cameraheight in
  let if cameraheight == nil then 1.7 else cameraheight -> cameraheight in
  let atof (getPluginInstanceParam inst "posx") -> posx in
  let if posx == nil then 0.0 else posx -> posx in
  let atof (getPluginInstanceParam inst "posy") -> posy in
  let if posy == nil then 0.0 else posy -> posy in
  let atof (getPluginInstanceParam inst "posz") -> posz in
  let if posz == nil then 0.0 else posz -> posz in
  
  let atof (getPluginInstanceParam inst "angx") -> angx in
  let atof (getPluginInstanceParam inst "angy") -> angy in
  let atof (getPluginInstanceParam inst "angz") -> angz in
  
  let if angx == nil then 0.0 else angx -> angx in
  let if angy == nil then 0.0 else angy -> angy in
  let if angz == nil then 0.0 else angz -> angz in

  let atof (getPluginInstanceParam inst "maxlookup") -> maxlookup in
  let atof (getPluginInstanceParam inst "maxlookdown") -> maxlookdown in
  let if maxlookup == nil then SO3MathsDegreeToRadian 80.0 else SO3MathsDegreeToRadian maxlookup -> maxlookup in
  let if maxlookdown == nil then SO3MathsDegreeToRadian -.85.0 else SO3MathsDegreeToRadian maxlookdown -> maxlookdown in
  
  let atoi (getPluginInstanceParam inst "init") -> init in
  let if init == nil then 1 else init -> init in
  
  let atoi (getPluginInstanceParam inst "initcam") -> initcam in
  let if initcam == nil then 1 else initcam -> initcam in  
  
  let atoi (getPluginInstanceParam inst "flymode") -> flymode in
  let if flymode == nil then 0 else flymode -> flymode in
  
  let atoi (getPluginInstanceParam inst "disabledefcontrol") -> disabledefcontrol in
  let if disabledefcontrol == nil then 0 else disabledefcontrol -> disabledefcontrol in
  
  let atof (getPluginInstanceParam inst "fovy") -> fovy in
  let if fovy == nil then 45.0 else fovy -> fovy in
  let atof (getPluginInstanceParam inst "nclip") -> nclip in
  let if nclip == nil then 0.1 else nclip -> nclip in
  let atof (getPluginInstanceParam inst "fclip") -> fclip in
  let if fclip == nil then 10000.0 else fclip -> fclip in
  let atof (getPluginInstanceParam inst "focalelength") -> focalelength in
  let if focalelength == nil then 1.0 else focalelength -> focalelength in
  let (strreplace (getPluginInstanceName inst) " " "_") -> instname in
  let V3DaddShell c3dXsession strcat instname "_BasicNav_dummy" nil nil [posx posy posz] [0.0 0.0 0.0 1.0] -> shell in
  let SO3BodyCreateCapsuleExt shell 0.20 (cameraheight +. 0.25) [0.0 ((cameraheight +. 0.25) *. 0.5) 0.0] [0.0 0.0 0.0 1.0] -> body in
  let V3DaddShell c3dXsession strcat instname "_BasicNav_neck_dummy" nil nil [0.0 cameraheight 0.0] [0.0 0.0 0.0 1.0] -> neckshell in
  let V3DaddShell c3dXsession strcat instname "_BasicNav_cam_dummy" nil nil [0.0 0.0 0.0] [0.0 0.0 0.0 1.0] -> camshell in
  let V3DaddCamera c3dXsession strcat instname "_BasicNav_camera" -> cam in
  let SO3MathsEulerXYZToQuat [(SO3MathsDegreeToRadian angy) 0.0 0.0] -> initquat in
  let mkBasicNavigation [shell cam neckshell camshell body [[posx posy posz] [angx angy angz]] [[0.0 0.0 0.0] [0.0 0.0 0.0]] initquat nil 0 fovy speed mass jumpforce 0 cameraheight 0.0 nil disabledefcontrol !disabledefcontrol nil nil [maxlookdown maxlookup] flymode init 0] -> bnavstr in
  (
    V3DgetAvatarMaterial c3dXsession; // init
    V3DphysSetBodyMaterial c3dXsession body "avatar";
    SO3BodySetLinearDamping body 0.0;
    SO3BodySetAngularDamping body [0.0 0.0 0.0];
    SO3BodyCreateBasicJointUpVector body [0.0 1.0 0.0];
    SO3BodySetContiniousCollisionMode body 1;
    SO3BodySetType body 1;
    
    SO3ObjectLink neckshell shell;
    SO3ObjectLink camshell neckshell;
    SO3ObjectLink cam camshell;
    V3DsetCamera cam focalelength (SO3MathsDegreeToRadian fovy) nclip fclip;
    
    SO3ObjectSetGlobalOrientation bnavstr.BNAV_neckShell initquat;
    SO3ObjectSetOrientation bnavstr.BNAV_camShell (SO3MathsEulerXYZToQuat [0.0 (SO3MathsDegreeToRadian angx) (SO3MathsDegreeToRadian angz)]);
    
    SO3BodySetInitialState body;
    SO3BodySetMass body if mass <=. 0.0 then 1.0 else mass;
    SO3BodySetAutoSleep body 0;
    SO3BodySetFreeze body 0;
    V3DphysEnableBody body 0;
    
    setPluginInstanceCbDel inst mkfun2 @deleteOb bnavstr;
    setPluginInstanceCbNavigateControlState inst mkfun4 @cbNavigationControlState bnavstr;
    PluginRegisterAction inst "Control" mkfun6 @cbControl bnavstr;
    PluginRegisterAction inst "Set position" mkfun6 @cbSetPosition bnavstr;
    PluginRegisterAction inst "Get position" mkfun6 @cbGetPosition bnavstr;
    PluginRegisterAction inst "Set angle limit" mkfun6 @cbSetAngleLimit bnavstr;
    PluginRegisterAction inst "Jump" mkfun6 @cbJump bnavstr;
    PluginRegisterAction inst "Enable control" mkfun6 @cbEnable bnavstr;
    PluginRegisterAction inst "Disable control" mkfun6 @cbDisable bnavstr;
    PluginRegisterAction inst "Set speed" mkfun6 @cbSetSpeed bnavstr;
    PluginRegisterAction inst "Active camera" mkfun6 @cbActiveCamera bnavstr;
    PluginRegisterAction inst "Deactivate camera" mkfun6 @cbDeactivateCamera bnavstr;
    PluginRegisterAction inst "Set camera FOV" mkfun6 @cbCameraFov bnavstr;
    PluginRegisterAction inst "Set camera clipping" mkfun6 @cbCameraClipping bnavstr;
    PluginRegisterAction inst "Set camera focal length" mkfun6 @cbCameraFocalLength bnavstr;
    PluginRegisterAction inst "Enable fly mode" mkfun6 @cbEnableFlyMode bnavstr;
    PluginRegisterAction inst "Disable fly mode" mkfun6 @cbDisableFlyMode bnavstr;
    PluginRegisterAction inst "Set camera height" mkfun6 @cbSetCameraHeight bnavstr;
    
    // keep for retro compatibility 
    PluginRegisterAction inst "Enable" mkfun6 @cbEnable bnavstr;
    PluginRegisterAction inst "Disable" mkfun6 @cbDisable bnavstr;
    
    if (!initcam) then nil else
      cbActiveCamera inst nil nil nil nil bnavstr;
      
    if (!initcam || !init) then      
      cbDisable inst nil nil nil nil bnavstr
    else
      cbEnable inst nil nil nil nil bnavstr;
  );
  0;;


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