#include "PajaTypes.h"
#include "Vector3C.h"
#include "ControllerC.h"
#include "ContFloatC.h"
#include "ContVector3C.h"
#include "WSMObjectC.h"
#include "noise.h"

using namespace Composition;
using namespace PajaTypes;
using namespace FileIO;

static
void
TRACE( const char* szFormat, ...  )
{
	char	szMsg[256];
	va_list	rList;
	va_start( rList, szFormat );
	_vsnprintf( szMsg, 255, szFormat, rList );
	va_end( rList );
	OutputDebugString( szMsg );
}

// 3dsmax constants
#define TIME_TICKSPERSEC	4800
#define	FORCE_SCALEFACTOR	float32(1200 * 1200) / float32(TIME_TICKSPERSEC * TIME_TICKSPERSEC)




//
// Wind
//

WindC::WindC( WSMObjectC* pObj ) :
	ForceFieldI( pObj ),
	m_f32Strength( 0 ),
	m_f32Decay( 0 ),
	m_ui32Type( FORCE_PLANAR ),
	m_f32Size( 0 ),
	m_f32Turbulence( 0 ),
	m_f32Frequency( 0 ),
	m_f32Scale( 0 ),
	m_pStrengthCont( 0 ),
	m_pDecayCont( 0 ),
	m_pTurbulenceCont( 0 ),
	m_pFrequencyCont( 0 ),
	m_pScaleCont( 0 )
{
	// empty
}

WindC::~WindC()
{
	delete m_pStrengthCont;
	delete m_pDecayCont;
	delete m_pTurbulenceCont;
	delete m_pFrequencyCont;
	delete m_pScaleCont;
}

void
WindC::eval_state( int32 i32Time )
{
	if( m_pStrengthCont )
		m_f32Strength = m_pStrengthCont->get_value( i32Time );
	if( m_pTurbulenceCont )
		m_f32Turbulence = m_pTurbulenceCont->get_value( i32Time );
	if( m_pFrequencyCont )
		m_f32Frequency = m_pFrequencyCont->get_value( i32Time );
	if( m_pScaleCont )
		m_f32Scale = m_pScaleCont->get_value( i32Time );
	if( m_pDecayCont )
		m_f32Decay = m_pDecayCont->get_value( i32Time );
}


static float32
r_turbulence( const Vector3C& rP, float32 f32Freq )
{
	float32	f32Vec[3];
	f32Vec[0] = rP[0] * f32Freq;
	f32Vec[1] = rP[1] * f32Freq;
	f32Vec[2] = rP[2] * f32Freq;
	return noise3( f32Vec );
}

Vector3C
WindC::force( int32 i32T, float32* pPos, float32* pVel, int32 i32Index )
{

	Vector3C	rForce;
	float32		f32Strength, f32Decay, f32Turb;

	f32Decay = get_decay();
	f32Turb = get_turbulence();

	if( f32Decay < 0.0f )
		f32Decay = 0.0f;

	Matrix3C	rTM = m_pObject->get_tm();
	
//	if( get_wind_type() == FORCE_SPHERICAL || f32Decay != 0.0f ) {

		f32Strength = get_strength();

		if( get_wind_type() == FORCE_PLANAR ) {
			rForce = rTM[1].normalize();
			if( f32Decay != 0.0f ) {
				Vector3C	rDiff = Vector3C( pPos ) - rTM[3];
				float32 f32Dist = rForce.dot( rDiff );
				f32Strength *= (float32)exp( -f32Decay * f32Dist );
			}			
			rForce *= f32Strength * 0.0001f * FORCE_SCALEFACTOR;
		}
		else {
			float32 f32Dist;
			rForce = Vector3C( pPos ) - rTM[3];
			f32Dist = rForce.length();
			if( f32Dist != 0.0f )
				rForce /= f32Dist;
			if( f32Decay != 0.0f ) {				
				f32Strength *= (float32)exp( -f32Decay * f32Dist );
			}			
			rForce *= f32Strength * 0.0001f * FORCE_SCALEFACTOR;
		}
//	}	

	if( f32Turb != 0.0f ) {

		float32		f32Freq = get_frequency();
		float32		f32Scale = get_scale();

		Vector3C	rTF;
		Vector3C	rPT = Vector3C( pPos ) - rTM[3];
		Vector3C	rP;

		f32Freq *= 0.01f;
		f32Turb *= 0.0001f * FORCE_SCALEFACTOR;
		
		rP     = rPT;
		rP[0]  = f32Freq * float( i32T );
		rTF[0] = r_turbulence( rP, f32Scale );

		rP     = rPT;
		rP[1]  = f32Freq * float( i32T );
		rTF[1] = r_turbulence( rP, f32Scale );

		rP     = rPT;
		rP[2]  = f32Freq * float( i32T );
		rTF[2] = r_turbulence( rP, f32Scale );
		
		rForce += (f32Turb * rTF);
	}
	
	return rForce;
}

uint32
WindC::get_type()
{
	return WSM_WIND;
}

void
WindC::set_strength( float32 f32Val )
{
	m_f32Strength = f32Val;
}

float32
WindC::get_strength() const
{
	return m_f32Strength;
}

void
WindC::set_decay( float32 f32Val )
{
	m_f32Decay = f32Val;
}

float32
WindC::get_decay() const
{
	return m_f32Decay;
}

void
WindC::set_size( float32 f32Val )
{
	m_f32Size = f32Val;
}

float32
WindC::get_size() const
{
	return m_f32Size;
}

void
WindC::set_turbulence( float32 f32Val )
{
	m_f32Turbulence = f32Val;
}

float32
WindC::get_turbulence() const
{
	return m_f32Turbulence;
}

