//**************************************************************************
//
// when the mouse goes down, remember where. also, clear out the now
// position by putting it into start
//==========================================================================
void ArcBallCam::down(const float x, const float y)
//==========================================================================
{
	start = now * start;
	now = Quat();		// identity

	downX = x;
	downY = y;

	panX = 0;
	panY = 0;
}

    void TransformComponent::Translate(const Vec3& t)
    {
        if(m_DidRotate && m_FirstPerson) {
            Quat pitch = Quat(Vec3(1,0,0), m_FirstPersonYRot);
            Quat yaw = Quat(Vec3(0,1,0), m_FirstPersonXRot);
            m_Orientation = yaw * pitch;
        }
        if(m_FirstPerson)
            m_Position += (m_Orientation * t);
        else
            m_Position += t;
        m_Updated = true;
        m_DidMove = true;

        if(m_Parent->HasComponentType(CT_PHYSICS)) {
            btRigidBody* body = ((PhysicsComponent*)m_Parent->GetComponentByType(CT_PHYSICS))->m_Body;
            btTransform tr = body->getWorldTransform();
            tr.setOrigin(m_Position);
            body->setWorldTransform(tr);
        }
    }

Quat Quat::Normalized() const
{
#ifdef MATH_AUTOMATIC_SSE
	return Quat(vec4_normalize(q));
#else
	Quat copy = *this;
	float success = copy.Normalize();
	assume(success > 0 && "Quat::Normalized failed!");
	MARK_UNUSED(success);
	return copy;
#endif
}

// From "Uniform Random Rotations", Ken Shoemake, Graphics Gems III.
Quat Quat::unitRandom() {
    float x0 = uniformRandom();
    float r1 = sqrtf(1 - x0), 
          r2 = sqrtf(x0);
    float t1 = (float)G3D::twoPi() * uniformRandom();
    float t2 = (float)G3D::twoPi() * uniformRandom();
    float c1 = cosf(t1), 
          s1 = sinf(t1);
    float c2 = cosf(t2), 
          s2 = sinf(t2);
    return Quat(s1 * r1, c1 * r1, s2 * r2, c2 * r2);
}

// find normalize bisection of two quaternions
static Quat bisect (const Quat &v1, const Quat &v2)
{
	assert(!v1.w && !v2.w);

	Quat v = v1 + v2;
	double n = v.normSquared();

	if (n < 1.0e-5)
		return Quat(0, 0, 1);
	else
		return v * (1.0 / sqrt(n));
}

static void parseAnimKeys( Animation *anim,int type ){

	int cnt=0;
	short t_flags;
	in.sgetn( (char*)&t_flags,2 );
	in.pubseekoff( 8,ios_base::cur );
	in.sgetn( (char*)&cnt,2 );
	in.pubseekoff( 2,ios_base::cur );
	_log( "ANIM_TRACK: frames="+itoa( cnt ) );
	Vector pos,axis,scale;
	float angle;
	Quat quat;
	for( int k=0;k<cnt;++k ){
		int time;
		short flags;
		in.sgetn( (char*)&time,4 );
		in.sgetn( (char*)&flags,2 );
		float tens=0,cont=0,bias=0,ease_to=0,ease_from=0;
		if( flags & 1 ) in.sgetn( (char*)&tens,4 );
		if( flags & 2 ) in.sgetn( (char*)&cont,4 );
		if( flags & 4 ) in.sgetn( (char*)&bias,4 );
		if( flags & 8 ) in.sgetn( (char*)&ease_to,4 );
		if( flags & 16 ) in.sgetn( (char*)&ease_from,4 );
		switch( type ){
		case 0xb020:	//POS_TRACK_TAG
			in.sgetn( (char*)&pos,12 );
			if( conv ) pos=conv_tform*pos;
//			_log( "POS_KEY: time="+itoa(time)+" pos="+ftoa( pos.x )+","+ftoa( pos.y )+","+ftoa( pos.z ) );
			if( time<=anim_len ) anim->setPositionKey( time,pos );
			break;
		case 0xb021:	//ROT_TRACK_TAG
			in.sgetn( (char*)&angle,4 );
			in.sgetn( (char*)&axis,12 );
//			_log( "ROT_KEY: time="+itoa(time)+" angle="+ftoa(angle)+" axis="+ftoa(axis.x)+","+ftoa(axis.y)+","+ftoa(axis.z) );
			if( axis.length()>EPSILON ){
				if( flip_tris ) angle=-angle;
				if( conv ) axis=conv_tform.m*axis;
				quat=Quat( cosf( angle/2 ),axis.normalized()*sinf( angle/2 ) )*quat;
				quat.normalize();
			}
			if( time<=anim_len ) anim->setRotationKey( time,quat );
			break;
		case 0xb022:	//SCL_TRACK_TAG
			in.sgetn( (char*)&scale,12 );
			if( conv ) scale=conv_tform.m*scale;
//			scale.x=fabs(scale.x);scale.y=fabs(scale.y);scale.z=fabs(scale.z);
			_log( "SCL_KEY: time="+itoa(time)+" scale="+ftoa( scale.x )+","+ftoa( scale.y )+","+ftoa( scale.z ) );
			if( time<=anim_len ) anim->setScaleKey( time,scale );
			break;
		}
	}
}

