/*
-----------------------------------------------------------------------------
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 ObjTireStr = [
  PTIRE_wheelObject           : SO3_OBJECT,
  PTIRE_wheelBody             : SO3_PHYSICBODY,
  PTIRE_bRemoveWheelBody      : I,
  PTIRE_wheelHinge            : SO3_PHYSICCONTRAINT,
  PTIRE_dirObject             : SO3_OBJECT,
  PTIRE_dirBody               : SO3_PHYSICBODY,
  PTIRE_dirHinge              : SO3_PHYSICCONTRAINT,
  PTIRE_damperObject          : SO3_OBJECT,
  PTIRE_damperBody            : SO3_PHYSICBODY,
  PTIRE_bRemoveDamper         : I,
  PTIRE_bRemoveDamperBody     : I,
  PTIRE_damperSlider          : SO3_PHYSICCONTRAINT,
  PTIRE_fDamping              : F,
  PTIRE_bDriving              : I,
  PTIRE_bDirectional          : I,
  PTIRE_restoreWheel          : [F [F F F] [F F F F]],
  PTIRE_restoreDamper         : [F [F F F] [F F F F]] 
  
  ]mkObjTireStr;;

struct ObjCameraPhys = [
  PCAMPHYS_camera             : SO3_OBJECT,
  PCAMPHYS_shell              : SO3_OBJECT,
  PCAMPHYS_camBody            : SO3_PHYSICBODY,
  PCAMPHYS_shellBody          : SO3_PHYSICBODY,
  PCAMPHYS_slider             : SO3_PHYSICCONTRAINT,
  PCAMPHYS_ball               : SO3_PHYSICCONTRAINT
    
  ]mkObjCameraPhysStr;;

struct ObjVehicleStr = [
  PVEHICLE_inst               : PInstance,
  PVEHICLE_child              : SO3_OBJECT,
  PVEHICLE_body               : SO3_PHYSICBODY,
  PVEHICLE_bRemoveBody        : I,
  PVEHICLE_physiqueMat        : SO3_PHYSICMATERIAL,
  PVEHICLE_tireMat            : SO3_PHYSICMATERIAL,
  PVEHICLE_fMass              : F,
  PVEHICLE_fEnginePower       : F,
  PVEHICLE_iNbGears           : I,
  PVEHICLE_bAutoGear          : I,
  PVEHICLE_matBrake           : [SO3_MATERIAL I I],
  PVEHICLE_sndEngineStart     : ObjAudio,
  PVEHICLE_sndEngineLoop      : ObjAudio,
  PVEHICLE_sndScreech         : ObjAudio,
  PVEHICLE_sndShock           : ObjAudio,
  PVEHICLE_camera             : ObjCameraPhys,
  PVEHICLE_lTires             : [ObjTireStr r1],
  PVEHICLE_iNbDirTires        : I,
  PVEHICLE_iNbDriveTires      : I,
  PVEHICLE_iNbTires           : I,
  PVEHICLE_fWheelRadius       : F,
  PVEHICLE_iCurGear           : I,
  PVEHICLE_fCurAccel          : F,
  PVEHICLE_iLastGear          : I,
  PVEHICLE_fLastAccel         : F,
  PVEHICLE_fCurRpm            : F,
  PVEHICLE_fLastRpm           : F,
  PVEHICLE_bBrake             : I,
  PVEHICLE_bStarted           : I,
  PVEHICLE_bTireOverlap       : I,
  PVEHICLE_restoreVehicle     : [F [F F F] [F F F F] [I I I I]]        // car mass, car position
  
  ]mkObjVehicleStr;;

var lGearRatios = [-1 2.90]::[0 1.0]::[1 2.66]::[2 1.78]::[3 1.3]::[4 1.0]::[5 0.74]::[6 0.5]::nil;;

fun deleteOb(inst, obstr)=
  //reset objects bodies and positions
  let obstr.PVEHICLE_restoreVehicle -> [omass pos quat [ambcolor diffcolor speccolor illcolor]] in
  (
    if (obstr.PVEHICLE_bRemoveBody) then
    (
      SO3BodyDestroy obstr.PVEHICLE_body;
      0;
    )
    else
    (
      SO3BodySetMass obstr.PVEHICLE_body omass;
      0;
    );
    SO3ObjectSetPosition obstr.PVEHICLE_child pos;
    SO3ObjectSetOrientation obstr.PVEHICLE_child quat;
      
    //restore material
    let obstr.PVEHICLE_matBrake -> [mat itech ipass] in
    (
      SO3MaterialSetAmbientByTechAndPass mat itech ipass ambcolor;
      SO3MaterialSetDiffuseByTechAndPass mat itech ipass diffcolor;
      SO3MaterialSetSpecularByTechAndPass mat itech ipass speccolor;
      SO3MaterialSetSelfIlluminationByTechAndPass mat itech ipass illcolor;
    );
  );
  
  while (obstr.PVEHICLE_lTires != nil) do
  (
    let hd obstr.PVEHICLE_lTires -> tirestr in
    let tirestr.PTIRE_restoreWheel -> [wmass wpos wquat] in
    let tirestr.PTIRE_restoreDamper -> [dmass dpos dquat] in
    (
      SO3PhysicsContraintDestroy tirestr.PTIRE_wheelHinge;
      SO3PhysicsContraintDestroy tirestr.PTIRE_damperSlider;
      
      if (tirestr.PTIRE_bRemoveWheelBody) then
      (
        SO3BodyDestroy tirestr.PTIRE_wheelBody;
        0;
      )
      else
      (
        SO3BodySetMass tirestr.PTIRE_wheelBody wmass;
        0;
      );
      
      if (tirestr.PTIRE_bRemoveDamperBody) then
      (
        SO3BodyDestroy tirestr.PTIRE_damperBody;
        0;
      )
      else
      (
        SO3BodySetMass tirestr.PTIRE_damperBody dmass;
        0;
      );
      
      if (!tirestr.PTIRE_bRemoveDamper) then nil else
      (
        SO3ObjectDestroy tirestr.PTIRE_damperObject;
        0;
      );
      
      if (!tirestr.PTIRE_bDirectional) then nil else
      (
        SO3BodyDestroy tirestr.PTIRE_dirBody;
        SO3ObjectDestroy tirestr.PTIRE_dirObject;
        SO3PhysicsContraintDestroy tirestr.PTIRE_dirHinge;
      );
      
      SO3ObjectSetPosition tirestr.PTIRE_wheelObject wpos;
      SO3ObjectSetOrientation tirestr.PTIRE_wheelObject wquat;
      
      SO3ObjectSetPosition tirestr.PTIRE_damperObject dpos;
      SO3ObjectSetOrientation tirestr.PTIRE_damperObject dquat;
    );
    
    set obstr.PVEHICLE_lTires = tl obstr.PVEHICLE_lTires;
  );
  
  SO3PhysicsMaterialDestroy obstr.PVEHICLE_physiqueMat;
  
  setPluginInstanceCbScenePreRender inst nil;
  
  //stop sounds
  _DSAudio obstr.PVEHICLE_sndEngineStart;
  _DSAudio obstr.PVEHICLE_sndEngineLoop;
  _DSAudio obstr.PVEHICLE_sndScreech;
  _DSAudio obstr.PVEHICLE_sndShock;
  
  //destroy camera
  V3DdelCamera c3dXsession obstr.PVEHICLE_camera.PCAMPHYS_camera;
  SO3ObjectDestroy obstr.PVEHICLE_camera.PCAMPHYS_shell;
  0;;


fun cbContraintPreRender(inst, sessionstr, etime, obstr)=  
  if (!obstr.PVEHICLE_bStarted) then nil else
  (
    let if (obstr.PVEHICLE_iCurGear == 0) then 1000.0 else 1000.0 *. itof obstr.PVEHICLE_iNbGears -> maxRpm in
    let switch lGearRatios obstr.PVEHICLE_iCurGear -> gearRatio in
    let if (abs obstr.PVEHICLE_iCurGear < 1) then 1.0 else switch lGearRatios obstr.PVEHICLE_iCurGear - 1 -> prevGearRatio in
    let 3.42 -> differential in
    let obstr.PVEHICLE_fEnginePower -> epower in
    let (itof (abs obstr.PVEHICLE_iCurGear)) -> fgear in
    let SO3BodyGetVelocity obstr.PVEHICLE_body -> [vx vy vz] in
    let sqrt ((vx *. vx) +. (vy *. vy) +. (vz *. vz)) -> mspeed in
    let mspeed *. 3.6 -> kspeed in
    let if (obstr.PVEHICLE_fWheelRadius == 0.0) then 0.0 else mspeed *. obstr.PVEHICLE_fWheelRadius -> curRate in
    let (curRate *. gearRatio *. differential) *. 60.0 /. 2.0 *. PIf -> curRpm in
    let 
        let 0.0 -> avom in
        (
          let sizelist obstr.PVEHICLE_lTires -> size in
          let 0 -> i in
          while (i < size) do
          (
            let nth_list obstr.PVEHICLE_lTires i -> tirestr in
            if (!tirestr.PTIRE_bDriving) then nil else
            (
              let SO3PhysicsContraintGetHingeOmega tirestr.PTIRE_wheelHinge -> homega in
              set avom = avom +. homega;
            );
            set i = i +1;
          );
          
          if (avom == 0.0) then avom else
            set avom = avom /. (itof obstr.PVEHICLE_iNbDriveTires);
        )
    -> avomega in
    let absf ((avomega *. gearRatio *. 60.0) *. obstr.PVEHICLE_fWheelRadius) -> realRpm in
    let (curRpm /. maxRpm) -> fx in
    // (- (0.1-x)*(1-x) + addtorque) * x*x + starttorque
    //let if (obstr.PVEHICLE_iCurGear == 0) then 0.0 else epower *. ((-.(0.1 -. fx) *. (1.0 -. fx) +. (1.0 -. (1.0 /. fgear))) *. (fx *. fx) +. (1.0 /. fgear)) -> engineTorque in
    
    // ((- (x*x) + (2 - 1/5)) * (x*x) + (1/5)) 
    let if (obstr.PVEHICLE_iCurGear == 0) then 0.0 else epower *. ((-.(fx *. fx) +. (2.0 -. (1.0 /. fgear))) *. (fx *. fx) +. (1.0 /. fgear)) -> engineTorque in
    let if (obstr.PVEHICLE_fCurAccel == 0.0) || (engineTorque <. 0.0) then 0.0 else (2.0 *. PIf *. engineTorque *. differential) /. (60.0 *. gearRatio *. obstr.PVEHICLE_fCurAccel) -> womega in
    let engineTorque *. gearRatio *. differential *. obstr.PVEHICLE_fCurAccel -> wheelTorque in
    // auto gear
    (
      //addLogMessage ftoa engineTorque;
      if (obstr.PVEHICLE_bAutoGear && (obstr.PVEHICLE_iCurGear == obstr.PVEHICLE_iLastGear) && obstr.PVEHICLE_iCurGear > 0) then
      (
        if ((curRpm <. 800.0) && (obstr.PVEHICLE_iCurGear > 1)) then
        (
          //gear down
          set obstr.PVEHICLE_iCurGear = obstr.PVEHICLE_iCurGear - 1;
          SendPluginEvent inst "Gear" itoa obstr.PVEHICLE_iCurGear nil;
        )
        else if ((engineTorque >=. (epower -. (epower *. 0.1))) && (obstr.PVEHICLE_iCurGear < obstr.PVEHICLE_iNbGears) /*&& (avomega >. 0.0)*/ && obstr.PVEHICLE_bTireOverlap && (obstr.PVEHICLE_fCurAccel >. 0.0)) then
        (
          //gear up
          set obstr.PVEHICLE_iCurGear = obstr.PVEHICLE_iCurGear + 1;
          SendPluginEvent inst "Gear" itoa obstr.PVEHICLE_iCurGear nil;
        )
        else nil;
      )
      else nil;
      
      // update gear
      if (/*(obstr.PVEHICLE_fLastAccel == obstr.PVEHICLE_fCurAccel && obstr.PVEHICLE_iCurGear == obstr.PVEHICLE_iLastGear) ||*/ obstr.PVEHICLE_bBrake) then nil else
      (
        set obstr.PVEHICLE_iLastGear = obstr.PVEHICLE_iCurGear;
        set obstr.PVEHICLE_fLastAccel = obstr.PVEHICLE_fCurAccel;
        
        let sizelist obstr.PVEHICLE_lTires -> size in
        let 0 -> i in
        while (i < size) do
        (
          let nth_list obstr.PVEHICLE_lTires i -> tirestr in
          if (!tirestr.PTIRE_bDriving) then nil else
          (
            if (obstr.PVEHICLE_iCurGear == 0) || (obstr.PVEHICLE_fCurAccel == 0.0) then 
            (
              //SO3PhysicsContraintSetHingeTorque tirestr.PTIRE_wheelHinge 0.0;
              SO3PhysicsContraintSetHingeMotorEnable tirestr.PTIRE_wheelHinge 0;
              0;
            )
            else
            (
              SO3PhysicsContraintSetHingeMotorOmega tirestr.PTIRE_wheelHinge if (obstr.PVEHICLE_iCurGear < 0) then -.womega else womega 0.5;
              SO3PhysicsContraintSetHingeMotorEnable tirestr.PTIRE_wheelHinge 1;
              //let SO3MathsQuatGetDirection SO3ObjectGetGlobalOrientation tirestr.PTIRE_wheelObject [if (obstr.PVEHICLE_iCurGear < 0) then -.womega else womega 0.0 0.0] -> wdir in
                //SO3BodySetOmega tirestr.PTIRE_wheelBody wdir;
              //SO3PhysicsContraintSetHingeTorque tirestr.PTIRE_wheelHinge wheelTorque;
              0;
            );
          );
          set i = i +1;
        );
        0;
      );
      
      // update sounds position
      let SO3ObjectGetGlobalPosition obstr.PVEHICLE_child -> pos in
      let SO3ObjectGetDerivedDirectionAxis obstr.PVEHICLE_child [0.0 0.0 1.0] -> dir in
      (
        _AudioSetPosition obstr.PVEHICLE_sndEngineStart pos dir;
        _AudioSetPosition obstr.PVEHICLE_sndEngineLoop pos dir;
        _AudioSetPosition obstr.PVEHICLE_sndScreech pos dir;
        _AudioSetPosition obstr.PVEHICLE_sndShock pos dir;
        
        //_AudioSetVolume obstr.PVEHICLE_sndEngineLoop 100;
        let (realRpm *. 0.0001) -> pitch in
        (
          _AudioSetPitch obstr.PVEHICLE_sndEngineLoop (0.5 +. pitch);
          _AudioSetVolume obstr.PVEHICLE_sndEngineLoop 30 + (ftoi (pitch *. 150.0));
        );
        
        let ((curRpm /. 60.0) -. (absf womega)) /. 100.0 -> diff in
        (
          if ((diff >. 0.0) && obstr.PVEHICLE_bTireOverlap) then
          (
            _AudioSetPitch obstr.PVEHICLE_sndScreech (1.0 +. (absf diff) /. 10.0);
            _AudioSetVolume obstr.PVEHICLE_sndScreech (ftoi ((absf diff) *. 100.0));
            SendPluginEvent inst "Drift" ftoa (absf diff) nil;
            0;
          )
          else
          (
            _AudioSetVolume obstr.PVEHICLE_sndScreech 0;
            0;
          );
        );
      );
        
      if (obstr.PVEHICLE_iCurGear < 0) then
      (
        SO3PhysicsContraintSetHingeLimits obstr.PVEHICLE_camera.PCAMPHYS_ball (SO3MathsDegreeToRadian (-.180.0)) (SO3MathsDegreeToRadian 180.0);
        SO3PhysicsContraintSetHingeMotorAngle obstr.PVEHICLE_camera.PCAMPHYS_ball (SO3MathsDegreeToRadian 180.0) 0.0 0.0;
      )
      else
      (
        SO3PhysicsContraintSetHingeLimits obstr.PVEHICLE_camera.PCAMPHYS_ball (SO3MathsDegreeToRadian (-.30.0)) (SO3MathsDegreeToRadian 30.0);
        SO3PhysicsContraintSetHingeMotorAngle obstr.PVEHICLE_camera.PCAMPHYS_ball 0.0 (-.30.0) 30.0;
      );
      
      set obstr.PVEHICLE_fCurRpm = curRpm;
      SendPluginEvent inst "Speed" itoa (ftoi kspeed) nil;
      SendPluginEvent inst "Rpm" itoa (ftoi realRpm) nil;
    );
  );
  0;;