void
WindC::set_frequency( float32 f32Val )
{
	m_f32Frequency = f32Val;
}

float32
WindC::get_frequency() const
{
	return m_f32Frequency;
}

void
WindC::set_scale( float32 f32Val )
{
	m_f32Scale = f32Val;
}

float32
WindC::get_scale() const
{
	return m_f32Scale;
}

void
WindC::set_wind_type( uint32 ui32Val )
{
	m_ui32Type = ui32Val;
}

uint32
WindC::get_wind_type() const
{
	return m_ui32Type;
}

void
WindC::set_strength_cont( ContFloatC* pCont )
{
	m_pStrengthCont = pCont;
}

void
WindC::set_decay_cont( ContFloatC* pCont )
{
	m_pDecayCont = pCont;
}

void
WindC::set_turbulence_cont( ContFloatC* pCont )
{
	m_pTurbulenceCont = pCont;
}

void
WindC::set_frequency_cont( ContFloatC* pCont )
{
	m_pFrequencyCont = pCont;
}

void
WindC::set_scale_cont( ContFloatC* pCont )
{
	m_pScaleCont = pCont;
}

enum WindChunksE {
	CHUNK_WIND_BASE				= 0x1000,
	CHUNK_WIND_STRENGTH_CONT	= 0x2000,
	CHUNK_WIND_DECAY_CONT		= 0x2001,
	CHUNK_WIND_TURBULENCE_CONT	= 0x2002,
	CHUNK_WIND_FREQUENCY_CONT	= 0x2003,
	CHUNK_WIND_SCALE_CONT		= 0x2004,
};

const uint32	WIND_VERSION = 1;

uint32
WindC::save( FileIO::SaveC* pSave )
{
	uint32	ui32Error = IO_OK;

	eval_state( 0 );

	std::string	sStr;

	// base
	pSave->begin_chunk( CHUNK_WIND_BASE, WIND_VERSION );
		ui32Error = pSave->write( &m_f32Strength, sizeof( m_f32Strength ) );
		ui32Error = pSave->write( &m_f32Decay, sizeof( m_f32Decay ) );
		ui32Error = pSave->write( &m_ui32Type, sizeof( m_ui32Type ) );
		ui32Error = pSave->write( &m_f32Size, sizeof( m_f32Size ) );
		ui32Error = pSave->write( &m_f32Turbulence, sizeof( m_f32Turbulence ) );
		ui32Error = pSave->write( &m_f32Frequency, sizeof( m_f32Frequency ) );
		ui32Error = pSave->write( &m_f32Scale, sizeof( m_f32Scale ) );
	pSave->end_chunk();

	if( m_pDecayCont ) {
		pSave->begin_chunk( CHUNK_WIND_DECAY_CONT, WIND_VERSION );
			ui32Error = m_pDecayCont->save( pSave );
		pSave->end_chunk();
	}

	if( m_pFrequencyCont ) {
		pSave->begin_chunk( CHUNK_WIND_FREQUENCY_CONT, WIND_VERSION );
			ui32Error = m_pFrequencyCont->save( pSave );
		pSave->end_chunk();
	}

	if( m_pScaleCont ) {
		pSave->begin_chunk( CHUNK_WIND_SCALE_CONT, WIND_VERSION );
			ui32Error = m_pScaleCont->save( pSave );
		pSave->end_chunk();
	}

	if( m_pStrengthCont ) {
		pSave->begin_chunk( CHUNK_WIND_STRENGTH_CONT, WIND_VERSION );
			ui32Error = m_pStrengthCont->save( pSave );
		pSave->end_chunk();
	}

	if( m_pTurbulenceCont ) {
		pSave->begin_chunk( CHUNK_WIND_TURBULENCE_CONT, WIND_VERSION );
			ui32Error = m_pTurbulenceCont->save( pSave );
		pSave->end_chunk();
	}

	return ui32Error;
}

uint32
WindC::load( FileIO::LoadC* pLoad )
{
	uint32	ui32Error = IO_OK;

	while( (ui32Error = pLoad->open_chunk()) == IO_OK ) {

		switch( pLoad->get_chunk_id() ) {

		case CHUNK_WIND_BASE:
			if( pLoad->get_chunk_version() == WIND_VERSION ) {
				ui32Error = pLoad->read( &m_f32Strength, sizeof( m_f32Strength ) );
				ui32Error = pLoad->read( &m_f32Decay, sizeof( m_f32Decay ) );
				ui32Error = pLoad->read( &m_ui32Type, sizeof( m_ui32Type ) );
				ui32Error = pLoad->read( &m_f32Size, sizeof( m_f32Size ) );
				ui32Error = pLoad->read( &m_f32Turbulence, sizeof( m_f32Turbulence ) );
				ui32Error = pLoad->read( &m_f32Frequency, sizeof( m_f32Frequency ) );
				ui32Error = pLoad->read( &m_f32Scale, sizeof( m_f32Scale ) );
			}
			break;

		case CHUNK_WIND_STRENGTH_CONT:
			if( pLoad->get_chunk_version() == WIND_VERSION ) {
				ContFloatC*	pCont = new ContFloatC;
				ui32Error = pCont->load( pLoad );
				m_pStrengthCont = pCont;
			}
			break;

		case CHUNK_WIND_DECAY_CONT:
			if( pLoad->get_chunk_version() == WIND_VERSION ) {
				ContFloatC*	pCont = new ContFloatC;
				ui32Error = pCont->load( pLoad );
				m_pDecayCont = pCont;
			}
			break;

		case CHUNK_WIND_TURBULENCE_CONT:
			if( pLoad->get_chunk_version() == WIND_VERSION ) {
				ContFloatC*	pCont = new ContFloatC;
				ui32Error = pCont->load( pLoad );
				m_pTurbulenceCont = pCont;
			}
			break;

		case CHUNK_WIND_FREQUENCY_CONT:
			if( pLoad->get_chunk_version() == WIND_VERSION ) {
				ContFloatC*	pCont = new ContFloatC;
				ui32Error = pCont->load( pLoad );
				m_pFrequencyCont = pCont;
			}
			break;

		case CHUNK_WIND_SCALE_CONT:
			if( pLoad->get_chunk_version() == WIND_VERSION ) {
				ContFloatC*	pCont = new ContFloatC;
				ui32Error = pCont->load( pLoad );
				m_pScaleCont = pCont;
			}
			break;

		default:
			assert( 0 );
		}

		pLoad->close_chunk();

		if( ui32Error != IO_OK && ui32Error != IO_END ) {
			return ui32Error;
		}
	}

	return ui32Error;
}


