/**
 @file Entity.h

 Dojo Game Engine

 Copyright 2005, Morgan McGuire
 All rights reserved.
 */
#ifndef DOJO_ENTITY_H
#define DOJO_ENTITY_H

#include "g3dhelper.h"
#include "GraphicsData.h"
#include "NamedArray.h"
#include <ode/ode.h>

namespace dojo {

class World;

typedef ReferenceCountedPointer<class Entity> EntityRef;

/** An object in the World that is simulated and poseable. 

  To create a new kind of object:
  

  - Subclass Entity
  - In the constructor, call the addPart(), addPhysicsGeometry(), and addGraphicsGeometry() methods.

 */
class Entity : public ReferenceCountedObject {
public:

    static const bool MOVABLE  = true;
    static const bool FIXED    = false;

    static const bool STILL    = false;
    static const bool ANIMATED = true;

    static const bool GROUND   = true;

    friend class World;
    friend class PhysicsData;

    /** Used temporarily while sorting */    
    float                       sortKey;

public:

    class Part {
    public:
        
        /** Back pointer to the containing Entity. */
        Entity*                 entity;

    private:

        friend Entity;
        friend World;

        const std::string       m_name;

        /** Owned by the Part and deleted on destruction. */
        Array<GraphicsData*>    graphicsArray;

        /** NULL if this is the root. */
        Part*                   parent;

        /** Array of children. */
        Array<Part*>            childArray;

        /** Angular velocity in radians/s about the center of mass, 
            in world space. Updated by afterSimulation. */
        Vector3                 m_angularVelocity;
        
        /** In world space. Updated by afterSimulation. */
        Vector3                 m_linearVelocity;

        /** Object to world space transformation.  Note that this does <b>not</b> map to the ODE body space
            or to the parent's frame.

            Updated by afterSimulation by offsetting ODE's current transformation by 
            m_massOffset in body space.*/
        CoordinateFrame         m_frame;

        /** Object space offset of the center of mass (ODE frame) from m_frame. 
            Set during createODEGeometry.*/
        Vector3                 m_massOffset;

        float                   m_mass;

        /** The mass must be centered at the origin after createODEGeometry.*/
        dMass                   odeMass;

        /** ID of the ODE object corresponding to this entity. 
            The userdata of the body is a back-pointer to this object.
        */
        dBodyID                 body;

        /** If this is a trimesh, the ID, otherwise NULL */
        dTriMeshDataID          triMeshDataID;

        /** ODE geometry that matches the G3D physics model */
        dGeomID                 odeGeometry;

        /** Physics uses simplified models for objects.        
            Deleted on destruction.*/
        Shape*                  g3dGeometry;

        Part(const std::string& name, Entity* e);

        /** The shape will be owned by this object after it is inserted. */
        void addRelativeGeometry(
            Shape*               bsShape,
            float                density);

        /**
        Sets the following fields: odeGeometry, body, m_massOffset
        The following have already been set: entity, odeMass (in body space), m_frame, m_linearVelocity, m_angularVelocity
        // TODO: pass in a new mass and update odeMass
        Called from createODEGeometry only.
        */
        void createRelativeGeom(
            dGeomID                 encapsulatedGeom, 
            const CoordinateFrame&  bodyToObject, 
            dSpaceID                space);

        /** */
        void createODEGeometry();

        /** Copies all information from G3D to ODE if odeDirty is true.
            Executes at the beginning of simulation.*/
        void beforeSimulation();

        /** Copies velocity and position data from ODE to G3D. 
            Executes at the end of simulation.*/
        void afterSimulation();

        /** Called during onGraphics to reduce this object to a set of PosedModels for rendering. */
        void pose(Array<PosedModelRef>& posedModels);

        /** Multiplies the local frame by c (moving ODE objects appropriately)
        and then recurses into children. */
        void multiplyFrameRecurse(const CoordinateFrame& c);

        /** Invokes on its own PhysicsData and then calls 
             recursively on all of its children. */
        void beforeSimulationRecurse();

        /** Invokes on its own PhysicsData and then calls 
             recursively on all of its children. */
        void afterSimulationRecurse();

        void renderPhysicsModel(RenderDevice* rd) const;

        void drawLabels(RenderDevice* rd);

    public:

        /** Destroys ODE objects */
        ~Part();

        /** Returns one of the graphics for this part*/
        GraphicsData* graphicsData(int i = 0) {
            return graphicsArray[i];
        }

