/*
Copyright (C) 2009 Sony Computer Entertainment Inc.
All rights reserved.

This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:

1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.

*/

#ifndef BT_TRBSTATEVEC_H__
#define BT_TRBSTATEVEC_H__

#include <stdlib.h>
#ifdef PFX_USE_FREE_VECTORMATH
#include "vecmath/vmInclude.h"
#else
#include "vectormath/vmInclude.h"
#endif //PFX_USE_FREE_VECTORMATH


#include "PlatformDefinitions.h"


static inline vmVector3 read_Vector3(const float* p)
{
vmVector3 v;
loadXYZ(v, p);
return v;
}

static inline vmQuat read_Quat(const float* p)
{
vmQuat vq;
loadXYZW(vq, p);
return vq;
}

static inline void store_Vector3(const vmVector3 &src, float* p)
{
vmVector3 v = src;
storeXYZ(v, p);
}

static inline void store_Quat(const vmQuat &src, float* p)
{
vmQuat vq = src;
storeXYZW(vq, p);
}

// Motion Type
enum {
PfxMotionTypeFixed = 0,
PfxMotionTypeActive,
PfxMotionTypeKeyframe,
PfxMotionTypeOneWay,
PfxMotionTypeTrigger,
PfxMotionTypeCount
};

#define PFX_MOTION_MASK_DYNAMIC 0x0a // Active,OneWay
#define PFX_MOTION_MASK_STATIC 0x95 // Fixed,Keyframe,Trigger,Sleeping
#define PFX_MOTION_MASK_SLEEP 0x0e // Can sleep
#define PFX_MOTION_MASK_TYPE 0x7f

//
// Rigid Body state
//

#ifdef __CELLOS_LV2__
ATTRIBUTE_ALIGNED128(class) TrbState
#else
ATTRIBUTE_ALIGNED16(class) TrbState
#endif

{
public:
TrbState()
{
setMotionType(PfxMotionTypeActive);
contactFilterSelf=contactFilterTarget=0xffffffff;
deleted = 0;
mSleeping = 0;
useSleep = 1;
trbBodyIdx=0;
mSleepCount=0;
useCcd = 0;
useContactCallback = 0;
useSleepCallback = 0;
linearDamping = 1.0f;
angularDamping = 0.99f;
}

TrbState(const uint8_t m, const vmVector3& x, const vmQuat& q, const vmVector3& v, const vmVector3& omega );

uint16_t mSleepCount;
uint8_t mMotionType;
uint8_t deleted : 1;
uint8_t mSleeping : 1;
uint8_t useSleep : 1;
uint8_t useCcd : 1;
uint8_t useContactCallback : 1;
uint8_t useSleepCallback : 1;

uint16_t trbBodyIdx;
uint32_t contactFilterSelf;
uint32_t contactFilterTarget;

float center[3]; // AABB center(World)
float half[3]; // AABB half(World)

float linearDamping;
float angularDamping;

float deltaLinearVelocity[3];
float deltaAngularVelocity[3];

float fX[3]; // position
float fQ[4]; // orientation
float fV[3]; // velocity
float fOmega[3]; // angular velocity

inline void setZero(); // Zeroes out the elements
inline void setIdentity(); // Sets the rotation to identity and zeroes out the other elements

bool isDeleted() const {return deleted==1;}

uint16_t getRigidBodyId() const {return trbBodyIdx;}
void setRigidBodyId(uint16_t i) {trbBodyIdx = i;}


uint32_t getContactFilterSelf() const {return contactFilterSelf;}
void setContactFilterSelf(uint32_t filter) {contactFilterSelf = filter;}

uint32_t getContactFilterTarget() const {return contactFilterTarget;}
void setContactFilterTarget(uint32_t filter) {contactFilterTarget = filter;}

float getLinearDamping() const {return linearDamping;}
float getAngularDamping() const {return angularDamping;}

void setLinearDamping(float damping) {linearDamping=damping;}
void setAngularDamping(float damping) {angularDamping=damping;}


uint8_t getMotionType() const {return mMotionType;}
void setMotionType(uint8_t t) {mMotionType = t;mSleeping=0;mSleepCount=0;}

uint8_t getMotionMask() const {return (1<<mMotionType)|(mSleeping<<7);}

bool isAsleep() const {return mSleeping==1;}
bool isAwake() const {return mSleeping==0;}

void wakeup() {mSleeping=0;mSleepCount=0;}
void sleep() {if(useSleep) {mSleeping=1;mSleepCount=0;}}

uint8_t getUseSleep() const {return useSleep;}
void setUseSleep(uint8_t b) {useSleep=b;}

uint8_t getUseCcd() const {return useCcd;}
void setUseCcd(uint8_t b) {useCcd=b;}

uint8_t getUseContactCallback() const {return useContactCallback;}
void setUseContactCallback(uint8_t b) {useContactCallback=b;}

uint8_t getUseSleepCallback() const {return useSleepCallback;}
void setUseSleepCallback(uint8_t b) {useSleepCallback=b;}

void incrementSleepCount() {mSleepCount++;}
void resetSleepCount() {mSleepCount=0;}
uint16_t getSleepCount() const {return mSleepCount;}

vmVector3 getPosition() const {return read_Vector3(fX);}
vmQuat getOrientation() const {return read_Quat(fQ);}
vmVector3 getLinearVelocity() const {return read_Vector3(fV);}
vmVector3 getAngularVelocity() const {return read_Vector3(fOmega);}
vmVector3 getDeltaLinearVelocity() const {return read_Vector3(deltaLinearVelocity);}
vmVector3 getDeltaAngularVelocity() const {return read_Vector3(deltaAngularVelocity);}

void setPosition(const vmVector3 &pos) {store_Vector3(pos, fX);}
void setLinearVelocity(const vmVector3 &vel) {store_Vector3(vel, fV);}
void setAngularVelocity(const vmVector3 &vel) {store_Vector3(vel, fOmega);}
void setDeltaLinearVelocity(const vmVector3 &vel) {store_Vector3(vel, deltaLinearVelocity);}
void setDeltaAngularVelocity(const vmVector3 &vel) {store_Vector3(vel, deltaAngularVelocity);}
void setOrientation(const vmQuat &rot) {store_Quat(rot, fQ);}

inline void setAuxils(const vmVector3 &centerLocal,const vmVector3 &halfLocal);
inline void setAuxilsCcd(const vmVector3 &centerLocal,const vmVector3 &halfLocal,float timeStep);
inline void reset();
};