Quat Quat::operator *(const Quat &r) const
{
#if defined(MATH_AUTOMATIC_SSE) && defined(MATH_SSE)
	// Best: 3.456 nsecs / 9.752 ticks, Avg: 3.721 nsecs, Worst: 3.840 nsecs
	return quat_mul_quat(q, r.q);
#else
	// Best: 12.289 nsecs / 33.216 ticks, Avg: 12.585 nsecs, Worst: 13.442 nsecs
	return Quat(x*r.w + y*r.z - z*r.y + w*r.x,
	           -x*r.z + y*r.w + z*r.x + w*r.y,
	            x*r.y - y*r.x + z*r.w + w*r.z,
	           -x*r.x - y*r.y - z*r.z + w*r.w);
#endif
}

// Quat *= Matrix 
const Quat & Quat :: operator*=(const Matrix &m)
{
	// Make sure that matrix is a rotation matrix
	assert( m.isRotation(MATH_TOLERANCE) );

	// Make Quaternion a Matrix
	Matrix mtmp(*this);

	//Multiply matrix by matrix, replace the original quaternion
	*this = Quat( mtmp * m );

	return ( *this );
}

Quat::Quat(const class Any& a) {
    *this = Quat();
    if (beginsWith(toLower(a.name()), "matrix3")) {
        *this = a;
    } else {
        a.verifyName("Quat");
        a.verifyType(Any::ARRAY);
        x = a[0];
        y = a[1];
        z = a[2];
        w = a[3];
    }
}

 DynamicsComponent::DynamicsComponent()
    : mEntity(NULL)
    , mVelocity(
       dtEntity::DynamicVec3Property::SetValueCB(this, &DynamicsComponent::SetVelocity),
       dtEntity::DynamicVec3Property::GetValueCB(this, &DynamicsComponent::GetVelocity)
    )
    , mAngularVelocity(Quat(0,0,0,1))
    , mIsMoving(false)
 {
    Register(VelocityId, &mVelocity);
    Register(AngularVelocityId, &mAngularVelocity);
    Register(AccelerationId, &mAcceleration);
 }

Quat Basis::get_quat() const {

#ifdef MATH_CHECKS
	if (!is_rotation()) {
		ERR_EXPLAIN("Basis must be normalized in order to be casted to a Quaternion. Use get_rotation_quat() or call orthonormalized() instead.");
		ERR_FAIL_V(Quat());
	}
#endif
	/* Allow getting a quaternion from an unnormalized transform */
	Basis m = *this;
	real_t trace = m.elements[0][0] + m.elements[1][1] + m.elements[2][2];
	real_t temp[4];

	if (trace > 0.0) {
		real_t s = Math::sqrt(trace + 1.0);
		temp[3] = (s * 0.5);
		s = 0.5 / s;

		temp[0] = ((m.elements[2][1] - m.elements[1][2]) * s);
		temp[1] = ((m.elements[0][2] - m.elements[2][0]) * s);
		temp[2] = ((m.elements[1][0] - m.elements[0][1]) * s);
	} else {
		int i = m.elements[0][0] < m.elements[1][1] ?
						(m.elements[1][1] < m.elements[2][2] ? 2 : 1) :
						(m.elements[0][0] < m.elements[2][2] ? 2 : 0);
		int j = (i + 1) % 3;
		int k = (i + 2) % 3;

		real_t s = Math::sqrt(m.elements[i][i] - m.elements[j][j] - m.elements[k][k] + 1.0);
		temp[i] = s * 0.5;
		s = 0.5 / s;

		temp[3] = (m.elements[k][j] - m.elements[j][k]) * s;
		temp[j] = (m.elements[j][i] + m.elements[i][j]) * s;
		temp[k] = (m.elements[k][i] + m.elements[i][k]) * s;
	}

	return Quat(temp[0], temp[1], temp[2], temp[3]);
}