        inline const std::string& name() const {
            return m_name;
        }

        void setMass(float m);

        /** Equivalent to setting the mass. */
        void setDensity(float d);

        inline float mass() const {
            return m_mass;
        }

        /** Returns the world space position of the center of mass, which is not
            necessarily the center of the reference frame.*/
        Vector3 centerOfMass() const {
            return m_frame.pointToWorldSpace(m_massOffset);
        }

        /** In world space */
        inline const Vector3& linearVelocity() const {
            return m_linearVelocity;
        }

        /** In world space */
        inline const Vector3& angularVelocity() const {
            return m_angularVelocity;
        }

        void setLinearVelocity(const Vector3& v);

        void setAngularVelocity(const Vector3& v);

        /** In world space.  <b>Avoid setting the frame explicitly
            after an object has been created.</b>  Instead, apply forces 
            (or set the velocity) to move it.
            
            The problem with calling setFrame is that the object warps to
            the new position and appears there with no additional velocity.
            If collisions should have occured between the old and new position
            they will be missed entirely.  If the new position produces interpenetration
            then it will be resolved, but not by applying the momentum from the movement.            
            */
        void setFrame(const PhysicsFrame& p);


        /** In world space.  <b>Avoid setting the frame explicitly
            after an object has been created.</b>  Instead, apply forces 
            (or set the velocity) to move it.
            
            The problem with calling setFrame is that the object warps to
            the new position and appears there with no additional velocity.
            If collisions should have occured between the old and new position
            they will be missed entirely.  If the new position produces interpenetration
            then it will be resolved, but not by applying the momentum from the movement.          
            */
        void setFrame(const CoordinateFrame& c);

        /** In world space */
        const CoordinateFrame& frame() const {
            return m_frame;
        }

        /** Apply a world-space force to this part at its center of mass.
            Units are Newtons.*/
        void applyForce(const Vector3& ws_F);

        /** Apply a world-space torque to this part.  Units are Newton-meters.*/
        void applyTorque(const Vector3& ws_T);

        /** Apply an object-space force to this part at its center of mass.*/
        void applyRelForce(const Vector3& os_F);
        void applyRelTorque(const Vector3& os_T);

        void applyForceAtPosition(const Vector3& ws_F, const Vector3& os_P);
        void applyRelForceAtPosition(const Vector3& os_F, const Vector3& ws_P);
        void applyRelForceAtRelPosition(const Vector3& os_F, const Vector3& os_P);
        void applyForceAtRelPosition(const Vector3& ws_F, const Vector3& os_P);
    };

    /** Used by World::rayCast. This must be here because Part is not exposed by Entity.*/
    class RayCast {
    public:
        Entity*         ignore;

        /** Distance to closest intersection */
        float           distance;

        /** Normal at the first intersection. */
        Vector3         normal;

        /** Part hit */
        Entity::Part*   part;

        /** Entity hit */
        Entity*         entity;

        RayCast();

        /** Called when two objects <b>might</b> intersect.  This will be called at some point with
            (ray, ray), but dCollide always returns 0 for that case anyway.*/
        static void odeCallback(void* instance, dGeomID ray, dGeomID geom);
    };

private:

    /** True if this object has physics geometry (it still might not be simulated, just provide a collision
	    surface). */
    bool                        hasPhysics;

    dSpaceID                    spaceID;

    class Joint {
    public:
        const std::string       name;
        dJointID                ID;

        /** AMotor */
        dJointID                ID2;
    
        /** Rough scale of the joint, used for debug rendering. */
        const float             size;

        Joint(const std::string& n, const float s);
        ~Joint();

        /** For debugging */
        void render(RenderDevice* rd) const;
    };

    /** The root part is also in the partArray. */
    Part*                       rootPart;

    /** Extended by create joint.  Deleted on destruction.*/
    NamedArray<Joint*>          jointArray;

    /** Must be true if this model ever changes location or shape, e.g., a rotating
        gun turret can move.  If false, the physics and rendering systems may
        optimize the model based on its static position. 
        
        An Entity with isSimulated = false can still be moved via setFrame.
        */
    const bool                  m_isSimulated;

    const bool                  m_partsCanAnimate;

	/** True for m_isSimulated == false objects for one frame after they have been
	    moved with setFrame.  This allows 
		
		Set by setFrame, unset by afterSimulation.  This allows the World to enable
		bodies that are hit by a moving piece of non-simulated geometry.
		*/
	bool						m_justMoved;