fun computeMass(obstr)=
  if (obstr.PVEHICLE_iNbTires == 0) then nil else
  (
    let obstr.PVEHICLE_fMass -> mass in
    let itof obstr.PVEHICLE_iNbTires -> nbtire in
    let mass /. nbtire *. 0.3 -> tiremass in
    let mass -. (tiremass *. nbtire) -> vehiclemass in
    (
      SO3BodySetMass obstr.PVEHICLE_body vehiclemass;
      //SO3BodySetContiniousCollisionMode obstr.PVEHICLE_body 1;
      let sizelist obstr.PVEHICLE_lTires -> size in
      let 0 -> i in
      while (i < size) do
      (
        let nth_list obstr.PVEHICLE_lTires i -> tirestr in
        let if tirestr.PTIRE_bDirectional then tiremass /. 12.0 else tiremass /. 6.0 -> wmass in
        (
          SO3BodySetMass tirestr.PTIRE_wheelBody wmass *. 3.0;
          SO3BodySetMass tirestr.PTIRE_damperBody wmass *. 3.0;
          SO3BodySetMass tirestr.PTIRE_dirBody wmass *. 6.0;
          
          //SO3BodySetContiniousCollisionMode tirestr.PTIRE_wheelBody 1;
          //SO3BodySetContiniousCollisionMode tirestr.PTIRE_damperBody 1;
          //SO3BodySetContiniousCollisionMode tirestr.PTIRE_dirBody 1;
        );
        set i = i +1;
      );
    );
  );
  0;;