Quat MUST_USE_RESULT Quat::RotateFromTo(const float3 &sourceDirection, const float3 &targetDirection)
{
	assume(sourceDirection.IsNormalized());
	assume(targetDirection.IsNormalized());
	float angle = sourceDirection.AngleBetweenNorm(targetDirection);
	assume(angle >= 0.f);
	// If sourceDirection == targetDirection, the cross product comes out zero, and normalization would fail. In that case, pick an arbitrary axis.
	float3 axis = sourceDirection.Cross(targetDirection);
	float oldLength = axis.Normalize();
	if (oldLength == 0)
		axis = float3(1, 0, 0);
	return Quat(axis, angle);
}

 TransformComponent::TransformComponent() : Component(CT_TRANSFORM), m_IsStatic(false),
                                               m_DidMove(true), m_DidRotate(true), m_DidScale(true),
                                               m_Clickable(true), m_InheritOrientation(true),
                                               m_InheritPosition(true), m_InheritScale(true), m_Simulated(false), m_Updated(false),
                                               m_FirstPersonXRot(0), m_FirstPersonYRot(0), m_FirstPerson(false), m_HasParent(false)
 {
     if(m_Position.x + m_Position.y + m_Position.z != 0.0f) m_DidMove = true;
     m_Transform = Mat4::Identity;
     m_Position = Vec3();
     m_Orientation = Quat(0.0f,1.0f,0.0f,0.0f);
     m_Scale = Vec3(1.0f,1.0f,1.0f);
     m_Updated = false;
 }

AnimationClip loadClipFromFile(Platform *platform, Skeleton *skeleton, const char *filepath) {
	Assimp::Importer importer;
	// const aiScene *scene = importer.ReadFileFromMemory(file_data.contents, file_data.size, aiProcess_MakeLeftHanded);
	const aiScene *scene = importer.ReadFile(filepath, aiProcess_MakeLeftHanded|aiProcess_Triangulate);
	if(scene == 0) {
		printf("Error: %s", importer.GetErrorString());
		printf("Couldn't load scene\n");
	}

	Assert(scene->mNumAnimations > 0);

	
	const aiAnimation *anim = scene->mAnimations[0];
	float duration_seconds = anim->mDuration / anim->mTicksPerSecond;
	AnimationChannel *channels = (AnimationChannel *)platform->alloc(sizeof(AnimationChannel) * anim->mNumChannels);
	for(u32 channel_index = 0; channel_index < anim->mNumChannels; channel_index++) {
		const aiNodeAnim *channel = anim->mChannels[channel_index];
		Assert(channel->mNumPositionKeys == channel->mNumScalingKeys);
		Assert(channel->mNumRotationKeys == channel->mNumScalingKeys);
		u32 frame_count = channel->mNumPositionKeys;
		JointPose *poses = (JointPose *)platform->alloc(sizeof(JointPose) * frame_count);
		f32 *frame_times = (f32 *)platform->alloc(sizeof(f32) * frame_count);
		for(u32 key_index = 0; key_index < frame_count; key_index++) {
			aiVectorKey pos_key = channel->mPositionKeys[key_index];
			aiQuatKey rot_key = channel->mRotationKeys[key_index];
			aiVectorKey scale_key = channel->mScalingKeys[key_index];

			frame_times[key_index] = (pos_key.mTime / anim->mDuration) * duration_seconds;

			JointPose pose = {};
			pose.translation = Vec3(pos_key.mValue.x, pos_key.mValue.y, pos_key.mValue.z);
			pose.scale = Vec3(scale_key.mValue.x, scale_key.mValue.y, scale_key.mValue.z);
			pose.rotation = Quat(rot_key.mValue.x, rot_key.mValue.y, rot_key.mValue.z, rot_key.mValue.w);
			poses[key_index] = pose;
		}

		AnimationChannel result_channel = {};
		result_channel.frame_count = frame_count;
		result_channel.frame_times = frame_times;
		result_channel.joint_index = skeleton->getJointIndexFromName(channel->mNodeName.C_Str());
		result_channel.poses = poses;
		channels[channel_index] = result_channel;
	}

	AnimationClip result = {};
	result.channel_count = anim->mNumChannels;
	result.channels = channels;
	result.duration_seconds = duration_seconds;

	return result;
}

