/**
 @file Entity.cpp

 Dojo Game Engine

 Copyright 2005, Morgan McGuire
 All rights reserved.
 */
#include "dojo/Entity.h"
#include "dojo/ASFModel.h"
#include "dojo/Entity.h"
#include "dojo/World.h"
#include "dojo/odeHelper.h"

namespace dojo {

Entity::Entity(
    const std::string&      name, 
    const std::string&      rootPartName, 
    bool                    isSimulated,
    bool                    partsCanAnimate,
    bool                    isGround) :
        partArray("part"),
        jointArray("joint"),
        m_name(name), 
        m_isSimulated(isSimulated), 
        m_partsCanAnimate(partsCanAnimate), 
        m_isGround(isGround), 
        inWorld(false), 
        hasPhysics(false),
        m_mass(0),
        spaceID(World::world()->physics.spaceID) {

    alwaysAssertM(rootPartName != "", "Root part name must be non-empty");
    rootPart = new Part(rootPartName, this);

    partArray.append(rootPart->name(), rootPart);
}


Entity::~Entity() {
    partArray.deleteAll();
    jointArray.deleteAll();
}


void Entity::addGraphicsGeometry(
    const std::string&      partName,
    const MD2ModelRef&      graphicsModel0,
    const TextureRef&       texture0,
    const MD2ModelRef&      graphicsModel1,
    const TextureRef&       texture1,
    const CoordinateFrame&  transform) {

    Part* part = partArray[partName];

    GraphicsData* graphics = new GraphicsData(graphicsModel0, texture0, graphicsModel1, texture1);
    graphics->cframe = transform;
    part->graphicsArray.append(graphics);
}


void Entity::addPart(
    const std::string&      partName,
    const std::string&      _parentName,
    const CoordinateFrame&  localFrame) {

    alwaysAssertM(! inWorld, "Cannot add to an Entity that has been inserted into the World.");
    alwaysAssertM(partName != "", "Part name must be non-empty");

    std::string parentName = _parentName;

    if (parentName == "") {
        parentName = rootPart->name();
    }

    Part* parent = partArray[parentName];

    Part* part = new Part(partName, this);
    partArray.append(part->name(), part);

    // Connect the parent and child
    part->parent = parent;
    parent->childArray.append(part);

    part->setFrame(parent->frame() * localFrame);
}


void Entity::addPhysicsGeometry(
    const std::string&      partName,
    Shape*                  bsShape,
    float                   density) {

    alwaysAssertM(! inWorld, "Cannot add to an Entity that has been inserted into the World.");
    Part* part = partArray[partName];

    /*
    if (bsShape->type() == Shape::CAPSULE) {
        // Temporary hack
        const Capsule& c = bsShape->capsule();
        Shape* s2 = new SphereShape(Sphere(Vector3::zero(), c.getRadius())); 
        part->addRelativeGeometry(s2, density);
    } else if (bsShape->type() == Shape::BOX) {
        // Temporary hack
        const Box& c = bsShape->box();
        Shape* s2 = new SphereShape(Sphere(Vector3::zero(), c.extent(0)/2)); 
        part->addRelativeGeometry(s2, density);
    } else */
        part->addRelativeGeometry(bsShape, density);
    

    m_mass += part->mass();

    hasPhysics = true;
}


void Entity::pose(Array<PosedModelRef>& posedModels) {
    alwaysAssertM(inWorld, "Must insert Entitys into the World after construction.");

    for (int p = 0; p < partArray.size(); ++p) {
        partArray[p]->pose(posedModels);
    }
}


void Entity::drawLabels(RenderDevice* rd) {
    const CoordinateFrame& cframe = frame();
    for (int i = 0; i < partArray.size(); ++i) {
        partArray[i]->drawLabels(rd);
    }
}

void Entity::renderPhysicsModel(RenderDevice* rd) {
    for (int j = 0; j < jointArray.size(); ++j) {
        jointArray[j]->render(rd);
    }

    for (int p = 0; p < partArray.size(); ++p) {
        partArray[p]->renderPhysicsModel(rd);
    }
}


void Entity::beforeSimulation() {
    if (isSimulated()) {
        rootPart->beforeSimulationRecurse();
    }
}


void Entity::afterSimulation() {
	m_justMoved = false;
	rootPart->afterSimulationRecurse();
}


void Entity::setFrame(const CoordinateFrame& wsC) {
    setFrame(rootPart, wsC);
}


void Entity::setFrame(Part* p, const CoordinateFrame& wsC) {

	if (! m_isSimulated) {
		m_justMoved = true;
	}

    // Only change if there is a difference from the current frame.
    // This helps avoid incremental error as well as recursion through
    // children in cases where the frame is repeatedly set to the same
    // value.
    if (wsC != p->frame()) {
        // Take us out of the old frame and into the new one
        CoordinateFrame change(wsC * p->frame().inverse());

        // Fix any remaining numerical error from the inverse operation.
        change.rotation.orthonormalize();

        p->multiplyFrameRecurse(change);
    }
}


void Entity::setFrame(const std::string& partName, const CoordinateFrame& wsC) {
    setFrame(part(partName), wsC);
}


void Entity::setRelFrame(const std::string& partName, const CoordinateFrame& osC) {
    Part* p = part(partName);

    if (p == rootPart) {
        // The object space of the root's parent is world space
        setFrame(p, osC);
    } else {
        alwaysAssertM(p->parent, "Internal error: non-root with no parent!");
        // Compute the net transformation
        setFrame(p, p->parent->frame() * osC);
    }
}


///////////////////////////////////////////////////////////////////////////

void Entity::applyForce(const std::string& part, const Vector3& ws_F) {
    partArray[part]->applyForce(ws_F);
}

void Entity::setLinearVelocity(const std::string& part,const Vector3& ws_F) {
	partArray[part]->setLinearVelocity(ws_F);
}

void Entity::setAngularVelocity(const std::string& part,const Vector3& ws_F) {
	partArray[part]->setAngularVelocity(ws_F);
}


void Entity::applyTorque(const std::string& part, const Vector3& ws_T) {
    partArray[part]->applyTorque(ws_T);
}


void Entity::applyBalanceTorque(SimTime dt, float magnitude) {

    // Remove all angular velocity not about Y
    Vector3 currentV = rootPart->angularVelocity();
    currentV.x = 0;
    currentV.z = 0;
    rootPart->setAngularVelocity(currentV);

    // Direction of root Y
    Vector3 current = rootPart->m_frame.rotation.getColumn(1);

    // Goal direction
    const Vector3& target = Vector3::unitY();

    // Rotation axis.  The length is proportional to the 
    // (sine of the) distance between the current and target,
    // which is what we want.


    // The magnitude here depends on the friction that we'll experience at 
    // the base.
    Vector3 T = current.cross(target) * mass() * magnitude * 5.0f / dt;

    float L = T.length();
    if (L > 0.0000001) {
        rootPart->applyTorque(T);
    }
}

void Entity::applyLevitateForce(
    SimTime         dt, 
    float           desiredHeight, 
    const Vector3&  osOffset, 
    float           magnitude) {

    Vector3 wsOffset = frame().pointToWorldSpace(osOffset);

    float actualHeight;

    World::RayCastHit hit;
    World::world()->rayCast(wsOffset, -Vector3::unitY(), hit, this);
    actualHeight = hit.distance;

    if (actualHeight < desiredHeight) {
        // Euler integration equation:
        //
        //   v1 = v0 + (gravity + F) * dt / m
        //   dy = vf * dt
        //
        // Choose F to meet desired dy and 

        // Desired velocity
        float desiredV = (desiredHeight - actualHeight) / dt;
        float actualV = rootPart->linearVelocity().y;
    
        float G = World::world()->gravity().y;

        // Achieve a fraction of the desired velocity, and overcome gravity
        float F = magnitude * G3D::min(desiredV - actualV, 0.5f) * mass() / dt - G;

        rootPart->applyForce(Vector3::unitY() * F);
    }
}


///////////////////////////////////////////////////////////////////////////

Entity::Part::Part(const std::string& name, Entity* e) : 
    m_name(name), 
    entity(e), 
    parent(NULL),
    g3dGeometry(NULL), 
    odeGeometry(NULL),
    body(NULL),
    triMeshDataID(NULL),
    m_mass(0),
    m_linearVelocity(Vector3::zero()), 
    m_angularVelocity(Vector3::zero()) {
}


Entity::Part::~Part() {
    if (odeGeometry != 0) {
        if (body != NULL) {
            dBodyDestroy(body);
            body = NULL;
        } else {
            dGeomDestroy(odeGeometry);
        }
        odeGeometry = NULL;
    }

    if (triMeshDataID != NULL) {
        dGeomTriMeshDataDestroy(triMeshDataID);
        triMeshDataID = NULL;
    }

    delete g3dGeometry;
    graphicsArray.deleteAll();
}


void Entity::Part::drawLabels(RenderDevice* rd) {
    for (int g = 0; g < graphicsArray.size(); ++g) {
        graphicsArray[g]->drawLabels(rd, frame());
    }
}


void Entity::Part::pose(Array<PosedModelRef>& posedModels) {
    for (int g = 0; g < graphicsArray.size(); ++g) {
        graphicsArray[g]->pose(posedModels, frame());
    }
}

void Entity::Part::multiplyFrameRecurse(const CoordinateFrame& change) {
    setFrame(change * frame());
    for (int c = 0; c < childArray.size(); ++c) {
        childArray[c]->multiplyFrameRecurse(change);
    }
}


void Entity::Part::beforeSimulationRecurse() {
    beforeSimulation();
    for (int i = 0; i < childArray.size(); ++i) {
        childArray[i]->beforeSimulationRecurse();
    }
}

void Entity::Part::afterSimulationRecurse() {
    afterSimulation();
    for (int i = 0; i < childArray.size(); ++i) {
        childArray[i]->afterSimulationRecurse();
    }
}


void Entity::Part::addRelativeGeometry(
    Shape*                  bsShape,
    float                   density) {

    g3dGeometry = bsShape;
    m_mass = density * g3dGeometry->volume();    
    createODEGeometry();
}


void Entity::Part::setFrame(const CoordinateFrame& f) {
    m_frame = f;

    if (g3dGeometry == NULL) {
        // Don't try to change the position of an empty
        // part.
        return;
    }

    if (entity->isSimulated()) {
        debugAssert(body != NULL);
        dBodyEnable(body);

        // Normal case; we have a body.
        // Compute the *body frame* that corresponds to this object frame
        CoordinateFrame bodyFrame(m_frame.rotation, m_frame.translation + m_frame.rotation * m_massOffset);

        dBodySetPositionAndRotation(body, bodyFrame);

    } else if (g3dGeometry->type() == Shape::PLANE) {
        // Planes are not placeable.  We must destroy and regenerate the geometry.
        // Destroying a geom automatically removes it from the space.
        dGeomDestroy(odeGeometry);
        odeGeometry = NULL;
        createODEGeometry();
    } else {
        // Some placeable geom that is not in a body.  Just move it.
        CoordinateFrame bodyFrame(m_frame.rotation, m_frame.translation + m_frame.rotation * m_massOffset);
        dGeomSetPositionAndRotation(odeGeometry, bodyFrame);
    }
}

void Entity::Part::beforeSimulation() {
}


void Entity::Part::afterSimulation() {
    if (body != NULL) {
        dBodyGetLinearAndAngularVel(body, m_linearVelocity, m_angularVelocity);

        // Get the center of mass position
        dBodyGetPositionAndRotation(body, m_frame);

        // The object frame is shifted from the center of mass but has
        // the same rotation matrix.
        m_frame.translation -= m_frame.rotation * m_massOffset;
    }
}


void Entity::Part::createRelativeGeom(
    dGeomID                 encapsulatedGeom, 
    const CoordinateFrame&  bodyToObject, 
    dSpaceID                space) {
    
    dWorldID world = World::world()->worldID();
    bool isSimulated = entity->isSimulated();

    dGeomSetData(encapsulatedGeom, this);

    // Position our geom in object space as specified
    dGeomSetPositionAndRotation(encapsulatedGeom, bodyToObject);

    // We always create a transform geom to wrap the underlying
    // geom.  This allows arbitrary orientation relative to body 
    // space.
    odeGeometry = dCreateGeomTransform(space);
    dGeomTransformSetCleanup(odeGeometry, 1);
    dGeomTransformSetInfo(odeGeometry, 1); // Return the transform's body when interrogated
    dGeomTransformSetGeom(odeGeometry, encapsulatedGeom);

    if (isSimulated) {
        // Create the body
        body = dBodyCreate(world);

        // Temporarily take the mass from its local frame to object space.
        dMassToWorldSpace(&odeMass, bodyToObject);

        // We must now transform this object so that its center of mass
        // is at the ODE body origin.  We didn't do this during creation
        // since in the future we will add multiple geoms to the same part 
        // and need to correct all at once.
        m_massOffset.x = odeMass.c[0]; m_massOffset.y = odeMass.c[1]; m_massOffset.z = odeMass.c[2];
        dMassTranslate(&odeMass, -m_massOffset);

        debugAssert(G3D::fuzzyEq(odeMass.c[0], 0) &&
                    G3D::fuzzyEq(odeMass.c[1], 0) &&
                    G3D::fuzzyEq(odeMass.c[2], 0));

        // Move the underlying geom as well.  Note that we do note move the 
        // transform geom, which must always have the same location as the body
        // to which it is attached.
        dGeomTranslate(encapsulatedGeom, -m_massOffset);

        dBodySetMass(body, &odeMass);

        // Attach geometry to the body
        dGeomSetBody(odeGeometry, body);

        // Set the object->world transformation (this will set the underlying
        // body's body->world transformation appropriately, using m_massOffset).
        setFrame(m_frame);

        dBodySetLinearVel(body, m_linearVelocity);
        dBodySetAngularVel(body, m_angularVelocity);

    } else {
        // Part of the static world; just translate
        dGeomSetPositionAndRotation(odeGeometry, m_frame);
    }
}


void Entity::Part::createODEGeometry() {
    dWorldID world = World::world()->worldID();
    dSpaceID space = entity->spaceID;
    bool isSimulated = entity->isSimulated();
    dMassSetZero(&odeMass);
    alwaysAssertM(odeGeometry == NULL,
        "This version of Dojo does not support multiple shapes per part.");

    switch (g3dGeometry->type()) {
    case Shape::MESH:
        {
            // Trimeshes are unstable for dynamic simulation because the collisions between two trimeshes
            // are complicated.  Also, we wouldn't have a good penetration depth or moment of inertia
            // approximation--just avoid issues by requiring that trimeshes be static.
            alwaysAssertM(! isSimulated, "Illegal to make a MeshShape part with isSimulated = true.");

            triMeshDataID = dGeomTriMeshDataCreate();

            const MeshShape* mesh = dynamic_cast<MeshShape*>(g3dGeometry);

            alwaysAssertM(sizeof(Vector3) == 3 * sizeof(float),
                "G3D vectors have the wrong size in this build; cannot create an "
                "ODE trimesh from G3D data without conversion.");

            alwaysAssertM(sizeof(mesh->indexArray()[0]) == sizeof(unsigned int),
                "ODE and G3D have different vertex index sizes in this build; cannot create an "
                "ODE trimesh from G3D data without conversion.");


            // ODE will use the G3D data directly.
            const void* vertices = mesh->vertexArray().getCArray();
            const void* indices  = mesh->indexArray().getCArray();

            dGeomTriMeshDataBuildSingle(
                triMeshDataID,
                vertices, sizeof(Vector3), mesh->vertexArray().size(),
                indices, mesh->indexArray().size(), 3 * sizeof(unsigned int));

            // The NULL arguments are the optional callbacks that can be used to
            // determine if a collision occured by sampling the texture of a triangle.
            odeGeometry = dCreateTriMesh(space, triMeshDataID, NULL, NULL, NULL);
            break;
        }

    case Shape::PLANE:
        {
            alwaysAssertM(! isSimulated, "Illegal to make a PlaneShape part with isSimulated = true.");
            // Plane won't have real physics attached to it, so we just need
            // to make it part of the space.
            float a, b, c, d;
            Plane plane = g3dGeometry->plane();
            // Take the plane to world space
            plane = frame().toWorldSpace(plane);

            plane.getEquation(a, b, c, d);

            Vector3 N(a, b, c);
            Vector3 X = Vector3::unitX();
            if (G3D::abs(X.dot(N)) > 0.9) {
                X = Vector3::unitY();
            }

            // How thick is the plane on the underside
            float thickness = 10;

            CoordinateFrame orient;
            orient.lookAt(-N, X);
            orient.translation = N * -(thickness/2 + d);

            // Cylinder vs. Plane collisions don't work correctly in ODE
            // so we replace the plane with a giant box where z = normal
            odeGeometry = dCreateBox(space, 10000, 1000, thickness);
            
            dGeomSetPositionAndRotation(odeGeometry, frame() * orient);

            // Code to use a plane:
            //odeGeometry = dCreatePlane(space, a, b, c, -d);            
        }
        break;

    case Shape::BOX:
        {
            const Box& box = g3dGeometry->box();

            dGeomID geom = dCreateBox(ODE_NO_SPACE, box.extent(0), box.extent(1), box.extent(2));
            dMassSetBox(&odeMass, 1, box.extent().x, box.extent().y, box.extent().z);
            dMassAdjust(&odeMass, m_mass);

            CoordinateFrame customOrient;
            box.getLocalFrame(customOrient);
            createRelativeGeom(geom, customOrient, space);
        }
        break;

    case Shape::SPHERE:
        {
            const Sphere& sphere = g3dGeometry->sphere();

            dGeomID geom = dCreateSphere(ODE_NO_SPACE, sphere.radius);
            dMassSetSphere(&odeMass, 1, sphere.radius);
            dMassAdjust(&odeMass, m_mass);
            CoordinateFrame customOrient(sphere.center);

            createRelativeGeom(geom, customOrient, space);
        }
        break;

    case Shape::CAPSULE:
        {
            // Create an object space geom.  Then transform it to body space (where center of mass = origin).

            const Capsule& capsule = g3dGeometry->capsule();

            float r = capsule.getRadius();
            float h = (capsule.getPoint2() - capsule.getPoint1()).length();

            dGeomID geom = dCreateCCylinder(ODE_NO_SPACE, r, h);

            // ODE capsules stand along the z axis in their local frame with the center of mass at (0,0,0);
            // we want them pointing along the axis of the input capsule.
            CoordinateFrame transform;
            {
                Vector3 Z = (capsule.getPoint2() - capsule.getPoint1()).direction();
                Vector3 X = Vector3::unitX();

                if (G3D::abs(X.dot(Z)) > 0.98) {
                    X = -Vector3::unitY();
                }

                // Make X orthogonal to Z
                X = X - Z.dot(X) * Z;
                Vector3 Y = Z.cross(X);
                transform.rotation.setColumn(0, X);
                transform.rotation.setColumn(1, Y);
                transform.rotation.setColumn(2, Z);
            }

            // Translate center of mass
            transform.translation = capsule.center();

            // Start the cylinder at (0,0,0) along the Z axis (3) like an ODE CCylinder, and then
            // apply the same transformation we applied to the geom.  
            // Note that we do not apply the m_frame of the body.
            dMassSetCappedCylinderTotal(&odeMass, m_mass, 3, r, h);

            createRelativeGeom(geom, transform, space);
        }
        break;

    case Shape::CYLINDER:
       {
            // Create an object space geom.  Then transform it to body space (where center of mass = origin).
            const Cylinder& cylinder = g3dGeometry->cylinder();

            float r = cylinder.radius();
            float h = (cylinder.getPoint2() - cylinder.getPoint1()).length();

            // TODO: Change to dCreateCylinder from dCreateCCylinder (remove a 'C')
            // Right now the collisions are missed.
            dGeomID geom = dCreateCCylinder(ODE_NO_SPACE, r, h);

            // ODE cylinders stand along the z axis in their local frame with the center of mass at (0,0,0);
            // we want them pointing along the axis of the input cylinder.
            CoordinateFrame transform;
            {
                Vector3 Z = (cylinder.getPoint2() - cylinder.getPoint1()).direction();
                Vector3 X = Vector3::unitX();

                if (G3D::abs(X.dot(Z)) > 0.98) {
                    X = -Vector3::unitY();
                }

                // Make X orthogonal to Z
                X = X - Z.dot(X) * Z;
                Vector3 Y = Z.cross(X);
                transform.rotation.setColumn(0, X);
                transform.rotation.setColumn(1, Y);
                transform.rotation.setColumn(2, Z);
            }

            // Translate center of mass
            transform.translation = cylinder.center();

            // Start the cylinder at (0,0,0) along the Z axis (3) like an ODE CCylinder, and then
            // apply the same transformation we applied to the geom.  
            // Note that we do not apply the m_frame of the body.
            dMassSetCylinderTotal(&odeMass, m_mass, 3, r, h);

            createRelativeGeom(geom, transform, space);
        }
        break;

    default:
        debugAssertM(false, "This shape is not supported by Entity.");
    }

    // Set the back pointer
    dGeomSetData(odeGeometry, this);

    if (isSimulated) {
        // Set up auto-disable
        dBodySetAutoDisableDefaults(body);
        dBodySetAutoDisableFlag(body, 1);
    }
}


void Entity::Part::renderPhysicsModel(RenderDevice* rd) const {

    if (g3dGeometry != NULL) {
        static const Color4 staticColor(0.8f, 0.8f, 0.8f, 1.0f);
        static const Color4 dynamicColor(.75f, .75f, 0.3f, 0.6f);
        static const Color4 disabledDynamicColor(0.5f, 0.5f, 0.68f, 0.7f);

        g3dGeometry->render(rd, frame(),
            entity->isSimulated() ? 
            (dBodyIsEnabled(body) ? dynamicColor : disabledDynamicColor) : staticColor, Color4(0,0,0,0.5f));

        // Render center of mass (currently disabled)
        if (false && (body != NULL)) {
            rd->pushState();
            rd->setObjectToWorldMatrix(frame());
            Draw::sphere(
                Sphere(m_massOffset, 0.1f), 
                rd, Color3::green(), Color4::clear());
            Draw::axes(rd);
            rd->popState();
        }

        // Render velocity vector (currently disabled)
        if (false && m_linearVelocity.squaredLength() > 0.001f) {
            Draw::ray(Ray::fromOriginAndDirection(frame().pointToWorldSpace(m_massOffset), 
                (float)G3D::max(m_linearVelocity.length(), 0.1f) * m_linearVelocity.direction()), 
                rd, Color3::white() * 0.7f,
                0.5f);
        }
    }
}


void Entity::Part::applyForce(const Vector3& ws_F) {
    if (entity->isSimulated() && (ws_F.squaredMagnitude() > 0)) {
        dBodyEnable(body);
        dBodyAddForce(body, ws_F.x, ws_F.y, ws_F.z);
    }
}


void Entity::Part::applyTorque(const Vector3& ws_T) {
    if (entity->isSimulated() && (ws_T.squaredMagnitude() > 0)) {
        // We must explicitly wake an object if it is sleeping
        dBodyEnable(body);
        dBodyAddTorque(body, ws_T.x, ws_T.y, ws_T.z);
    }
}


void Entity::Part::applyRelTorque(const Vector3& os_T) {
    if (entity->isSimulated()) {
        // We must explicitly wake an object if it is sleeping
        dBodyEnable(body);
        dBodyAddRelTorque(body, os_T.x, os_T.y, os_T.z);
    }
}


void Entity::Part::setLinearVelocity(const Vector3& v) {
    if (body != NULL) {
        m_linearVelocity = v;
        dBodySetLinearVel(body, v);
    }
}


void Entity::Part::setAngularVelocity(const Vector3& v) {
    if (body != NULL) {
        m_angularVelocity = v;
        dBodySetAngularVel(body, v);
    }
}

} // namespace