    float                       m_mass;

    const std::string           m_name;

    const bool                  m_isGround;

    /** Called by the World before simulation to update all ODE data. */
    void beforeSimulation();

    /** Called by World. Copies velocity and position data from ODE to G3D. 
        Executes at the end of simulation.*/
    void afterSimulation();

    /** Recursively set the frame of p and move its children. 
        Called from other setFrame routines.*/
    void setFrame(Part* p, const CoordinateFrame& wsC);

protected:

	NamedArray<Part*>           partArray;
    /** Once initialized, the object cannot be added to. This is set by the world. */
    bool                        inWorld;

    /////////////////////////////////////////////////////////
    // Call these routines to build your Entity.

    /** 
     @param isSimulated If false, this object is not moved by simulation of forces and velocities.   It can still be moved with Entity::setFrame.

     @param partsCanAnimate If false, the individual parts cannot change shape.
     @param isGround If true shadows, collisions, and debug rendering are handled specially for 
     this object.  Should only be set for one Entity per scene.  isSimulated must be false and canAnimate must be false.
    */
    Entity(
        const std::string&      name, 
        const std::string&      rootPartName        = "Root", 
        bool                    isSimulated         = true,
        bool                    partsCanAnimate     = true,
        bool                    isGround            = false);

    /** 
      Add a new part to this model.  Parts must form a tree
      
      @param parentName Name of the parent. If empty, the parent will be the root.

      @param localFrame Reference frame of this part relative to its parent's reference frame
      Geometry will be transformed by the product of this reference frame and all
      ancestors's reference frames when it is first added.
     */
    void addPart(
        const std::string&      partName,
        const std::string&      parentName      = "",
        const CoordinateFrame&  localFrame      = CoordinateFrame());

    /**
      Append visible geometry to a named part.  The graphicsModel will be appended to the 
      part at its <i>body space</i> position (i.e., in the local reference frame of the part).
      Since most models are centered on the origin, the optional @a transform allows the models 
      to be glued to the part away from the part's local origin.
     */
    template<class ModelTypeRef>
    void addGraphicsGeometry(
        const std::string&      partName,
        const ModelTypeRef&     graphicsModel,
        const CoordinateFrame&  transform = CoordinateFrame()) {

        Part* part = partArray[partName];

        GraphicsData* graphics = new GraphicsData(graphicsModel);
        graphics->cframe = transform;
        part->graphicsArray.append(graphics);
    }

    void addGraphicsGeometry(
        const std::string&      partName,
        const MD2ModelRef&      graphicsModel0,
        const TextureRef&       texture0,
        const MD2ModelRef&      graphicsModel1 = NULL,
        const TextureRef&       texture1 = NULL,
        const CoordinateFrame&  transform = CoordinateFrame());

    /** Append physics geometry to an existing part. The simulation
        will be more realistic if the physics geometry closely matches
        the shape of the graphics geometry, however they are not constrained
        to be the same.  It is common practice to use spheres and cylinders to
        approximate complex geometry.

        More than one piece of geometry can be attached to the same
        part (TODO: although not in this build!).

        Collisions are ignored between:
        
        <ul>
        <li>all physics geometry within the same part
        <li>two parts that are connected by a joint.
        <li>any pair of cylinders (a limitation of the underlying ODE library)
        </ul>
        
        A mesh can only be used on an entity for which Entity::isSimulated is false;
        it is intended primarily for terrains and building.

        @param bsShape The shape, body space space (i.e., the local reference frame of the part).  
        Will be owned and deleted by the Entity at destruction.  In terms of simulation speed, 
        the shapes from fastest to slowest are: G3D::SphereShape, G3D::PlaneShape, G3D::CapsuleShape,
        G3D::BoxShape, G3D::CylinderShape, G3D::MeshShape.

        @param density Density in kg/m^3.  Density is more convenient than mass
         because it is independent of the size of the object.
      */
    void addPhysicsGeometry(
        const std::string&      partName,
        Shape*                  bsShape,
        float                   density);

