/**
 @file World.cpp

 Dojo Game Engine

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

//#include "drawstuff/drawstuff.h"

namespace dojo {

/** Used exclusively by dojo::world() and the destructor*/
static World* globalWorld;

World* World::world() {
    static bool initialized = false;
    if (! initialized) {
        // On the first call, we can't depend on the global 
        // variable being initialized.
        globalWorld = NULL;
        initialized = true;
    }
    if (globalWorld == NULL) {
        // Create a world.
        globalWorld = new World();
    }

    return globalWorld;
}


World::World() : entityArray("Entity"), debugContacts(false) {
    debugAssertM(globalWorld == NULL, "Can only create one World object.");
}


void World::init() {
    renderMode = RENDER_NORMAL;

}


World::~World() {
    onCleanup();
    globalWorld = NULL;
}


void World::remove(const std::string& name) {
    //debugAssert(entityArray.containsKey(name));
    removeList.append(name);
}


void World::onCleanup() {
    removeList.clear();
    entityArray.clear();
    physics.simArray.clear();

    dJointGroupDestroy(physics.contactGroup);
    dSpaceDestroy(physics.spaceID);

    dWorldDestroy(physics.worldID);
}


void World::insert(EntityRef& e, const CoordinateFrame& cframe) {
    alwaysAssertM(! e->inWorld, "Cannot insert entity into the world twice.");
    e->inWorld = true;

    if (e->hasPhysics) {
        physics.simArray.append(e);
    }

    if (e->isGround()) {
        groundArray.append(e);
    }

    entityArray.append(e->name(), e);

    e->setFrame(cframe);
}


Entity::Part* World::geomToPart(dGeomID g) {
    void* data = dGeomGetData(g);
    debugAssertM(data != NULL, "Incorrectly created Geom with no Entity.");
    return (Entity::Part*)data;
}


void World::ODENearCallback(void* data, dGeomID o1, dGeomID o2) {
    int i, n;
    
    World* world = (World*)data;

    if (dAreConnectedExcluding(o1, o2, dJointTypeContact)) {
        // These are connected by a joint; ignore the collision.
        return;
    }

    // Must be at least 12 for trimesh; we've had problems with values smaller than 20 just 
    // crashing the system.
    const int maxContacts = 20;
    dContact contact[maxContacts];
    n = dCollide(o1, o2, maxContacts, &contact[0].geom, sizeof(dContact));
    if (n > 0) {
        Entity::Part* p1 = geomToPart(o1);
        Entity::Part* p2 = geomToPart(o2);

        // Collision location
        Vector3 pos(contact[0].geom.pos[0], contact[0].geom.pos[1], contact[0].geom.pos[2]);

        debugAssertM(! G3D::isNaN(pos.x) &&
                    ! G3D::isNaN(pos.y) &&
                    ! G3D::isNaN(pos.z), 
            "NaN returned from ode::dCollide.");

        // Put this test here since ODE may invoke ODENearCallback even if no collision has
        // actually occured.
        bool veto1 = ! p1->entity->onCollision(p1->name(), p2->entity, p2->name(), pos);
        bool veto2 = ! p2->entity->onCollision(p2->name(), p1->entity, p1->name(), pos);

        if (veto1 || veto2) {
            // Ignore this collision
            return;
        }

        // Either b1 or b2 may be null. 
        dBodyID b1 = dGeomGetBody(o1);
        dBodyID b2 = dGeomGetBody(o2);

		if ((b1 == NULL) && (b2 == NULL)) {
			// No bodies, so no point in adding joints.
			return;
		}

		// If one of these is a piece of static geometry,
		// wake the other one up so it can respond to the
		// collision.
		if ((b1 == NULL) && (b2 != NULL) && (p1->entity->m_justMoved)) {
			dBodyEnable(b2);
		}
		if ((b2 == NULL) && (b1 != NULL) && (p2->entity->m_justMoved)) {
			dBodyEnable(b1);
		}

        for (i = 0; i < n; ++i) {
           if (world->debugContacts) {
                world->debugContactArray.append(pos);
            }

            contact[i].surface.mode = 0;
            //dContactApprox1;
//                | dContactSlip1 | dContactSlip2  | ;
//            | dContactSoftERP | dContactSoftCFM;

            // Note: In terms of processing performance, 
            // zero friction is faster than inf friction, 
            // but both are faster than finite friction.
            contact[i].surface.mu = 50;//dInfinity;//
//            contact[i].surface.slip1 = 0.1;
//            contact[i].surface.slip2 = 0.1;

            // Soft ERP/CFM makes objects squishy on collision
//            contact[i].surface.soft_erp = 0.5;
//            contact[i].surface.soft_cfm = 0.3;
            
            dJointID c = dJointCreateContact(
                world->physics.worldID, 
                world->physics.contactGroup, 
                &contact[i]);

            dJointAttach(c, b1, b2);
        }
    }
}