fun loadWheels(inst, obstr)=
  let 0 -> i in
  (
    let getPluginInstanceParam inst (strcat "wheel_" (itoa i)) -> wheel in
    let getPluginInstanceParam inst (strcat "damper_" (itoa i)) -> damper in
    let atof getPluginInstanceParam inst (strcat "damping_" (itoa i)) -> damping in
    let atoi getPluginInstanceParam inst (strcat "driving_" (itoa i)) -> driving in
    let atoi getPluginInstanceParam inst (strcat "directional_" (itoa i)) -> directional in
    let 1 -> wtype in
    while (wheel != nil) do
    (
      let SO3SceneGetObject (V3DgetSession c3dXsession) wheel -> wheelobj in
      let SO3SceneNodeGetBody wheelobj -> wheelbody in
      let if wheelbody == nil then 1 else 0 -> removewheelbody in      
      let let SO3BodyGetMassMatrix wheelbody -> [omass _] in [omass (SO3ObjectGetPosition wheelobj) (SO3ObjectGetOrientation wheelobj)] -> restorewheel in
              
      let SO3SceneGetObject (V3DgetSession c3dXsession) damper -> damperobj in
      let if damperobj == nil then 1 else 0 -> removedamper in
      let SO3SceneNodeGetBody damperobj -> damperbody in
      let if damperbody == nil then 1 else 0 -> removedamperbody in      
      let let SO3BodyGetMassMatrix damperbody -> [omass _] in [omass (SO3ObjectGetPosition damperobj) (SO3ObjectGetOrientation damperobj)] -> restoredamper in
      
      let (SO3ObjectGetGlobalPosition wheelobj) -> gpos in
      let gpos -> [gx gy gz] in

      let mkObjTireStr [wheelobj wheelbody removewheelbody nil nil nil nil damperobj damperbody removedamper removedamperbody nil damping driving directional restorewheel restoredamper] -> tirestr in
      
      let SO3ObjectGetBoundingBoxInfo tirestr.PTIRE_wheelObject 0 -> [[x y z] _ _] in
      let [(if x <=. 0.0 then 0.01 else x) (if y <=. 0.0 then 0.01 else y) (if z <=. 0.0 then 0.01 else z)] -> [x y z] in
      let if x >=. y && x >=. z then [(x /. 2.0) (if y >. z then z else y)] else if y >=. x && y >=. z then [(y /. 2.0) (if x >. z then z else x)] else [(z /. 2.0) (if x >. y then y else x)] -> [wradius wheight] in
      (
        set obstr.PVEHICLE_fWheelRadius = obstr.PVEHICLE_fWheelRadius +. wradius;
        
        // create the tire body if it is not already set
        if (tirestr.PTIRE_wheelBody != nil) then nil else
          set tirestr.PTIRE_wheelBody = SO3BodyCreateChamferCylinder tirestr.PTIRE_wheelObject wradius wheight;
        //SO3BodyCreateEllipsoid tirestr.PTIRE_wheelObject [wheight wradius wradius];
        
        SO3BodySetAngularDamping tirestr.PTIRE_wheelBody [0.1 0.1 0.1];
        //SO3BodySetLinearDamping tirestr.PTIRE_wheelBody 0.1;
        // create the damper node if it is not already set
        if (tirestr.PTIRE_damperObject != nil) then nil else
        (
          set tirestr.PTIRE_damperObject = SO3SceneNodeCreate (V3DgetSession c3dXsession) strcatn (getPluginInstanceName inst)::".Ndamper"::(itoa i)::nil;
          SO3ObjectLink tirestr.PTIRE_damperObject obstr.PVEHICLE_child;
          SO3ObjectSetGlobalPosition tirestr.PTIRE_damperObject [gx (gy +. 0.0) gz];
        );
        
        // create the damper body if it is not already set
        if (tirestr.PTIRE_damperBody != nil) then nil else
          set tirestr.PTIRE_damperBody = SO3BodyCreateEllipsoid tirestr.PTIRE_damperObject [wradius /. 2.0 wradius /. 2.0 wradius /. 2.0];
        
        SO3BodySetCenterOfMass tirestr.PTIRE_damperBody [0.0 0.1 0.0];
        
        // create the body for directionnal joint
        if (!tirestr.PTIRE_bDirectional) then nil else
        (
          set tirestr.PTIRE_dirObject = SO3SceneNodeCreate (V3DgetSession c3dXsession) strcatn (getPluginInstanceName inst)::".Ndir"::(itoa i)::nil;
          SO3ObjectLink tirestr.PTIRE_dirObject obstr.PVEHICLE_child;
          let (SO3ObjectGetGlobalPosition tirestr.PTIRE_damperObject) -> [dx dy dz] in
            SO3ObjectSetGlobalPosition tirestr.PTIRE_dirObject [dx (dy +. wradius) dz];
          set tirestr.PTIRE_dirBody = SO3BodyCreateBox tirestr.PTIRE_dirObject [wradius /. 2.0 wradius /. 2.0 wradius /. 2.0];
        );
        
        //set a not collidable physic material
        SO3BodySetMaterial tirestr.PTIRE_wheelBody obstr.PVEHICLE_tireMat;
        SO3BodySetMaterial tirestr.PTIRE_damperBody obstr.PVEHICLE_physiqueMat;
        SO3BodySetMaterial tirestr.PTIRE_dirBody obstr.PVEHICLE_physiqueMat;
        
        // now ready to create the joints
        
        // tire hinge joint
        let SO3MathsQuatGetDirection (SO3ObjectGetGlobalOrientation tirestr.PTIRE_wheelObject) [-.1.0 0.0 0.0] -> dir in
          set tirestr.PTIRE_wheelHinge = SO3PhysicsContraintCreateHinge (V3DgetSession c3dXsession) (if tirestr.PTIRE_bDirectional then tirestr.PTIRE_dirBody else tirestr.PTIRE_damperBody) tirestr.PTIRE_wheelBody gpos dir 0;
        
        // damper slider joint
        let SO3MathsQuatGetDirection (SO3ObjectGetGlobalOrientation tirestr.PTIRE_damperObject) [0.0 1.0 0.0] -> dir in
          set tirestr.PTIRE_damperSlider = SO3PhysicsContraintCreateSlider (V3DgetSession c3dXsession) obstr.PVEHICLE_body tirestr.PTIRE_damperBody gpos dir 1 (if tirestr.PTIRE_fDamping >. 0.0 then 1 else 0);
        
        SO3PhysicsContraintSetSliderLimits tirestr.PTIRE_damperSlider 0.0 (0.15);
        //TODO calculate damping with mass
        SO3PhysicsContraintSetSliderSpring tirestr.PTIRE_damperSlider 1000.0 tirestr.PTIRE_fDamping;
        
        // directional hinge
        if (!tirestr.PTIRE_bDirectional) then nil else
        (
          let SO3MathsQuatGetDirection (SO3ObjectGetGlobalOrientation tirestr.PTIRE_wheelObject) [0.0 1.0 0.0] -> dir in
            set tirestr.PTIRE_dirHinge = SO3PhysicsContraintCreateHinge (V3DgetSession c3dXsession) tirestr.PTIRE_dirBody tirestr.PTIRE_damperBody gpos dir 0;
          
          //TODO angle in editor ?
          SO3PhysicsContraintSetHingeLimits tirestr.PTIRE_dirHinge (SO3MathsDegreeToRadian (-.40.0)) (SO3MathsDegreeToRadian 40.0);
          SO3PhysicsContraintSetHingeMotorAngle tirestr.PTIRE_dirHinge SO3PhysicsContraintGetHingeAngle tirestr.PTIRE_dirHinge 0.0 0.0;
          SO3PhysicsContraintSetHingeMotorEnable tirestr.PTIRE_dirHinge 1;
        );
        
        //set wheel type
        set wtype = wtype + wtype;
        SO3BodySetType tirestr.PTIRE_wheelBody wtype;
        
        set obstr.PVEHICLE_iNbTires = obstr.PVEHICLE_iNbTires + 1;                
        set obstr.PVEHICLE_lTires = tirestr::obstr.PVEHICLE_lTires;
        set obstr.PVEHICLE_iNbDirTires = obstr.PVEHICLE_iNbDirTires + tirestr.PTIRE_bDirectional;
        set obstr.PVEHICLE_iNbDriveTires = obstr.PVEHICLE_iNbDriveTires + tirestr.PTIRE_bDriving;
      );
        
      set i = i + 1;
      set wheel = getPluginInstanceParam inst (strcat "wheel_" (itoa i));
      set damper = getPluginInstanceParam inst (strcat "damper_" (itoa i));
      set damping = atof getPluginInstanceParam inst (strcat "damping_" (itoa i));
      set driving = atoi getPluginInstanceParam inst (strcat "driving_" (itoa i));
      set directional = atoi getPluginInstanceParam inst (strcat "directional_" (itoa i));
    );
    
    if (obstr.PVEHICLE_iNbTires == 0) then nil else
      set obstr.PVEHICLE_fWheelRadius = obstr.PVEHICLE_fWheelRadius /. (itof obstr.PVEHICLE_iNbTires);
    0;
  );;