void ConnectingBone::setRotations()
{
	//cprintf("childjoint: %f %f %f\n", m_childJoint[0], m_childJoint[1], m_childJoint[2]);
	//cprintf("parentjoint: %f %f %f\n", m_parentJoint[0], m_parentJoint[1], m_parentJoint[2]);

	float xDist = m_childJoint[0] - m_parentJoint[0];
	float yDist = m_childJoint[1] - m_parentJoint[1];
	float zDist = m_childJoint[2] - m_parentJoint[2];
	
	//cprintf("xDist: %f\n", xDist);
	//cprintf("yDist: %f\n", yDist);
	//cprintf("zDist: %f\n", zDist);

	double zero = 1.0e-3;
	if (xDist == 0){
		xDist = zero;
	}
	if (yDist == 0){
		yDist = zero;
	}
	if (zDist == 0){
		zDist = zero;
	}
	float cBoneLengthTemp = m_foldedLength;
	if (cBoneLengthTemp == 0){
		cBoneLengthTemp = zero;
	}

	double angleInDegrees = 0.0;
	angleInDegrees = 57.2957795*acos(zDist / cBoneLengthTemp);
	//cprintf("angleInDegrees: %f\n", angleInDegrees);

	if (zDist <= 0.0){
		angleInDegrees = -angleInDegrees;
	}

	if (angleInDegrees == 0){
		m_foldAngle = Vec4f(0, 0, 0, 1);
		m_foldQuat = Quat();
	}
	else {
		float rx = -yDist*zDist;
		float ry = xDist*zDist;

		// Rotate about rotation vector
		m_foldAngle = Vec4f(angleInDegrees, rx, ry, 0);
		m_foldQuat.axisToQuat(Vec3d(rx, ry, 0), angleInDegrees*3.142 / 180);
		m_foldQuat.normalize();
	}
	//cprintf("global rotations: %f %f %f %f\n", m_foldQuat[0], m_foldQuat[1], m_foldQuat[2], m_foldQuat[3]);
}

    static Transform LookAtRH(const Vec3 eye, const Vec3 at, const Vec3 up)
    {
        Vec3 n = (eye - at).Normalized();
        Vec3 u = up.Cross(n).Normalized();
        Vec3 v = n.Cross(u);

        float w = sqrtf(1.0f + u.x + v.y + n.z) * 0.5f;
        float w4 = 1.0f / (4.0f * w);
        
        return Transform(   Vec3(-u.Dot(eye), -v.Dot(eye), -n.Dot(eye)), // Position
                            Quat((v.z-n.y) * w4, (n.x-u.z) * w4, (u.y-v.x) * w4, -w).Normalized(), // Rotation
                            1); // Scale

    }

