/*==LICENSE==*

CyanWorlds.com Engine - MMOG client, server and tools
Copyright (C) 2011  Cyan Worlds, Inc.

This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 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 General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program.  If not, see <http://www.gnu.org/licenses/>.

Additional permissions under GNU GPL version 3 section 7

If you modify this Program, or any covered work, by linking or
combining it with any of RAD Game Tools Bink SDK, Autodesk 3ds Max SDK,
NVIDIA PhysX SDK, Microsoft DirectX SDK, OpenSSL library, Independent
JPEG Group JPEG library, Microsoft Windows Media SDK, or Apple QuickTime SDK
(or a modified version of those libraries),
containing parts covered by the terms of the Bink SDK EULA, 3ds Max EULA,
PhysX SDK EULA, DirectX SDK EULA, OpenSSL and SSLeay licenses, IJG
JPEG Library README, Windows Media SDK EULA, or QuickTime SDK EULA, the
licensors of this Program grant you additional
permission to convey the resulting work. Corresponding Source for a
non-source form of such a combination shall include the source code for
the parts of OpenSSL and IJG JPEG Library used as well as that of the covered
work.

You can contact Cyan Worlds, Inc. by email legal@cyan.com
 or by snail mail at:
      Cyan Worlds, Inc.
      14617 N Newport Hwy
      Mead, WA   99021

*==LICENSE==*/
#include "plPXPhysicalControllerCore.h"
#include "plgDispatch.h"
#include "plSimulationMgr.h"
#include "plPXPhysical.h"
#include "plPXConvert.h"
#include "pnSceneObject/plSimulationInterface.h"
#include "pnSceneObject/plSceneObject.h"
#include "pnMessage/plCorrectionMsg.h"
#include "plAvatar/plArmatureMod.h"
#include "pnSceneObject/plCoordinateInterface.h"
#include "plDrawable/plDrawableGenerator.h"
#include "plPhysical/plPhysicalProxy.h"
#include "pnMessage/plSetNetGroupIDMsg.h"
#include "plMessage/plCollideMsg.h"
#include "plModifier/plDetectorLog.h"
//#include "NxVecExtendedVec3.h"

#include "NxPhysics.h"
#include "ControllerManager.h" 
#include "NxCapsuleController.h"
#include "NxCapsuleShape.h"

#include "plSurface/hsGMaterial.h"
#include "plSurface/plLayerInterface.h"


#ifndef PLASMA_EXTERNAL_RELEASE
#include "plPipeline/plDebugText.h"
#endif

#define kPhysxSkinWidth 0.1f
#define kPhysZOffset ((fRadius + (fHeight / 2)) + kPhysxSkinWidth)
//#define kSLOPELIMIT (cosf(NxMath::degToRad(55.f)))
//#define kPhysicalHeightFudge 0.4f   // this fudge was used for PhysX 2.4
#define kPhysicalHeightFudge 0.0f

//#define STEP_OFFSET   1.0f
#define STEP_OFFSET 0.5f
//#define STEP_OFFSET   0.15f


#ifndef PLASMA_EXTERNAL_RELEASE
hsBool plPXPhysicalControllerCore::fDebugDisplay = false;
#endif // PLASMA_EXTERNAL_RELEASE
int plPXPhysicalControllerCore::fPXControllersMax = 0;

static ControllerManager gControllerMgr;
static std::vector<plPXPhysicalControllerCore*> gControllers;
static bool gRebuildCache=false;

#define AvatarMass 200.f

