#pragma once
#include "AftrConfig.h"
#ifdef AFTR_CONFIG_USE_ODE

#include "WO.h"
#include "IFaceWOODE.h"

namespace Aftr
{

   enum class ODE_PRIM_TYPE : unsigned char { SPHERE = 0, BOX, CYCLINDER, CAPSULE, PLANE, RAY, TRIMESH };
   std::ostream& operator<<( std::ostream& out, const ODE_PRIM_TYPE& opt );

/**
   \class WOODE
   \author Scott Nykl 
   \brief An inheritable class used for creating World Objects in your virtual world. One WOODE
          represents a single entity within the virtual world that serves some purpose. WOs
          may have a graphical representation (\see Model) and may also have corresponding
          physics properties, collision detection, internal state, internal intelligence,
          and may also be link hierarchically via a parent or multiple children.

   Creating Complex Objects:
      -# create multiple graphical models, each shall be able to be made up of one ODE prim.
      -# inherit this complex model from WOODE. ie: "public class Human : WOODE"
      -# in Human Constructor, create many children WorldObjects from multiple model files. ie: WOODE* arm, leg, torso, etc.
      -# create appropriate ODE prims for each child WOODE. ie: inside of "virtual void ODEinitializeInODE();" loop through
      each child object and call "void PhysicsEngineODE::createODEPrimitive( WOODE* wo, PhysicsEngineODE::ODE_PRIMITIVE_OBJ shape );"
      using each child WOODE (arm, leg, torso, etc). 
      Also, create the appropriate ODE Joints to hold the complex object together. 
      ie: "void dJointAttach( dJointID, arm->odeID, torso->odeID);" // link arm to torso. (angular contraints are also specified here!)  
      -# Within "virtual void ODEupdateForcesAndJoints();" do the following:
      Update the forces acting upon the bodies. ie: arm torque generated by human bicep
      Update the associated joints that link the Prims together. ie: left human arm bends 45 degrees.
 NOTE: Collision detection will not detect children objects colliding with their parent. ie: a human's arm will be able to go through
   the corresponding parent torso.
    \{
*/
class WOODE : public WO, public IFaceWOODE
{
public:

   /**
       Default constructor, sets pointers to NULL, sets everything up to be a static or ghost world object
   */
   static WOODE* New();

   /**
       Constructor, accepts a vrml file as a parameter and processes the file and creates a graphical and physical model of the file
       \param modelFileName The path to the model's filename.
   */
   static WOODE* New( const std::string& modelFileName, Vector scale = Vector(1,1,1), MESH_SHADING_TYPE shadingType = MESH_SHADING_TYPE::mstAUTO );

   /// The destructor for the WOODE
   virtual ~WOODE();

   /**
      Serializes this WOODE instance and its specific member properties to the output stream.
      This method serializes enough state such that when this WOODE's corresponding fromFile(...)
      is invoked, all attributes and state are preserved.

      This method invokes the protected toFileHelper(...)
   */
   virtual void toFile( std::ostream& sout );

   /**
      Deserializes a WOODE from the input stream and returns an instance of that
      specific WOODE type whos member properties are restored to the same state when the WOODE
      was serialized.
   */
   static WO* fromFile( std::istream& sin );
   
   /**
      Returns the ODE_PRIM_TYPE of this WOODE. This is the geometric shape ODE (Physics Engine)
      shall use to approximate the graphical model.
      \see ODE_PRIM_TYPE
      \return The ODE_PRIM_TYPE this WOODE is set to.
   */
   virtual Aftr::ODE_PRIM_TYPE getODEPrimType() { return this->odePrimType; }

   /**
      Sets the ODE_PRIM_TYPE of this WOODE. This is the geometric shape ODE (Physics Engine)
      shall use to approximate the graphical model.
      \see ODE_PRIM_TYPE
      \param The ODE_PRIM_TYPE this WOODE shall be set to.
   */   
   virtual void setODEPrimType( Aftr::ODE_PRIM_TYPE type ) { odePrimType = type; }

   //Wrappers Around the WOODE's graphical "Model" instance
   //Each of these virtual methods has a DEFAULT behavior (they do exactly what you would expect them to do); however, children 
   //classes may overload these to customize behavior as desired!


