#include <iostream>
#include <Ogre.h>
#include <OgreBullet.h>
#include "BulletCollision/CollisionDispatch/btGhostObject.h"
#include "LinearMath/btTransform.h"
#include "character.h"

#define RUN_SPEED 17 // character running speed in units per second
#define TURN_SPEED 500.0f // character turning in degrees per second
#define ANIM_FADE_SPEED \
	7.5f // animation crossfade speed in % of full weight per second

Character::Character(Ogre::SceneManager *scnMgr, const Ogre::String &model,
		     Ogre::Bullet::DynamicsWorld *world)
	: mModelName(model)
	, mScnMgr(scnMgr)
	, mPivotPitch(0)
	, mVerticalVelocity(0)
	, mAnimID(ANIM_NONE)
	, mRunning(false)
	, mCollisionShape(nullptr)
	, mGhostObject(nullptr)
	, mWorld(world)
	, mGoalDirection(0, 0, 0)
	, mUpdate(false)
{
	setupBody();
	setupAnimations();
}

Character::~Character()
{
}

bool Character::frameStarted(const Ogre::FrameEvent &evt)
{
	return true;
}

bool Character::frameRenderingQueued(const Ogre::FrameEvent &evt)
{
	if (mUpdate) {
		updateBody(evt.timeSinceLastFrame);
		updateAnimations(evt.timeSinceLastFrame);
		if (evt.timeSinceLastFrame > 0)
			updateRootMotion(evt.timeSinceLastFrame);
	}
	return true;
}

bool Character::frameEnded(const Ogre::FrameEvent &evt)
{
	return true;
}

