/*
-----------------------------------------------------------------------------
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 ObjPosStr = [
  POBP_father               : SO3_OBJECT,
  POBP_iCameraMode          : I,
  POBP_initpos              : [[F F F] [F F F F]],
  POBP_lControls            : [[S [[F F F] [F F F] F I I]] r1],
  POBP_bState               : I
  
  ]mkObjPosStr;;


fun deleteOb(inst, objpstr)=
  setPluginInstanceCbScenePreRenderPhysic inst nil;
  setPluginInstanceCbCameraChange inst nil;
  
  let objpstr.POBP_initpos -> [pos quat] in
  (
    SO3ObjectSetPosition objpstr.POBP_father pos;
    SO3ObjectSetOrientation objpstr.POBP_father quat;
  );
  
  set objpstr.POBP_lControls = nil;
  0;;


fun usePhysic(strobj)=
  let SO3SceneNodeGetBody strobj.POBP_father -> body in
  let SO3BodyGetMassMatrix body -> [mass _] in
    if ((body == nil) || !(V3DphysGetState c3dXsession) || (SO3BodyGetFluid body) || (mass <=. 0.0)) then 0 else 1;;


fun computeControls(objpstr, 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] -> hvec 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 0 -> prevbehavior in
  (
    let sizelist objpstr.POBP_lControls -> size in
    let 0 -> i in
    while i < size do
    (
      let nth_list objpstr.POBP_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 hvec = addVectorF hvec 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];
          set prevbehavior = 1;
          0;
        );
        
        set havedata = 1;
      );
      
      set i = i + 1;
    );
    
    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) hvec fang ycam angOverride addCamHeight havedata prevbehavior];
  );;


fun cbControlPreRender(inst, sessionstr, etime, objpstr)=
  if (!objpstr.POBP_bState) then nil else
  let (itof etime) /. 1000.0 -> ftime in
  let computeControls objpstr ftime 1.0 -> [vec hvec [pitch yaw roll] ycam angOverride addCamHeight havedata prevbehavior] in
  let addVectorF vec [0.0 ycam 0.0] -> vec in
  (
    if (!usePhysic objpstr) then
    (
      SO3ObjectTranslate objpstr.POBP_father vec SO3_LOCAL_TS;
      if (angOverride) then
      (
        let SO3MathsQuatToEulerPYR SO3ObjectGetOrientation objpstr.POBP_father -> [_ oyaw _] in
        (
          SO3ObjectSetOrientation objpstr.POBP_father SO3MathsEulerPYRToQuat [pitch oyaw +. yaw roll];
        );
      )
      else
      (
        SO3ObjectRotatePitch objpstr.POBP_father pitch SO3_LOCAL_TS;
        SO3ObjectRotateYaw objpstr.POBP_father yaw SO3_LOCAL_TS;
        SO3ObjectRotateRoll objpstr.POBP_father roll SO3_LOCAL_TS;
      );
      0;
    )
    else
    (
      let SO3SceneNodeGetBody objpstr.POBP_father -> body in
      if (!prevbehavior) then
      (
        let SO3ObjectGetGlobalPosition objpstr.POBP_father -> gvec in
        let SO3ObjectGetGlobalOrientation SO3ObjectGetParent objpstr.POBP_father -> grot in
        let SO3MathsQuatGetDirection grot vec -> vec in
        (
          SO3BodyMoveTo body addVectorF gvec vec 0.8;
          if (angOverride) then
          (
            let SO3MathsQuatToEulerPYR SO3ObjectGetOrientation objpstr.POBP_father -> [_ oyaw _] in
            (
              SO3BodyRotateTo body SO3MathsEulerPYRToQuat [pitch oyaw +. yaw roll] 0.6;
            );
          )
          else
          (
            let SO3MathsQuatToEulerPYR SO3ObjectGetOrientation objpstr.POBP_father -> [opitch oyaw oroll] in
              SO3BodyRotateTo body SO3MathsEulerPYRToQuat [opitch +. pitch oyaw +. yaw oroll +. roll] 0.6;
            0;
          );
        );
        0;
      )
      else
      (
        let SO3BodyGetVelocity body -> [velx vely velz] in
        let SO3ObjectGetDerivedDirectionAxis objpstr.POBP_father vec -> vec in
          SO3BodySetVelocity body addVectorF vec [0.0 vely 0.0];
         
        SO3BodySetOmega body [pitch yaw roll];
        0;
      );
    );    
  );
  0;;


fun cbSetPosObject(inst, from, action, param, reply, objpstr)=
  if (!objpstr.POBP_bState) then nil else
  if (!strcmp "" (strtrim param)) || (param == nil) then nil else
  let strextr param -> lp in
  let if (atof (nth_list (hd lp) 0)) == nil then 0.0 else (atof (nth_list (hd lp) 0)) -> px in
  let if (atof (nth_list (hd lp) 1)) == nil then 0.0 else (atof (nth_list (hd lp) 1)) -> py in
  let if (atof (nth_list (hd lp) 2)) == nil then 0.0 else (atof (nth_list (hd lp) 2)) -> pz in
    SO3ObjectSetGlobalPosition objpstr.POBP_father [px py pz];
  0;;


fun cbSetOrientationObject(inst, from, action, param, reply, objpstr)=
  if (!objpstr.POBP_bState) then nil else
  if (!strcmp "" (strtrim param)) || (param == nil) then nil else
  let strextr param -> lp in
  let (SO3MathsEulerXYZToQuat [SO3MathsDegreeToRadian (atof (nth_list (hd lp) 0)) SO3MathsDegreeToRadian (atof (nth_list (hd lp) 1)) SO3MathsDegreeToRadian (atof (nth_list (hd lp) 2))]) -> quat in
    SO3ObjectSetGlobalOrientation objpstr.POBP_father quat;
  0;;


fun cbChangeCamera(inst, viewstr, sessionstr, camera, objpstr)=
  let objpstr.POBP_father -> oldson in
  let V3DgetCameraByType sessionstr camera objpstr.POBP_iCameraMode -> father in
  let SO3ObjectGetPosition father -> cpos in
  let SO3ObjectGetOrientation father -> cquat in
  (
    set objpstr.POBP_father = father;
    
    let objpstr.POBP_initpos -> [pos quat] in
    (
      SO3ObjectSetPosition oldson pos;
      SO3ObjectSetOrientation oldson quat;
    );
    
    set objpstr.POBP_initpos = [cpos cquat];
  );
  0;;


fun cbSetObject(inst, from, action, param, reply, objpstr)=
  let strextr param -> lp in
  let hd hd lp -> objname in
  let atoi hd hd tl lp -> reset in
  let if reset == nil then 1 else reset -> reset in
  let V3DgetObjectByName c3dXsession objname -> father in
  let objpstr.POBP_father -> oldson in
  (  
    set objpstr.POBP_father = father;    
    
    if (!reset) then nil else
    let objpstr.POBP_initpos -> [pos quat] in
    (
      SO3ObjectSetPosition oldson pos;
      SO3ObjectSetOrientation oldson quat;
    );
    
    if (father == nil) then nil else
    (
      set objpstr.POBP_father = father;
      
      let V3DgetObjectTypeByName objname -> icammode in
      (
        set objpstr.POBP_iCameraMode = icammode;
        if (icammode == 0) then
          setPluginInstanceCbCameraChange inst nil
        else
          setPluginInstanceCbCameraChange inst mkfun5 @cbChangeCamera objpstr;
      );
      
      let SO3ObjectGetPosition father -> cpos in
      let SO3ObjectGetOrientation father -> cquat in
        set objpstr.POBP_initpos = [cpos cquat];
    );
  );
  0;;


fun cbControl(inst, from, action, param, reply, objpstr)=
  //get the input control from the link
  let if reply == nil then "control" else reply -> reply in
  let switchstr objpstr.POBP_lControls reply -> icontrol in
  (
    // if not already int the list we add it
    if (icontrol != nil) then nil else
    (
      set objpstr.POBP_lControls = [reply [[0.0 0.0 0.0] [0.0 0.0 0.0] 0.0 0 1]]::objpstr.POBP_lControls;
      set icontrol = switchstr objpstr.POBP_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) 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 cbTranslate(inst, from, action, param, reply, objpstr)=
  if (!objpstr.POBP_bState) then nil else
  let strextr param -> l in
  let hd l -> sv in
  let [atof(nth_list sv 0) atof(nth_list sv 1) atof(nth_list sv 2)] -> [vx vy vz] in
  let atoi(nth_list sv 3) -> mode in
  let if mode == 1 then SO3_WORLD_TS else if mode == 2 then SO3_PARENT_TS else SO3_LOCAL_TS -> nmode in
  (
    if (!usePhysic objpstr) then 
    (
      SO3ObjectTranslate objpstr.POBP_father [vx vy vz] nmode;
      0;
    )
    else
    (
      let if !mode then SO3ObjectGetDerivedDirectionAxis objpstr.POBP_father [vx vy vz] else [vx vy vz] -> vec in
        SO3BodySetVelocity (SO3SceneNodeGetBody objpstr.POBP_father) vec;
      0;
    );
  );
  0;;


fun cbRotate(inst, from, action, param, reply, objpstr)=
  if (!objpstr.POBP_bState) then nil else
  let strextr param -> l in
  let hd l -> sa in
  let [atof(nth_list sa 0) atof(nth_list sa 1) atof(nth_list sa 2)] -> [ax ay az] in
  (
    if (!usePhysic objpstr) then 
    (
      SO3ObjectRotatePitch objpstr.POBP_father (SO3MathsDegreeToRadian ax) SO3_LOCAL_TS;
      SO3ObjectRotateYaw objpstr.POBP_father (SO3MathsDegreeToRadian ay) SO3_LOCAL_TS;
      SO3ObjectRotateRoll objpstr.POBP_father (SO3MathsDegreeToRadian az) SO3_LOCAL_TS;
      0;
    )
    else
    (
      let [(SO3MathsDegreeToRadian ax) (SO3MathsDegreeToRadian ay) (SO3MathsDegreeToRadian az)] -> vec in
        SO3BodySetOmega (SO3SceneNodeGetBody objpstr.POBP_father) vec;
      0;
    );
  );
  0;;


fun setPitch(inst, from, action, param, rep, objpstr)=
  if (!objpstr.POBP_bState) then nil else
  let strextr param -> l in
  let atof hd hd l -> ang in
  let atoi hd tl hd l -> mode in
  if (param == nil) || (ang == nil) then nil else
  let if mode == 1 then SO3_WORLD_TS else if mode == 2 then SO3_PARENT_TS else SO3_LOCAL_TS -> nmode in
  (
    if (!usePhysic objpstr) then 
    (
      SO3ObjectRotatePitch objpstr.POBP_father (SO3MathsDegreeToRadian ang) nmode;
      0;
    )
    else
    (
      let if !mode then SO3ObjectGetDerivedDirectionAxis objpstr.POBP_father [(SO3MathsDegreeToRadian ang) 0.0 0.0 ] else [(SO3MathsDegreeToRadian ang) 0.0 0.0] -> vec in
        SO3BodySetOmega (SO3SceneNodeGetBody objpstr.POBP_father) vec;
      0;
    );
  );
  0;;


fun setYaw(inst, from, action, param, rep, objpstr)=
  if (!objpstr.POBP_bState) then nil else
  let strextr param -> l in
  let atof hd hd l -> ang in
  let atoi hd tl hd l -> mode in
  if (param == nil) || (ang == nil) then nil else
  let if mode == 1 then SO3_WORLD_TS else if mode == 2 then SO3_PARENT_TS else SO3_LOCAL_TS -> nmode in
  (
    if (!usePhysic objpstr) then 
    (
      SO3ObjectRotateYaw objpstr.POBP_father (SO3MathsDegreeToRadian ang) nmode;
      0;
    )
    else
    (
      let if !mode then SO3ObjectGetDerivedDirectionAxis objpstr.POBP_father [0.0 (SO3MathsDegreeToRadian ang) 0.0 ] else [0.0 (SO3MathsDegreeToRadian ang) 0.0] -> vec in
        SO3BodySetOmega (SO3SceneNodeGetBody objpstr.POBP_father) vec;
      0;
    );
  );
  0;;


fun setRoll(inst, from, action, param, rep, objpstr)=
  if (!objpstr.POBP_bState) then nil else
  let strextr param -> l in
  let atof hd hd l -> ang in
  let atoi hd tl hd l -> mode in
  if (param == nil) || (ang == nil) then nil else
  let if mode == 1 then SO3_WORLD_TS else if mode == 2 then SO3_PARENT_TS else SO3_LOCAL_TS -> nmode in
  (
    if (!usePhysic objpstr) then 
    (
      SO3ObjectRotateRoll objpstr.POBP_father (SO3MathsDegreeToRadian ang) nmode;
      0;
    )
    else
    (
      let if !mode then SO3ObjectGetDerivedDirectionAxis objpstr.POBP_father [0.0 0.0 (SO3MathsDegreeToRadian ang)] else [0.0 0.0 (SO3MathsDegreeToRadian ang)] -> vec in
        SO3BodySetOmega (SO3SceneNodeGetBody objpstr.POBP_father) vec;
      0;
    );
  );
  0;;


fun cbEnable(inst, from, action, param, rep, objpstr)=  
  let SO3SceneNodeGetBody objpstr.POBP_father -> body in
  if (body == nil) then nil else
  (
    SO3BodySetAutoSleep body 0;
    V3DphysFreezeBody body 1;
    SO3BodySetVelocity body [0.0 0.0 0.0];
    SO3BodySetOmega body [0.0 0.0 0.0];
  );
  
  set objpstr.POBP_bState = 1;
  0;;


fun cbDisable(inst, from, action, param, rep, objpstr)= 
  let SO3SceneNodeGetBody objpstr.POBP_father -> body in
  if (body == nil) then nil else
  (
    SO3BodySetAutoSleep body 0;
    V3DphysFreezeBody body 1;
    SO3BodySetVelocity body [0.0 0.0 0.0];
    SO3BodySetOmega body [0.0 0.0 0.0];
  );
  
  set objpstr.POBP_bState = 0;
  0;;


fun newOb(inst)=
  let (getPluginInstanceParam inst "object") -> objname in
  // for compatibility
  let if ((atoi (getPluginInstanceParam inst "iscamera")) == 1) then "Current camera" else objname -> objname in

  let V3DgetObjectByName c3dXsession objname -> father in
  let V3DgetObjectTypeByName objname -> icammode in

  let atoi (getPluginInstanceParam inst "init") -> init in
  let if (init == nil) then 1 else init -> init in
  
  let SO3ObjectGetPosition father -> cpos in
  let SO3ObjectGetOrientation father -> cquat in
  let mkObjPosStr [father icammode [cpos cquat] nil init] -> objpstr in
  (
    if icammode == 0 then nil else
      setPluginInstanceCbCameraChange inst mkfun5 @cbChangeCamera objpstr;

    if (!init) then nil else
    (      
      let SO3SceneNodeGetBody father -> body in
      (
        SO3BodySetAutoSleep body 0;
        V3DphysFreezeBody body 1;
      );
    );
    
    setPluginInstanceCbDel inst mkfun2 @deleteOb objpstr;
    
    PluginRegisterAction inst "Set position" mkfun6 @cbSetPosObject objpstr;
    PluginRegisterAction inst "Set orientation" mkfun6 @cbSetOrientationObject objpstr;
    PluginRegisterAction inst "Control" mkfun6 @cbControl objpstr;
    PluginRegisterAction inst "Translate" mkfun6 @cbTranslate objpstr;
    PluginRegisterAction inst "Rotate" mkfun6 @cbRotate objpstr;
    PluginRegisterAction inst "Pitch" mkfun6 @setPitch objpstr;
	  PluginRegisterAction inst "Roll" mkfun6 @setRoll objpstr;
	  PluginRegisterAction inst "Yaw" mkfun6 @setYaw objpstr;
	  PluginRegisterAction inst "Set object" mkfun6 @cbSetObject objpstr;
	  
	  PluginRegisterAction inst "Enable" mkfun6 @cbEnable objpstr;
	  PluginRegisterAction inst "Disable" mkfun6 @cbDisable objpstr;
	  	  
    setPluginInstanceCbScenePreRenderPhysic inst mkfun4 @cbControlPreRender objpstr;
  );
  0;;


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