/*
-----------------------------------------------------------------------------
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 ObjStructureStr = [
  PSTRUCT_inst               : PInstance,
  PSTRUCT_child              : SO3_OBJECT,
  PSTRUCT_parent             : SO3_OBJECT,
  PSTRUCT_bCollide           : I,
  PSTRUCT_iTargetMode        : I,
  PSTRUCT_iSourceMode        : I,
  PSTRUCT_lBodies            : [SO3_PHYSICBODY r1],
  PSTRUCT_lContraints        : [SO3_PHYSICCONTRAINT r1],
  PSTRUCT_lNodes             : [[SO3_OBJECT [F F F] [F F F F]] r1],
  PSTRUCT_iNbSons            : I,
  PSTRUCT_iPosMode           : I,
  PSTRUCT_tPosition          : [F F F],
  PSTRUCT_tLimits            : [F F F],
  PSTRUCT_bUseLimits         : I,
  PSTRUCT_fBreakForce        : F
  
  ]mkObjStructureStr;;


fun deleteStructure(obstr)=
  while (obstr.PSTRUCT_lContraints != nil) do
  (
    SO3PhysicsContraintDestroy hd obstr.PSTRUCT_lContraints;
    set obstr.PSTRUCT_lContraints = tl obstr.PSTRUCT_lContraints;
  );
  
  while (obstr.PSTRUCT_lBodies != nil) do
  (
    SO3BodyDestroy hd obstr.PSTRUCT_lBodies;
    set obstr.PSTRUCT_lBodies = tl obstr.PSTRUCT_lBodies;
  );
  
  setPluginInstanceCbScenePreRender obstr.PSTRUCT_inst nil;
  
  while (obstr.PSTRUCT_lNodes != nil) do
  (
    let hd obstr.PSTRUCT_lNodes -> [node pos rot] in
    (
      SO3ObjectSetPosition node pos;
      SO3ObjectSetOrientation node rot;
    );
    set obstr.PSTRUCT_lNodes = tl obstr.PSTRUCT_lNodes;
  );
  0;;


fun deleteOb(inst, obstr)= 
  deleteStructure obstr;
  0;;


fun cbScenePreRender(inst, sessionstr, etime, obstr)=
  /*let SO3ObjectGetAnimations mesh -> lanims in
  while (lanims != nil) do
  (
    let hd lanims -> anim in
    if (((SO3AnimationGetType anim) != SO3_SKELETAL_ANIM) && ((SO3AnimationGetType anim) != SO3_SEQUENCE_ANIM)) then nil else
    if (!SO3AnimationGetEnable anim) then nil else
    (
  
    );
    
    set lanims = tl lanims;
  );*/
  0;;