void Character::setupBody()
{
	mBodyEnt = mScnMgr->createEntity(mModelName);
	mBodyNode = mScnMgr->getRootSceneNode()->createChildSceneNode();
	mBodyNode->attachObject(mBodyEnt);
	mSkeleton = mBodyEnt->getSkeleton();
	// mRigidBody = world->addCharacter(mBodyEnt, 0);
	// mCollisionShape = static_cast<btCompoundShape *>(mRigidBody->getCollisionShape());
	mGhostObject = new btPairCachingGhostObject();
	mCollisionShape = new btCompoundShape;
	mGhostObject->setCollisionShape(mCollisionShape);

	{
		btVector3 inertia(0, 0, 0);
		//        mCollisionShape = new btCompoundShape();
		btScalar height = 1.0f;
		btScalar radius = 0.3f;
		btCapsuleShape *shape =
			new btCapsuleShape(radius, 2 * height - 2 * radius);
		btTransform transform;
		transform.setIdentity();
		transform.setOrigin(btVector3(0, 1, 0));
		static_cast<btCompoundShape *>(mCollisionShape)
			->addChildShape(transform, shape);
		btScalar masses[1] = { 0 };
		btTransform principal;
		static_cast<btCompoundShape *>(mCollisionShape)
			->calculatePrincipalAxisTransform(masses, principal,
							  inertia);
	}
	mGhostObject->setCollisionFlags(
		btCollisionObject::CF_KINEMATIC_OBJECT /* |
		btCollisionObject::CF_NO_CONTACT_RESPONSE */);
	mGhostObject->setActivationState(DISABLE_DEACTIVATION);
	Ogre::Bullet::KinematicMotionSimple *controller =
		new Ogre::Bullet::KinematicMotionSimple(mGhostObject,
							mBodyNode);
	mWorld->attachCollisionObject(mGhostObject, mBodyEnt,
				      btBroadphaseProxy::AllFilter,
				      btBroadphaseProxy::AllFilter);
	mWorld->getBtWorld()->addAction(controller);

	assert(mCollisionShape);
#if 0
    if (mRigidBody->getMass() == 0) {
#if 0
        mRigidBody->setCollisionFlags(mRigidBody->getCollisionFlags()
		    | btCollisionObject::CF_KINEMATIC_OBJECT
		    | btCollisionObject::CF_NO_CONTACT_RESPONSE
		    );
#endif
#if 0
	mGhostObject->setWorldTransform(mRigidBody->getWorldTransform());
	WorldData::get_singleton()->getBtWorld()
		->getBroadphase()->getOverlappingPairCache()
			->setInternalGhostPairCallback(new btGhostPairCallback());
#endif
    }
#endif
#if 0
    mRigidBody->setActivationState(DISABLE_DEACTIVATION);
#endif
#if 0
    {
        Ogre::Entity *e2 = mScnMgr->createEntity("normal-male.glb");
	Ogre::SceneNode *e2node = mScnMgr->getRootSceneNode()->createChildSceneNode();
    	e2node->attachObject(e2);
	mGhostObject = WorldData::get_singleton()->addGhostObject(e2, mCollisionShape);
	mController = new btKinematicCharacterController(mGhostObject, mCollisionShape, 0.5f);
	WorldData::get_singleton()->getBtWorld()->addAction(mController);
    }
#endif
	assert(mSkeleton->hasBone("Root"));
	mRootBone = mSkeleton->getBone("Root");
	assert(mRootBone);
}
void Character::setupAnimations()
{
	int i, j;
	mSkeleton->setBlendMode(Ogre::ANIMBLEND_CUMULATIVE);
	Ogre::String animNames[NUM_ANIMS] = { "idle", "walking", "running" };
	for (i = 0; i < NUM_ANIMS; i++) {
		mAnims[i] = mBodyEnt->getAnimationState(animNames[i]);
		mAnims[i]->setLoop(true);
		mAnims[i]->setEnabled(true);
		mAnims[i]->setWeight(0);
		mFadingIn[i] = false;
		mFadingOut[i] = false;
		mSkelAnimations[i] = mSkeleton->getAnimation(animNames[i]);
		for (const auto &it : mSkelAnimations[i]->_getNodeTrackList()) {
			Ogre::NodeAnimationTrack *track = it.second;
			Ogre::String trackName =
				track->getAssociatedNode()->getName();
			if (trackName == "mixamorig:Hips") {
				mHipsTracks[i] = track;
			} else if (trackName == "Root") {
				mRootTracks[i] = track;
				//				mRootTracks[i]->removeAllKeyFrames();
			}
		}
		Ogre::Vector3 delta = Ogre::Vector3::ZERO;
		Ogre::Vector3 motion = Ogre::Vector3::ZERO;
		for (j = 0; j < mRootTracks[i]->getNumKeyFrames(); j++) {
			Ogre::Vector3 trans = mRootTracks[i]
						      ->getNodeKeyFrame(j)
						      ->getTranslate();
			if (j == 0)
				delta = trans;
			else
				delta = trans - motion;
			mRootTracks[i]->getNodeKeyFrame(j)->setTranslate(delta);
			motion = trans;
		}
	}
#if 0
	for(i = 0; i < NUM_ANIMS - 1; i++) {
		// need to cache
		int j;
		Ogre::String animName = mAnims[i]->getAnimationName();
		Ogre::Animation *anim = mSkeleton->getAnimation(animName);
		Ogre::NodeAnimationTrack *hips_track = nullptr, *root_track = nullptr;
		Ogre::Node *root_node = nullptr;
		for (const auto& it : anim->_getNodeTrackList()) {
			Ogre::NodeAnimationTrack* track = it.second;
			Ogre::String trackName = track->getAssociatedNode()->getName();
			std::cout << animName << " track: " << trackName << "\n";
			if (trackName == "mixamorig:Hips")
				hips_track = track;
			else if (trackName == "Root") {
				root_track = track;
				root_node = track->getAssociatedNode();
			}
		}
		assert(false);
		root_track->removeAllKeyFrames();
		std::cout << hips_track << " " << root_track << "\n";
		std::cout << hips_track->getNumKeyFrames() << " " << root_track->getNumKeyFrames() << "\n";
		assert(hips_track && root_track);
		Ogre::Vector3 delta = Ogre::Vector3::ZERO;
		for(j = 0; j < hips_track->getNumKeyFrames(); j++) {
			float timePos = hips_track->getNodeKeyFrame(j)->getTime();
			Ogre::Vector3 trans = hips_track->getNodeKeyFrame(j)->getTranslate();
			Ogre::Vector3 hips_trans(0, 0, 0);
			Ogre::Vector3 root_trans(0, 0, 0);
			hips_track->getNodeKeyFrame(j)->setTranslate(hips_trans);
			Ogre::TransformKeyFrame *nk = root_track->createNodeKeyFrame(timePos);
			nk->setTranslate(root_trans - delta);
			nk->setScale(Ogre::Vector3(1, 1, 1));
			nk->setRotation(Ogre::Quaternion());
			std::cout << animName << " delta: " << j << " " << timePos << " " << root_trans - delta << "\n";
			delta = root_trans;
		}
		for(j = 0; j < root_track->getNumKeyFrames(); j++) {
			float timePos = hips_track->getNodeKeyFrame(j)->getTime();
			Ogre::Vector3 root_trans = hips_track->getNodeKeyFrame(j)->getTranslate();
			std::cout << animName << " delta: root: " << j << " " << timePos << " " << root_trans << "\n";
		}
	}
//	assert(false);
#endif
	setAnimation(ANIM_IDLE);
}
void Character::updateBody(Ogre::Real delta)
{
	Ogre::Quaternion toGoal =
		mBodyNode->getOrientation().zAxis().getRotationTo(
			mGoalDirection);
	// calculate how much the character has to turn to face goal direction
	Ogre::Real yawToGoal = toGoal.getYaw().valueDegrees();
	// this is how much the character CAN turn this frame
	Ogre::Real yawAtSpeed =
		yawToGoal / Ogre::Math::Abs(yawToGoal) * delta * TURN_SPEED;
	// reduce "turnability" if we're in midair
	// if (mBaseAnimID == ANIM_JUMP_LOOP) yawAtSpeed *= 0.2f;
	if (yawToGoal < 0)
		yawToGoal = std::min<Ogre::Real>(
			0,
			std::max<Ogre::Real>(
				yawToGoal,
				yawAtSpeed)); //yawToGoal = Math::Clamp<Real>(yawToGoal, yawAtSpeed, 0);
	else if (yawToGoal > 0)
		yawToGoal = std::max<Ogre::Real>(
			0,
			std::min<Ogre::Real>(
				yawToGoal,
				yawAtSpeed)); //yawToGoal = Math::Clamp<Real>(yawToGoal, 0, yawAtSpeed);
	mBodyNode->yaw(Ogre::Degree(yawToGoal));
}
void Character::updateAnimations(Ogre::Real delta)
{
	int i, j, k;
	Ogre::Real animSpeed = 1;
	mTimer += delta;
	{
		Ogre::Quaternion rot = mBodyNode->getOrientation();
		OgreAssert(!Ogre::Math::isNaN(rot.x), "NaN");
		OgreAssert(!Ogre::Math::isNaN(rot.y), "NaN");
		OgreAssert(!Ogre::Math::isNaN(rot.z), "NaN");
	}
	if (mAnimID != ANIM_NONE) {
		if (mAnimID == ANIM_WALK)
			mAnims[mAnimID]->addTime(delta * 1.0f);
		else
			mAnims[mAnimID]->addTime(delta * animSpeed);
	}
	fadeAnimations(delta);
}
void Character::updateRootMotion(Ogre::Real delta)
{
	Ogre::Vector3 boneMotion = mRootBone->getPosition();
	OgreAssert(delta > 0.0f, "Zero delta");
	Ogre::Vector3 motion = boneMotion - rootMotion;
	if (motion.squaredLength() > 0.1f * 0.1f)
		motion = Ogre::Vector3();
	rootMotion = boneMotion;
#if 0
	float mass = mRigidBody->getMass();
	std::cout << "Root bone position: " << boneMotion << "\n";
	std::cout << "body mass: " << mass << "\n";
#endif
	/* Kinematic motion */
	Ogre::Quaternion rot = mBodyNode->getOrientation();
	// Ogre::Vector3 gravity(0, -9.8, 0);
	Ogre::Vector3 gravity(0, 0, 0);
	Ogre::Vector3 velocity = rot * boneMotion / delta;
	velocity += gravity * delta;
	Ogre::Vector3 rotMotion = velocity * delta;
	btTransform from(convert(mBodyNode->getOrientation()),
			 convert(mBodyNode->getPosition()));
	mBodyNode->setPosition(mBodyNode->getPosition() + rotMotion);
	//	WorldData::get_singleton()->getWorld()->testBodyMotion(mRigidBody, from, Ogre::Bullet::convert(rotMotion), true,
	//            nullptr, false, std::set<btCollisionObject *>());
}
void Character::fadeAnimations(Ogre::Real delta)
{
	int i;
	for (i = 0; i < NUM_ANIMS; i++) {
		if (mFadingIn[i]) {
			// slowly fade this animation in until it has full weight
			Ogre::Real newWeight = mAnims[i]->getWeight() +
					       delta * ANIM_FADE_SPEED;
			mAnims[i]->setWeight(
				Ogre::Math::Clamp<Ogre::Real>(newWeight, 0, 1));
			if (newWeight >= 1)
				mFadingIn[i] = false;
		} else if (mFadingOut[i]) {
			// slowly fade this animation out until it has no weight, and then disable it
			Ogre::Real newWeight = mAnims[i]->getWeight() -
					       delta * ANIM_FADE_SPEED;
			mAnims[i]->setWeight(
				Ogre::Math::Clamp<Ogre::Real>(newWeight, 0, 1));
			if (newWeight <= 0) {
				mAnims[i]->setEnabled(false);
				mFadingOut[i] = false;
			}
		}
	}
}
void Character::setAnimation(AnimID id, bool reset)
{
	assert(id >= 0 && id < NUM_ANIMS);
	if (mAnimID != ANIM_NONE) {
		mFadingIn[mAnimID] = false;
		mFadingOut[mAnimID] = true;
	}
	mAnimID = id;
	if (id != ANIM_NONE) {
		mAnims[id]->setEnabled(true);
		mAnims[id]->setWeight(0);
		mFadingOut[id] = false;
		mFadingIn[id] = true;
		if (reset)
			mAnims[id]->setTimePosition(0);
	}
}

bool Character::act_run()
{
	if (mAnimID == ANIM_IDLE)
		setAnimation(ANIM_RUN, true);
	else if (mAnimID == ANIM_WALK)
		setAnimation(ANIM_RUN);
	return true;
}

bool Character::act_walk()
{
	if (mAnimID == ANIM_IDLE)
		setAnimation(ANIM_WALK, true);
	else if (mAnimID == ANIM_RUN)
		setAnimation(ANIM_WALK);
	return true;
}

bool Character::act_idle()
{
	setAnimation(ANIM_IDLE);
	return true;
}

bool Character::isRunning()
{
	return mAnimID == ANIM_RUN;
}

bool Character::isWalking()
{
	return mAnimID == ANIM_WALK;
}

Ogre::Vector3 Character::getPosition()
{
	return mBodyNode->_getDerivedPosition();
}