//
// Gravity
//

GravityC::GravityC( WSMObjectC* pObj ) :
	ForceFieldI( pObj ),
	m_f32Strength( 0 ),
	m_f32Decay( 0 ),
	m_f32Size( 0 ),
	m_ui32Type( FORCE_PLANAR ),
	m_pStrengthCont( 0 ),
	m_pDecayCont( 0 )
{
	// empty
}

GravityC::~GravityC()
{
	delete m_pStrengthCont;
	delete m_pDecayCont;
}

void
GravityC::eval_state( int32 i32Time )
{
	if( m_pStrengthCont )
		m_f32Strength = m_pStrengthCont->get_value( i32Time );
	if( m_pDecayCont )
		m_f32Decay = m_pDecayCont->get_value( i32Time );
}

Vector3C
GravityC::force( int32 i32T, float32* pPos, float32* pVel, int32 i32Index )
{
	Vector3C	rForce;

	float32	f32Strength, f32Decay;

	f32Decay = get_decay();
	if( f32Decay < 0.0f )
		f32Decay = 0.0f;

//	if( get_gravity_type() == FORCE_SPHERICAL || f32Decay != 0.0f ) {

		Matrix3C	rTM = m_pObject->get_tm();

		f32Strength = get_strength();

		if( get_gravity_type() == FORCE_PLANAR ) {
			rForce = rTM[1].normalize();
			if( f32Decay != 0.0f ) {
				Vector3C	rDiff = Vector3C( pPos ) - rTM[3];
				float32 f32Dist = rForce.dot( rDiff );
				f32Strength *= (float32)exp( -f32Decay * f32Dist );
			}			
			rForce *= f32Strength * 0.0001f * FORCE_SCALEFACTOR;

		}
		else {
			float32 f32Dist;
			rForce = rTM[3] - Vector3C( pPos );
			f32Dist = rForce.length();
			if( f32Dist != 0.0f )
				rForce /= f32Dist;
			if( f32Decay != 0.0f ) {				
				f32Strength *= (float32)exp( -f32Decay * f32Dist );
			}			
			rForce *= f32Strength * 0.0001f * FORCE_SCALEFACTOR;
		}
//	}	

	return rForce;
}

uint32
GravityC::get_type()
{
	return WSM_GRAVITY;
}

void
GravityC::set_strength( float32 f32Val )
{
	m_f32Strength = f32Val;
}

float32
GravityC::get_strength() const
{
	return m_f32Strength;
}

void
GravityC::set_decay( float32 f32Val )
{
	m_f32Decay = f32Val;
}

float32
GravityC::get_decay() const
{
	return m_f32Decay;
}

void
GravityC::set_size( float32 f32Val )
{
	m_f32Size = f32Val;
}

float32
GravityC::get_size() const
{
	return m_f32Size;
}

void
GravityC::set_gravity_type( uint32 ui32Val )
{
	m_ui32Type = ui32Val;
}

uint32
GravityC::get_gravity_type() const
{
	return m_ui32Type;
}

void
GravityC::set_strength_cont( ContFloatC* pCont )
{
	m_pStrengthCont = pCont;
}

void
GravityC::set_decay_cont( ContFloatC* pCont )
{
	m_pDecayCont = pCont;
}

enum GravityChunksE {
	CHUNK_GRAVITY_BASE				= 0x1000,
	CHUNK_GRAVITY_STRENGTH_CONT		= 0x2000,
	CHUNK_GRAVITY_DECAY_CONT		= 0x2001,
	CHUNK_GRAVITY_TURBULENCE_CONT	= 0x2002,
	CHUNK_GRAVITY_FREQUENCY_CONT	= 0x2003,
	CHUNK_GRAVITY_SCALE_CONT		= 0x2004,
};

const uint32	GRAVITY_VERSION = 1;


uint32
GravityC::save( SaveC* pSave )
{
	uint32	ui32Error = IO_OK;

	eval_state( 0 );

	std::string	sStr;

	// base
	pSave->begin_chunk( CHUNK_GRAVITY_BASE, GRAVITY_VERSION );
		ui32Error = pSave->write( &m_f32Strength, sizeof( m_f32Strength ) );
		ui32Error = pSave->write( &m_f32Decay, sizeof( m_f32Decay ) );
		ui32Error = pSave->write( &m_ui32Type, sizeof( m_ui32Type ) );
		ui32Error = pSave->write( &m_f32Size, sizeof( m_f32Size ) );
	pSave->end_chunk();

	if( m_pDecayCont ) {
		pSave->begin_chunk( CHUNK_GRAVITY_DECAY_CONT, GRAVITY_VERSION );
			ui32Error = m_pDecayCont->save( pSave );
		pSave->end_chunk();
	}

	if( m_pStrengthCont ) {
		pSave->begin_chunk( CHUNK_GRAVITY_STRENGTH_CONT, GRAVITY_VERSION );
			ui32Error = m_pStrengthCont->save( pSave );
		pSave->end_chunk();
	}

	return ui32Error;
}