fun loadBody(obstr, obj, stage)=
  let if ((SO3ObjectGetParent obj) == nil) then obstr.PSTRUCT_parent else (SO3ObjectGetParent obj) -> parent in
  let hd SO3ObjectGetChildren obj -> son in
  let SO3SceneNodeGetBody parent -> pbody in
  let SO3SceneNodeGetBody obj -> bbody in
  let SO3BodyGetMassMatrix pbody -> [mass _] in
  (
    if (mass != nil && mass != 0.0) then nil else
      set mass = 100.0 /. (itof (obstr.PSTRUCT_iNbSons + 1));
    
    if ((obstr.PSTRUCT_parent != parent) || (SO3SceneNodeGetBody obstr.PSTRUCT_parent) != nil) then nil else
    (
      set pbody = if (((SO3ObjectGetType parent) == SO3_TYPE_ENTITY) && ((SO3GetRootBonesFromMesh parent) == nil)) then SO3BodyCreateShape parent 0.0 else SO3BodyCreateEllipsoidExt parent [0.005 0.005 0.005] [0.0 0.0 0.0] nil;
      SO3BodySetMass pbody mass;
      set obstr.PSTRUCT_lBodies = pbody::obstr.PSTRUCT_lBodies;
    );
    
    // search for a parent with physics
    if (pbody != nil) then nil else
    (
      while ((pbody == nil) && ((SO3ObjectGetParent parent) != nil)) do
      (
        set parent = SO3ObjectGetParent parent;
        set pbody = SO3SceneNodeGetBody parent;    
      );
      
      if (pbody != nil) then nil else
      (
        set parent = obstr.PSTRUCT_parent;
        set pbody = SO3SceneNodeGetBody parent;
      ); 
    );
    
    if (bbody != nil) then nil else
    let [0.03 0.03 0.03] -> [mx my mz] in

    let SO3ObjectGetGlobalPosition obj -> gpos in 
    let if (son == nil) then gpos else SO3ObjectGetGlobalPosition son -> ppos in
    let SO3ObjectGetGlobalOrientation obj -> gquat in
    let SO3MathsQuatGetDirection (quatInverse gquat) subVectorF ppos gpos -> ldir in
    
    let SO3ObjectGetOrientation obj -> lquat in
    let quatInverse lquat -> iquat in
    
    let ldir -> [ldx ldy ldz] in
    let [(absf ldx) (absf ldy) (absf ldz)] -> [ldx ldy ldz] in
    let [(maxf ldx mx) (maxf ldy my) (maxf ldz mz)] -> lpos in
    let lpos -> ssize in
    let multiplyVectorF ldir [0.5 0.5 0.5] -> soffset in
    //if (ldx <. mx && ldy <. my && ldz <. mz) then nil else //too small
    (
      //addLogMessageVector3F "dir :" dir;
      set bbody = if ((SO3ObjectGetType obj) == SO3_TYPE_ENTITY) then SO3BodyCreateShape obj 0.0 else SO3BodyCreateBoxExt obj ssize soffset nil;
      SO3BodySetMass bbody mass;
      SO3BodySetMaterial bbody SO3BodyGetMaterial pbody;
      SO3BodySetCenterOfMass bbody [0.0 0.0 0.0];
      SO3BodySetContiniousCollisionMode bbody 1;
      set obstr.PSTRUCT_lBodies = bbody::obstr.PSTRUCT_lBodies;
    );
    
    if (pbody == nil) then nil else
    let SO3ObjectGetGlobalPosition obj -> pos in
    let SO3ObjectGetGlobalPosition parent -> ppos in
    // pos between bodies
    //let addVectorF pos divideVectorF (subVectorF ppos pos) [2.0 2.0 2.0] -> pos in
    let SO3MathsQuatGetDirection (SO3ObjectGetGlobalOrientation obj) [1.0 0.0 0.0] -> dir in
    let 1 -> type in
    let if (type == 2) then
          SO3PhysicsContraintCreateHinge (V3DgetSession c3dXsession) bbody pbody pos dir 0
        else if (type == 2) then
          SO3PhysicsContraintCreateBallAndSocket (V3DgetSession c3dXsession) bbody pbody pos 0
        else
          SO3PhysicsContraintCreateSlider (V3DgetSession c3dXsession) bbody pbody pos [0.0 0.1 0.0] 1 0
    -> joint in
    (
      //SO3PhysicsContraintSetHingeLimits joint (SO3MathsDegreeToRadian 4.0) (SO3MathsDegreeToRadian 85.0);
      SO3PhysicsContraintSetHingeMotorEnable joint 0;
      //SO3PhysicsContraintSetBallAndSocketLimits joint (SO3MathsDegreeToRadian 60.0) (SO3MathsDegreeToRadian -.60.0) (SO3MathsDegreeToRadian 60.0);
      SO3PhysicsContraintSetStiffness joint 1.0;
      SO3PhysicsContraintSetSliderLimits joint 0.0 0.0;
      //SO3PhysicsContraintSetSliderSpring joint 100.0 0.1;
      
      SO3PhysicsContraintSetCollisionState joint 0;
      set obstr.PSTRUCT_lContraints = joint::obstr.PSTRUCT_lContraints;
    );
  );
  0;;


fun countNbSons(obj, i)=
  let SO3ObjectGetChildren obj -> lchild in
  let SO3GetRootBonesFromMesh obj -> lbones in
  (
    while (lchild != nil) do
    (
      set i = countNbSons (hd lchild) i + 1;
      set lchild = tl lchild;
    );
    
    while (lbones != nil) do
    (
      set i = countNbSons (hd lbones) i + 1;
      set lbones = tl lbones;
    );
  );
  i;;