void World::beforeSimulation() {
    for (int s = 0; s < physics.simArray.size(); ++s) {
        EntityRef entity = physics.simArray[s];
        entity->beforeSimulation();
    }
}

void World::onSimulation(SimTime sdt) {
    if (sdt <= 0) {
        return;
    }

    for (int e = 0; e < entityArray.size(); ++e) {
        entityArray[e]->onSimulation(sdt);
    }

    beforeSimulation();

    // Do physics on the models
    dSpaceCollide(physics.spaceID, this, &ODENearCallback);

    if (physics.fastMode) {
        dWorldQuickStep(physics.worldID, sdt);
    } else {
        dWorldStep(physics.worldID, sdt);
    }

    // Remove all contact joints
    dJointGroupEmpty(physics.contactGroup);

    afterSimulation();
}


void World::processRemoveList() {
    // Remove everything on the removeList
    for (int e = 0; e < removeList.size(); ++e) {
        EntityRef entity;
        
        entityArray.get(removeList[e], entity);

        if (entity.notNull()) {
            // Not already removed
            entityArray.remove(entity->name());
			for(int j = 0; j < physics.simArray.size(); ++j){
				if(physics.simArray[j]->name() == entity->name()){
					physics.simArray.fastRemove(j);
				}
			}
			
            if (entity->isGround()) {
                for (int i = 0; i < groundArray.size(); ++i) {
                    if (groundArray[i]->name() == entity->name()) {
                        groundArray.fastRemove(i);
                    }
                }
            }
        }
    }
    removeList.clear();
}

void World::afterSimulation() {
    // Update from simulated data
    CoordinateFrame c;
    for (int s = 0; s < physics.simArray.size(); ++s) {
        EntityRef entity = physics.simArray[s];
        entity->afterSimulation();
    }

    processRemoveList();
}

static bool entitySortBackToFront(const EntityRef& a, const EntityRef& b) {
    return a->sortKey > b->sortKey;
}


static void sortBackToFront(const CoordinateFrame& camera, Array<EntityRef>& array) {
    Vector3 z = camera.getLookVector();

    // Create keys
    for (int e = 0; e < array.size(); ++e) {
        EntityRef entity = array[e];
        // Draw planes first
        entity->sortKey = entity->isGround() ? inf() : entity->frame().translation.dot(z);
    }

    array.sort(entitySortBackToFront);
}


void World::renderPhysicsModels(RenderDevice* rd) {
    sortBackToFront(rd->getCameraToWorldMatrix(), physics.simArray);

    rd->pushState();
        rd->setPolygonOffset(0);
        // Draw opaque (static) objects first
        for (int s = 0; s < physics.simArray.size(); ++s) {
            if (! physics.simArray[s]->isSimulated()) {
                physics.simArray[s]->renderPhysicsModel(rd);
            }
        }
        for (int s = 0; s < physics.simArray.size(); ++s) {
            if (physics.simArray[s]->isSimulated()) {
                physics.simArray[s]->renderPhysicsModel(rd);
            }
        }
    rd->popState();
}


static void configureFixedFunctionLighting(RenderDevice* rd, const LightingRef& lighting) {
    rd->setAmbientLightColor(lighting->ambientTop);
    if (lighting->ambientBottom != lighting->ambientTop) {
        rd->setLight(0, GLight::directional(-Vector3::unitY(), 
            lighting->ambientBottom - lighting->ambientTop, false)); 
    }
    
    // Lights
    for (int L = 0; L < iMin(8, lighting->lightArray.size()); ++L) {
        rd->setLight(L + 1, lighting->lightArray[L]);
    }
}