   /**
      Returns a Vector representing the X,Y,Z location of this WOODE in global coordinates.
      \return A Vector representing the X,Y,Z location of this WOODE in global coordinates.
   */
   virtual Vector getPosition();

   /**
      Returns a normalized vector pointing in the direction the WO is "facing". According to convention,
      the WO's "front" (think nose of airplane) points along the +X-axis, the plane's tail points towards
      the +Z-axis, and the wings lie along the Y axis. The left wing points along the +Y-axis and the right wing
      points towards the -Y-axis.

      \return Normalize vector pointing in the direction this WO is currently facing.
   */
   virtual Vector getLookDirection();
   
   /**
      Returns a normalized vector pointing perpendicular to the direction the WO is "facing". According to convention,
      the WO's "front" (think nose of airplane) points along the +X-axis, the plane's tail points towards
      the +Z-axis, and the wings lie along the Y axis. The left wing points along the +Y-axis and the right wing
      points towards the -Y-axis. This method would return a vector pointing along the +Z-axis.

      \return Normalize vector pointing in the direction perpendicular to where this WO is currently facing. The
              current roll of the object is accounted for.
   */
   virtual Vector getNormalDirection();


   /**
      Sets the X,Y,Z location of this WOODE in global coordinates.

      IMPORTANT: The user typically calls this method inside of GLViewXYZ::LoadMap() to position the WOODE 
      initially. However, once the simulation "begins" and the physics engine is running; the physics engine is responsible
      for moving this object. Thus, while the simulation is running, a call to setPosition() on a WOODE that has a corresponding \see odeID
      will result in undesired behavior.
      
      This undesired behavior will most likely be an instant acceleration towards infinity (because of speed of 
      instantaneous travel), and the associated WOODE will be "blown away" probably by several billions units.

      If you do not want to have a specific WOODE "use" or "interact" with the physics engine, use a
      \see WO or one of its child classes that best suits your needs.

      \param newXYZ A Vector representing the new X,Y,Z location of this WOODE in global coordinates.
   */
   virtual void setPosition( const Vector& newXYZ );
   virtual void setPosition( float x, float y, float z );
   virtual void moveRelative( const Vector& dxdydz );

   /** 
      Sets the X,Y,Z global coordinate of this WOODE. The children are intentionally left UNCHANGED. This method
      is invoked by the network subsystem to sync each newly received NetWO's position (from the server) to
      the local graphical representation of the WOODE stored locally on this client.
      \param newXYZ The new X,Y,Z global coordinate of only this WOODE. All The WOODE's children are UNCHANGED.
   */
   virtual void setPositionIgnoringAllChildren( const Vector& newXYZ );
   virtual void moveRelativeIgnoringAllChildren( const Vector& dxdydz );


   virtual void getDisplayMatrix( float* outParamDispMat4x4 );
   
   
   //ODE Specific
   //-------------------------------------------------------------------------
   //Methods specific for integrating this WOODE with ODE
   //Children of this class need to overload these methods so ODE can use these WorldObjects appropriately

   /**
      ONE of FOUR virtual ODE callback functions required for proper interaction with the Physics Engine.

      This method is automatically called by ODE when the user invokes the Physics Engine constructor; ie, "new's" the
      Physics Engine.

      For any children WOODE objects of this parent to have their ODE callbacks invoked, 
      the parent must push the child pointers into the 'std::vector< WO* > children' 
      list AND also into the 'std::vector< WOODE* > WOODEPtrsIntoChildren' list. The user
      must also ensure that for any additions/removals of a WOODE child to/from 
      the 'std::vector< WO* > children' list, the corresponding pointer must be added/removed
      from the 'std::vector< WOODE* > WOODEPtrsIntoChildren' list.

      This method creates the ODE representation of this WOODE. That is, this creates a body and geometric shape that is used
      by ODE for collision detection and force accumulation.
      
      For example, if this WOODE's \see ODE_PRIM_TYPE is set to a BOX, then when ODE invokes
      this method, an ODE "Box" will be created. This WOODE's \see odeID represents an "ODE Pointer" to the
      corresponding ODE body.
      
      The box's geometric dimensions will be taken from this WOODE's Model's Bounding Box. 
      The box's mass will be taken from this WOODE's mass.
      The box's initial orientation will be taken from this WOODE's Model's displayMatrix.
      Again, this WOODE's \see odeID shall point to the newly created ODE body (Physics Object).

      If you will be created complex objects with engines and joints, etc. Visit this site for the official
      ODE API documentation and VERY useful description of how ODE works:
      http://www.ode.org/ode-latest-userguide.html
   */
   virtual void ODEinitializeInODE();