    /** 
       Creates a joint that constrains two joints to only rotate about a single axis and
       not translate at all with respect to one another.  This is useful as a car axle
       or as a human knee. The hinge is automatically connected to a motor that can
       target specific positions or velocities.       

       Do not call after insertion into the World.

       Both parts must already be added.
       The hinge axis and anchor are in the body space of part1.

       Collisions are automatically disabled between parts that are connected to one another
       (this allows interpenetration near the joint without creating instability).
       Use joint limits to prevent parts from swinging too far.

       The position of the anchor along the axis does not matter for a hinge joint.

       The hinge is anchored to the first part.

       @param size The rough scale of the joint for debug rendering
       @param maxTorque The maximum torque that the motor can exert in N*m.  Set to zero to disable
       the motor.
      */
    void addHinge(
        const std::string&      jointName,
        const std::string&      part1Name,
        const std::string&      part2Name,
        const Vector3&          bsAxis,
        const Vector3&          bsAnchor,
        float                   loLimit = -dInfinity,
        float                   hiLimit = dInfinity,
        float                   maxTorque = 0,
        float                   scale = 0.1);

    /** Axes 1 and 2 should be perpendicular */
    void addUniversalJoint(
        const std::string&      jointName,
        const std::string&      partAName,
        const std::string&      partBName,
        const Vector3&          bsAxis1,
        const Vector3&          bsAxis2,
        const Vector3&          bsAnchor,
		float                   maxTorque,
		const float				loLimit1 = -inf(),
		const float				loLimit2 = -inf(),
		const float				hiLimit1 = inf(),
		const float				hiLimit2 = inf(),
        float                   scale = 0.1);

	/** @param loLimitN Joint limit around axis N */
    void addBallJoint(
        const std::string&      jointName,
        const std::string&      partAName,
        const std::string&      partBName,
        const Vector3&          bsAnchor,
		float                   maxTorque,
		const float				loLimit1 = -inf(),
		const float				loLimit2 = -inf(),
		const float				loLimit3 = -inf(),
		const float				hiLimit1 = inf(),
		const float				hiLimit2 = inf(),
		const float				hiLimit3 = inf(),
        float                   scale = 0.1);

    /**
      The hinge will use up to its maxTorque to attempt to reach and
      maintain this velocity in radians/sec.
     */
    void setHingeTargetVelocity(
        const std::string&  jointName,
        float               targetVel);

	public:

    inline Part* part(const std::string& partName) {
        return partArray[partName];
    }

    const inline Part* part(const std::string& partName) const {
        return partArray[partName];
    }

    /** True if this is the ground plane and must be rendered specially. 
      */
    inline bool isGround() const {
        debugAssert(! (m_isSimulated && m_isGround));
        return m_isGround;
    }

    inline bool hasPart(const std::string& partName) const {
        return partArray.containsKey(partName);
    }

    inline bool hasJoint(const std::string& jointName) const {
        return jointArray.containsKey(jointName);
    }

    inline bool isSimulated() const {
        return m_isSimulated;
    }

    inline bool partsCanAnimate() const {
        return m_partsCanAnimate;
    }

    inline const std::string& name() const {
        return m_name;
    }

    ~Entity();

    /** Root coordinate frame. */
    inline const CoordinateFrame& frame() const {
        return rootPart->frame();
    }

    /** Set the world-space position of the root (the rest of the Entity will
        be transformed so as to preserve joint positions).
    
        <b>Avoid setting the frame explicitly
        after an Entity has been created.</b>  Instead, apply forces 
        (or set the velocity) to move it.
        
        The problem with calling setFrame is that the object warps to
        the new position and appears there with no additional velocity.
        If collisions should have occured between the old and new position
        they will be missed entirely.  If the new position produces interpenetration
        then it will be resolved, but not by applying the momentum from the movement.          
        */
    void setFrame(const CoordinateFrame& c);

    /** Set the world-space position of the part (its children will
        be transformed so as to preserve joint positions).

        Using this method is not recommended. Applying forces/torques and setting velocities/angularVelocities
        is preferrable because it produces better interaction between objects.

        If you move a part in a way that violates a joint constraint the simulation
        will apply forces to restore the constraint.
        */
    void setFrame(const std::string& partName, const CoordinateFrame& wsC);

    /** Set the position of the part relative to its parent (its children will
        be transformed so as to preserve joint positions).

        Calling this on the root is the same as calling setFrame.

        Using this method is not recommended. Applying forces/torques and setting velocities/angularVelocities
        is preferrable because it produces better interaction between objects.

        If you move a part in a way that violates a joint constraint the simulation
        will apply forces to restore the constraint.

        @sa setFrame
        */
    void setRelFrame(const std::string& partName, const CoordinateFrame& osC);