class PXControllerHitReportWalk : public NxUserControllerHitReport
{
public:
    virtual NxControllerAction onShapeHit(const NxControllerShapeHit& hit)
    {
        plPXPhysicalControllerCore* ac = plPXPhysicalControllerCore::FindController(hit.controller);
        NxActor& actor = hit.shape->getActor();
        plPXPhysical* phys = (plPXPhysical*)actor.userData;
        static hsScalar SlopeLimit = kSLOPELIMIT;
        hsVector3 normal = plPXConvert::Vector(hit.worldNormal);
        ac->fMovementInterface->IAddContactNormals(normal);
#ifndef PLASMA_EXTERNAL_RELEASE
        plDbgCollisionInfo info;
        info.fNormal = normal;
        info.fSO = plSceneObject::ConvertNoRef(phys->GetObjectKey()->ObjectIsLoaded());
        info.fOverlap = false;
        NxShape* const *shapes = hit.controller->getActor()->getShapes();
        int numShapes = hit.controller->getActor()->getNbShapes();
        int i;
        for (i = 0; i < numShapes; i++)
        {
            // should only be one capsule shape
            const NxCapsuleShape *capShape = shapes[i]->isCapsule();
            if (capShape)
            {
                NxCapsule cap;
                capShape->getWorldCapsule(cap);
                if (hit.shape->checkOverlapCapsule(cap))
                    info.fOverlap = true;
            }
        }
        ac->fDbgCollisionInfo.Append(info);
#endif PLASMA_EXTERNAL_RELEASE
        // If the avatar hit a movable physical, apply some force to it.
        hsVector3 dir = plPXConvert::Vector(hit.dir);
        float dirdotup=dir.fZ;
        hsPoint3 pos((hsScalar)hit.worldPos.x, (hsScalar)hit.worldPos.y, (hsScalar)hit.worldPos.z);
        NxExtendedVec3 controllerPos=hit.controller->getPosition();
        hsVector3 bottomOfTheCapsule((hsScalar)controllerPos.x,(hsScalar)controllerPos.y,(hsScalar)controllerPos.z);
        bottomOfTheCapsule.fZ=bottomOfTheCapsule.fZ-(ac->fHeight/2.0f + ac->fRadius);
        if (actor.isDynamic() )
        {
            if((hit.worldPos.z- bottomOfTheCapsule.fZ)<=ac->fRadius)//bottom hemisphere
            {
                //onTopOfSlopeLimit
                if (phys && phys->GetProperty(plSimulationInterface::kPhysAnim))
                {
                    if(normal.fZ>=0)
                    {//we consider this ground
                        ac->fMovementInterface->AddOnTopOfObject(phys);
                    }
                }
            }
            if ( !actor.readBodyFlag(NX_BF_KINEMATIC) && !actor.readBodyFlag(NX_BF_FROZEN))
            {
                // If this is the local avatar, we need to take ownership of this
                // dynamic if we haven't already
                if (ac->fLOSDB == plSimDefs::kLOSDBLocalAvatar && !phys->IsLocallyOwned() &&
                    !phys->GetProperty(plSimulationInterface::kNoOwnershipChange))
                {
                    plSynchedObject* obj = plSynchedObject::ConvertNoRef(phys->GetObjectKey()->ObjectIsLoaded());
                    obj->SetNetGroupConstant(plNetGroup::kNetGroupLocalPhysicals);
                    // Tell all the other clients that we own this physical
                    plSetNetGroupIDMsg* setNetGroupID = TRACKED_NEW plSetNetGroupIDMsg;
                    setNetGroupID->fId = plNetGroup::kNetGroupRemotePhysicals;
                    setNetGroupID->SetBCastFlag(plMessage::kNetPropagate | plMessage::kNetForce);
                    setNetGroupID->SetBCastFlag(plMessage::kLocalPropagate, false);
                    setNetGroupID->Send(obj->GetKey());
                }
                plSimulationMgr::GetInstance()->ConsiderSynch(phys, nil);
                // We only allow horizontal pushes. Vertical pushes when we stand on
                // dynamic objects creates useless stress on the solver.
                
                hsVector3 vel=ac->GetLinearVelocity()- plPXConvert::Vector( actor.getLinearVelocity());
                if(dirdotup>=0)vel.fZ=0.001f;
                else vel.fZ=0.0f;
                static hsScalar kAvieMass = 140.f/32.f;         
                if (!vel.IsEmpty())
                {
                    static hsScalar kForceScale = 140.0f;
                    NxF32 coeff;
                    NxExtendedVec3 norm2=hit.controller->getPosition();
                    norm2.x=hit.worldPos.x-bottomOfTheCapsule.fX;
                    norm2.y=hit.worldPos.y-bottomOfTheCapsule.fY;
                    if((hit.worldPos.z- bottomOfTheCapsule.fZ)<ac->fRadius)//bottom hemisphere
                    {
                        norm2.normalize();
                        norm2.z=0.01f;
                    }
                    else if((hit.worldPos.z- bottomOfTheCapsule.fZ)<(ac->fRadius+ac->fHeight))
                    {
                        norm2.z=0.0f;
                        norm2.normalize();
                    }
                    else
                    {//must be the top so the normal is displacement from the pos - center
                        //of top hemisphere
                        norm2.z=hit.worldPos.z - ((ac->fRadius+ac->fHeight + bottomOfTheCapsule.fZ));
                        norm2.normalize();
                    }
                    
                    
                    float proj=(float)(norm2.x*dir.fX+dir.fY*norm2.y+dir.fZ*norm2.z);
                    coeff =abs(proj*kForceScale*vel.Magnitude());
                    vel.fZ=(hsScalar)norm2.z;
                    vel.fY=(hsScalar)norm2.y;
                    vel.fX=(hsScalar)norm2.x;
                    phys->SetHitForce(vel*coeff, pos);
                }
            }
        }
        else  // else if the avatar hit a static
        {
            return NX_ACTION_NONE;
        }
        if (phys && phys->GetProperty(plSimulationInterface::kAvAnimPushable))
        {
            hsQuat inverseRotation = ac->fLocalRotation.Inverse();
            hsVector3 normal = plPXConvert::Vector(hit.worldNormal);
            ac->SetPushingPhysical( phys);
            ac->SetFacingPushingPhysical((inverseRotation.Rotate(&kAvatarForward).InnerProduct(normal) < 0 ? true : false));
        }
        return NX_ACTION_NONE;
    }
    virtual NxControllerAction onControllerHit(const NxControllersHit& hit)
    {
        return NX_ACTION_NONE;
    }

} gMyReport;


plPhysicalControllerCore* plPhysicalControllerCore::Create(plKey ownerSO, hsScalar height, hsScalar width)
{
    // Test to see how many controller there already is
    if ( !plPXPhysicalControllerCore::fPXControllersMax || plPXPhysicalControllerCore::NumControllers() < plPXPhysicalControllerCore::fPXControllersMax )
    {
        hsScalar radius = width / 2.f;
        hsScalar realHeight = height - width + kPhysicalHeightFudge;
        return TRACKED_NEW plPXPhysicalControllerCore(ownerSO, realHeight,radius);
    }
    return nil;
}

//Static Helper Func
plPXPhysicalControllerCore* plPXPhysicalControllerCore::FindController(NxController* controller)
{
    for (int i = 0; i < gControllers.size(); i++)
    {
        plPXPhysicalControllerCore* ac = gControllers[i];
        if (ac->fController == controller)
            return ac;
    }
    return nil;
}
void plPXPhysicalControllerCore::RebuildCache(){gRebuildCache=true;}

plPXPhysicalControllerCore* plPXPhysicalControllerCore::GetController(NxActor& actor, bool* isController)
{
    *isController = false;
    for (int i = 0; i < gControllers.size(); i++)
    {
        plPXPhysicalControllerCore* ac = gControllers[i];
        if (ac->fController && ac->fController->getActor() == &actor)
        {
            *isController = true;
            return ac;
        }
        if ( ac->fKinematicActor == &actor)
        {
            return ac;
        }
    }

    return nil;
}
void plPXPhysicalControllerCore::GetWorldSpaceCapsule(NxCapsule& cap) const
{
    if(this->fKinematicActor)
    {
        int numshapes=fKinematicActor->getNbShapes();
        if (numshapes==1) 
        {
            //there should only be one shape on a controller
            NxShape* const *shapes=fKinematicActor->getShapes();
            //and since it is a capsule controller it better be a capsule;
            NxCapsuleShape *capShape = shapes[0]->isCapsule();
            if(capShape) capShape->getWorldCapsule(cap);
        }
    }
}
bool plPXPhysicalControllerCore::AnyControllersInThisWorld(plKey world)
{
    for (int i = 0; i < gControllers.size(); i++)
    {
        plPXPhysicalControllerCore* ac = gControllers[i];
        if (ac->GetSubworld() == world)
            return true;
    }
    return false;
}