   /**
      ONE of FOUR virtual ODE callback functions required for proper interaction with the Physics Engine.

      This method destroys all corresponding physics engine objects (ODE objects) created within
      \see ODEinitializeInODE(). This method does not touch any graphical elements of the WOODE
      such as the model, etc; it simply removes all bodies, joints, etc that were created when this object
      was initialized within ODE.

      This method is called in a clustered environment, when a WOODE residing on this server cluster instance
      is handed off to a neighboring server. This method essentially removes all phsyics pertaining to this
      WOODE out of the current simulation thereby reducing the computational load of this cluster server.

      Note that after invokation of this method, this WOODE should be removed from the GLView::worldLst, otherwise,
      during the next physics tick, the ODEupdateForcesAndJoints will attempt to use ODE objects (bodies, joint,etc)
      that no longer exist causing an error.

      This method only needs to be overloaded for "complex" physics objects; that is, any object that uses
      multiple ODE bodies that are connected to each other via joints.
      
      In otherwords, if the ODEinitializeInODE creates more ODE objects than the default WOODE::ODEinitializeInODE 
      method, then this method will need to be overloaded to properly destroy ALL of those ODE objects created
      in the corresponding ODEinitializeInODE.

      For any children WOODE objects of this parent to have their ODE callbacks invoked, 
      the parent must push the child pointers into the 'std::vector< WO* > children' 
      list AND also into the 'std::vector< WOODE* > WOODEPtrsIntoChildren' list. The user
      must also ensure that for any additions/removals of a WOODE child to/from 
      the 'std::vector< WO* > children' list, the corresponding pointer must be added/removed
      from the 'std::vector< WOODE* > WOODEPtrsIntoChildren' list.
   */
   virtual void ODEdestroyInODE();

   /**
      ONE of FOUR virtual ODE callback functions required for proper interaction with the Physics Engine.

      This method is automatically called by ODE JUST BEFORE an ODE physics step is taken; ie, just before the
      physics engine detects collisions and applies all the accumulated forces over the last
      time step interval.

      For any children WOODE objects of this parent to have their ODE callbacks invoked, 
      the parent must push the child pointers into the 'std::vector< WO* > children' 
      list AND also into the 'std::vector< WOODE* > WOODEPtrsIntoChildren' list. The user
      must also ensure that for any additions/removals of a WOODE child to/from 
      the 'std::vector< WO* > children' list, the corresponding pointer must be added/removed
      from the 'std::vector< WOODE* > WOODEPtrsIntoChildren' list.

      Typically, most objects will be "dumb" like a pile of bricks sitting in a playground. Because the only
      motion these type of object are exposed to are collisions, nothing needs to be performed here. In fact, typically,
      only "smart" objects like a "user controlled human avatar, car, boat, or other complex object" will need any thing
      done in this method.

      This method is responsible for updating this WOODE's force and/or joint information of the ODE Body 
      JUST BEFORE an ODE step is taken.

      For example, if this WOODE is a car, the torque force from the engines and joints attaching the wheels to 
      the car body shall be updated here.

      If the object is made up of more than one ODE Prim (This WOODE has 1 or more Children with corresponding physics objects),
      the associated joints linking the Prims together are updated in this method.

      This method will be used ONLY by ODE. Do not manually call this, otherwise ODE's state machine will become undefined.
      
      All one needs to do here is:
      1. Apply forces to the bodies as necessary.
      2. Adjust the joint parameters as necessary.

      Look at \see WOFourWheeled::ODEupdateForcesAndJoints() for an example of updating a car's wheels and
      steering column.

      If you will be created complex objects with engines and joints, etc. Visit this site for the official
      ODE API documentation and VERY useful description of how ODE works:
      http://www.ode.org/ode-latest-userguide.html
   */
   virtual void ODEupdateForcesAndJoints();