    /** Renders labels on the sub-parts */
    void drawLabels(RenderDevice* rd);

    /** Total mass of all parts. */
    const float mass() const {
        return m_mass;
    }

    /** For debugging.  This draws the physics model using comparatively slow G3D::Draw calls. */
    void renderPhysicsModel(RenderDevice* rd);

    /** Called by World during onGraphics to reduce this object to a set of PosedModels for rendering. */
    void pose(Array<PosedModelRef>& posedModels);

    /** Apply a world-space force to this part at its center of mass.
        Units are Newtons.*/
    void applyForce(const std::string& part, const Vector3& ws_F);

    /** Apply a world-space torque to this part.  Units are Newton-meters.*/
    void applyTorque(const std::string& part, const Vector3& ws_T);

    /** Applies sufficient torque to make this object balance with its 
        root y-axis pointing straight up.  If the object contains non-root parts
        they will be dragged along by their joints, but not have torques 
        directly applied.

        Friction can make it difficult for an object to balance using only
        torque.  See also applyLevitateForce, which maintains a vertical distance
        between an object and a surface below it.  A good way to represent
        a simple character in a game is to create the physics geometry of a Capsule that 
        levitates slightly above the ground and balances.  Note that the graphics geometry
        can still appear to have feet in contact with the ground.  Also, the balance and
        levitation are only applied at the root--other simulation or animation can be independently
        applied at the character's limbs.

        @param dt Simulation time step, which is needed to scale the impulse

        @param magnitude Depending on the moment of inertia of the body and the
         frictional forces in the system, the torque applied may be 
         scaled inappropriately.  If you encounter a body that is too slow to balance
         or that overcompensates and either swings or becomes unstable, 
         alter this constant.
     */
    void applyBalanceTorque(SimTime dt, float magnitude = 1);

    /** 
      Applies sufficient force to keep object-space point @a osOffset on the
      root part at @a height above any surface vertically below that point.  If the point is already
      above @a height, no force is applied.
      
      It is frequently useful to apply several levitation forces around the base
      of an object to prevent falling through small holes.

      @param height Desired vertical height between osOffset and the surface 
       (which will be determined by a ray-cast) immediately below it.
     */
    void applyLevitateForce(
        SimTime         dt, 
        float           height, 
        const Vector3&  osOffset = Vector3::zero(), 
        float           magnitude = 1);

    /** Returns the world space velocity of the named part. */
    inline const Vector3& velocity(const std::string& part) {
        return partArray[part]->linearVelocity();
    }

    /** Returns the world space angular velocity of the named part. */
    const Vector3& angularVelocity(const std::string& part) {
        return partArray[part]->angularVelocity();
    }

	void setLinearVelocity(const std::string& part,const Vector3& ws_F);

	void setAngularVelocity(const std::string& part,const Vector3& ws_F);

    /**
     Name a class of objects; e.g., "Player", "Powerup", etc. so that
     they can be detected during the onCollision event.

     This is recommended but not required to be the name of the Entity subclass.
     Default returns "Entity".
     */
    virtual const std::string& className() const {
        static const std::string n = "Entity";
        return n;
    }

    /** 
      Invoked by the collision detection system when two objects come into contact.
      Override to implement your own collision reactions (e.g., damage objects).

      This method will be called on each of the objects involved in the collision.

      Return true if the collision should be processed, false if the physics system
      should ignore the collision.  The collision will be ignored if <b>either</b>
      of the objects wants to ignore it.  This veto resolution is 
      necessary because most static objects in the world (e.g., ground plane) are 
      implemented without an awareness of the other objects.

      Note that if an Entity is destroyed in this method, the actual destruction will
      not occur until the following timestep so that its other events may be processed.
      */
    virtual bool onCollision(
        const std::string&  myPart, 
        const EntityRef&    other, 
        const std::string&  otherPart,
        const Vector3&      wsLocation) {
        
        return true;
    }

    /** 
      Event invoked immediately before the time step is simulated. Override to add forces
      inherent in this object, e.g. balancing force.
      Invoked on all objects, even if they cannot move.  Not invoked when the system is paused.

      @internal
      Called <b>before</b> Entity::beforeSimulation
      */
    virtual void onSimulation(SimTime timeStep) {}
};

} // namespace

#endif