fun cbAccelerate(inst, from, action, param, reply, obstr)=
  let if (atof param) == nil then 0.0 else if (atof param) <. 0.0 then 0.0 else if (atof param) >. 1.0 then 1.0 else (atof param) -> accel in
    set obstr.PVEHICLE_fCurAccel = accel;
  0;;
  
  
fun cbTurn(inst, from, action, param, reply, obstr)=
  let if (atof param) == nil then 0.0 else if (atof param) <. (-.1.0) then (-.1.0) else if (atof param) >. 1.0 then 1.0 else (atof param) -> turn in  
  let sizelist obstr.PVEHICLE_lTires -> size in
  let 0 -> i in
  while (i < size) do
  (
    let nth_list obstr.PVEHICLE_lTires i -> tirestr in
    if (!tirestr.PTIRE_bDirectional) then nil else
    (
      SO3PhysicsContraintSetHingeMotorAngle tirestr.PTIRE_dirHinge (SO3MathsDegreeToRadian (turn *. 40.0)) 0.0 0.0;
    );
    set i = i +1;
  );
  0;;


fun cbStartBrake(inst, from, action, param, reply, obstr)=
  let if (atof param) == nil then 0.0 else (atof param) -> brake in  
  let sizelist obstr.PVEHICLE_lTires -> size in
  let 0 -> i in
  while (i < size) do
  (
    let nth_list obstr.PVEHICLE_lTires i -> tirestr in
      SO3PhysicsContraintSetHingeBrake tirestr.PTIRE_wheelHinge brake;
    set i = i +1;
  );
  
  //brake material
  let obstr.PVEHICLE_matBrake -> [mat itech ipass] in
  let make_rgba 255 0 0 128 -> brakeColor in
  (
    SO3MaterialSetAmbientByTechAndPass mat itech ipass brakeColor;
    SO3MaterialSetDiffuseByTechAndPass mat itech ipass brakeColor;
    //SO3MaterialSetSpecularByTechAndPass mat itech ipass speccolor;
    SO3MaterialSetSelfIlluminationByTechAndPass mat itech ipass brakeColor;
  );
  
  set obstr.PVEHICLE_bBrake = 1;
  0;;