void Bvh::PreCalculateMotion()
{
	motion.poses.resize(motionFrames);
	for (int i = 0; i < motionFrames; i++) {
		const float* mot = &rawMotion[i * channels];
		Pose& pose = motion.poses[i];

		for (auto& it : m_frames) {
			Quat q[3];
			int idxMin = std::min((uint32_t)it.rotIndices.x, std::min((uint32_t)it.rotIndices.y, (uint32_t)it.rotIndices.z));
			if (it.rotIndices.x >= 0) {
				q[it.rotIndices.x - idxMin] = Quat(Vec3(1,0,0), -mot[it.rotIndices.x] * (float)M_PI / 180);
			}
			if (it.rotIndices.y >= 0) {
				q[it.rotIndices.y - idxMin] = Quat(Vec3(0,1,0), -mot[it.rotIndices.y] * (float)M_PI / 180);
			}
			if (it.rotIndices.z >= 0) {
				q[it.rotIndices.z - idxMin] = Quat(Vec3(0,0,1), mot[it.rotIndices.z] * (float)M_PI / 180);
			}
			pose.quats.push_back(q[2] * q[1] * q[0]);
		}
	}
}

ConvexPolyhedronID createConvexPolyhedron( BodyStorage& globalStorage, BlockStorage& blocks, BlockDataID storageID,
                                           id_t uid, Vec3 gpos, TriangleMesh mesh,
                                           MaterialID material,
                                           bool global, bool communicating, bool infiniteMass )
{
   WALBERLA_ASSERT_UNEQUAL( ConvexPolyhedron::getStaticTypeID(), std::numeric_limits<id_t>::max(), "ConvexPolyhedron TypeID not initalized!");

   ConvexPolyhedronID poly = nullptr;

   Vec3 centroid = toWalberla( computeCentroid( mesh ) );
   translate( mesh, -centroid );

   gpos += centroid;

   if (global)
   {
      const id_t sid = UniqueID<RigidBody>::createGlobal();
      WALBERLA_CHECK_EQUAL(communicating, false, "Global bodies can not be communicating!" );
      WALBERLA_CHECK_EQUAL(infiniteMass, true, "Global bodies must have infinite mass!" );

      auto cp = std::make_unique<ConvexPolyhedron>(sid, uid, gpos, Vec3(0,0,0), Quat(), mesh, material, global, false, true);
      poly = static_cast<ConvexPolyhedronID>(&globalStorage.add(std::move(cp)));
   } else
   {
      for (auto& block : blocks){
         if (block.getAABB().contains(gpos))
         {
            const id_t sid( UniqueID<RigidBody>::create() );

            BodyStorage& bs = (*block.getData<Storage>(storageID))[0];
            auto cp = std::make_unique<ConvexPolyhedron>(sid, uid, gpos, Vec3(0,0,0), Quat(), mesh, material, global, communicating, infiniteMass);
            cp->MPITrait.setOwner(Owner(MPIManager::instance()->rank(), block.getId().getID()));
            poly = static_cast<ConvexPolyhedronID>(&bs.add( std::move(cp) ) );
         }
      }
   }

   if (poly != nullptr)
   {
      // Logging the successful creation of the box
      WALBERLA_LOG_DETAIL(
                "Created ConvexPolyhedron " << poly->getSystemID() << "\n"
             << "   User-ID         = " << uid << "\n"
             << "   Global position = " << gpos << "\n"
             << "   Material        = " << Material::getName( material )
               );
   }

   return poly;
}

//FIXME:use the animated character view filter for this
void CPlayerView::ViewFollowCharacterFirstPerson(SViewParams &viewParams)
{
	viewParams.viewID = 3;

	//to avoid clipping
	viewParams.nearplane = 0.1f;
	viewParams.position = m_in.entityWorldMatrix *m_in.localEyePos;

	if (m_in.stats_isRagDoll)
		viewParams.position.z += 0.05f;

	if (m_in.stats_followCharacterHead)
		viewParams.rotation = Quat(Matrix33(m_in.entityWorldMatrix)*m_in.headMtxLocal*Matrix33::CreateRotationY(gf_PI*0.5f));
}

void SpatialCamera::setFrame(const Vec3& position, const Vec3& lookAtPoint) {
	Vec3 back = position - lookAtPoint;
	back.normalize();
	Vec3 up = Vec3(0.0f, 1.0f, 0.0f);
	Vec3 right = cross(up, back);
	right.normalize();
	up = cross(back, right);
	up.normalize();
	m_worldTransform.setTranslate(position);
	m_worldTransform.setRotate(Quat(right, up, back));
	updateViewMatrix();
	updateViewProjectionMatrix();
	updateFrustumPlanesAndPoints();
}