/*==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/>.

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 "plPhysicalSDLModifier.h"

#include "hsGeometry3.h"
#include "plPhysical.h"
#include "plSDL/plSDL.h"
#include "pnSceneObject/plSceneObject.h"
#include "pnSceneObject/plSimulationInterface.h"
#include "pnNetCommon/plNetApp.h"
#include "hsQuat.h"
//#include "plHavok1/plSimulationMgr.h"
#include "plStatusLog/plStatusLog.h"

// static vars
static const char* kStrLinear       = "linear";
static const char* kStrAngular      = "angular";
static const char* kStrPosition     = "position";
static const char* kStrOrientation  = "orientation";

int plPhysicalSDLModifier::fLogLevel = 0;

static void IGetVars(plStateDataRecord::SimpleVarsList& vars,
    hsPoint3& pos, bool& isPosSet,
    hsQuat& rot, bool& isRotSet,
    hsVector3& linV, bool& isLinVSet,
    hsVector3& angV, bool& isAngVSet);

//
// get current state from physical
// fill out state data rec
//
void plPhysicalSDLModifier::IPutCurrentStateIn(plStateDataRecord* dstState)
{
    plPhysical* phys = IGetPhysical();

    // get latest state
    hsPoint3 curPos;
    hsQuat curOrientation;
    hsVector3 curLinear, curAngular;
    phys->GetSyncState(curPos, curOrientation, curLinear, curAngular);

    // put it in sdl state record
    dstState->FindVar(kStrPosition)->Set(&curPos.fX);
    dstState->FindVar(kStrOrientation)->Set(&curOrientation.fX);
    dstState->FindVar(kStrLinear)->Set(&curLinear.fX);
    dstState->FindVar(kStrAngular)->Set(&curAngular.fX);

    if (fLogLevel > 1)
        ILogState(dstState, false, "PUT", plStatusLog::kWhite);
}

void plPhysicalSDLModifier::ISetCurrentStateFrom(const plStateDataRecord* srcState)
{
    plPhysical* phys = IGetPhysical();
    
    // FIXME PHYSX

//  if(phys->GetBody()->isFixed())
//  {
//      plSimulationMgr::Log("Received synch for fixed body %s", phys->GetKey()->GetName());
//      return;
//  }
//  else if (phys->GetProperty(plSimulationInterface::kPinned))
//  {
//      // This is mainly intended for avatars. When pinning them (like in a multistage),
//      // we don't want physical updates to sneak in due to network lag. If necessary,
//      // this could be made a separate property on the physical, orthagonal to kPinned.
//      return;
//  }
//  else
    {
        hsPoint3 pos;
        bool isPosSet;
        hsQuat rot;
        bool isRotSet;
        hsVector3 linV;
        bool isLinVSet;
        hsVector3 angV;
        bool isAngVSet;

        plStateDataRecord::SimpleVarsList vars;
        srcState->GetUsedVars(&vars);
        IGetVars(vars, pos, isPosSet, rot, isRotSet, linV, isLinVSet, angV, isAngVSet);

        if (fLogLevel > 0)
            ILogState(srcState, false, "RCV", plStatusLog::kGreen);

        phys->SetSyncState(
            isPosSet ? &pos : nil,
            isRotSet ? &rot : nil,
            isLinVSet ? &linV : nil,
            isAngVSet ? &angV : nil);
    }
}

void plPhysicalSDLModifier::ISentState(const plStateDataRecord* sentState)
{
    if (fLogLevel > 0)
    {
        ILogState(sentState, true, "SND", plStatusLog::kYellow);

//      plPhysical* phys = IGetPhysical();
//      if (!phys->GetBody()->isActive())
//          IGetLog()->AddLineF("Phys %s sent state because it deactivated", phys->GetKeyName());
    }
}

static void IGetVars(plStateDataRecord::SimpleVarsList& vars,
    hsPoint3& pos, bool& isPosSet,
    hsQuat& rot, bool& isRotSet,
    hsVector3& linV, bool& isLinVSet,
    hsVector3& angV, bool& isAngVSet)
{
    isPosSet = false;
    isRotSet = false;
    isLinVSet = false;
    isAngVSet = false;

    int num = vars.size();
    for (int i = 0; i < num; i++)
    {
        if (vars[i]->IsNamed(kStrPosition))
        {
            vars[i]->Get(&pos.fX);
            isPosSet= true;
        }
        else
        if (vars[i]->IsNamed(kStrOrientation))
        {
            vars[i]->Get(&rot.fX);
            isRotSet = true;
        }
        else
        if (vars[i]->IsNamed(kStrLinear))
        {
            vars[i]->Get(&linV.fX);
            isLinVSet = true;
        }
        else
        if (vars[i]->IsNamed(kStrAngular))
        {
            vars[i]->Get(&angV.fX);
            isAngVSet = true;
        }
        else
        if (vars[i]->IsNamed("subworld"))
        {
            // Unused
        }
        else
        {
            hsAssert(false, "Unknown var name");
        }
    }
}

void plPhysicalSDLModifier::ILogState(const plStateDataRecord* state, bool useDirty, const char* prefix, UInt32 color)
{
    hsPoint3 pos;
    bool isPosSet;
    hsQuat rot;
    bool isRotSet;
    hsVector3 linV;
    bool isLinVSet;
    hsVector3 angV;
    bool isAngVSet;

    plStateDataRecord::SimpleVarsList vars;
    if (useDirty)
        state->GetDirtyVars(&vars);
    else
        state->GetUsedVars(&vars);

    IGetVars(vars, pos, isPosSet, rot, isRotSet, linV, isLinVSet, angV, isAngVSet);

    plPhysical* phys = IGetPhysical();

    std::string log = xtl::format("%s: %s", phys->GetKeyName(), prefix);

    if (isPosSet)
        log += xtl::format(" Pos=%.1f %.1f %.1f", pos.fX, pos.fY, pos.fZ);
    else
        log += " Pos=None";

    if (isLinVSet)
        log += xtl::format(" LinV=%.1f %.1f %.1f", linV.fX, linV.fY, linV.fZ);
    else
        log += " LinV=None";

    if (isAngVSet)
        log += xtl::format(" AngV=%.1f %.1f %.1f", angV.fX, angV.fY, angV.fZ);
    else
        log += " AngV=None";

    if (isRotSet)
        log += xtl::format(" Rot=%.1f %.1f %.1f %.1f", rot.fX, rot.fY, rot.fZ, rot.fW);
    else
        log += " Rot=None";

    IGetLog()->AddLine(log.c_str(), color);
}

plStatusLog* plPhysicalSDLModifier::IGetLog()
{
    static plStatusLog* gLog = nil;
    if (!gLog)
    {
        gLog = plStatusLogMgr::GetInstance().CreateStatusLog(20, "PhysicsSDL.log",
                    plStatusLog::kFilledBackground |
                    plStatusLog::kTimestamp |
                    plStatusLog::kDeleteForMe |
                    plStatusLog::kAlignToTop);
    }

    return gLog;
}

plPhysical* plPhysicalSDLModifier::IGetPhysical()
{
    plPhysical* phys = nil;

    plSceneObject* sobj = GetTarget();
    if (sobj)
    {
        const plSimulationInterface* si = sobj->GetSimulationInterface();
        if (si)
            phys = si->GetPhysical();
    }

    hsAssert(phys, "nil hkPhysical");
    return phys;
}