uint32
GravityC::load( LoadC* pLoad )
{
	uint32	ui32Error = IO_OK;

	while( (ui32Error = pLoad->open_chunk()) == IO_OK ) {

		switch( pLoad->get_chunk_id() ) {

		case CHUNK_GRAVITY_BASE:
			if( pLoad->get_chunk_version() == GRAVITY_VERSION ) {
				ui32Error = pLoad->read( &m_f32Strength, sizeof( m_f32Strength ) );
				ui32Error = pLoad->read( &m_f32Decay, sizeof( m_f32Decay ) );
				ui32Error = pLoad->read( &m_ui32Type, sizeof( m_ui32Type ) );
				ui32Error = pLoad->read( &m_f32Size, sizeof( m_f32Size ) );
			}
			break;

		case CHUNK_GRAVITY_STRENGTH_CONT:
			if( pLoad->get_chunk_version() == GRAVITY_VERSION ) {
				ContFloatC*	pCont = new ContFloatC;
				ui32Error = pCont->load( pLoad );
				m_pStrengthCont = pCont;
			}
			break;

		case CHUNK_GRAVITY_DECAY_CONT:
			if( pLoad->get_chunk_version() == GRAVITY_VERSION ) {
				ContFloatC*	pCont = new ContFloatC;
				ui32Error = pCont->load( pLoad );
				m_pDecayCont = pCont;
			}
			break;

		default:
			assert( 0 );
		}

		pLoad->close_chunk();

		if( ui32Error != IO_OK && ui32Error != IO_END ) {
			return ui32Error;
		}
	}

	return ui32Error;
}


//
// Deflector
//

DeflectorC::DeflectorC( WSMObjectC* pObj ) :
	CollisionObjectI( pObj ),
	m_f32Bounce( 0 ),
	m_f32Width( 0 ),
	m_f32Height( 0 ),
	m_f32Variation( 0 ),
	m_f32Chaos( 0 ),
	m_f32Friction( 0 ),
	m_f32InheritVel( 0 ),
	m_pBounceCont( 0 ),
	m_pWidthCont( 0 ),
	m_pHeightCont( 0 ),
	m_pVariationCont( 0 ),
	m_pChaosCont( 0 ),
	m_pFrictionCont( 0 ),
	m_pInheritVelCont( 0 )
{
	// empty

	srand( 1264873 );
	for( uint32 i = 0; i < RANDOM_FLOAT_COUNT; i++ )
		m_f32RandomFloat[i] = (float32)rand() / (float32)RAND_MAX;
}

DeflectorC::~DeflectorC()
{
	delete m_pBounceCont;
	delete m_pWidthCont;
	delete m_pHeightCont;
	delete m_pVariationCont;
	delete m_pChaosCont;
	delete m_pFrictionCont;
	delete m_pInheritVelCont;
}

void
DeflectorC::eval_state( int32 i32Time )
{
	if( m_pBounceCont )
		m_f32Bounce = m_pBounceCont->get_value( i32Time );
	if( m_pWidthCont )
		m_f32Width = m_pWidthCont->get_value( i32Time );
	if( m_pHeightCont )
		m_f32Height = m_pHeightCont->get_value( i32Time );
	if( m_pVariationCont )
		m_f32Variation = m_pVariationCont->get_value( i32Time );
	if( m_pChaosCont )
		m_f32Chaos = m_pChaosCont->get_value( i32Time );
	if( m_pFrictionCont )
		m_f32Friction = m_pFrictionCont->get_value( i32Time );
	if( m_pInheritVelCont )
		m_f32InheritVel = m_pInheritVelCont->get_value( i32Time );
}

inline
Vector3C
transform_vector( const Vector3C& rV, const Matrix3C& rTM )
{
	return Vector3C( rV[0] * rTM[0][0] + rV[1] * rTM[1][0] + rV[2] * rTM[2][0],
					 rV[0] * rTM[0][1] + rV[1] * rTM[1][1] + rV[2] * rTM[2][1],
					 rV[0] * rTM[0][2] + rV[1] * rTM[1][2] + rV[2] * rTM[2][2] );
}