int plPXPhysicalControllerCore::NumControllers()
{
    return gControllers.size();
}
int plPXPhysicalControllerCore::GetControllersInThisSubWorld(plKey world, int maxToReturn,plPXPhysicalControllerCore** bufferout)
{
    int i=0;
    for (int j=0;j<gControllers.size();j++)
    {
        plPXPhysicalControllerCore* ac = gControllers[i];
        if (ac->GetSubworld()==world)
        {
            if(i<maxToReturn)
            {
                bufferout[i]=ac;
                i++;
            }
        }
    }
    return i;

}
int plPXPhysicalControllerCore::GetNumberOfControllersInThisSubWorld(plKey world)
{
    int i=0;
    for (int j=0;j<gControllers.size();j++)
    {
        plPXPhysicalControllerCore* ac = gControllers[i];
        if (ac->GetSubworld()==world)i++;
    }
    return i;
}
//
plPXPhysicalControllerCore::plPXPhysicalControllerCore(plKey ownerSO, hsScalar height, hsScalar radius)
    : plPhysicalControllerCore(ownerSO,height,radius)
    , fController(nil)
    , fProxyGen(nil)
    , fKinematicActor(nil)
    ,fPreferedRadius(radius)
    ,fPreferedHeight(height)
    , fBehavingLikeAnimatedPhys(true)
{
    fLocalPosition.Set(0, 0, 0);
    fLocalRotation.Set(0, 0, 0, 1);
    gControllers.push_back(this);
    fLastGlobalLoc.Reset();
    ICreateController();
    Enable(false);
}

void plPXPhysicalControllerCore::ISetGlobalLoc(const hsMatrix44& l2w)
{
    fLastGlobalLoc = l2w;
    // Update our subworld position and rotation
    const plCoordinateInterface* subworldCI = GetSubworldCI();
    if (subworldCI)
    {
        const hsMatrix44& w2s = fPrevSubworldW2L;
        hsMatrix44 l2s = w2s * l2w;

        l2s.GetTranslate(&fLocalPosition);
        fLocalRotation.SetFromMatrix44(l2s);
    }
    else
    {
        l2w.GetTranslate(&fLocalPosition);
        fLocalRotation.SetFromMatrix44(l2w);
    }
    hsMatrix44 w2l;
    l2w.GetInverse(&w2l);
    if (fProxyGen)
        fProxyGen->SetTransform(l2w, w2l);
    // Update the physical position
    NxExtendedVec3 nxPos(fLocalPosition.fX, fLocalPosition.fY, fLocalPosition.fZ + kPhysZOffset);
    fController->setPosition(nxPos);
    IMatchKinematicToController();
}
plPXPhysicalControllerCore::~plPXPhysicalControllerCore()
{
    IDeleteController();
    for (int i = 0; i < gControllers.size(); i++)
    {
        if (gControllers[i] == this)
        {
            gControllers.erase(gControllers.begin()+i);
            break;
        }
    }
    delete fProxyGen;
}
void plPXPhysicalControllerCore::IMatchKinematicToController()
{
    if ( fKinematicActor)
    {
        NxExtendedVec3 cPos = fController->getPosition();
        NxVec3 prevKinPos = fKinematicActor->getGlobalPosition();
        NxVec3 kinPos;
        kinPos.x = (NxReal)cPos.x;
        kinPos.y = (NxReal)cPos.y;
        kinPos.z = (NxReal)cPos.z;
        if (plSimulationMgr::fExtraProfile)
            SimLog("Match setting kinematic from %f,%f,%f to %f,%f,%f",prevKinPos.x,prevKinPos.y,prevKinPos.z,kinPos.x,kinPos.y,kinPos.z );
        fKinematicActor->moveGlobalPosition(kinPos);
    }
}
void plPXPhysicalControllerCore::UpdateControllerAndPhysicalRep()
{
    if ( fKinematicActor)
    {
        if(this->fBehavingLikeAnimatedPhys)
        {//this means we are moving the controller and then synchnig the kin
            NxExtendedVec3 ControllerPos= fController->getPosition();
            NxVec3 NewKinPos((NxReal)ControllerPos.x, (NxReal)ControllerPos.y, (NxReal)ControllerPos.z);
            if (fEnabled && fKinematicActor->readBodyFlag(NX_BF_KINEMATIC))
            {
                if (plSimulationMgr::fExtraProfile)
                    SimLog("Moving kinematic to %f,%f,%f",NewKinPos.x, NewKinPos.y, NewKinPos.z );
                // use the position
                fKinematicActor->moveGlobalPosition(NewKinPos);

            }
            else if (fEnabled)
            {
                if (plSimulationMgr::fExtraProfile)
                    SimLog("Setting kinematic to %f,%f,%f", NewKinPos.x, NewKinPos.y, NewKinPos.z );
                fKinematicActor->setGlobalPosition(NewKinPos);
            }

        }
        else
        {
            NxVec3 KinPos= fKinematicActor->getGlobalPosition();
            NxExtendedVec3 NewControllerPos(KinPos.x, KinPos.y, KinPos.z);
            if (plSimulationMgr::fExtraProfile)
                    SimLog("Setting Controller to %f,%f,%f", NewControllerPos.x, NewControllerPos.y, NewControllerPos.z );
            fController->setPosition(NewControllerPos);
        }
        hsPoint3 curLocalPos;   
        GetPositionSim(curLocalPos);
        fLocalPosition = curLocalPos;
    }
}
void plPXPhysicalControllerCore::MoveKinematicToController(hsPoint3& pos)
{
    if ( fKinematicActor)
    {
        NxVec3 kinPos = fKinematicActor->getGlobalPosition();
        if ( abs(kinPos.x-pos.fX) + abs(kinPos.y-pos.fY) + (abs(kinPos.z-pos.fZ+kPhysZOffset)) > 0.0001f)
        {
            NxVec3 newPos;
            newPos.x = (NxReal)pos.fX;
            newPos.y = (NxReal)pos.fY;
            newPos.z = (NxReal)pos.fZ+kPhysZOffset;
            if (fEnabled && fKinematicActor->readBodyFlag(NX_BF_KINEMATIC))
            {
                if (plSimulationMgr::fExtraProfile)
                    SimLog("Moving kinematic from %f,%f,%f to %f,%f,%f",pos.fX,pos.fY,pos.fZ+kPhysZOffset,kinPos.x,kinPos.y,kinPos.z );
                // use the position
                fKinematicActor->moveGlobalPosition(newPos);
            }
            else if (fEnabled)
            {
                if (plSimulationMgr::fExtraProfile)
                    SimLog("Setting kinematic from %f,%f,%f to %f,%f,%f",pos.fX,pos.fY,pos.fZ+kPhysZOffset,kinPos.x,kinPos.y,kinPos.z );
                fKinematicActor->setGlobalPosition(newPos);
            }
        }
    }
}