fun loadChildren(obstr, obj, stage)=
  set obstr.PSTRUCT_lNodes = [obj SO3ObjectGetPosition obj SO3ObjectGetOrientation obj]::obstr.PSTRUCT_lNodes;
  let SO3ObjectGetChildren obj -> lchild in
  let SO3GetRootBonesFromMesh obj -> lbones in
  (
    while (lchild != nil) do
    (
      loadBody obstr (hd lchild) stage;
      loadChildren obstr (hd lchild) stage + 1;
      set lchild = tl lchild;
    );
    
    while (lbones != nil) do
    (
      loadBody obstr (hd lbones) stage;
      loadChildren obstr (hd lbones) stage + 1;
      set lbones = tl lbones;
    );
  );
  0;;


fun cbEnable(inst, from, action, param, reply, obstr)=
  if (obstr.PSTRUCT_lContraints != nil) then nil else
  (
    set obstr.PSTRUCT_iNbSons = countNbSons obstr.PSTRUCT_parent 0;
    loadChildren obstr obstr.PSTRUCT_parent 0;
    /*
    let if obstr.PSTRUCT_iPosMode == 0 then
          SO3ObjectGetGlobalPosition obstr.PSTRUCT_child
        else if obstr.PSTRUCT_iPosMode == 1 then
          SO3ObjectGetGlobalPosition obstr.PSTRUCT_parent
        else
          obstr.PSTRUCT_tPosition
    -> pos in
    set obstr.PSTRUCT_contraint = SO3PhysicsContraintCreateBallAndSocket (V3DgetSession c3dXsession) (SO3SceneNodeGetBody obstr.PSTRUCT_child) (SO3SceneNodeGetBody obstr.PSTRUCT_parent) pos obstr.PSTRUCT_bUseLimits;
    
    if (!obstr.PSTRUCT_bUseLimits) then nil else
    let obstr.PSTRUCT_tLimits -> [cone mint maxt] in
      SO3PhysicsContraintSetBallAndSocketLimits obstr.PSTRUCT_contraint (SO3MathsDegreeToRadian cone) (SO3MathsDegreeToRadian mint) (SO3MathsDegreeToRadian maxt);
    
    SO3PhysicsContraintSetCollisionState obstr.PSTRUCT_contraint obstr.PSTRUCT_bCollide;
    
    setPluginInstanceCbScenePreRender inst mkfun4 @cbScenePreRender obstr; 
    */
  );
  0;;


fun cbDisable(inst, from, action, param, reply, obstr)=
  deleteStructure obstr;
  0;;


fun cbSetBreakForce(inst, from, action, param, reply, obstr)=
  set obstr.PSTRUCT_fBreakForce = atof param;
  0;;


fun newOb(inst)=
  let (getPluginInstanceParam inst "object") -> objsource in
  let (getPluginInstanceParam inst "parent") -> objparent in
  let atoi (getPluginInstanceParam inst "collide") -> collide in

  let atoi (getPluginInstanceParam inst "posmode") -> posmode in
  let atof (getPluginInstanceParam inst "posx") -> fx in
  let atof (getPluginInstanceParam inst "posy") -> fy in
  let atof (getPluginInstanceParam inst "posz") -> fz in
  let [fx fy fz] -> offset in
  
  let atoi (getPluginInstanceParam inst "useLimits") -> useLimits in  
  let atof (getPluginInstanceParam inst "coneangle") -> coneangle in
  let atof (getPluginInstanceParam inst "mintwist") -> mintwist in
  let atof (getPluginInstanceParam inst "maxtwist") -> maxtwist in      
  let atof (getPluginInstanceParam inst "breakforce") -> breakforce in  
  let atoi (getPluginInstanceParam inst "init") -> init in
  
  let V3DgetObjectByName c3dXsession objparent -> parent in
  let V3DgetObjectTypeByName objparent -> iobj1mode in
  let V3DgetObjectByName c3dXsession objsource -> objsrc in
  let V3DgetObjectTypeByName objsource -> iobj2mode in
   
  let mkObjStructureStr [inst objsrc parent collide iobj1mode iobj2mode nil nil nil 0 posmode offset [coneangle mintwist maxtwist] useLimits breakforce] -> obstr in
  (
    if (!init) then nil else
      cbEnable inst nil nil nil nil obstr;
    
    PluginRegisterAction inst "Enable" mkfun6 @cbEnable obstr;
    PluginRegisterAction inst "Disable" mkfun6 @cbDisable obstr;
    PluginRegisterAction inst "Set break force" mkfun6 @cbSetBreakForce obstr;
    
    setPluginInstanceCbDel inst mkfun2 @deleteOb obstr;
  );
  0;;


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