bool
DeflectorC::check_collision( int32 i32T, float32* pPos, float32* pVel, float32 f32DT, int32 i32Index, float32 *pColTime, bool bUpdatePastCollide )
{

	float32	f32Bounce = get_bounce();
	float32	f32Friction = get_friction();
	float32	f32Chaos = get_chaos();
	float32	f32Variation = get_variation();
	float32	f32InheritVel = get_inherit_velocity();
	float32	f32Width = get_width() * 0.5f;
	float32	f32Height = get_height() * 0.5f;
	float32	f32AT = 0;
	
	Vector3C	rP, rV, rPH;
	Matrix3C	rInvTM = m_pObject->get_inv_tm();
	Matrix3C	rTM = m_pObject->get_tm();

	// Transform the point and velocity into our space

	rP = Vector3C( pPos[0], pPos[1], pPos[2] ) * rInvTM;
	rV = transform_vector( Vector3C( pVel[0], pVel[1], pVel[2] ), rInvTM );

	// Compute the point of intersection
	if( fabs(rP[1] ) < 0.001 ) {
		rP[1] = 0.0f;
		f32AT  = 0.0f;		
	} 
	else {
		if( rV[1] == 0.0f ) 
			return false;
		f32AT = -rP[1] / rV[1];
		if( f32AT < 0.0f || f32AT > f32DT ) 
			return false;
	}
	rPH = rP + f32AT * rV;

	// See if the point is within our range
	if( rPH[0] < -f32Width || rPH[0] > f32Width || rPH[2] < -f32Height || rPH[2] > f32Height )
		return false;

	// Remove the part of dt we used to get to the collision point
	float32 f32HoldDT = f32DT;
	f32DT -= f32AT;

	// Reflect the velocity about the XY plane and attenuate with the bounce factor
	Vector3C	rBounce( rV );
	rBounce[1] = -rBounce[1];

	Vector3C	rFriction( rV );
	rFriction[1] = 0;
	rFriction = -rFriction;

	// Reflect the velocity about the XY plane and attenuate with the bounce factor
	// and add in inherited motion
	float32	f32RefVariation = 1.0f;
	float32	f32RefChaos = 1.0f;
	if( f32Variation != 0.0f )
		f32RefVariation = 1.0f - (f32Variation * m_f32RandomFloat[i32Index & RANDOM_FLOAT_MASK] );
	if( f32Chaos != 0.0f )
		f32RefChaos = 1.0f - (f32Chaos * m_f32RandomFloat[i32Index & RANDOM_FLOAT_MASK]);

	// Bayboro: New vector calculation scheme
	rV = rBounce * (f32Bounce * f32RefVariation) + rFriction * (1.0f - f32Friction);

	if( f32RefChaos < 0.9999f ) { // randomize vector direction
		Vector3C	rAxis( m_f32RandomFloat[(i32Index + 33) & RANDOM_FLOAT_MASK], m_f32RandomFloat[(i32Index + 34) & RANDOM_FLOAT_MASK], m_f32RandomFloat[(i32Index + 35) & RANDOM_FLOAT_MASK] );
		rAxis = rAxis - Vector3C( 0.5f, 0.5f, 0.5f );
		float32	f32LenghSq = rAxis.dot( rAxis );
		rAxis -= rBounce * rAxis.dot( rBounce ) / f32LenghSq;
		float32 f32Angle = M_PI * (1.0f - f32RefChaos) * m_f32RandomFloat[(i32Index + 57) & RANDOM_FLOAT_MASK];
		QuatC rQuat;
		rQuat.from_axis_angle( rAxis, f32Angle );
		Matrix3C	rRot = rQuat.to_rot_matrix();
		rV = rV * rRot;
		// vector should not go beyond surface
		float32 f32DotProd = rV.dot( rBounce );
		if( f32DotProd < 0.0f )
			rV -= 2.0f * rBounce * f32DotProd / f32LenghSq;
	}
//	vel += inheritedVel * inherit;
//	pos = hitpoint;



	if( bUpdatePastCollide ) {
		rPH += rV * f32DT;  // move to collision point
		if( pColTime )
			(*pColTime) = f32HoldDT;
	}
	else {
		if( pColTime )
			(*pColTime) = f32AT;
	}

	// Tranform back into world space.
	rPH *= rTM;
	rV = transform_vector( rV, rTM );

	pPos[0] = rPH[0];
	pPos[1] = rPH[1];
	pPos[2] = rPH[2];

	pVel[0] = rV[0];
	pVel[1] = rV[1];
	pVel[2] = rV[2];

	return true;
}

uint32
DeflectorC::get_type()
{
	return WSM_DEFLECTOR;
}

void
DeflectorC::set_bounce( float32 f32Val )
{
	m_f32Bounce = f32Val;
}

float32
DeflectorC::get_bounce() const
{
	return m_f32Bounce;
}

void
DeflectorC::set_width( float32 f32Val )
{
	m_f32Width = f32Val;
}

float32
DeflectorC::get_width() const
{
	return m_f32Width;
}

void
DeflectorC::set_height( float32 f32Val )
{
	m_f32Height = f32Val;
}

float32
DeflectorC::get_height() const
{
	return m_f32Height;
}

void
DeflectorC::set_variation( float32 f32Val )
{
	m_f32Variation = f32Val;
}

float32
DeflectorC::get_variation() const
{
	return m_f32Variation;
}

void
DeflectorC::set_chaos( float32 f32Val )
{
	m_f32Chaos = f32Val;
}

float32
DeflectorC::get_chaos() const
{
	return m_f32Chaos;
}

void
DeflectorC::set_friction( float32 f32Val )
{
	m_f32Friction = f32Val;
}

float32
DeflectorC::get_friction() const
{
	return m_f32Friction;
}

void
DeflectorC::set_inherit_velocity( float32 f32Val )
{
	m_f32InheritVel = f32Val;
}

float32
DeflectorC::get_inherit_velocity() const
{
	return m_f32InheritVel;
}

void
DeflectorC::set_bounce_cont( ContFloatC* pCont )
{
	m_pBounceCont = pCont;
}

void
DeflectorC::set_width_cont( ContFloatC* pCont )
{
	m_pWidthCont = pCont;
}

void
DeflectorC::set_height_cont( ContFloatC* pCont )
{
	m_pHeightCont = pCont;
}

void
DeflectorC::set_variation_cont( ContFloatC* pCont )
{
	m_pVariationCont = pCont;
}

void
DeflectorC::set_chaos_cont( ContFloatC* pCont )
{
	m_pChaosCont = pCont;
}

void
DeflectorC::set_friction_cont( ContFloatC* pCont )
{
	m_pFrictionCont = pCont;
}

void
DeflectorC::set_inherit_vel_cont( ContFloatC* pCont )
{
	m_pInheritVelCont = pCont;
}

