#include <iostream>
#include <Ogre.h>
#include <OgreBullet.h>
#include "GameData.h"
#include "CharacterModule.h"
#include "WaterModule.h"
#include "Components.h"
namespace ECS
{
CharacterModule::CharacterModule(flecs::world &ecs)
{
	ecs.module<CharacterModule>();
	player = ecs.entity("player");
	player.set<AnimationControl>({ AnimationControl::ANIM_NONE,
				       AnimationControl::ANIM_NONE, false,
				       false });
	player.set<CharacterBase>(
		{ "normal-male.glb", 0.0f, nullptr, nullptr, nullptr });
	player.set<CharacterBody>(
		{ nullptr, nullptr, nullptr, { 0, 0, 0 }, false, false });
	player.add<Character>();
	player.add<Player>();
	ecs.system<EngineData, CharacterBase>("UpdateTimer")
		.kind(flecs::OnUpdate)
		.each([this](EngineData &eng, CharacterBase &ch) {
			ch.mTimer += eng.delta;
		});
	ecs.system<Input, Camera>("HandleInput")
		.kind(flecs::OnUpdate)
		.each([this](Input &input, Camera &camera) {
			/* handle input */
			//			if (input.control == input.control_prev)
			//				return;
			uint32_t pressed = input.control & ~input.control_prev;
			uint32_t released = input.control_prev & ~input.control;
			uint32_t active = input.control;
			float zaxis = input.motion.z;
			zaxis *= 0.9f;
			if (pressed & 1)
				std::cout << "W pressed\n";
			if (released & 1)
				std::cout << "W released\n";
			if (active & 1)
				zaxis -= 1.0f;
			if (active & 4)
				zaxis += 1.0f;
			if (zaxis > -1.0f && zaxis < 1.0f)
				zaxis = 0.0f;
			else
				zaxis = Ogre::Math::Sign(zaxis);
			input.motion.z = zaxis;
			float xaxis = input.motion.x;
			xaxis *= 0.9f;
			if (active & 2)
				xaxis = -1.0f;
			if (active & 8)
				xaxis += 1.0f;
			if (xaxis > -1.0f && xaxis < 1.0f)
				xaxis = 0.0f;
			else
				xaxis = Ogre::Math::Sign(xaxis);
			input.motion.x = xaxis;
			if (active & 16)
				input.fast = true;
			else
				input.fast = false;
			input.control_prev = input.control;
			if (input.mouse_moved) {
				updateCameraGoal(camera, -0.18f * input.mouse.x,
						 -0.12f * input.mouse.y, 0);
				input.mouse_moved = false;
				input.mouse.x = 0;
				input.mouse.y = 0;
			}
			if (input.wheel_moved) {
				updateCameraGoal(camera, 0, 0,
						 -0.15f * input.wheel_y);
				input.wheel_moved = false;
				input.wheel_y = 0;
			}
			ECS::get().modified<ECS::Input>();
		});
	ecs.system<const CharacterBase, AnimationControl>("HandleAnimations")
		.kind(flecs::OnUpdate)
		.each([this](flecs::entity e, const CharacterBase &ch,
			     AnimationControl &anim) {
			if (!anim.configured && ch.mSkeleton) {
				int i, j;
				ch.mSkeleton->setBlendMode(
					Ogre::ANIMBLEND_CUMULATIVE);
				Ogre::String
					animNames[AnimationControl::NUM_ANIMS] = {
						"idle", "walking", "running"
					};
				for (i = 0; i < AnimationControl::NUM_ANIMS;
				     i++) {
					anim.mAnims[i] =
						ch.mBodyEnt->getAnimationState(
							animNames[i]);
					anim.mAnims[i]->setLoop(true);
					anim.mAnims[i]->setEnabled(true);
					anim.mAnims[i]->setWeight(0);
					anim.mFadingIn[i] = false;
					anim.mFadingOut[i] = false;
					anim.mSkelAnimations[i] =
						ch.mSkeleton->getAnimation(
							animNames[i]);
					for (const auto &it :
					     anim.mSkelAnimations[i]
						     ->_getNodeTrackList()) {
						Ogre::NodeAnimationTrack *track =
							it.second;
						Ogre::String trackName =
							track->getAssociatedNode()
								->getName();
						if (trackName ==
						    "mixamorig:Hips") {
							anim.mHipsTracks[i] =
								track;
						} else if (trackName ==
							   "Root") {
							anim.mRootTracks[i] =
								track;
							//				mRootTracks[i]->removeAllKeyFrames();
						}
					}
					Ogre::Vector3 delta =
						Ogre::Vector3::ZERO;
					Ogre::Vector3 motion =
						Ogre::Vector3::ZERO;
					for (j = 0;
					     j < anim.mRootTracks[i]
							 ->getNumKeyFrames();
					     j++) {
						Ogre::Vector3 trans =
							anim.mRootTracks[i]
								->getNodeKeyFrame(
									j)
								->getTranslate();
						if (j == 0)
							delta = trans;
						else
							delta = trans - motion;
						anim.mRootTracks[i]
							->getNodeKeyFrame(j)
							->setTranslate(delta);
						motion = trans;
					}
				}
				anim.nextAnim = AnimationControl::ANIM_IDLE;
				setAnimation(anim);
				anim.configured = true;
			}
		});
	ecs.system<AnimationControl>("HandleAnimations0")
		.kind(flecs::OnUpdate)
		.each([this](flecs::entity e, AnimationControl &anim) {
			if (anim.currentAnim != anim.nextAnim)
				setAnimation(anim);
		});
	ecs.system<EngineData, CharacterBase, AnimationControl>(
		   "HandleAnimations1")
		.kind(flecs::OnUpdate)
		.each([this](EngineData &eng, CharacterBase &ch,
			     AnimationControl &anim) {
			float delta = eng.delta;
			Ogre::Real animSpeed = 1;
			if (anim.currentAnim != AnimationControl::ANIM_NONE) {
				if (anim.currentAnim ==
				    AnimationControl::ANIM_WALK)
					anim.mAnims[anim.currentAnim]->addTime(
						delta * 1.0f);
				else
					anim.mAnims[anim.currentAnim]->addTime(
						delta * animSpeed);
			}
			fadeAnimations(anim, delta);
			if (!ch.mRootBone)
				return;
			ch.mBoneMotion = ch.mRootBone->getPosition();
		});
	ecs.system<CharacterBase, CharacterBody, AnimationControl>(
		   "HandleRootMotion")
		.kind(flecs::OnUpdate)
		.each([this](flecs::entity e, CharacterBase &ch,
			     CharacterBody &body, AnimationControl &anim) {
			float delta = e.world().delta_time();
			if (!ch.mBodyNode)
				return;
			Ogre::Quaternion rot = ch.mBodyNode->getOrientation();
			Ogre::Vector3 pos = ch.mBodyNode->getPosition();
			Ogre::Vector3 boneMotion = ch.mBoneMotion;
			Ogre::Vector3 velocity = rot * boneMotion / delta;
			OgreAssert(delta > 0.0f, "Zero delta");
			int maxPen = 0;
			Ogre::Vector3 colNormal;
			bool is_on_floor = false;
			bool penetration = false;
			Ogre::Vector3 gravity(0, -9.8f, 0);
			if (e.has<InWater>()) {
				float volume = 2.0f * 0.5f * 0.5f;
				float density = 900.0f;
				float full_subm = 2.0f;
				float mass = 80.0f;
				float multiplier = 0.25f;
				float current_subm = -Ogre::Math::Clamp(
					pos.y + Ogre::Math::Sin(ch.mTimer *
									0.13f +
								130.0f) *
							0.07f,
					-full_subm, 0.0f);

				Ogre::Vector3 b = -gravity * density * volume *
						  multiplier * current_subm /
						  full_subm / mass;
				body.gvelocity += (gravity + b) * delta;
				body.gvelocity.y = Ogre::Math::Clamp(
					body.gvelocity.y, -2.5f, 2.5f);
				std::cout << "InWater!!!!!!!\n";
			} else
				body.gvelocity += gravity * delta;
			body.gvelocity *= 0.99;
			velocity += body.gvelocity;
			Ogre::Vector3 rotMotion = velocity * delta;
			btVector3 currentPosition =
				body.mGhostObject->getWorldTransform()
					.getOrigin();
			is_on_floor = body.mController->isOnFloor();
			penetration = body.mController->isPenetrating();
			if (is_on_floor)
				body.gvelocity =
					Ogre::Vector3(0.0f, 0.0f, 0.0f);

			btTransform from(
				Ogre::Bullet::convert(
					ch.mBodyNode->getOrientation()),
				Ogre::Bullet::convert(
					ch.mBodyNode->getPosition()));
			ch.mBodyNode->setPosition(ch.mBodyNode->getPosition() +
						  rotMotion);
			ch.mBoneMotion = Ogre::Vector3(0, 0, 0);
		});
	ecs.system<const Input, AnimationControl>("HandlePlayerAnimations")
		.kind(flecs::OnUpdate)
		.with<Character>()
		.with<Player>()
		.each([](const Input &input, AnimationControl &anim) {
			if (!anim.configured)
				return;
			bool controls_idle = input.motion.zeroLength();
			bool anim_is_idle = anim.currentAnim ==
					    AnimationControl::ANIM_IDLE;
			bool anim_is_walking = anim.currentAnim ==
					       AnimationControl::ANIM_WALK;
			bool anim_is_running = anim.currentAnim ==
					       AnimationControl::ANIM_RUN;
			bool anim_is_motion = anim_is_walking ||
					      anim_is_running;
			if (!controls_idle && anim_is_idle) {
				anim.reset = true;
				if (input.fast)
					anim.nextAnim =
						AnimationControl::ANIM_RUN;
				else
					anim.nextAnim =
						AnimationControl::ANIM_WALK;
			} else
				anim.reset = false;
			if (controls_idle && anim_is_motion)
				anim.nextAnim = AnimationControl::ANIM_IDLE;
			else if (!controls_idle && anim_is_motion) {
				if (input.fast && anim_is_walking)
					anim.nextAnim =
						AnimationControl::ANIM_RUN;
				else if (!input.fast && anim_is_running)
					anim.nextAnim =
						AnimationControl::ANIM_WALK;
			}
		});
	ecs.system<const EngineData, CharacterBase, CharacterBody>(
		   "UpdateCharacterBase")
		.kind(flecs::OnUpdate)
		.with<Character>()
		.each([](const EngineData &eng, CharacterBase &ch,
			 CharacterBody &body) {
			if (!ch.mBodyNode) {
				ch.mBodyEnt = eng.mScnMgr->createEntity(
					"normal-male.glb");
				ch.mBodyNode = eng.mScnMgr->getRootSceneNode()
						       ->createChildSceneNode();
				ch.mBodyNode->attachObject(ch.mBodyEnt);
				ch.mSkeleton = ch.mBodyEnt->getSkeleton();
				body.mGhostObject =
					new btPairCachingGhostObject();
				body.mCollisionShape = new btCompoundShape;
				body.mGhostObject->setCollisionShape(
					body.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 *>(
						body.mCollisionShape)
						->addChildShape(transform,
								shape);
					btScalar masses[1] = { 0 };
					btTransform principal;
					static_cast<btCompoundShape *>(
						body.mCollisionShape)
						->calculatePrincipalAxisTransform(
							masses, principal,
							inertia);
				}
				body.mGhostObject->setCollisionFlags(
					btCollisionObject::CF_KINEMATIC_OBJECT /*|
					btCollisionObject::CF_NO_CONTACT_RESPONSE */);
				body.mGhostObject->setActivationState(
					DISABLE_DEACTIVATION);
				eng.mWorld->attachCollisionObject(
					body.mGhostObject, ch.mBodyEnt, 1,
					0x7FFFFFFF);
				body.mController =
					new Ogre::Bullet::KinematicMotionSimple(
						body.mGhostObject,
						ch.mBodyNode);
				OgreAssert(body.mGhostObject,
					   "Need GhostObject");
				OgreAssert(body.mController, "Need controller");
				eng.mWorld->getBtWorld()->addAction(
					body.mController);

				OgreAssert(body.mCollisionShape,
					   "No collision shape");
				OgreAssert(ch.mSkeleton->hasBone("Root"),
					   "No root bone");
				ch.mRootBone = ch.mSkeleton->getBone("Root");
				OgreAssert(ch.mRootBone, "No root bone");
			}
		});
#define CAM_HEIGHT 1.6f // height of camera above character's center of mass
	ecs.system<const EngineData, Camera, const CharacterBase>(
		   "UpdateCamera")
		.kind(flecs::OnUpdate)
		.with<Player>()
		.each([](const EngineData &eng, Camera &camera,
			 const CharacterBase &ch) {
			float delta = eng.delta;
			if (!camera.configured) {
				// create a pivot at roughly the character's shoulder
				camera.mCameraPivot =
					eng.mScnMgr->getRootSceneNode()
						->createChildSceneNode();
				camera.mCameraGoal =
					camera.mCameraPivot
						->createChildSceneNode(
							Ogre::Vector3(0, 2, 3));
				camera.mCameraNode->setPosition(
					camera.mCameraPivot->getPosition() +
					camera.mCameraGoal->getPosition());
				camera.mCameraPivot->setFixedYawAxis(true);
				camera.mCameraGoal->setFixedYawAxis(true);
				camera.mCameraNode->setFixedYawAxis(true);
				// our model is quite small, so reduce the clipping planes
				camera.mCamera->setNearClipDistance(0.1f);
				camera.mCamera->setFarClipDistance(700);

				camera.mPivotPitch = 0;
				camera.configured = true;
			} else {
				// place the camera pivot roughly at the character's shoulder
				camera.mCameraPivot->setPosition(
					ch.mBodyNode->getPosition() +
					Ogre::Vector3::UNIT_Y * CAM_HEIGHT);
				// move the camera smoothly to the goal
				Ogre::Vector3 goalOffset =
					camera.mCameraGoal
						->_getDerivedPosition() -
					camera.mCameraNode->getPosition();
				camera.mCameraNode->translate(goalOffset *
							      delta * 9.0f);
				// always look at the pivot
				camera.mCameraNode->lookAt(
					camera.mCameraPivot
						->_getDerivedPosition(),
					Ogre::Node::TS_PARENT);
			}
		});
#define TURN_SPEED 500.0f // character turning in degrees per second
	ecs.system<const Input, const Camera, CharacterBase>("UpdateBody")
		.kind(flecs::OnUpdate)
		.with<Character>()
		.with<Player>()
		.each([](flecs::entity e, const Input &input,
			 const Camera &camera, CharacterBase &ch) {
			ch.mGoalDirection = Ogre::Vector3::ZERO;
			float delta = e.world().delta_time();
			if (!input.motion.zeroLength()) {
				// calculate actually goal direction in world based on player's key directions
				ch.mGoalDirection +=
					input.motion.z *
					camera.mCameraNode->getOrientation()
						.zAxis();
				ch.mGoalDirection +=
					input.motion.x *
					camera.mCameraNode->getOrientation()
						.xAxis();
				ch.mGoalDirection.y = 0;
				ch.mGoalDirection.normalise();

				Ogre::Quaternion toGoal =
					ch.mBodyNode->getOrientation()
						.zAxis()
						.getRotationTo(
							ch.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);
				ch.mBodyNode->yaw(Ogre::Degree(yawToGoal));
			}
		});
	class ClosestNotMeRayResultCallback
		: public btCollisionWorld::ClosestRayResultCallback {
		btCollisionObject *mMe;

	public:
		ClosestNotMeRayResultCallback(btCollisionObject *me,
					      const btVector3 &from,
					      const btVector3 &to)
			: btCollisionWorld::ClosestRayResultCallback(from, to)
		{
			mMe = me;
		}
		virtual btScalar
		addSingleResult(btCollisionWorld::LocalRayResult &rayResult,
				bool normalInWorldSpace)
		{
			if (rayResult.m_collisionObject == mMe)
				return 1.0f;
			return ClosestRayResultCallback::addSingleResult(
				rayResult, normalInWorldSpace);
		}
	};
	ecs.system<const EngineData, CharacterBody>("CheckGround")
		.kind(flecs::OnUpdate)
		.with<Character>()
		.each([](const EngineData &eng, CharacterBody &body) {
			if (body.checkGround) {
				btVector3 from =
					body.mGhostObject->getWorldTransform()
						.getOrigin() +
					btVector3(0, 0.2f, 0);
				btVector3 to = from + btVector3(0, -3000.0f, 0);
				ClosestNotMeRayResultCallback resultCallback(
					body.mGhostObject, from, to);
				body.mGhostObject->rayTest(from, to,
							   resultCallback);
				body.checkGroundResult =
					resultCallback.hasHit();
				body.checkGround = false;
			}
		});
	ecs.system<const WaterBody, const CharacterBase, CharacterBody>(
		   "CharacterWater1")
		.kind(flecs::OnUpdate)
		.with<Character>()
		.without<InWater>()
		.each([](flecs::entity e, const WaterBody &waterb,
			 const CharacterBase &ch, CharacterBody &body) {
			if (waterb.mInWater.find(body.mGhostObject) !=
				    waterb.mInWater.end() &&
			    ch.mBodyNode->_getDerivedPosition().y < -0.05f) {
				e.add<InWater>();
				std::cout << "Big Splash\n";
			}
#if 0
			if (waterb.mInWater.find(body.mGhostObject) ==
			    waterb.mInWater.end())
				e.add<InWater>();
			std::cout << waterb.mInWater.size() << " InWater\n";
#endif
		});
	ecs.system<const WaterBody, const CharacterBase, CharacterBody>(
		   "CharacterWater2")
		.kind(flecs::OnUpdate)
		.with<Character>()
		.with<InWater>()
		.each([](flecs::entity e, const WaterBody &waterb,
			 const CharacterBase &ch, CharacterBody &body) {
			if (waterb.mInWater.find(body.mGhostObject) ==
				    waterb.mInWater.end() &&
			    ch.mBodyNode->_getDerivedPosition().y > 0.05f)
				e.remove<InWater>();
		});
	ecs.system<const EngineData, CharacterBase, CharacterBody>(
		   "DisplayPlayerPos")
		.kind(flecs::OnUpdate)
		.with<Character>()
		.with<Player>()
		.each([](const EngineData &eng, CharacterBase &ch,
			 CharacterBody &body) {
			std::cout << "player: " << ch.mBodyNode->getPosition()
				  << "\n";
		});
}

void CharacterModule::setAnimation(AnimationControl &anim)
{
	OgreAssert(anim.nextAnim >= 0 &&
			   anim.nextAnim < AnimationControl::NUM_ANIMS,
		   "Bad animation");
	if (anim.currentAnim != AnimationControl::ANIM_NONE) {
		anim.mFadingIn[anim.currentAnim] = false;
		anim.mFadingOut[anim.currentAnim] = true;
	}
	if (anim.nextAnim != AnimationControl::ANIM_NONE) {
		anim.mAnims[anim.nextAnim]->setEnabled(true);
		anim.mAnims[anim.nextAnim]->setWeight(0);
		anim.mFadingOut[anim.nextAnim] = false;
		anim.mFadingIn[anim.nextAnim] = true;
		if (anim.reset)
			anim.mAnims[anim.nextAnim]->setTimePosition(0);
	}
	anim.currentAnim = anim.nextAnim;
	anim.reset = false;
}
#define ANIM_FADE_SPEED \
	7.5f // animation crossfade speed in % of full weight per second

void CharacterModule::fadeAnimations(AnimationControl &anim, Ogre::Real delta)
{
	int i;
	for (i = 0; i < AnimationControl::NUM_ANIMS; i++) {
		if (anim.mFadingIn[i]) {
			// slowly fade this animation in until it has full weight
			Ogre::Real newWeight = anim.mAnims[i]->getWeight() +
					       delta * ANIM_FADE_SPEED;
			anim.mAnims[i]->setWeight(
				Ogre::Math::Clamp<Ogre::Real>(newWeight, 0, 1));
			if (newWeight >= 1)
				anim.mFadingIn[i] = false;
		} else if (anim.mFadingOut[i]) {
			// slowly fade this animation out until it has no weight, and then disable it
			Ogre::Real newWeight = anim.mAnims[i]->getWeight() -
					       delta * ANIM_FADE_SPEED;
			anim.mAnims[i]->setWeight(
				Ogre::Math::Clamp<Ogre::Real>(newWeight, 0, 1));
			if (newWeight <= 0) {
				anim.mAnims[i]->setEnabled(false);
				anim.mFadingOut[i] = false;
			}
		}
	}
}
void CharacterModule::updateCameraGoal(Camera &camera, Ogre::Real deltaYaw,
				       Ogre::Real deltaPitch,
				       Ogre::Real deltaZoom)
{
	camera.mCameraPivot->yaw(Ogre::Degree(deltaYaw), Ogre::Node::TS_PARENT);
	if (!(camera.mPivotPitch + deltaPitch > 25 && deltaPitch > 0) &&
	    !(camera.mPivotPitch + deltaPitch < -60 && deltaPitch < 0)) {
		camera.mCameraPivot->pitch(Ogre::Degree(deltaPitch),
					   Ogre::Node::TS_LOCAL);
		camera.mPivotPitch += deltaPitch;
	}
	Ogre::Real dist = camera.mCameraGoal->_getDerivedPosition().distance(
		camera.mCameraPivot->_getDerivedPosition());
	Ogre::Real distChange = deltaZoom * dist;

	// bound the zoom
	if (!(dist + distChange < 8 && distChange < 0) &&
	    !(dist + distChange > 25 && distChange > 0))
		camera.mCameraGoal->translate(0, 0, distChange,
					      Ogre::Node::TS_LOCAL);
}
}