void plPXPhysicalControllerCore::ISetKinematicLoc(const hsMatrix44& l2w)
{
    hsPoint3 kPos;
    // Update our subworld position and rotation
    const plCoordinateInterface* subworldCI = GetSubworldCI();
    if (subworldCI)
    {
        const hsMatrix44& w2s = subworldCI->GetWorldToLocal();
        hsMatrix44 l2s = w2s * l2w;

        l2s.GetTranslate(&kPos);
    }
    else
    {
        l2w.GetTranslate(&kPos);
    }

    hsMatrix44 w2l;
    l2w.GetInverse(&w2l);
    if (fProxyGen)
        fProxyGen->SetTransform(l2w, w2l);

    // add z offset
    kPos.fZ += kPhysZOffset;
    // Update the physical position of kinematic
    if (fEnabled && fKinematicActor->readBodyFlag(NX_BF_KINEMATIC))
        fKinematicActor->moveGlobalPosition(plPXConvert::Point(kPos));
    else if (fEnabled)
        fKinematicActor->setGlobalPosition(plPXConvert::Point(kPos));
}
void plPXPhysicalControllerCore::IGetPositionSim(hsPoint3& pos) const
{
    
    if(this->fBehavingLikeAnimatedPhys)
    {
        const NxExtendedVec3& nxPos = fController->getPosition();
        pos.Set(hsScalar(nxPos.x), hsScalar(nxPos.y), hsScalar(nxPos.z) - kPhysZOffset);
    }
    else
    {
        NxVec3 Pos = fKinematicActor->getGlobalPosition();
        pos.Set(hsScalar(Pos.x), hsScalar(Pos.y), hsScalar(Pos.z) - kPhysZOffset);
    }
}
void plPXPhysicalControllerCore::ICreateController()
{
NxScene* scene = plSimulationMgr::GetInstance()->GetScene(fWorldKey);

    NxCapsuleControllerDesc desc;
    desc.position.x     = 0;
    desc.position.y     = 0;
    desc.position.z     = 0;
    desc.upDirection    = NX_Z;
    desc.slopeLimit     = kSLOPELIMIT;
    desc.skinWidth      = kPhysxSkinWidth;
    desc.stepOffset     = STEP_OFFSET;
    desc.callback       = &gMyReport;
    desc.userData       = this;
    desc.radius         = fRadius;
    desc.height         = fHeight;
    desc.interactionFlag = NXIF_INTERACTION_EXCLUDE;
    //desc.interactionFlag = NXIF_INTERACTION_INCLUDE;
    fController = (NxCapsuleController*)gControllerMgr.createController(scene, desc);

    // Change the avatars shape groups.  The avatar doesn't actually use these when
    // it's determining collision, but if you're standing still and an object runs
    // into you, it'll pass through without this.
    NxActor* actor = fController->getActor();
    NxShape* shape = actor->getShapes()[0];
    shape->setGroup(plSimDefs::kGroupAvatar);

    // need to create the non-bouncing object that can be used to trigger things while the avatar is doing behaviors.
    NxActorDesc actorDesc;
    NxCapsuleShapeDesc capDesc;
    capDesc.radius = fRadius;
    capDesc.height = fHeight;
    capDesc.group = plSimDefs::kGroupAvatar;
    capDesc.materialIndex= plSimulationMgr::GetInstance()->GetMaterialIdx(scene, 0.0,0.0);
    actorDesc.shapes.pushBack(&capDesc);
    NxBodyDesc bodyDesc;
    bodyDesc.mass = AvatarMass;//1.f;
    actorDesc.body = &bodyDesc;
    bodyDesc.flags = NX_BF_KINEMATIC;
    bodyDesc.flags |=NX_BF_DISABLE_GRAVITY ;
    
    actorDesc.name = "AvatarTriggerKinematicGuy";
    fSeeking=false;
    try
    {
        fKinematicActor = scene->createActor(actorDesc);
    } catch (...)
    {
        hsAssert(false, "Actor creation crashed");
    }
#ifdef PHYSX_KINEMATIC_IS_DISABLED
    // initially start as in-active
    fKinematicActor->raiseActorFlag(NX_AF_DISABLE_COLLISION);
#endif
    // set the matrix to be the same as the controller's actor... that should orient it to be the same
    fKinematicActor->moveGlobalPose(actor->getGlobalPose());

    // the proxy for the debug display
    //hsAssert(!fProxyGen, "Already have proxy gen, double read?");

    hsColorRGBA physColor;
    hsScalar opac = 1.0f;

    // local avatar is light purple and transparent
    physColor.Set(.2f, .1f, .2f, 1.f);
    opac = 0.8f;

    /*
    // the avatar proxy doesn't seem to work... not sure why?
    fProxyGen = TRACKED_NEW plPhysicalProxy(hsColorRGBA().Set(0,0,0,1.f), physColor, opac);
    fProxyGen->Init(this);
    */
}
void plPXPhysicalControllerCore::ICreateController(const hsPoint3& pos)
{
    NxScene* scene = plSimulationMgr::GetInstance()->GetScene(fWorldKey);
    NxCapsuleControllerDesc desc;
    desc.position.x     = pos.fX;
    desc.position.y     = pos.fY;
    desc.position.z     = pos.fZ;
    desc.upDirection    = NX_Z;
    desc.slopeLimit     = kSLOPELIMIT;
    desc.skinWidth      = kPhysxSkinWidth;
    desc.stepOffset     = STEP_OFFSET;
    desc.callback       = &gMyReport;
    desc.userData       = this;
    desc.radius         = fRadius;
    desc.height         = fHeight;
    desc.interactionFlag = NXIF_INTERACTION_EXCLUDE;
    //desc.interactionFlag = NXIF_INTERACTION_INCLUDE;
    fController = (NxCapsuleController*)gControllerMgr.createController(scene, desc);

    // Change the avatars shape groups.  The avatar doesn't actually use these when
    // it's determining collision, but if you're standing still and an object runs
    // into you, it'll pass through without this.
    NxActor* actor = fController->getActor();
    NxShape* shape = actor->getShapes()[0];
    shape->setGroup(plSimDefs::kGroupAvatar);

    // need to create the non-bouncing object that can be used to trigger things while the avatar is doing behaviors.
    NxActorDesc actorDesc;
    NxCapsuleShapeDesc capDesc;
    capDesc.radius = fRadius;
    capDesc.height = fHeight;
    capDesc.group = plSimDefs::kGroupAvatar;
    actorDesc.shapes.pushBack(&capDesc);
    capDesc.materialIndex= plSimulationMgr::GetInstance()->GetMaterialIdx(scene, 0.0,0.0);
    actorDesc.globalPose=actor->getGlobalPose();
    NxBodyDesc bodyDesc;
    bodyDesc.mass = AvatarMass;
    actorDesc.body = &bodyDesc;
    bodyDesc.flags = NX_BF_KINEMATIC;
    bodyDesc.flags |=NX_BF_DISABLE_GRAVITY ;
    actorDesc.name = "AvatarTriggerKinematicGuy";
    fSeeking=false;
    try
    {
        fKinematicActor = scene->createActor(actorDesc);
    }
    catch (...)
    {
        hsAssert(false, "Actor creation crashed");
    }

    // set the matrix to be the same as the controller's actor... that should orient it to be the same
    //fKinematicActor->setGlobalPose(actor->getGlobalPose());

    // the proxy for the debug display
    //hsAssert(!fProxyGen, "Already have proxy gen, double read?");

    hsColorRGBA physColor;
    hsScalar opac = 1.0f;

    // local avatar is light purple and transparent
    physColor.Set(.2f, .1f, .2f, 1.f);
    opac = 0.8f;

    /*
    // the avatar proxy doesn't seem to work... not sure why?
    fProxyGen = TRACKED_NEW plPhysicalProxy(hsColorRGBA().Set(0,0,0,1.f), physColor, opac);
    fProxyGen->Init(this);
    */

}
void plPXPhysicalControllerCore::IDeleteController()
{
    if (fController)
    {
        gControllerMgr.releaseController(*fController);
        fController = nil;

        if (fKinematicActor)
        {
            NxScene* scene = plSimulationMgr::GetInstance()->GetScene(fWorldKey);
            scene->releaseActor(*fKinematicActor);
            fKinematicActor = nil;
        }
        plSimulationMgr::GetInstance()->ReleaseScene(fWorldKey);
    }
}