/** Renders what ODE sees.  For debugging purposes only. */
static void odeCheck(RenderDevice* rd, dSpaceID space)
{
  int i;

  int n = dSpaceGetNumGeoms(space);
  static float Z_OFFSET = 0;

  // draw all other objects
  for (i=0; i<n; i++) {
    dGeomID g = dSpaceGetGeom (space,i);
    dVector3 pos;

    if (dGeomGetClass(g) == dPlaneClass) {
        // Not a placeable geom
        continue;
    }

    if (dGeomGetClass (g) != dPlaneClass) {
      memcpy (pos,dGeomGetPosition(g),sizeof(pos));
      pos[2] += Z_OFFSET;
    }
    
    CoordinateFrame cframe;
    dGeomGetPositionAndRotation(g, cframe);
    rd->setObjectToWorldMatrix(cframe);

    if (dGeomGetClass (g) == dGeomTransformClass) {
        g = dGeomTransformGetGeom(g);
        CoordinateFrame cframe2; 
        dGeomGetPositionAndRotation(g, cframe2);
        rd->setObjectToWorldMatrix(cframe * cframe2);
    }

    switch (dGeomGetClass (g)) {

    case dSphereClass: {
        /*
      dsSetColorAlpha (1,0,0,0.8);
      dReal radius = dGeomSphereGetRadius (g);
      dsDrawSphere (pos,dGeomGetRotation(g),radius);
      */
      break;
    }

    case dBoxClass: {/*
      dsSetColorAlpha (1,1,0,0.8);
      dVector3 sides;
      dGeomBoxGetLengths (g,sides);
      dsDrawBox (pos,dGeomGetRotation(g),sides);
      */
      break;
    }

    case dCCylinderClass: {
      dReal radius,length;
      dGeomCCylinderGetParams (g,&radius,&length);
      Draw::capsule(Capsule(Vector3(0,0,-length/2), Vector3(0,0,length/2), radius), rd);
      break;
    }

    case dPlaneClass: {
        /*
      dVector4 n;
      dMatrix3 R,sides;
      dVector3 pos2;
      dGeomPlaneGetParams (g,n);
      dRFromZAxis (R,n[0],n[1],n[2]);
      for (j=0; j<3; j++) pos[j] = n[j]*n[3];
      pos[2] += Z_OFFSET;
      sides[0] = 2;
      sides[1] = 2;
      sides[2] = 0.001;
      dsSetColor (1,0,1);
      for (j=0; j<3; j++) pos2[j] = pos[j] + 0.1*n[j];
      dsDrawLine (pos,pos2);
      dsSetColorAlpha (1,0,1,0.8);
      dsDrawBox (pos,R,sides);
      */
      break;
    }

    }
  }
}