enum DeflectorChunksE {
	CHUNK_DEFLECTOR_BASE				= 0x1000,
	CHUNK_DEFLECTOR_BOUNCE_CONT			= 0x2000,
	CHUNK_DEFLECTOR_WIDTH_CONT			= 0x2001,
	CHUNK_DEFLECTOR_HEIGHT_CONT			= 0x2002,
	CHUNK_DEFLECTOR_VARIATION_CONT		= 0x2003,
	CHUNK_DEFLECTOR_CHAOS_CONT			= 0x2004,
	CHUNK_DEFLECTOR_FRICTION_CONT		= 0x2005,
	CHUNK_DEFLECTOR_INHERITVEL_CONT		= 0x2006,
};

const uint32	DEFLECTOR_VERSION = 1;


uint32
DeflectorC::save( SaveC* pSave )
{
	uint32	ui32Error = IO_OK;

	eval_state( 0 );

	std::string	sStr;

	// base
	pSave->begin_chunk( CHUNK_DEFLECTOR_BASE, DEFLECTOR_VERSION );
		ui32Error = pSave->write( &m_f32Bounce, sizeof( m_f32Bounce ) );
		ui32Error = pSave->write( &m_f32Width, sizeof( m_f32Width ) );
		ui32Error = pSave->write( &m_f32Height, sizeof( m_f32Height ) );
		ui32Error = pSave->write( &m_f32Variation, sizeof( m_f32Variation ) );
		ui32Error = pSave->write( &m_f32Chaos, sizeof( m_f32Chaos ) );
		ui32Error = pSave->write( &m_f32Friction, sizeof( m_f32Friction ) );
		ui32Error = pSave->write( &m_f32InheritVel, sizeof( m_f32InheritVel ) );
	pSave->end_chunk();


	if( m_pBounceCont ) {
		pSave->begin_chunk( CHUNK_DEFLECTOR_BOUNCE_CONT, DEFLECTOR_VERSION );
			ui32Error = m_pBounceCont->save( pSave );
		pSave->end_chunk();
	}

	if( m_pChaosCont ) {
		pSave->begin_chunk( CHUNK_DEFLECTOR_CHAOS_CONT, DEFLECTOR_VERSION );
			ui32Error = m_pChaosCont->save( pSave );
		pSave->end_chunk();
	}

	if( m_pFrictionCont ) {
		pSave->begin_chunk( CHUNK_DEFLECTOR_FRICTION_CONT, DEFLECTOR_VERSION );
			ui32Error = m_pFrictionCont->save( pSave );
		pSave->end_chunk();
	}

	if( m_pHeightCont ) {
		pSave->begin_chunk( CHUNK_DEFLECTOR_HEIGHT_CONT, DEFLECTOR_VERSION );
			ui32Error = m_pHeightCont->save( pSave );
		pSave->end_chunk();
	}

	if( m_pInheritVelCont ) {
		pSave->begin_chunk( CHUNK_DEFLECTOR_INHERITVEL_CONT, DEFLECTOR_VERSION );
			ui32Error = m_pInheritVelCont->save( pSave );
		pSave->end_chunk();
	}

	if( m_pVariationCont ) {
		pSave->begin_chunk( CHUNK_DEFLECTOR_VARIATION_CONT, DEFLECTOR_VERSION );
			ui32Error = m_pVariationCont->save( pSave );
		pSave->end_chunk();
	}

	if( m_pWidthCont ) {
		pSave->begin_chunk( CHUNK_DEFLECTOR_WIDTH_CONT, DEFLECTOR_VERSION );
			ui32Error = m_pWidthCont->save( pSave );
		pSave->end_chunk();
	}

	return ui32Error;
}

uint32
DeflectorC::load( LoadC* pLoad )
{
	uint32	ui32Error = IO_OK;

	while( (ui32Error = pLoad->open_chunk()) == IO_OK ) {

		switch( pLoad->get_chunk_id() ) {

		case CHUNK_DEFLECTOR_BASE:
			if( pLoad->get_chunk_version() == DEFLECTOR_VERSION ) {
				ui32Error = pLoad->read( &m_f32Bounce, sizeof( m_f32Bounce ) );
				ui32Error = pLoad->read( &m_f32Width, sizeof( m_f32Width ) );
				ui32Error = pLoad->read( &m_f32Height, sizeof( m_f32Height ) );
				ui32Error = pLoad->read( &m_f32Variation, sizeof( m_f32Variation ) );
				ui32Error = pLoad->read( &m_f32Chaos, sizeof( m_f32Chaos ) );
				ui32Error = pLoad->read( &m_f32Friction, sizeof( m_f32Friction ) );
				ui32Error = pLoad->read( &m_f32InheritVel, sizeof( m_f32InheritVel ) );
			}
			break;

		case CHUNK_DEFLECTOR_BOUNCE_CONT:
			if( pLoad->get_chunk_version() == DEFLECTOR_VERSION ) {
				ContFloatC*	pCont = new ContFloatC;
				ui32Error = pCont->load( pLoad );
				m_pBounceCont = pCont;
			}
			break;

		case CHUNK_DEFLECTOR_WIDTH_CONT:
			if( pLoad->get_chunk_version() == DEFLECTOR_VERSION ) {
				ContFloatC*	pCont = new ContFloatC;
				ui32Error = pCont->load( pLoad );
				m_pWidthCont = pCont;
			}
			break;

		case CHUNK_DEFLECTOR_HEIGHT_CONT:
			if( pLoad->get_chunk_version() == DEFLECTOR_VERSION ) {
				ContFloatC*	pCont = new ContFloatC;
				ui32Error = pCont->load( pLoad );
				m_pHeightCont = pCont;
			}
			break;

		case CHUNK_DEFLECTOR_VARIATION_CONT:
			if( pLoad->get_chunk_version() == DEFLECTOR_VERSION ) {
				ContFloatC*	pCont = new ContFloatC;
				ui32Error = pCont->load( pLoad );
				m_pVariationCont = pCont;
			}
			break;

		case CHUNK_DEFLECTOR_CHAOS_CONT:
			if( pLoad->get_chunk_version() == DEFLECTOR_VERSION ) {
				ContFloatC*	pCont = new ContFloatC;
				ui32Error = pCont->load( pLoad );
				m_pChaosCont = pCont;
			}
			break;

		case CHUNK_DEFLECTOR_FRICTION_CONT:
			if( pLoad->get_chunk_version() == DEFLECTOR_VERSION ) {
				ContFloatC*	pCont = new ContFloatC;
				ui32Error = pCont->load( pLoad );
				m_pFrictionCont = pCont;
			}
			break;

		case CHUNK_DEFLECTOR_INHERITVEL_CONT:
			if( pLoad->get_chunk_version() == DEFLECTOR_VERSION ) {
				ContFloatC*	pCont = new ContFloatC;
				ui32Error = pCont->load( pLoad );
				m_pInheritVelCont = pCont;
			}
			break;

		default:
			assert( 0 );
		}

		pLoad->close_chunk();

		if( ui32Error != IO_OK && ui32Error != IO_END ) {
			return ui32Error;
		}
	}

	return ui32Error;
}