void plPXPhysicalControllerCore::IInformDetectors(bool entering,bool deferUntilNextSim=true)
{
    static const NxU32 DetectorFlag= 1<<plSimDefs::kGroupDetector;
    if (fController)
    {
#ifndef PLASMA_EXTERNAL_RELEASE
        DetectorLog("Informing from plPXPhysicalControllerCore::IInformDetectors");
#endif  
        NxScene* scene = plSimulationMgr::GetInstance()->GetScene(fWorldKey);
        int kNumofShapesToStore=30;
        NxCapsule cap;
        GetWorldSpaceCapsule(cap);
        NxShape* shapes[30];
        int numCollided=scene->overlapCapsuleShapes(cap,NX_ALL_SHAPES,kNumofShapesToStore,shapes,NULL,DetectorFlag,NULL,true);
        for (int i=0;i<numCollided;i++)
        {
            NxActor* myactor=&(shapes[i]->getActor());
            
            if (myactor)
            {
                plPXPhysical* physical = (plPXPhysical*)myactor->userData;
                if (physical)
                {
                    bool doReport = physical->DoReportOn(plSimDefs::kGroupAvatar);
                    if(doReport)
                    {
                        plCollideMsg* msg = TRACKED_NEW plCollideMsg;
                        msg->fOtherKey = fOwner;
                        msg->fEntering = entering;
                        msg->AddReceiver(physical->GetObjectKey());
                        if(!deferUntilNextSim)
                        {
                            DetectorLog("Sending an %s msg to %s" , entering? "entering":"exit", physical->GetObjectKey()->GetName());                    
                            msg->Send();
                        }
                        else
                        {
                            DetectorLog("Queuing an %s msg to %s, which will be sent after the client update" , entering? "entering":"exit", physical->GetObjectKey()->GetName());    
                            plgDispatch::Dispatch()->MsgQueue(msg);
                        }
                    }
                }
            }
        }
        DetectorLog("Done informing from plPXPhysicalControllerCore::IInformDetectors");
    }
}
void plPXPhysicalControllerCore::Move(hsVector3 displacement, unsigned int collideWith, unsigned int &collisionResults)
{
    collisionResults=0;
    if(fController)
    {
        NxVec3 dis(displacement.fX,displacement.fY,displacement.fZ);
        NxU32 colFlags = 0;
        this->fController->move(dis,collideWith,.00001,colFlags);
        if(colFlags&NXCC_COLLISION_DOWN)collisionResults|=kBottom;
        if(colFlags&NXCC_COLLISION_UP)collisionResults|=kTop;
        if(colFlags&&NXCC_COLLISION_SIDES)collisionResults|=kSides;
    }
    return; 
}
void plPXPhysicalControllerCore::Enable(bool enable)
{
    if (fEnabled != enable)
    {
        fEnabled = enable;
        if (fEnabled)
            fEnableChanged = true;
        else
        {
            // See ISendUpdates for why we don't re-enable right away
            fController->setCollision(fEnabled);
        }
    }
}