fun cbHandBrake(inst, from, action, param, reply, obstr)=
  let sizelist obstr.PVEHICLE_lTires -> size in
  let 0 -> i in
  while (i < size) do
  (
    let nth_list obstr.PVEHICLE_lTires i -> tirestr in
      SO3PhysicsContraintSetHingeBrake tirestr.PTIRE_wheelHinge 0.0;
    set i = i +1;
  );
  
  //brake material
  let obstr.PVEHICLE_matBrake -> [mat itech ipass] in
  let make_rgba 255 0 0 128 -> brakeColor in
  (
    SO3MaterialSetAmbientByTechAndPass mat itech ipass brakeColor;
    SO3MaterialSetDiffuseByTechAndPass mat itech ipass brakeColor;
    //SO3MaterialSetSpecularByTechAndPass mat itech ipass speccolor;
    SO3MaterialSetSelfIlluminationByTechAndPass mat itech ipass brakeColor;
  );
  
  set obstr.PVEHICLE_bBrake = 1;
  0;;


fun cbStopBrake(inst, from, action, param, reply, obstr)=
  let sizelist obstr.PVEHICLE_lTires -> size in
  let 0 -> i in
  while (i < size) do
  (
    let nth_list obstr.PVEHICLE_lTires i -> tirestr in
      SO3PhysicsContraintSetHingeBrake tirestr.PTIRE_wheelHinge nil;
    set i = i +1;
  );
  
  
  //restore brake material
  let obstr.PVEHICLE_restoreVehicle -> [omass pos quat [ambcolor diffcolor speccolor illcolor]] in
  let obstr.PVEHICLE_matBrake -> [mat itech ipass] in
  (
    SO3MaterialSetAmbientByTechAndPass mat itech ipass ambcolor;
    SO3MaterialSetDiffuseByTechAndPass mat itech ipass diffcolor;
    SO3MaterialSetSpecularByTechAndPass mat itech ipass speccolor;
    SO3MaterialSetSelfIlluminationByTechAndPass mat itech ipass illcolor;
  );
  
  set obstr.PVEHICLE_bBrake = 0;
  0;;
  