   /**
      ONE of FOUR virtual ODE callback functions required for proper interaction with the Physics Engine.

      This methos is automatically invoked by ODE JUST BEFORE a render of the world occurs; ie, all the ODE body's
      are now in their new locations AND new orientations; however, the corresponding WorldObjects now need to be
      synchronized to its corresponding ODE body.

      For any children WOODE objects of this parent to have their ODE callbacks invoked, 
      the parent must push the child pointers into the 'std::vector< WO* > children' 
      list AND also into the 'std::vector< WOODE* > WOODEPtrsIntoChildren' list. The user
      must also ensure that for any additions/removals of a WOODE child to/from 
      the 'std::vector< WO* > children' list, the corresponding pointer must be added/removed
      from the 'std::vector< WOODE* > WOODEPtrsIntoChildren' list.

      This method is responsible for updating this WOODE's position and orientation according to 
      ODE's calculations JUST BEFORE the entire world is rendered. This means that GLView->numPhysicsStepsPerRender
      ODE steps have been taken.
      
      The DEFAULT action simply updates the position and orientation of this WOODE based on the newly calculated
      position and orientation of this WOODE's corresponding ODE body. If the WOODE contains any children
      this method is invoked on them as well.

      If you will be created complex objects with engines and joints or you need to do something "non-standard" here.
      Visit this site for the official ODE API documentation and VERY useful description of how ODE works:
      http://www.ode.org/ode-latest-userguide.html
   */
   virtual void ODEupdateToNewPositionOrientation();

   //-------------------------------------------------------------------------

   //-------------------------------------------------------------------------
   //Call backs that are invoked at different times on each WOODE.

   //-------------------------------------------------------------------------

   //ODE specific data
   dBodyID odeID; ///< An ODE "pointer" to ODE's representation of this world object in the universe; ie, its physics engine component.
   dGeomID odeGeomID; ///< An ODE "pointer" to ODE's geometric representation of this world object in the universe; ie, its collision-detection component.
   dSurfaceParameters odeSurface; ///< The surface parameters of this WOODE. Useful to make stuff bouncy, slippery, hard, soft, etc...
   float mass; ///< Mass of world object used by ODE given in kilograms
   //ODE Auto disable parameters
   bool odeAutoDisableFlag;
   float odeAutoDisableLinearThreshold;
   float odeAutoDisableAngularThreshold;
   unsigned int odeAutoDisableSteps;
   float odeAutoDisableTime;
   
   //ODE Collision Properties   
   /**
      Returns true iff this WOODE is currently an ODE collision Sink; ie, it can receive a collision.
      Returns false iff this WOODE is NOT a collision sink; ie, it cannot recieve collision, BUT CAN CAUSE COLLISIONS.
      See the documentation for IsODECollisionSink protected bool inside of WOODE for detailed info.
   */
   virtual bool isODECollisionSink();
   /** Allows user to set if this instance is an ODE Collision Sink or not. */
   virtual void isODECollisionSink( bool isODECollisionSink );
   /** Sets this instance and all siblings to be an ODE Collision Sink or not. */
   virtual void isODECollisionSinkRecursive( bool isODECollisionSink );
   virtual Vector getODECollisionSourceLinearVel();
   virtual Vector getODECollisionSourceAngVel();

   ///disables the ODE bodies of me and all my children if non-zero
   virtual void makeMeAndAllMyChildrenDisabled();
   ///enables the ODE bodies of me and all my children if non-zero
   virtual void makeMeAndAllMyChildrenEnabled();

   ///zeros out the linear and angular velocity of entire family tree (downwards) if non-zero odeID
   virtual void makeMeAndAllMyChildrenStill();

   ///Teleports any WOODE with a dynamic (not static) ODE body to a new location.
   virtual void teleportMeAndAllMyChildrenTo( Vector to );

   /**
      Sets the orientation of this WOODE's physics body as well as the bodies
      of all children to the in passed odeRotationMatrix.
   */
   virtual void makeMeAndAllMyChildrenOriented(dReal* odeRotationMatrix );

   //virtual std::vector< WOODE* >* getWOODEPtrsIntoChildren() { return &this->WOODEPtrsIntoChildren; }

protected:
   