//
// WSM Object
//

WSMObjectC::WSMObjectC() :
	m_pPosCont( 0 ),
	m_pRotCont( 0 ),
	m_pCollisionObject( 0 ),
	m_pForceField( 0 )
{
	// empty
}

WSMObjectC::~WSMObjectC()
{
	delete m_pPosCont;
	delete m_pForceField;
	delete m_pCollisionObject;
}

int32
WSMObjectC::get_type()
{
	return CGITEM_WSMOBJECT;
}

void
WSMObjectC::set_position( const Vector3C& rPos )
{
	m_rPosition = rPos;
}

const
Vector3C&
WSMObjectC::get_position()
{
	return m_rPosition;
}

void
WSMObjectC::set_rotation( const QuatC& rRot )
{
	m_rRotation = rRot;
}

const QuatC&
WSMObjectC::get_rotation()
{
	return m_rRotation;
}

void
WSMObjectC::eval_state( int32 i32Time )
{
	if( m_pPosCont )
		m_rPosition = m_pPosCont->get_value( i32Time ); 
	if( m_pRotCont )
		m_rRotation = m_pRotCont->get_value( i32Time );

	Matrix3C	rRotMat;
	QuatC		rRot( m_rRotation );

	rRotMat.set_rot( rRot );

	// Compose matrix
	m_rTM = rRotMat;
	m_rTM[3] = m_rPosition;

	if( m_pParent ) {
		m_rTM *= m_pParent->get_tm();
	}

	m_rInvTM = m_rTM.inverse();

	// update modifiers
	if( m_pForceField )
		m_pForceField->eval_state( i32Time );
	if( m_pCollisionObject )
		m_pCollisionObject->eval_state( i32Time );
}

void
WSMObjectC::eval_geom( int32 i32Time )
{
	// empty
}


const Matrix3C&
WSMObjectC::get_inv_tm()
{
	return m_rInvTM;
}

void
WSMObjectC::set_position_controller( ContVector3C* pCont )
{
	m_pPosCont = pCont;
}

ContQuatC*
WSMObjectC::get_rotation_controller()
{
	return m_pRotCont;
}

void
WSMObjectC::set_rotation_controller( ContQuatC* pCont )
{
	m_pRotCont = pCont;
}

ContVector3C*
WSMObjectC::get_position_controller()
{
	return m_pPosCont;
}

void
WSMObjectC::update_dependencies( std::vector<ScenegraphItemI*> rScenegraph )
{
	uint32	i;
	// find parent
	m_pParent = 0;
	if( !m_sParentName.empty() ) {
		for( i = 0; i < rScenegraph.size(); i++ ) {
			if( m_sParentName.compare( rScenegraph[i]->get_name() ) == 0 ) {
				m_pParent = rScenegraph[i];
				break;
			}
		}
	}
}

void
WSMObjectC::set_force_field( ForceFieldI* pField )
{
	delete m_pForceField;
	m_pForceField = pField;
}

ForceFieldI*
WSMObjectC::get_force_field()
{
	return m_pForceField;
}

void
WSMObjectC::set_collision_object( CollisionObjectI* pObj )
{
	delete m_pCollisionObject;
	m_pCollisionObject = pObj;
}

CollisionObjectI*
WSMObjectC::get_collision_object()
{
	return m_pCollisionObject;
}



enum WSMObjectChunksE {
	CHUNK_WSMOBJECT_BASE			= 0x1000,
	CHUNK_WSMOBJECT_STATIC			= 0x1001,
	CHUNK_WSMOBJECT_CONT_POS		= 0x2000,
	CHUNK_WSMOBJECT_CONT_ROT		= 0x2001,
	CHUNK_WSMOBJECT_FORCEFIELD		= 0x3000,
	CHUNK_WSMOBJECT_COLLISIONOBJ	= 0x3001,
};

const uint32	WSMOBJECT_VERSION = 1;