void World::onGraphics(RenderDevice* rd) {
    LightingRef        lighting      = toneMap.prepareLighting(this->lighting);
    LightingParameters skyParameters = toneMap.prepareLightingParameters(this->skyParameters);

    // Pose all
    Array<PosedModelRef> posedModels;

    for (int e = 0; e < entityArray.size(); ++e) {
        EntityRef& entity = entityArray[e];
        entity->pose(posedModels);
    }
    Array<PosedModelRef> opaque, transparent;
    PosedModel::sort(posedModels, app->debugCamera.getCoordinateFrame().lookVector(), opaque, transparent);

    bool shadows = false;
    if (!shadows && (lighting->shadowedLightArray.size() > 0)) {
        // We're not going to be able to draw shadows, so move the shadowed lights into
        // the unshadowed category.
        lighting->lightArray.append(lighting->shadowedLightArray);
        lighting->shadowedLightArray.clear();
    }

    rd->setProjectionAndCameraMatrix(app->debugCamera);
    rd->setObjectToWorldMatrix(CoordinateFrame());

    debugNumOpaqueRendered = opaque.size();
    debugNumTransparentRendered = transparent.size();

    // Cyan background
    rd->setColorClearValue((lighting->ambientTop + lighting->ambientBottom) / 2);

    rd->clear(sky.notNull(), true, true);
    if (sky.notNull()) {
        sky->render(rd, skyParameters);
    }

    rd->pushState();

        switch (renderMode) {
        case RENDER_NORMAL:
            //rd->setRenderMode(RenderDevice::RENDER_WIREFRAME);

            // Opaque unshadowed
            for (int m = 0; m < opaque.size(); ++m) {
                opaque[m]->renderNonShadowed(rd, lighting);
            }

            // Drop shadow
            if (false) {
            rd->pushState();
                rd->disableLighting();
                rd->setColor(Color3::black());
                CoordinateFrame cam = rd->getCameraToWorldMatrix();
                rd->setCameraToWorldMatrix(cam);
                for (int m = 0; m < opaque.size(); ++m) {                
                    opaque[m]->render(rd);
                }
            rd->popState();
            }

            // Opaque shadowed
            if (lighting->shadowedLightArray.size() > 0) {
                for (int m = 0; m < opaque.size(); ++m) {
                //    opaque[m]->renderShadowMappedLightPass(rd, lighting->shadowedLightArray[0], lightMVP, shadowMap);
                }
            }

            // Transparent
            for (int m = 0; m < transparent.size(); ++m) {
                transparent[m]->renderNonShadowed(rd, lighting);
                if (lighting->shadowedLightArray.size() > 0) {
                //    transparent[m]->renderShadowMappedLightPass(rd, lighting->shadowedLightArray[0], lightMVP, shadowMap);
                }
            }
            break;

        case RENDER_PHYSICS:
            rd->enableLighting();
                configureFixedFunctionLighting(rd, lighting);
                rd->setSpecularCoefficient(Color3::white() * 0.5);
                rd->setShininess(30);
                Draw::axes(CoordinateFrame(Vector3(0, 0, 0)), rd, Color3::red() * 0.6, Color3::green() * 0.8, Color3::blue() * 0.8);

                debugContacts = true;
                for (int i = 0; i < debugContactArray.size(); ++i) {
                    static const Vector3 v(0.05, 0.05, 0.05);
                    Draw::box(
                        AABox(debugContactArray[i] - v, debugContactArray[i] + v),
                        rd, Color3::red());
                }
                debugContactArray.clear();

                renderPhysicsModels(rd);
            rd->disableLighting();
            break;
        }

        // Debugging statement to verify that the ODE objects are where we think they are.
        //odeCheck(app->renderDevice, physics.spaceID);

    rd->popState();

    toneMap.apply(app->renderDevice);

//    for (int e = 0; e < entityArray.size(); ++e) {
//        entityArray[e]->drawLabels(rd);
//    }

    if (sky.notNull()) {
        sky->renderLensFlare(rd, skyParameters);
    }
}


void World::setGravity(const Vector3& g) {
    physics.gravity = g;
    dWorldSetGravity(physics.worldID, physics.gravity.x, physics.gravity.y, physics.gravity.z);
}

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

World::Physics::Physics() {
    worldID = dWorldCreate();
    spaceID = dHashSpaceCreate(0);

    // Sleeping: 
    // Allow objects to sleep
    dWorldSetAutoDisableFlag(worldID, 1);
    dWorldSetAutoDisableLinearThreshold(worldID, 0.01);
    dWorldSetAutoDisableAngularThreshold(worldID, 0.0075 * 2 * G3D_PI);

    // Don't wake up objects unless they are
    // very close to each other
    //    dWorldSetAutoEnableDepthSF1(worldID, 10);

    // How much error should be corrected during the next timestep.
    // Useful range is 0.1 -- 0.8.  Higher corrects more error but can
    // lead to instability.  ODE default is 0.2.
    dWorldSetERP(worldID, 0.1);

    // How much error is acceptable in constraints. ODE default is 10e-5 
    // Larger values make the system more spongy.  Smaller values can help
    // resolve singular (overconstrained) systems, but lead to instability.
    dWorldSetCFM(worldID, 5e-6);

    contactGroup = dJointGroupCreate(0);

    gravity = Vector3(0, 0, 0);
    dWorldSetGravity(worldID, gravity.x, gravity.y, gravity.z);
}

} // namespace