   /**
      Use WOODE::New(...) to instantiate a WOODE. The WOODE Constructors are by convention private. This 
      enables a programmer to mimic polymorphism in constructors via the static WOODE::New method
      and the onCreate() method.
   */
   WOODE();

   virtual void onCreate();
   virtual void onCreate( const std::string& modelFileName, Vector scale = Vector(1,1,1), MESH_SHADING_TYPE shadingType = MESH_SHADING_TYPE::mstAUTO );

   virtual void toFileConstructorParams( std::ostream& sout );
   virtual void toFileWOState( std::ostream& sout );
   static ThisClassType* fromFileConstructorParams( std::istream& sin );
   virtual bool fromFileWOState( std::istream& sin );
   bool fromFileWOStateHelper( std::istream& sin, std::string& inLhs, std::string& inRhs );

   /**
      A WOODE may have some children that are of type WOODE or a decendent; it also may have
      some children that are a WO or some non WOODE decedent of WO. All of these children
      are stored inside of the children vector. This vector maintains pointers into the
      children vector that are of type WOODE or a WOODE decendent; this list is then used
      to invoke all necessary ODE callbacks at the appropriate time. The user is responsible
      for pushing a pointer into this list at the same time he pushes a pointer into the
      children list. The user is also responsible for properly removing invalid pointers
      from this list.
   */
   //std::vector< WOODE* > WOODEPtrsIntoChildren;

   /// Recursive method used by teleportMeAndAllMyChildrenTo( Vector to ) to take care of all children
   void recurseTeleportMeAndAllMyChildrenTo(Vector to, Vector from);


   virtual void initODESurface(); ///< Sets initial values for odeSurface of this object. Used during ODE collisions.
   void initODEAutoDisableParams(); ///< Sets ODE auto disable parameters
   /**
      If true, this WOODE can be 'hit' by any other WOODE (via ODE collision) provided that
      the other WOODE has a valid ODE body participating in the simulation. This is true by default
      and provides typical 'real-world' behavior between two objects when they collide. That is,
      if two spheres of equal mass collide, both trajectories will be effected by the collision.

      If false, this WOODE cannot be 'hit' by any other WOODE via ODE's collision detection. However,
      this WOODE can 'hit' other WOs. An ODE collision will be generated and a corrective force will
      be applied against the WOODE with which this instance collided. In other words, assume two spheres
      A and B, of equal mass collide. Let sphere A have this flag set to false and let sphere B 
      have this flag set to true. Upon collision, B will bounce off of A and adjust its trajectory
      accordingly; A will continue along its original trajectory completely unaffected by B's
      interaction with A.
   */
   bool IsODECollisionSink;
   
   /**
      Only used iff(IsODECollisionSink == false). This is only used when this WOODE's dBody is not
      being used (perhaps because it is a WOStatic, or the user overloaded the ODE callback behavior).
      
      This specifies the linear velocity the ODE Collision geometry is 'traveling when another
      object collides into the geometry. This handles how friction between the two object is computed. 
      
      Imagine this WOODE is a large platform that is flying through 3-space completely unaffected by all
      physics, but still needs to be able to have other objects collide against it. All objects sitting
      on the moving platform must have friction against the platform, otherwise, the platform would slide
      out from beneath them. Thus, this parameter should be set equal to the velocity the platform is 
      moving through space. This way, all objects on the platform will travel w/ the platform as expected,
      assuming the coefficient of friction is properly set.

      The x,y,z values of this vector specify the corresponding velocity in m/s w.r.t to the global reference frame.

      This platform is most likely a WOStatic or it could be a regular WOODE that has
      an overloaded ODEupdateForcesAndJoints method.
   */
   Vector odeCollisionSourceLinearVel;
   /**
      This is analogous to the odeCollisionSourceLinearVel except this specifies the angular velocities.
      The x,y,z values of this vector specify the corresponding rotation in rad/sec w.r.t to the global reference frame.
   */
   Vector odeCollisionSourceAngVel;


   ODE_PRIM_TYPE odePrimType; ///< The \ODE_PRIM_TYPE of this WOODE. This is used by the physics engine to determine the geometric shape used for collision detection.
};

/** \} */

} //namespace Aftr

#endif //AFTR_CONFIG_USE_ODE