uint32
WSMObjectC::save( SaveC* pSave )
{
	uint32	ui32Error = IO_OK;

	eval_state( 0 );

	std::string	sStr;
	float32	f32Temp3[3];
	float32	f32Temp4[4];
	uint32	ui32Temp;

	// base
	pSave->begin_chunk( CHUNK_WSMOBJECT_BASE, WSMOBJECT_VERSION );
		ScenegraphItemI::save( pSave );
	pSave->end_chunk();

	// light base (static)
	pSave->begin_chunk( CHUNK_WSMOBJECT_STATIC, WSMOBJECT_VERSION );
		// position
		f32Temp3[0] = m_rPosition[0];
		f32Temp3[1] = m_rPosition[1];
		f32Temp3[2] = m_rPosition[2];
		ui32Error = pSave->write( f32Temp3, sizeof( f32Temp3 ) );

		// rotation
		f32Temp4[0] = m_rRotation[0];
		f32Temp4[1] = m_rRotation[1];
		f32Temp4[2] = m_rRotation[2];
		f32Temp4[3] = m_rRotation[3];
		ui32Error = pSave->write( f32Temp4, sizeof( f32Temp4 ) );
	pSave->end_chunk();

	// pos controller
	if( m_pPosCont ) {
		pSave->begin_chunk( CHUNK_WSMOBJECT_CONT_POS, WSMOBJECT_VERSION );
			ui32Error = m_pPosCont->save( pSave );
		pSave->end_chunk();
	}

	// rotation controller
	if( m_pRotCont ) {
		pSave->begin_chunk( CHUNK_WSMOBJECT_CONT_ROT, WSMOBJECT_VERSION );
			ui32Error = m_pRotCont->save( pSave );
		pSave->end_chunk();
	}

	// force field
	if( m_pForceField ) {
		pSave->begin_chunk( CHUNK_WSMOBJECT_FORCEFIELD, WSMOBJECT_VERSION );
			// type
			ui32Temp = m_pForceField->get_type();
			ui32Error = pSave->write( &ui32Temp, sizeof( ui32Temp ) );
			// field
			ui32Error = m_pForceField->save( pSave );
		pSave->end_chunk();
	}

	// collision obj
	if( m_pCollisionObject ) {
		pSave->begin_chunk( CHUNK_WSMOBJECT_COLLISIONOBJ, WSMOBJECT_VERSION );
			// type
			ui32Temp = m_pCollisionObject->get_type();
			ui32Error = pSave->write( &ui32Temp, sizeof( ui32Temp ) );
			// obj
			ui32Error = m_pCollisionObject->save( pSave );
		pSave->end_chunk();
	}

	return ui32Error;
}

uint32
WSMObjectC::load( LoadC* pLoad )
{
	uint32	ui32Error = IO_OK;
	float32	f32Temp3[3];
	float32	f32Temp4[4];
	uint32	ui32Temp;

	while( (ui32Error = pLoad->open_chunk()) == IO_OK ) {

		switch( pLoad->get_chunk_id() ) {
		case CHUNK_WSMOBJECT_BASE:
			if( pLoad->get_chunk_version() == WSMOBJECT_VERSION )
				ScenegraphItemI::load( pLoad );
			break;

		case CHUNK_WSMOBJECT_STATIC:
			if( pLoad->get_chunk_version() == WSMOBJECT_VERSION ) {
				// position
				ui32Error = pLoad->read( f32Temp3, sizeof( f32Temp3 ) );
				m_rPosition[0] = f32Temp3[0];
				m_rPosition[1] = f32Temp3[1];
				m_rPosition[2] = f32Temp3[2];

				// rotation
				ui32Error = pLoad->read( f32Temp4, sizeof( f32Temp4 ) );
				m_rRotation[0] = f32Temp4[0];
				m_rRotation[1] = f32Temp4[1];
				m_rRotation[2] = f32Temp4[2];
				m_rRotation[3] = f32Temp4[3];
			}
			break;

		case CHUNK_WSMOBJECT_CONT_POS:
			if( pLoad->get_chunk_version() == WSMOBJECT_VERSION ) {
				ContVector3C*	pCont = new ContVector3C;
				ui32Error = pCont->load( pLoad );
				m_pPosCont = pCont;
			}
			break;

		case CHUNK_WSMOBJECT_CONT_ROT:
			if( pLoad->get_chunk_version() == WSMOBJECT_VERSION ) {
				ContQuatC*	pCont = new ContQuatC;
				ui32Error = pCont->load( pLoad );
				m_pRotCont = pCont;
			}
			break;

		case CHUNK_WSMOBJECT_FORCEFIELD:
			if( pLoad->get_chunk_version() == WSMOBJECT_VERSION ) {
				//read type
				ui32Error = pLoad->read( &ui32Temp, sizeof( ui32Temp ) );
				if( ui32Temp == WSM_WIND ) {
					WindC*	pWind = new WindC( this );
					ui32Error = pWind->load( pLoad );
					m_pForceField = pWind;
				}
				else if( ui32Temp == WSM_GRAVITY ) {
					GravityC*	pGrav = new GravityC( this );
					ui32Error = pGrav->load( pLoad );
					m_pForceField = pGrav;
				}
			}
			break;

		case CHUNK_WSMOBJECT_COLLISIONOBJ:
			if( pLoad->get_chunk_version() == WSMOBJECT_VERSION ) {
				//read type
				ui32Error = pLoad->read( &ui32Temp, sizeof( ui32Temp ) );
				if( ui32Temp == WSM_DEFLECTOR ) {
					DeflectorC*	pDef = new DeflectorC( this );
					ui32Error = pDef->load( pLoad );
					m_pCollisionObject = pDef;
				}
			}
			break;

		default:
			assert( 0 );
		}

		pLoad->close_chunk();

		if( ui32Error != IO_OK && ui32Error != IO_END ) {
			return ui32Error;
		}
	}

	return ui32Error;
}