inline
TrbState::TrbState(const uint8_t m, const vmVector3& x, const vmQuat& q, const vmVector3& v, const vmVector3& omega)
{
setMotionType(m);
fX[0] = x[0];
fX[1] = x[1];
fX[2] = x[2];
fQ[0] = q[0];
fQ[1] = q[1];
fQ[2] = q[2];
fQ[3] = q[3];
fV[0] = v[0];
fV[1] = v[1];
fV[2] = v[2];
fOmega[0] = omega[0];
fOmega[1] = omega[1];
fOmega[2] = omega[2];
contactFilterSelf=contactFilterTarget=0xffff;
trbBodyIdx=0;
mSleeping = 0;
deleted = 0;
useSleep = 1;
useCcd = 0;
useContactCallback = 0;
useSleepCallback = 0;
mSleepCount=0;
linearDamping = 1.0f;
angularDamping = 0.99f;
}

inline void
TrbState::setIdentity()
{
fX[0] = 0.0f;
fX[1] = 0.0f;
fX[2] = 0.0f;
fQ[0] = 0.0f;
fQ[1] = 0.0f;
fQ[2] = 0.0f;
fQ[3] = 1.0f;
fV[0] = 0.0f;
fV[1] = 0.0f;
fV[2] = 0.0f;
fOmega[0] = 0.0f;
fOmega[1] = 0.0f;
fOmega[2] = 0.0f;
}

inline void
TrbState::setZero()
{
fX[0] = 0.0f;
fX[1] = 0.0f;
fX[2] = 0.0f;
fQ[0] = 0.0f;
fQ[1] = 0.0f;
fQ[2] = 0.0f;
fQ[3] = 0.0f;
fV[0] = 0.0f;
fV[1] = 0.0f;
fV[2] = 0.0f;
fOmega[0] = 0.0f;
fOmega[1] = 0.0f;
fOmega[2] = 0.0f;
}

inline void
TrbState::setAuxils(const vmVector3 &centerLocal,const vmVector3 &halfLocal)
{
vmVector3 centerW = getPosition() + rotate(getOrientation(),centerLocal);
vmVector3 halfW = absPerElem(vmMatrix3(getOrientation())) * halfLocal;
center[0] = centerW[0];
center[1] = centerW[1];
center[2] = centerW[2];
half[0] = halfW[0];
half[1] = halfW[1];
half[2] = halfW[2];
}

inline void
TrbState::setAuxilsCcd(const vmVector3 &centerLocal,const vmVector3 &halfLocal,float timeStep)
{
vmVector3 centerW = getPosition() + rotate(getOrientation(),centerLocal);
vmVector3 halfW = absPerElem(vmMatrix3(getOrientation())) * halfLocal;

vmVector3 diffvec = getLinearVelocity()*timeStep;

vmVector3 newCenter = centerW + diffvec;
vmVector3 aabbMin = minPerElem(newCenter - halfW,centerW - halfW);
vmVector3 aabbMax = maxPerElem(newCenter + halfW,centerW + halfW);

centerW = 0.5f * (aabbMin + aabbMax);
halfW =0.5f * (aabbMax - aabbMin);

center[0] = centerW[0];
center[1] = centerW[1];
center[2] = centerW[2];

half[0] = halfW[0];
half[1] = halfW[1];
half[2] = halfW[2];
}

inline
void TrbState::reset()
{
#if 0
mSleepCount = 0;
mMotionType = PfxMotionTypeActive;
mDeleted = 0;
mSleeping = 0;
mUseSleep = 1;
mUseCcd = 0;
mUseContactCallback = 0;
mUseSleepCallback = 0;
mRigidBodyId = 0;
mContactFilterSelf = 0xffffffff;
mContactFilterTarget = 0xffffffff;
mLinearDamping = 1.0f;
mAngularDamping = 0.99f;
mPosition = vmVector3(0.0f);
mOrientation = vmQuat::identity();
mLinearVelocity = vmVector3(0.0f);
mAngularVelocity = vmVector3(0.0f);
#endif

setMotionType(PfxMotionTypeActive);
contactFilterSelf=contactFilterTarget=0xffffffff;
deleted = 0;
mSleeping = 0;
useSleep = 1;
trbBodyIdx=0;
mSleepCount=0;
useCcd = 0;
useContactCallback = 0;
useSleepCallback = 0;
linearDamping = 1.0f;
angularDamping = 0.99f;
}

#endif //BT_TRBSTATEVEC_H__