void plPXPhysicalControllerCore::SetSubworld(plKey world) 
{   
    if (fWorldKey != world)
    {
        bool wasEnabled = fEnabled;
#ifdef USE_PHYSX_CONVEXHULL_WORKAROUND
        // PHYSX FIXME - before leaving this world, sending leaving detector events if we are inside a convex hull detector
        hsPoint3 pos;
        IGetPositionSim(pos);
        plSimulationMgr::GetInstance()->UpdateDetectorsInScene(fWorldKey,GetOwner(),pos,false);
#endif  // USE_PHYSX_CONVEXHULL_WORKAROUND
        //need to inform detectors in the old world that we are leaving
        IInformDetectors(false);
        //done informing old world
        SimLog("Changing subworlds!");
        IDeleteController();
        SimLog("Deleted old controller");
        fWorldKey = world;
        if (GetSubworldCI())
            fPrevSubworldW2L = GetSubworldCI()->GetWorldToLocal();
        // Update our subworld position and rotation
        const plCoordinateInterface* subworldCI = GetSubworldCI();
        if (subworldCI)
        {
            const hsMatrix44& w2s = fPrevSubworldW2L;
            hsMatrix44 l2s = w2s * fLastGlobalLoc;
            l2s.GetTranslate(&fLocalPosition);
            fLocalRotation.SetFromMatrix44(l2s);
        }
        else
        {
            fLastGlobalLoc.GetTranslate(&fLocalPosition);
            fLocalRotation.SetFromMatrix44(fLastGlobalLoc);
        }
        hsMatrix44 w2l;
        fLastGlobalLoc.GetInverse(&w2l);
        if (fProxyGen)
            fProxyGen->SetTransform(fLastGlobalLoc, w2l);
        // Update the physical position
        SimLog("creating new controller");
        hsPoint3 PositionPlusOffset=fLocalPosition;
        PositionPlusOffset.fZ +=kPhysZOffset;
        //placing new controller and kinematic in the appropriate location
        ICreateController(PositionPlusOffset);
        RebuildCache();
    }
}
const plCoordinateInterface* plPXPhysicalControllerCore::GetSubworldCI() const 
{
    if (fWorldKey)
    {
        plSceneObject* so = plSceneObject::ConvertNoRef(fWorldKey->ObjectIsLoaded());
        if (so)
            return so->GetCoordinateInterface();
    }
    return nil;
}
// For the avatar SDL only
void plPXPhysicalControllerCore::GetState(hsPoint3& pos, float& zRot)
{   
    // Temporarily use the position point while we get the z rotation
    fLocalRotation.NormalizeIfNeeded();
    fLocalRotation.GetAngleAxis(&zRot, (hsVector3*)&pos);

    if (pos.fZ < 0)
        zRot = (2 * hsScalarPI) - zRot; // axis is backwards, so reverse the angle too

    pos = fLocalPosition;

}

void plPXPhysicalControllerCore::SetState(const hsPoint3& pos, float zRot)
{
    plSceneObject* so = plSceneObject::ConvertNoRef(fOwner->ObjectIsLoaded());
    if (so)
    {
        hsQuat worldRot;
        hsVector3 zAxis(0.f, 0.f, 1.f);
        worldRot.SetAngleAxis(zRot, zAxis);

        hsMatrix44 l2w, w2l;
        worldRot.MakeMatrix(&l2w);
        l2w.SetTranslate(&pos);

        // Localize new position and rotation to global coords if we're in a subworld
        const plCoordinateInterface* ci = GetSubworldCI();
        if (ci)
        {
            const hsMatrix44& subworldL2W = ci->GetLocalToWorld();
            l2w = subworldL2W * l2w;
        }
        l2w.GetInverse(&w2l);
        so->SetTransform(l2w, w2l);
        so->FlushTransform();
    }
}
// kinematic stuff .... should be just for when playing a behavior...
void plPXPhysicalControllerCore::Kinematic(bool state)
{
    if (fKinematic != state)
    {
        fKinematic = state;
        if (fKinematic)
        {
            // See ISendUpdates for why we don't re-enable right away
            fController->setCollision(false);
#ifdef PHYSX_KINEMATIC_IS_DISABLED
            fKinematicActor->clearActorFlag(NX_AF_DISABLE_COLLISION);
#endif
        }
        else
        {
            fKinematicChanged = true;
        }
    }
}
bool plPXPhysicalControllerCore::IsKinematic()
{
    return fKinematic;
}
void plPXPhysicalControllerCore::GetKinematicPosition(hsPoint3& pos)
{
    pos.Set(-1,-1,-1);
    if ( fKinematicActor )
    {
        NxVec3 klPos = fKinematicActor->getGlobalPosition();
        pos.Set(hsScalar(klPos.x), hsScalar(klPos.y), hsScalar(klPos.z) - kPhysZOffset);
    }
}
void plPXPhysicalControllerCore::UpdatePoststep( hsScalar delSecs)
{
    // Apparently the user data field of the controllers is broken
//  UInt32 count = gControllerMgr.getNbControllers();
//  NxController* controllers = (NxController*)gControllerMgr.getControllers();
// 
//  for (int i = 0; i < count; i++)
//  {
//      plPXPhysicalController* ac = (plPXPhysicalController*)controllers[i].getAppData();
    for (int i = 0; i < gControllers.size(); i++)
    {
        plPXPhysicalControllerCore* ac = gControllers[i];

        hsAssert(ac, "Bad avatar controller");
    
            gControllerMgr.updateControllers();
            if(ac->fMovementInterface)
                ac->Update(delSecs);
                
            if (ac->GetSubworldCI())
                ac->fPrevSubworldW2L = ac->GetSubworldCI()->GetWorldToLocal();
            else
            {
                if (!ac->fPrevSubworldW2L.IsIdentity())
                    ac->fPrevSubworldW2L.Reset();
            }
    }
}
void plPXPhysicalControllerCore::UpdatePrestep(hsScalar delSecs)
{
    for (int i = 0; i < gControllers.size(); i++)
    {
        plPXPhysicalControllerCore* ac = gControllers[i];

        hsAssert(ac, "Bad avatar controller");
        //FIXME
#ifndef PLASMA_EXTERNAL_RELEASE
        ac->fDbgCollisionInfo.SetCount(0);
#endif // PLASMA_EXTERNAL_RELEASE

        if (gRebuildCache&&ac->fController) 
            ac->fController->reportSceneChanged();
        //hsAssert(ac->fMovementInterface,"Updating a controller with out a movement strategy");
        if(ac)
        {   
            if(ac->fNeedsResize)ac->IHandleResize();
            ac->Apply(delSecs);
        }
#ifndef PLASMA_EXTERNAL_RELEASE
        if (fDebugDisplay)
            ac->IDrawDebugDisplay();
#endif // PLASMA_EXTERNAL_RELEASE
    }
    gRebuildCache = false;
}
void plPXPhysicalControllerCore::UpdatePostSimStep(hsScalar delSecs)
{
    for (int i = 0; i < gControllers.size(); i++)
    {
        plPXPhysicalControllerCore* ac = gControllers[i];
        hsAssert(ac, "Bad avatar controller");
        ac->PostStep(delSecs);
        
    }
}
void plPXPhysicalControllerCore::HandleEnableChanged()
{
        fEnableChanged = false;
        if(this->fBehavingLikeAnimatedPhys)
        {
            fController->setCollision(fEnabled);
        }
        else
        {
            fController->setCollision(false);
        }
#ifdef USE_PHYSX_CONVEXHULL_WORKAROUND
        // PHYSX FIXME - after re-enabling check to see if we are inside any convex hull detector regions
        hsPoint3 pos;
        IGetPositionSim(pos);
        plSimulationMgr::GetInstance()->UpdateDetectorsInScene(fWorldKey,GetOwner(),pos,fEnabled);
#endif  // USE_PHYSX_CONVEXHULL_WORKAROUND
        //IInformDetectors(true);
}

