//////////////////////////////////////////////////////////////////////
//
//	Articulated Entity header
//	
//	File: articulatedentity.h
//	Description : CArticulatedEntity class declaration
//
//	History:
//	-:Created by Anton Knyazev
//
//////////////////////////////////////////////////////////////////////

#ifndef articulatedentity_h
#define articulatedentity_h
#pragma once

struct featherstone_data {
	Vec3i qidx2axidx,axidx2qidx;
	Vec3 Ya_vec[2];
	Vec3 dv_vec[2];
	Vec3 s_vec[3][2];
	Vec3 Q;
	float qinv[9];
	float qinv_down[9];
	float s[18];
	DEFINE_ALIGNED_DATA( float, Ia[6][6], 16 );
	float Ia_s[18];
	DEFINE_ALIGNED_DATA( float, Ia_s_qinv_sT[6][6], 16 );
	DEFINE_ALIGNED_DATA( float, s_qinv_sT[6][6], 16 );
	DEFINE_ALIGNED_DATA( float, s_qinv_sT_Ia[6][6], 16 );
	float qinv_sT[3][6];
	float qinv_sT_Ia[3][6];
	DEFINE_ALIGNED_DATA( float, Iinv[6][6], 16 );
};

enum joint_flags_aux { joint_rotate_pivot=010000000 };


struct ae_joint {
	ae_joint() { 
		nChildren=nChildrenTree=0; iParent=-2; idbody=-1;
		prev_q=q=Ang3(ZERO);	
		qext=Ang3(ZERO); 
		prev_dq=dq.zero(); 
		dqext.zero(); 
		ddq.zero();
		MARK_UNUSED dq_req.x,dq_req.y,dq_req.z; dq_limit.zero();
		bounciness.zero(); ks.zero(); kd.zero(); 
		qdashpot.zero(); kdashpot.zero();
		limits[0].Set(-1E10,-1E10,-1E10);
		limits[1].Set(1E10,1E10,1E10);
		flags=all_angles_locked; 
		quat0.SetIdentity();
		Pext.zero(); Lext.zero();
		Pimpact.zero(); Limpact.zero();
		//fs.Q.zero(); fs.Ya_vec[0].zero(); fs.Ya_vec[1].zero();
		//Matrix33(fs.qinv).identity();
		nActiveAngles = nPotentialAngles = 0;
		pivot[0].zero(); pivot[1].zero();
		iLevel = 0;
		iStartPart = nParts = 0;
		dv_body.zero(); dw_body.zero();
		selfCollMask = 0;
		fs = 0; fsbuf = 0;
		bQuat0Changed = 0;
		Fcollision.zero(); Tcollision.zero();
		vSleep.zero(); wSleep.zero();
		bAwake = 0;
	}
	~ae_joint() {
		if (fsbuf) delete[] fsbuf;
	}

	Ang3 q;
	Ang3 qext;
	Vec3 dq;
	Vec3 dqext;
	Vec3 dq_req;
	Vec3 dq_limit;
	Vec3 ddq;
	quaternionf quat;

	Ang3 prev_q;
	Vec3 prev_dq;
	Vec3 prev_pos,prev_v,prev_w;
	quaternionf prev_qrot;
	Ang3 q0;
	Vec3 Fcollision,Tcollision;
	Vec3 vSleep,wSleep;

	unsigned int flags;
	quaternionf quat0;
	Vec3 limits[2];
	Vec3 bounciness;
	Vec3 ks,kd;
	Vec3 qdashpot,kdashpot;
	Vec3 pivot[2];
	Vec3 hingePivot[2];

	int iStartPart,nParts;
	int iParent;
	int nChildren,nChildrenTree;
	int iLevel;
	masktype selfCollMask;
	entity_contact *pContact,*pContactEnd;
	entity_contact *pdummy0,pdummy1;
	int bAwake;
	int bQuat0Changed;
	int bHasExtContacts;
	
	int idbody;
	RigidBody body;
	Vec3 dv_body,dw_body;
	Vec3 Pext,Lext;
	Vec3 Pimpact,Limpact;
	int nActiveAngles,nPotentialAngles;
	Vec3 rotaxes[3];
	Matrix33 I;

	featherstone_data *fs;
	char *fsbuf;
};

struct ae_part_info {
	Vec3 pos;
	quaternionf q;
	float scale;
	Vec3 BBox[2];
	quaternionf q0;
	Vec3 pos0;
	int iJoint;
	int idbody;
};


class CArticulatedEntity : public CRigidEntity {
 public:
	CArticulatedEntity(CPhysicalWorld *pworld);
	virtual ~CArticulatedEntity();
	virtual pe_type GetType() { return PE_ARTICULATED; }
	virtual void AlertNeighbourhoodND();