fun cbGearUp(inst, from, action, param, reply, obstr)=
  let if obstr.PVEHICLE_iCurGear == obstr.PVEHICLE_iNbGears then obstr.PVEHICLE_iNbGears else obstr.PVEHICLE_iCurGear + 1 -> gear in
    set obstr.PVEHICLE_iCurGear = gear;
  set obstr.PVEHICLE_fLastRpm = obstr.PVEHICLE_fCurRpm;
  SendPluginEvent inst "Gear" itoa obstr.PVEHICLE_iCurGear nil;
  0;;
  
  
fun cbGearDown(inst, from, action, param, reply, obstr)=
  let if obstr.PVEHICLE_iCurGear <= (-1) then -1 else obstr.PVEHICLE_iCurGear - 1 -> gear in
    set obstr.PVEHICLE_iCurGear = gear;
  set obstr.PVEHICLE_fLastRpm = obstr.PVEHICLE_fCurRpm;
  SendPluginEvent inst "Gear" itoa obstr.PVEHICLE_iCurGear nil;
  0;;


fun cbReset(inst, from, action, param, reply, obstr)=
  if (obstr.PVEHICLE_iCurGear == 0) then nil else
  (
    set obstr.PVEHICLE_iCurGear = 0;
    SendPluginEvent inst "Gear" itoa obstr.PVEHICLE_iCurGear nil;
    set obstr.PVEHICLE_fLastRpm = 0.0;
  );
  set obstr.PVEHICLE_fCurAccel = 0.0;
  
  let sizelist obstr.PVEHICLE_lTires -> size in
  let 0 -> i in
  while (i < size) do
  (
    let nth_list obstr.PVEHICLE_lTires i -> tirestr in
    (
      SO3PhysicsContraintSetHingeMotorEnable tirestr.PTIRE_wheelHinge 0;
      SO3BodySetOmega tirestr.PTIRE_wheelBody [0.0 0.0 0.0];
    );
    set i = i +1;
  );
  
  let SO3ObjectGetGlobalOrientation obstr.PVEHICLE_child -> [ax ay az aw] in
  let SO3ObjectGetGlobalPosition obstr.PVEHICLE_child -> [x y z] in
  (
    SO3BodySetOmega obstr.PVEHICLE_body [0.0 0.0 0.0];
    SO3BodySetVelocity obstr.PVEHICLE_body [0.0 0.0 0.0];
    SO3ObjectSetGlobalPosition obstr.PVEHICLE_child [x (y +. 1.0) z];
    SO3ObjectSetGlobalOrientation obstr.PVEHICLE_child [0.0 ay 0.0 aw];
  );
  0;;


fun cbSoundStartEnd(snd, obstr)=
  //_AudioPlay3d obstr.PVEHICLE_sndEngineLoop 1.0 1;
  0;;


fun cbStart(inst, from, action, param, reply, obstr)=
  set obstr.PVEHICLE_bStarted = 1;
  _AudioPlay3d obstr.PVEHICLE_sndEngineStart 10.0 0;
  _AudioPlay3d obstr.PVEHICLE_sndEngineLoop 10.0 1;
  0;;


fun cbStop(inst, from, action, param, reply, obstr)=
  set obstr.PVEHICLE_bStarted = 0;
  _AudioStop obstr.PVEHICLE_sndEngineLoop;
  
  //stop hinges motor
  let sizelist obstr.PVEHICLE_lTires -> size in
  let 0 -> i in
  while (i < size) do
  (
    let nth_list obstr.PVEHICLE_lTires i -> tirestr in
      SO3PhysicsContraintSetHingeMotorEnable tirestr.PTIRE_wheelHinge 0;
    set i = i +1;
  );
  0;;


fun cbCarContact(scene, obstr, body1, body2, pos, normal, force, normalspeed)=
  if _AudioIsPlaying obstr.PVEHICLE_sndShock then nil else
  (
    _AudioSetVolume obstr.PVEHICLE_sndShock (ftoi (absf normalspeed *. 100.0));
    _AudioPlay3d obstr.PVEHICLE_sndShock 10.0 0;
  );
  0;;


fun cbWheelOverlapStarted(scene, obstr, body1, body2)=
  let 0 -> btype in
  (
    let sizelist obstr.PVEHICLE_lTires -> size in
    let 0 -> i in
    while ((i < size) && (btype == 0)) do
    (
      let nth_list obstr.PVEHICLE_lTires i -> tirestr in
      if (body1 == tirestr.PTIRE_wheelBody) then
        set btype = SO3BodyGetType tirestr.PTIRE_wheelBody
      else if (body2 == tirestr.PTIRE_wheelBody) then
        set btype = SO3BodyGetType tirestr.PTIRE_wheelBody
      else nil;
      
      set i = i +1;
    );
    set obstr.PVEHICLE_bTireOverlap = obstr.PVEHICLE_bTireOverlap|btype;
  );
  0;;