void plPXPhysicalControllerCore::HandleKinematicChanged()
{
        fKinematicChanged = false;
        if(this->fBehavingLikeAnimatedPhys)
        {
            fController->setCollision(true);
        }
        else
        {
            fController->setCollision(false);
        }
#ifdef PHYSX_KINEMATIC_IS_DISABLED
        fKinematicActor->raiseActorFlag(NX_AF_DISABLE_COLLISION);
#endif  // PHYSX_KINEMATIC_IS_DISABLED
}
void plPXPhysicalControllerCore::HandleKinematicEnableNextUpdate()
{
    fKinematicActor->clearActorFlag(NX_AF_DISABLE_COLLISION);
        fKinematicEnableNextUpdate = false;
}
void plPXPhysicalControllerCore::IHandleResize()
{

    UInt32 collideFlags =
        1<<plSimDefs::kGroupStatic |
        1<<plSimDefs::kGroupAvatarBlocker |
        1<<plSimDefs::kGroupDynamic;
    if(!IsSeeking())
    {
        collideFlags|=(1<<plSimDefs::kGroupExcludeRegion);
    }
    NxScene* myscene = plSimulationMgr::GetInstance()->GetScene(this->fWorldKey);
//  NxShape** response=TRACKED_NEW NxShape*[2];
    
    NxVec3 center(fLocalPosition.fX,fLocalPosition.fY,fLocalPosition.fZ+fPreferedRadius);
    NxSegment Seg(center,center);
    const NxCapsule newCap(Seg,fPreferedRadius);
    int numintersect =myscene->checkOverlapCapsule(newCap,NX_ALL_SHAPES,collideFlags);
    //with new capsule dimensions check for overlap
    //with objects we would collide with
    
    if(numintersect==0)
    {
        fHeight=fPreferedHeight;
        fRadius=fPreferedRadius;
        fController->setRadius(fRadius);
        fController->setHeight(fHeight);
        
        fNeedsResize=false;
    }

//  delete[] response;
}
void plPXPhysicalControllerCore::SetControllerDimensions(hsScalar radius, hsScalar height)
{
    fNeedsResize=false;
    if(fRadius!=radius)
    {
        fNeedsResize=true;
    }
    if(fHeight!=height)
    {
        fNeedsResize=true;
    }
    fPreferedRadius=radius;
    fPreferedHeight=height;
}

void plPXPhysicalControllerCore::LeaveAge()
{
    SetPushingPhysical(nil);
    if(fWorldKey) this->SetSubworld(nil);
    this->fMovementInterface->LeaveAge();
}
int plPXPhysicalControllerCore::SweepControllerPath(const hsPoint3& startPos, const hsPoint3& endPos, hsBool vsDynamics, hsBool vsStatics, 
                            UInt32& vsSimGroups, std::multiset< plControllerSweepRecord >& WhatWasHitOut)
{
    NxCapsule tempCap;
    tempCap.p0 =plPXConvert::Point( startPos);
    tempCap.p0.z = tempCap.p0.z + fPreferedRadius;
    tempCap.radius = fPreferedRadius ;
    tempCap.p1 = tempCap.p0;
    tempCap.p1.z = tempCap.p1.z + fPreferedHeight;

    NxVec3 vec;
    vec.x = endPos.fX - startPos.fX;
    vec.y = endPos.fY - startPos.fY;
    vec.z = endPos.fZ - startPos.fZ;

    int numberofHits = 0;
    int HitsReturned = 0;
    WhatWasHitOut.clear();
    NxScene *myscene = plSimulationMgr::GetInstance()->GetScene(fWorldKey);
    NxSweepQueryHit whatdidIhit[10];
    unsigned int flags = NX_SF_ALL_HITS;
    if(vsDynamics)
        flags |= NX_SF_DYNAMICS;
    if(vsStatics)
        flags |= NX_SF_STATICS;
    numberofHits = myscene->linearCapsuleSweep(tempCap, vec, flags, nil, 10, whatdidIhit, nil, vsSimGroups);
    if(numberofHits)
    {//we hit a dynamic object lets make sure it is not animatable
        for(int i=0; i<numberofHits; i++)
        {
            plControllerSweepRecord CurrentHit;
            CurrentHit.ObjHit=(plPhysical*)whatdidIhit[i].hitShape->getActor().userData;
            CurrentHit.Norm.fX = whatdidIhit[i].normal.x;
            CurrentHit.Norm.fY = whatdidIhit[i].normal.y;
            CurrentHit.Norm.fZ = whatdidIhit[i].normal.z;
            if(CurrentHit.ObjHit != nil)
            {
                hsPoint3 where;
                where.fX = whatdidIhit[i].point.x;
                where.fY = whatdidIhit[i].point.y;
                where.fZ = whatdidIhit[i].point.z;
                CurrentHit.locHit = where;
                CurrentHit.TimeHit = whatdidIhit[i].t ;
                WhatWasHitOut.insert(CurrentHit);
                HitsReturned++;
            }
        }
    }

    return HitsReturned;
}
void plPXPhysicalControllerCore::BehaveLikeAnimatedPhysical(hsBool actLikeAnAnimatedPhys)
{
    hsAssert(fKinematicActor, "Changing behavior, but plPXPhysicalControllerCore has no Kinematic actor associated with it");
    if(fBehavingLikeAnimatedPhys!=actLikeAnAnimatedPhys)
    {
        fBehavingLikeAnimatedPhys=actLikeAnAnimatedPhys;
        if(fKinematicActor)
        {
            if(actLikeAnAnimatedPhys)
            {
                //need to set BX Kinematic if true and kill any rotation
                fController->setCollision(fEnabled);
                fKinematicActor->raiseBodyFlag(NX_BF_KINEMATIC);
                fKinematicActor->clearBodyFlag(NX_BF_FROZEN_ROT);
                fKinematicActor->raiseBodyFlag(NX_BF_DISABLE_GRAVITY);
            }
            else
            {
                //don't really use the controller now don't bother with collisions 
                fController->setCollision(false);
                fKinematicActor->clearBodyFlag(NX_BF_KINEMATIC);
                fKinematicActor->raiseBodyFlag(NX_BF_FROZEN_ROT_X);
                fKinematicActor->raiseBodyFlag(NX_BF_FROZEN_ROT_Y);
                fKinematicActor->clearBodyFlag(NX_BF_DISABLE_GRAVITY);
                

            }
        }
    }
}