	virtual int AddGeometry(phys_geometry *pgeom, pe_geomparams* params,int id=-1,int bThreadSafe=1);
	virtual void RemoveGeometry(int id,int bThreadSafe=1);
	virtual int SetParams(pe_params *_params,int bThreadSafe=1);
	virtual int GetParams(pe_params *_params);
	virtual int GetStatus(pe_status *_status);
	virtual int Action(pe_action*,int bThreadSafe=1);

	virtual RigidBody *GetRigidBody(int ipart=-1,int bWillModify=0);
	virtual void GetContactMatrix(const Vec3 &pt, int ipart, Matrix33 &K);
	virtual void OnContactResolved(entity_contact *pcontact, int iop, int iGroupId);

	virtual void GetMemoryStatistics(ICrySizer *pSizer);

	enum snapver { SNAPSHOT_VERSION = 6 };
	virtual int GetStateSnapshot(CStream &stm, float time_back=0,int flags=0);
	virtual int SetStateFromSnapshot(CStream &stm, int flags);
	virtual int GetStateSnapshot(TSerialize ser, float time_back=0, int flags=0);
	virtual int SetStateFromSnapshot(TSerialize ser, int flags);

	virtual float GetMaxTimeStep(float time_interval);
	virtual int Step(float time_interval);
	virtual void StepBack(float time_interval);
	virtual int RegisterContacts(float time_interval,int nMaxPlaneContacts);
	virtual int Update(float time_interval, float damping);
	virtual float CalcEnergy(float time_interval);
	virtual float GetDamping(float time_interval);
	virtual void GetSleepSpeedChange(int ipart, Vec3 &v,Vec3 &w) { int i=m_infos[ipart].iJoint; v=m_joints[i].vSleep; w=m_joints[i].wSleep; }

	virtual int GetPotentialColliders(CPhysicalEntity **&pentlist, float dt=0);
	virtual int CheckSelfCollision(int ipart0,int ipart1);
	virtual int IsAwake(int ipart=-1);
	virtual void RecomputeMassDistribution(int ipart=-1,int bMassChanged=1);
	virtual void BreakableConstraintsUpdated();

	void SyncWithHost(int bRecalcJoints,float time_interval);
	void SyncBodyWithJoint(int idx, int flags=3);
	void SyncJointWithBody(int idx, int flags=1);
	void UpdateJointRotationAxes(int idx);
	void CheckForGimbalLock(int idx);
	int GetUnprojAxis(int idx, Vec3 &axis);

	int StepJoint(int idx, float time_interval,int &bBounced, int bFlying);
	void JointListUpdated();
	int CalcBodyZa(int idx, float time_interval, vectornf &Za_change);
	int CalcBodyIa(int idx, matrixf& Ia_change);
	void CalcBodiesIinv(int bLockLimits);
	int CollectPendingImpulses(int idx,int &bNotZero);
	void PropagateImpulses(const Vec3 &dv,int bLockLimits=0);
	void CalcVelocityChanges(float time_interval, const Vec3 &dv,const Vec3 &dw);
	void GetJointTorqueResponseMatrix(int idx, Matrix33 &K);
	void UpdatePosition(int bGridLocked);
	void UpdateJointDyn();

	int IsChildOf(int idx, int iParent) { return isnonneg(iParent) & isneg(iParent-idx) & isneg(idx-iParent-m_joints[iParent].nChildrenTree-1); }
	entity_contact *CreateConstraintContact(int idx);

	ae_part_info *m_infos;
	ae_joint *m_joints;
	int m_nJoints, m_nJointsAlloc;
	Vec3 m_posPivot, m_offsPivot;
	Vec3 m_acc,m_wacc;
	Matrix33 m_M0inv;
	Vec3 m_Ya_vec[2];
	float m_simTime,m_simTimeAux;
	float m_scaleBounceResponse;
	int m_bGrounded;
	int m_nRoots;
	int m_bInheritVel;
	CPhysicalEntity *m_pHost;
	Vec3 m_posHostPivot;
	quaternionf m_qHostPivot;
	int m_bCheckCollisions;
	int m_bCollisionResp;
	int m_bExertImpulse;
	int m_iSimType,m_iSimTypeLyingMode;
	int m_iSimTypeCur;
	int m_iSimTypeOverride;
	int m_bIaReady;
	int m_bPartPosForced;
	int m_bFastLimbs;
	int m_bExpandHinges;
	float m_maxPenetrationCur;
	int m_bUsingUnproj;
	Vec3 m_prev_pos,m_prev_vel;
	int m_bUpdateBodies;
	int m_nDynContacts,m_bInGroup;
	int m_bIgnoreCommands;

	int m_nCollLyingMode;
	Vec3 m_gravityLyingMode;
	float m_dampingLyingMode;
	float m_EminLyingMode;
	int m_nBodyContacts;

	volatile int m_lockJoints;

	CPhysicalEntity **m_pCollEntList;
	int m_nCollEnts;
};

#endif