fun cbWheelOverlapEnded(scene, obstr, body1, body2)=
  let 0 -> btype in
  (
    let sizelist obstr.PVEHICLE_lTires -> size in
    let 0 -> i in
    while ((i < size) && (btype == 0)) do
    (
      let nth_list obstr.PVEHICLE_lTires i -> tirestr in
      if (body1 == tirestr.PTIRE_wheelBody) then
        set btype = SO3BodyGetType tirestr.PTIRE_wheelBody
      else if (body2 == tirestr.PTIRE_wheelBody) then
        set btype = SO3BodyGetType tirestr.PTIRE_wheelBody
      else nil;
      
      set i = i +1;
    );
    set obstr.PVEHICLE_bTireOverlap = obstr.PVEHICLE_bTireOverlap & ~(btype);
  );
  0;;


fun cbEnableCamera(inst, from, action, param, reply, obstr)=
  V3DsetDefaultCamera c3dXsession obstr.PVEHICLE_camera.PCAMPHYS_camera;
  0;;


fun cbDisableCamera(inst, from, action, param, reply, obstr)=
  V3DremoveDefaultCamera c3dXsession obstr.PVEHICLE_camera.PCAMPHYS_camera;
  0;;


fun loadCamera(inst, obstr)=
  let atof (getPluginInstanceParam inst "nclip") -> nclip in
  let atof (getPluginInstanceParam inst "fclip") -> fclip in
  let atof (getPluginInstanceParam inst "fovy") -> fovy in
  let atof (getPluginInstanceParam inst "mindist") -> mindist in
  let atof (getPluginInstanceParam inst "maxdist") -> maxdist in
  let atoi (getPluginInstanceParam inst "enablecam") -> enablecam in
  let V3DaddCamera c3dXsession strcat (getPluginInstanceName inst) "_camera" -> cam in
  let V3DaddShell c3dXsession strcat (getPluginInstanceName inst) "_cam_dummy" nil nil [0.0 0.0 0.0] [0.0 (SO3MathsDegreeToRadian 180.0) (SO3MathsDegreeToRadian 20.0) 0.0] -> shell in
  let mkObjCameraPhysStr [cam shell nil nil nil nil] -> camstr in
  (
    set obstr.PVEHICLE_camera = camstr;
    SO3ObjectLink shell obstr.PVEHICLE_child;
    SO3ObjectLink cam shell;
    SO3ObjectSetPosition cam [0.0 0.0 maxdist];
    SO3ObjectSetOrientation cam SO3MathsEulerYXZToQuat [(SO3MathsDegreeToRadian 5.0) (SO3MathsDegreeToRadian 0.0) (SO3MathsDegreeToRadian 0.0)];
    V3DsetCamera cam 1.0 (SO3MathsDegreeToRadian fovy) nclip fclip;
    
    set camstr.PCAMPHYS_camBody = SO3BodyCreateEllipsoid cam [0.5 0.5 0.5];
    set camstr.PCAMPHYS_shellBody = SO3BodyCreateBox shell [0.1 0.1 0.1];
    SO3BodySetMass camstr.PCAMPHYS_shellBody 10.0;
    SO3BodySetMass camstr.PCAMPHYS_camBody 0.5;
    SO3BodySetMaterial camstr.PCAMPHYS_shellBody obstr.PVEHICLE_physiqueMat;
    
    let SO3ObjectGetGlobalPosition camstr.PCAMPHYS_shell -> gpos in
    (
      let SO3MathsQuatGetDirection (SO3ObjectGetGlobalOrientation camstr.PCAMPHYS_shell) [0.0 0.0 1.0] -> dir in
        set camstr.PCAMPHYS_slider = SO3PhysicsContraintCreateSlider (V3DgetSession c3dXsession) camstr.PCAMPHYS_camBody camstr.PCAMPHYS_shellBody gpos dir 1 0;
      SO3PhysicsContraintSetSliderLimits camstr.PCAMPHYS_slider (-. (maxdist -. mindist)) 0.0; 
    
      let SO3MathsQuatGetDirection (SO3ObjectGetGlobalOrientation camstr.PCAMPHYS_camera) [0.0 1.0 0.0] -> dir in
        set camstr.PCAMPHYS_ball = SO3PhysicsContraintCreateHinge (V3DgetSession c3dXsession) camstr.PCAMPHYS_shellBody obstr.PVEHICLE_body gpos dir 1;
      SO3PhysicsContraintSetHingeLimits camstr.PCAMPHYS_ball (SO3MathsDegreeToRadian (-.30.0)) (SO3MathsDegreeToRadian 30.0);
      SO3PhysicsContraintSetHingeMotorEnable camstr.PCAMPHYS_ball 1;
      SO3PhysicsContraintSetHingeMotorAngle camstr.PCAMPHYS_ball 0.0 (-.30.0) 30.0;
    );
    
    if (!enablecam) then nil else
      cbEnableCamera inst nil nil nil nil obstr;
  );
  0;;


  