hsBool plPXPhysicalControllerCore::BehavingLikeAnAnimatedPhysical()
{
    hsAssert(fKinematicActor, "plPXPhysicalControllerCore is missing a kinematic actor");
    return fBehavingLikeAnimatedPhys;
}

void plPXPhysicalControllerCore::SetLinearVelocity(const hsVector3& linearVel)
{
    plPhysicalControllerCore::SetLinearVelocity(linearVel);
    if(fKinematicActor && !fBehavingLikeAnimatedPhys)
    {
        NxVec3 vel= plPXConvert::Vector(linearVel);
        fKinematicActor->setLinearVelocity(vel);
    }
}
void plPXPhysicalControllerCore::SetAngularVelocity(const hsScalar angvel)
{
    plPhysicalControllerCore::SetAngularVelocity(angvel);
    if(fKinematicActor && !fBehavingLikeAnimatedPhys)
    {
        NxVec3 vel(0.0f, 0.0f, angvel);
        fKinematicActor->setAngularVelocity(vel);
    }
}
void plPXPhysicalControllerCore::SetVelocities(const hsVector3& linearVel, hsScalar angVel)
{
    SetLinearVelocity(linearVel);
    SetAngularVelocity(angVel);
}

void plPXPhysicalControllerCore::IMatchControllerToKinematic()
{
    NxExtendedVec3 newpos;
    NxVec3 pos=fKinematicActor->getGlobalPosition();
    newpos.x=pos.x;
    newpos.y=pos.y;
    newpos.z=pos.z;
    fController->setPosition(newpos);
}
const hsVector3& plPXPhysicalControllerCore::GetLinearVelocity()
{
    if(BehavingLikeAnAnimatedPhysical())
        return fLinearVelocity;
    else
    {
        fLinearVelocity = plPXConvert::Vector(fKinematicActor->getLinearVelocity());
        return fLinearVelocity;
    }
}

// Make a visible object that can be viewed by users for debugging purposes.
plDrawableSpans* plPXPhysicalControllerCore::CreateProxy(hsGMaterial* mat, hsTArray<UInt32>& idx, plDrawableSpans* addTo)
{
    plDrawableSpans* myDraw = addTo;
    hsBool blended = ((mat->GetLayer(0)->GetBlendFlags() & hsGMatState::kBlendMask));
    float radius = fRadius;
    myDraw = plDrawableGenerator::GenerateSphericalDrawable(fLocalPosition, radius,
        mat, fLastGlobalLoc, blended,
        nil, &idx, myDraw);

/*
    plSceneObject* so = plSceneObject::ConvertNoRef(fOwner->ObjectIsLoaded());
    if (so)
    {
        hsBool blended = ((mat->GetLayer(0)->GetBlendFlags() & hsGMatState::kBlendMask));

        myDraw = plDrawableGenerator::GenerateConicalDrawable(fRadius*10, fHeight*10,
            mat, so->GetLocalToWorld(), blended,
            nil, &idx, myDraw);
    }
*/
    return myDraw;
}
#ifndef PLASMA_EXTERNAL_RELEASE

void plPXPhysicalControllerCore::IDrawDebugDisplay()
{
    plDebugText     &debugTxt = plDebugText::Instance();
    char            strBuf[ 2048 ];
    int             lineHeight = debugTxt.GetFontSize() + 4;
    UInt32          scrnWidth, scrnHeight;

    debugTxt.GetScreenSize( &scrnWidth, &scrnHeight );
    int y = 10;
    int x = 10;

    sprintf(strBuf, "Controller Count: %d", gControllers.size());
    debugTxt.DrawString(x, y, strBuf);
    y += lineHeight;

    debugTxt.DrawString(x, y, "Avatar Collisions:");
    y += lineHeight;

    int i;
    for (i = 0; i < fDbgCollisionInfo.GetCount(); i++)
    {
        hsVector3 normal = fDbgCollisionInfo[i].fNormal;
        char *overlapStr = fDbgCollisionInfo[i].fOverlap ? "yes" : "no";
        hsScalar angle = hsScalarRadToDeg(hsACosine(normal * hsVector3(0, 0, 1)));
        sprintf(strBuf, "    Obj: %s, Normal: (%.2f, %.2f, %.2f), Angle(%.1f), Overlap(%3s)",
                fDbgCollisionInfo[i].fSO->GetKeyName(),
                normal.fX, normal.fY, normal.fZ, angle, overlapStr);
        debugTxt.DrawString(x, y, strBuf);
        y += lineHeight;
    }
}
#endif PLASMA_EXTERNAL_RELEASE