/*
-----------------------------------------------------------------------------
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 ObjHingeStr = [
  PHINGE_child              : SO3_OBJECT,
  PHINGE_parent             : SO3_OBJECT,
  PHINGE_bCollide           : I,
  PHINGE_iTargetMode        : I,
  PHINGE_iSourceMode        : I,
  PHINGE_contraint          : SO3_PHYSICCONTRAINT,
  PHINGE_iPosMode           : I,
  PHINGE_tPosition          : [F F F],
  PHINGE_tDirection         : [F F F],
  PHINGE_tLimits            : [F F],
  PHINGE_bUseLimits         : I,
  PHINGE_fBreakForce        : F,
  PHINGE_bUseMotor          : I,
  PHINGE_fMotor             : F,
  PHINGE_bLastLimit         : I,
  PHINGE_iDebounceTick      : I,
  PHINGE_iTick              : I
  
  ]mkObjHingeStr;;


fun deleteOb(inst, obstr)=
  setPluginInstanceCbCameraChange inst nil;
  SO3PhysicsContraintDestroy obstr.PHINGE_contraint;
  
  setPluginInstanceCbScenePreRender inst nil;
  0;;


fun cbChangeCamera(inst, viewstr, sessionstr, camera, obstr)=
  if (camera != V3DgetDefaultCamera c3dXsession) then nil else
  (
    if (!obstr.PHINGE_iSourceMode) then nil else
    (
      set obstr.PHINGE_child = V3DgetCameraByType sessionstr camera obstr.PHINGE_iSourceMode;
    );
  
    if (!obstr.PHINGE_iTargetMode) then nil else
    (
      set obstr.PHINGE_parent = V3DgetCameraByType sessionstr camera obstr.PHINGE_iTargetMode;
    );
  );
0;;


fun cbContraintPreRender(inst, sessionstr, etime, obstr)=
  let SO3PhysicsContraintGetHingeForce obstr.PHINGE_contraint -> [x y z] in
  let SO3PhysicsContraintGetHingeOmega obstr.PHINGE_contraint -> omega in
  (
    if (obstr.PHINGE_fBreakForce <=. 0.0) then nil else
    (
      let (absf x +. absf y +. absf z) /. 3.0 -> avforce in
      if (avforce <. obstr.PHINGE_fBreakForce) then nil else
      (
        SO3PhysicsContraintDestroy obstr.PHINGE_contraint;
        set obstr.PHINGE_contraint = nil;
        setPluginInstanceCbScenePreRender inst nil;
        SendPluginEvent inst "Broken" nil nil;
      );
    );
    SendPluginEvent inst "Force" strcatn (ftoa x)::" "::(ftoa y)::" "::(ftoa z)::nil nil;
    
    if (omega == nil) then nil else
      SendPluginEvent inst "Omega" (ftoa omega) nil;
    
    let (SO3MathsRadianToDegree SO3PhysicsContraintGetHingeAngle obstr.PHINGE_contraint) -> angle in
    if (angle == nil) then nil else
    (
      SendPluginEvent inst "Angle" (ftoa angle) nil;
      set obstr.PHINGE_iTick = obstr.PHINGE_iTick + (etime / 1000);
      
      if (!obstr.PHINGE_bUseLimits) then nil else
      if ((obstr.PHINGE_iTick - obstr.PHINGE_iDebounceTick) < 200) then nil else
      let obstr.PHINGE_tLimits -> [tmin tmax] in
      let if ((angle +. 2.0) >=. tmax) then 1 else 0 -> maxlimit in
      let if ((angle -. 2.0) <=. tmin) then 1 else 0 -> minlimit in
      let if (minlimit || maxlimit) then 1 else 0 -> hitlimit in
      (
        //addLogMessage ftoa angle;
        if (maxlimit && !obstr.PHINGE_bLastLimit) then
        (
          SendPluginEvent inst "In max limit" nil nil;
          set obstr.PHINGE_iDebounceTick = obstr.PHINGE_iTick;
          set obstr.PHINGE_bLastLimit = hitlimit;
        )
        else if (minlimit && !obstr.PHINGE_bLastLimit) then
        (
          SendPluginEvent inst "In min limit" nil nil;
          set obstr.PHINGE_iDebounceTick = obstr.PHINGE_iTick;
          set obstr.PHINGE_bLastLimit = hitlimit;
        )
        else if (!hitlimit && obstr.PHINGE_bLastLimit) then     
        (
          SendPluginEvent inst "Out limits" nil nil;
          set obstr.PHINGE_iDebounceTick = obstr.PHINGE_iTick;
          set obstr.PHINGE_bLastLimit = hitlimit;
        )
        else nil;
      );
    );
  );
  0;;


fun cbSetLimits(inst, from, action, param, reply, obstr)=
  let strextr param -> l in
  let hd l -> p in
  let (atof (hd p)) -> mint in
  let if mint == nil then 0.0 else mint -> mint in
  let atof (hd tl p) -> maxt in
  let if maxt == nil then 0.0 else maxt -> maxt in
    set obstr.PHINGE_tLimits = [mint maxt];
    
  if (obstr.PHINGE_contraint == nil) || (!obstr.PHINGE_bUseLimits) then nil else
  let obstr.PHINGE_tLimits -> [mint maxt] in
    SO3PhysicsContraintSetHingeLimits obstr.PHINGE_contraint (SO3MathsDegreeToRadian mint) (SO3MathsDegreeToRadian maxt);
  0;;


fun cbEnable(inst, from, action, param, reply, obstr)=
  if (obstr.PHINGE_contraint != nil) then nil else
  (
    let if obstr.PHINGE_iPosMode == 0 then
          SO3ObjectGetGlobalPosition obstr.PHINGE_child
        else if obstr.PHINGE_iPosMode == 1 then
          SO3ObjectGetGlobalPosition obstr.PHINGE_parent
        else
          obstr.PHINGE_tPosition
    -> pos in
    let SO3MathsQuatGetDirection (SO3ObjectGetGlobalOrientation obstr.PHINGE_child) obstr.PHINGE_tDirection -> dir in
    set obstr.PHINGE_contraint = SO3PhysicsContraintCreateHinge (V3DgetSession c3dXsession) (SO3SceneNodeGetBody obstr.PHINGE_child) (SO3SceneNodeGetBody obstr.PHINGE_parent) pos dir obstr.PHINGE_bUseLimits;

    if (!obstr.PHINGE_bUseLimits) then nil else
    let obstr.PHINGE_tLimits -> [mint maxt] in
      SO3PhysicsContraintSetHingeLimits obstr.PHINGE_contraint (SO3MathsDegreeToRadian mint) (SO3MathsDegreeToRadian maxt);
    
    if (!obstr.PHINGE_bUseMotor) then nil else
    (
      SO3PhysicsContraintSetHingeMotorOmega obstr.PHINGE_contraint (SO3MathsDegreeToRadian obstr.PHINGE_fMotor) 1.0;
      if (obstr.PHINGE_fMotor != 0.0) then nil else
        SO3PhysicsContraintSetHingeMotorAngle obstr.PHINGE_contraint SO3PhysicsContraintGetHingeAngle obstr.PHINGE_contraint 0.0 0.0;
      SO3PhysicsContraintSetHingeMotorEnable obstr.PHINGE_contraint 1;
    );
    
    SO3PhysicsContraintSetCollisionState obstr.PHINGE_contraint obstr.PHINGE_bCollide;
    
    setPluginInstanceCbScenePreRender inst mkfun4 @cbContraintPreRender obstr;
    set obstr.PHINGE_iTick = 0;
    set obstr.PHINGE_iDebounceTick = 0;
  );
  0;;


fun cbDisable(inst, from, action, param, reply, obstr)=
  SO3PhysicsContraintDestroy obstr.PHINGE_contraint;
  set obstr.PHINGE_contraint = nil;
  setPluginInstanceCbScenePreRender inst nil;
  0;;


fun cbSetAngle(inst, from, action, param, reply, obstr)=
  let strextr param -> l in
  let hd l -> p in
  let if ((atof (hd p)) == nil) then nil else SO3MathsDegreeToRadian (atof (hd p)) -> angle in
  let atof (hd tl p) -> minfric in
  let atof (hd tl tl p) -> maxfric in
  let if minfric == nil then 0.0 else minfric -> minfric in
  let if maxfric == nil then 0.0 else maxfric -> maxfric in
    SO3PhysicsContraintSetHingeMotorAngle obstr.PHINGE_contraint angle minfric maxfric;
  
  SO3PhysicsContraintSetHingeMotorEnable obstr.PHINGE_contraint 1;
  0;;


fun cbSetTorque(inst, from, action, param, reply, obstr)=
  let atof param -> value in
    SO3PhysicsContraintSetHingeTorque obstr.PHINGE_contraint value;
  0;;


fun cbSetMotor(inst, from, action, param, reply, obstr)=
  let strextr param -> l in
  let hd l -> p in
  let if ((atof (hd p)) == nil) then nil else SO3MathsDegreeToRadian (atof (hd p)) -> angle in
  let atof (hd tl p) -> force in
  (
    SO3PhysicsContraintSetHingeMotorOmega obstr.PHINGE_contraint angle force;
    
    //addLogMessage ftoa SO3MathsRadianToDegree angle;
    if ((angle != nil && angle == 0.0) && (force == nil || force >=. 1.0)) then
      SO3PhysicsContraintSetHingeMotorAngle obstr.PHINGE_contraint SO3PhysicsContraintGetHingeAngle obstr.PHINGE_contraint 0.0 0.0
    else nil;
    SO3PhysicsContraintSetHingeMotorEnable obstr.PHINGE_contraint 1;
  );
  0;;


fun cbBrake(inst, from, action, param, reply, obstr)=
  let atof param -> value in
    SO3PhysicsContraintSetHingeBrake obstr.PHINGE_contraint value;
  0;;
  

fun cbReleaseBrake(inst, from, action, param, reply, obstr)=
  SO3PhysicsContraintSetHingeBrake obstr.PHINGE_contraint nil;
  0;;


fun cbStopMotor(inst, from, action, param, reply, obstr)=
  SO3PhysicsContraintSetHingeMotorEnable obstr.PHINGE_contraint 0;
  0;;


fun cbSetBreakForce(inst, from, action, param, reply, obstr)=
  set obstr.PHINGE_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 atof (getPluginInstanceParam inst "dirx") -> dirx in
  let atof (getPluginInstanceParam inst "diry") -> diry in
  let atof (getPluginInstanceParam inst "dirz") -> dirz in
  let [dirx diry dirz] -> dir in
  
  let atoi (getPluginInstanceParam inst "useLimits") -> useLimits in  
  let atof (getPluginInstanceParam inst "minangle") -> minangle in
  let atof (getPluginInstanceParam inst "maxangle") -> maxangle in      
  let atof (getPluginInstanceParam inst "breakforce") -> breakforce in
  let atoi (getPluginInstanceParam inst "useMotor") -> usemotor in
  let atof (getPluginInstanceParam inst "motor") -> motor 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 mkObjHingeStr [objsrc parent collide iobj1mode iobj2mode nil posmode offset dir [minangle maxangle] useLimits breakforce usemotor motor 1 0 0] -> obstr in
  (
    if !iobj1mode && !iobj2mode then nil else
      setPluginInstanceCbCameraChange inst mkfun5 @cbChangeCamera obstr;
    
    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 angle" mkfun6 @cbSetAngle obstr;
    PluginRegisterAction inst "Set torque" mkfun6 @cbSetTorque obstr;
    PluginRegisterAction inst "Set motor" mkfun6 @cbSetMotor obstr;
    PluginRegisterAction inst "Brake" mkfun6 @cbBrake obstr;
    PluginRegisterAction inst "Release brake" mkfun6 @cbReleaseBrake obstr;
    PluginRegisterAction inst "Stop motor" mkfun6 @cbStopMotor obstr;
    PluginRegisterAction inst "Set break force" mkfun6 @cbSetBreakForce obstr;
    PluginRegisterAction inst "Set limits" mkfun6 @cbSetLimits obstr;
    
    setPluginInstanceCbDel inst mkfun2 @deleteOb obstr;
  );
  0;;


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