fun newOb(inst)=
  let (getPluginInstanceParam inst "object") -> objname in 
  let atof (getPluginInstanceParam inst "mass") -> mass in  
  let atof (getPluginInstanceParam inst "enginepower") -> enginepower in
  let atoi (getPluginInstanceParam inst "nbgears") -> nbgears in
  let atoi (getPluginInstanceParam inst "autogear") -> autogear in  
  let (getPluginInstanceParam inst "sndstart") -> sndstart in
  let (getPluginInstanceParam inst "sndloop") -> sndloop in
  let (getPluginInstanceParam inst "sndscreech") -> sndscreech in
  let (getPluginInstanceParam inst "sndshock") -> sndshock in
  
  let (getPluginInstanceParam inst "objectmat") -> objmatname in
  let (getPluginInstanceParam inst "material") -> matname in
  let atoi (getPluginInstanceParam inst "technique") -> itec in
  let if itec == nil then 0 else itec -> itec in
  let atoi (getPluginInstanceParam inst "pass") -> ipass in
  let if ipass == nil then 0 else ipass -> ipass in
  
  let atoi (getPluginInstanceParam inst "startengine") -> startengine in  
  let SO3SceneGetObject (V3DgetSession c3dXsession) objname -> vehicleobj in  
  let SO3SceneNodeGetBody vehicleobj -> vehiclebody in
  let if vehiclebody == nil then 1 else 0 -> removebody in
  
  // create the physic body if not already set
  let if vehiclebody == nil then 1 else 0 -> bnewbody in
  let if vehiclebody == nil then SO3BodyCreateShape vehicleobj 0.0 else vehiclebody -> vehiclebody in
  
  let SO3SceneGetMaterial (V3DgetSession c3dXsession) (getPluginInstanceGroupName inst) matname -> brakemat in
  let SO3MaterialGetAmbientByTechAndPass brakemat itec ipass -> defamb in
  let SO3MaterialGetDiffuseByTechAndPass brakemat itec ipass -> defdiff in
  let SO3MaterialGetSpecularByTechAndPass brakemat itec ipass -> defspec in
  let SO3MaterialGetSelfIlluminationByTechAndPass brakemat itec ipass -> defself in
  
  let let SO3BodyGetMassMatrix vehiclebody -> [omass _] in [omass (SO3ObjectGetPosition vehicleobj) (SO3ObjectGetOrientation vehicleobj) [defamb defdiff defspec defself]] -> restore in
  let SO3PhysicsMaterialCreate (V3DgetSession c3dXsession) (strcat (getPluginInstanceName inst) ".mat") -> pmat in
  let SO3PhysicsMaterialCreate (V3DgetSession c3dXsession) (strcat (getPluginInstanceName inst) ".wmat") -> wmat in
  let mkObjVehicleStr [inst vehicleobj vehiclebody removebody pmat wmat mass enginepower nbgears autogear [brakemat itec ipass] nil nil nil nil nil nil 0 0 0 0.0 0 0.0 0 0.0 0.0 0.0 0 startengine 0 restore] -> obstr in
  (
    if !bnewbody then nil else
      SO3BodySetCenterOfMass vehiclebody [0.0 (-.0.3) 0.0];
    
    // physic mat to disable car collision between bodies
    SO3PhysicsMaterialSetDefaultCollidable obstr.PVEHICLE_physiqueMat obstr.PVEHICLE_physiqueMat 0;
    SO3PhysicsMaterialSetDefaultCollidable obstr.PVEHICLE_tireMat obstr.PVEHICLE_physiqueMat 0;
    SO3BodySetMaterial obstr.PVEHICLE_body obstr.PVEHICLE_physiqueMat;
    
    let SO3PhysicsGetMaterial (V3DgetSession c3dXsession) "default" -> defmat in
    (
      SO3PhysicsMaterialSetContiniousCollisionMode obstr.PVEHICLE_tireMat defmat 1;
      SO3PhysicsMaterialSetDefaultElasticity obstr.PVEHICLE_tireMat defmat 0.002;
      SO3PhysicsMaterialSetDefaultFriction obstr.PVEHICLE_tireMat defmat 0.9 1.3;
      SO3PhysicsMaterialSetDefaultSoftness obstr.PVEHICLE_tireMat defmat 0.001;
      SO3PhysicsMaterialSetSurfaceThickness obstr.PVEHICLE_tireMat defmat 0.001;
      
      SO3CbMaterialCollision obstr.PVEHICLE_physiqueMat defmat @cbCarContact obstr;
      
      SO3CbMaterialOverlapStarted obstr.PVEHICLE_tireMat defmat @cbWheelOverlapStarted obstr;
      SO3CbMaterialOverlapEnded obstr.PVEHICLE_tireMat defmat @cbWheelOverlapEnded obstr;
    );
      
    SO3BodySetAutoSleep vehiclebody 0;
    
    loadWheels inst obstr;
    computeMass obstr;
    
    // sounds
    set obstr.PVEHICLE_sndEngineStart = _CRAudio _channel (strcat (getPluginInstanceName inst) ".start") (_checkpack sndstart) 0;
    set obstr.PVEHICLE_sndEngineLoop = _CRAudio _channel (strcat (getPluginInstanceName inst) ".engine") (_checkpack sndloop) 0;
    set obstr.PVEHICLE_sndScreech = _CRAudio _channel (strcat (getPluginInstanceName inst) ".steer") (_checkpack sndscreech) 0;
    set obstr.PVEHICLE_sndShock= _CRAudio _channel (strcat (getPluginInstanceName inst) ".shock") (_checkpack sndshock) 0;
    _AudioPlay3d obstr.PVEHICLE_sndScreech 10.0 1;
    _AudioSetVolume obstr.PVEHICLE_sndScreech 0;
    _CBAudioEnd obstr.PVEHICLE_sndEngineStart @cbSoundStartEnd obstr;
    
    //camera
    loadCamera inst obstr;
    
    if (!obstr.PVEHICLE_bStarted) then nil else
      cbStart inst nil nil nil nil obstr;
    
    /*
    PluginRegisterAction inst "Control" mkfun6 @cbControl obstr;
    
    */
    
    PluginRegisterAction inst "Start engine" mkfun6 @cbStart obstr;
    PluginRegisterAction inst "Stop engine" mkfun6 @cbStop obstr;
    PluginRegisterAction inst "Accelerate" mkfun6 @cbAccelerate obstr;
    PluginRegisterAction inst "Brake" mkfun6 @cbStartBrake obstr;
    PluginRegisterAction inst "Stop brake" mkfun6 @cbStopBrake obstr;
    PluginRegisterAction inst "Hand brake" mkfun6 @cbHandBrake obstr;
    PluginRegisterAction inst "Turn" mkfun6 @cbTurn obstr;
    PluginRegisterAction inst "Gear up" mkfun6 @cbGearUp obstr;
    PluginRegisterAction inst "Gear down" mkfun6 @cbGearDown obstr;
    PluginRegisterAction inst "Reset" mkfun6 @cbReset obstr;
    PluginRegisterAction inst "Enable camera" mkfun6 @cbEnableCamera obstr;
    PluginRegisterAction inst "Disable camera" mkfun6 @cbDisableCamera obstr;
    setPluginInstanceCbScenePreRender inst mkfun4 @cbContraintPreRender obstr;
    
    setPluginInstanceCbDel inst mkfun2 @deleteOb obstr;
  );
  0;;


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