#include <iostream>
#include "Components.h"
#include "EventTriggerModule.h"
#include "CharacterModule.h"
#include "PhysicsModule.h"
#include "CharacterAnimationModule.h"
#include "EventModule.h"
#include "TerrainModule.h"
#include "WaterModule.h"
#include "world-build.h"
namespace ECS
{
CharacterAnimationModule::CharacterAnimationModule(flecs::world &ecs)
{
	ecs.module<CharacterAnimationModule>();
	ecs.component<AnimationControl>();
	ecs.import <EventModule>();
	ecs.import <TerrainModule>();
	ecs.import <WaterModule>();
	ecs.import <PhysicsModule>();
	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;
				e.set<EventData>({});
				ch.mSkeleton->setBlendMode(
					Ogre::ANIMBLEND_CUMULATIVE);
                if (ch.mBodyEnt->getSkeleton()->getName() ==
                    "normal-male.skeleton") {
                    anim.mAnimationSystem =
                        new AnimationSystem(ch.mBodyEnt,
                                    e, false);
                    anim.mAnimationSystem
                        ->builder()
                        /* clang-format off */
					->output()
					->state_machine(ANIM_FADE_SPEED, "main")
						->state("locomotion")
							->state_machine(ANIM_FADE_SPEED, "locomotion-state")
								->state("idle")
									->animation("idle")
								->end()
								->state("walking")
									->animation("walking")
								->end()
								->state("running")
									->animation("running")
								->end()
								->state("treading_water")
									->animation("treading_water")
								->end()
								->state("swimming")
									->animation("swimming")
								->end()
								->state("swimming-fast")
									->speed(20.0f)
										->animation("swimming")
									->end()
								->end()
							->end()
						->end()
						->state("actuator")
							->state_machine(ANIM_FADE_SPEED * 10.0f, "actuator-state")
								->state("hanging-idle")
									->animation("hanging-idle")
								->end()
								->state("swimming-hold-edge")
									->animation("swimming-hold-edge")
								->end()
								->state("swimming-edge-climb")
									->animation("swimming-edge-climb")
									->trigger(e, "end_of_climb", 0.99f, "animation:swimming-edge-climb:end")
								->end()
								->state("hanging-climb")
									->animation("hanging-climb")
									->trigger(e, "end_of_climb2", 0.99f, "animation:hanging-climb:end")
								->end()
								->state("idle")
									->animation("idle-act")
								->end()
								->state("pass-character")
									->animation("pass-character")
									->trigger(e, "pass-character", 0.99f, "animation:pass-character:end")
								->end()
								->state("character-talk")
									->animation("character-talk")
								->end()
								->state("sitting")
									->animation("sitting-chair")
								->end()
								->state("sitting-ground")
									->animation("sitting-ground")
								->end()
								->transition_end("swimming-edge-climb", "idle")
								->transition_end("hanging-climb", "idle")
								->transition_end("pass-character", "idle")
							->end()
						->end()
					->end();
                    /* clang-format on */

                    anim.mAnimationSystem
                        ->get<AnimationNodeStateMachine>(
                            "main")
                        ->setAnimation("locomotion",
                                   true);
                    anim.mAnimationSystem
                        ->get<AnimationNodeStateMachine>(
                            "locomotion-state")
                        ->setAnimation("idle", true);
                } else
                    anim.mAnimationSystem = nullptr;

				anim.configured = true;
			}
		});
#if 0
	ecs.system<CharacterBase>("RootMotionStart")
		.kind(flecs::OnUpdate)
		.each([this](flecs::entity e, CharacterBase &ch) {
			ch.mBoneMotion = Ogre::Vector3::ZERO;
		});
#endif
	ecs.system<const EngineData, CharacterBase, AnimationControl>(
		   "HandleAnimations1")
		.kind(flecs::OnUpdate)
		.each([this](flecs::entity e, const EngineData &eng,
			     CharacterBase &ch, AnimationControl &anim) {
			float delta = eng.delta;
			// ch.mBoneMotion = Ogre::Vector3::ZERO;
            bool result = false;
            if (anim.mAnimationSystem) {
                result = anim.mAnimationSystem->addTime(delta);
                if (!ch.mRootBone)
                    return;
            } else
				return;
            // The value we get is interpolated value. When result is true it is new step
#if 0
			Ogre::Vector3 offset = ch.mRootBone->getPosition();
			ch.mBoneMotion = static_cast<RootMotionListener *>(
						 anim.mListener)
						 ->getDeltaMotion();
			ch.mRootBone->setPosition(Ogre::Vector3::ZERO);
			Ogre::Vector3 d = offset - ch.mBonePrevMotion;
			ch.mBonePrevMotion = offset;

			std::cout << "length: " << d.length() << std::endl;
			if (d.squaredLength() > 0.02f * 0.02f)
				d = offset;
			if (d.squaredLength() > 0.02f * 0.02f)
				d = Ogre::Vector3::ZERO;
			std::cout << "length2: " << d.length() << std::endl;
			OgreAssert(d.length() < 0.5f, "bad offset");
			ch.mBoneMotion = d;
#endif
#if 0
			if (result) {
				if (d.squaredLength() > 0.0f)
					ch.mBoneMotion =
						ch.mRootBone->getPosition() -
						ch.mBoneMotion;
				else
					ch.mBoneMotion =
						ch.mRootBone->getPosition();
			} else {
				ch.mBoneMotion = ch.mRootBone->getPosition() -
						 ch.mBoneMotion;
			}
#endif

#undef VDEBUG
#ifdef VDEBUG
			std::cout << "root motion: " << delta << ": "
				  << ch.mBoneMotion << " - "
				  << ch.mRootBone->getPosition()
				  << " result: " << result << std::endl;
#endif
#undef VDEBUG
#if 0
			// ch.mRootBone->setPosition(Ogre::Vector3::ZERO);
			ch.mBonePrevMotion = offset;
#endif
		});
	ecs.system<const EngineData, CharacterBase, CharacterVelocity>(
		   "HandleRootMotionVelocity")
		.kind(flecs::OnUpdate)
		.with<TerrainReady>()
		.with<WaterReady>()
		.each([this](flecs::entity e, const EngineData &eng,
			     CharacterBase &ch, CharacterVelocity &v) {
			if (eng.delta < 0.0000001f)
				return;
			if (!ch.mBodyNode)
				return;
			Ogre::Quaternion rot = ch.mBodyNode->getOrientation();
			Ogre::Vector3 pos = ch.mBodyNode->getPosition();
			Ogre::Vector3 boneMotion = ch.mBoneMotion;
			v.velocity = Ogre::Vector3::ZERO;
			float safeDelta =
				Ogre::Math::Clamp(eng.delta, 0.001f, 0.99f);
#if 0
			if (!e.has<CharacterInActuator>()) {
				v.velocity = Ogre::Math::lerp(
					v.velocity,
					rot * boneMotion / safeDelta, 0.99f);
			} else {
				// v.velocity = rot * boneMotion / safeDelta;
				v.velocity = Ogre::Math::lerp(
					v.velocity,
					rot * boneMotion / safeDelta, 0.99f);
			}
#endif
			v.velocity = rot * boneMotion / safeDelta;
#if 0
			if (!e.has<CharacterDisablePhysics>() &&
			    !e.has<CharacterInActuator>()) {
				if (eng.startupDelay <= 0.0f)
					v.velocity += v.gvelocity;
				v.velocity.y = Ogre::Math::Clamp(v.velocity.y,
								 -10.5f, 10.0f);
			}
#endif
			//			if (v.velocity.squaredLength() > 1.4f * 1.4f)
			//				v.velocity = v.velocity.normalisedCopy() * 1.4f;
			// ch.mBoneMotion = Ogre::Vector3::ZERO;
			// safety
			// std::cout << "velocity: " << v.velocity << std::endl;
			v.velocity.x =
				Ogre::Math::Clamp(v.velocity.x, -16.0f, 16.0f);
			v.velocity.z =
				Ogre::Math::Clamp(v.velocity.z, -16.0f, 16.0f);
			v.velocity.y =
				Ogre::Math::Clamp(v.velocity.y, -10.5f, 10.0f);
#if 0
			v.velocity.y = 0.0f;
#endif
		});
#if 0
	ecs.system<const EngineData, const AnimationControl,
		   const CharacterBase, CharacterVelocity>("HandleSwimming")
		.kind(flecs::OnUpdate)
		.with<TerrainReady>()
		.with<WaterReady>()
		.with<InWater>()
		.with<CharacterBuoyancy>()
		.without<CharacterDisablePhysics>()
		.without<CharacterInActuator>()
		.each([this](flecs::entity e, const EngineData &eng,
			     const AnimationControl &anim,
			     const CharacterBase &ch, CharacterVelocity &gr) {
			if (anim.mAnimationSystem
				    ->get<AnimationNodeStateMachine>(
					    "locomotion-state")
				    ->getCurrentState() == "swimming") {
				float h = Ogre::Math::Clamp(
					0.0f - ch.mBodyNode->getPosition().y,
					0.0f, 2000.0f);
				if (h > 0.05 && h < 2.0f)
					gr.gvelocity.y += 0.1f * (h + 1.0f) *
							  h * eng.delta;
			}
		});
#endif
	ecs.system<const EngineData, CharacterBase, AnimationControl,
		   CharacterVelocity>("HandleRootMotion")
		.kind(flecs::OnUpdate)
		.each([this](flecs::entity e, const EngineData &eng,
			     CharacterBase &ch, AnimationControl &anim,
			     CharacterVelocity &v) {
			if (!ch.mBodyNode)
				return;
			if (eng.delta < 0.0000001f)
				return;
			OgreAssert(eng.delta > 0.0f, "Zero delta");
			int maxPen = 0;
			Ogre::Vector3 colNormal;
			bool is_on_floor = false;
			bool penetration = false;
#if 0
			if (eng.startupDelay < 0.0f) {
				if (body.mController) {
					Ogre::Vector3 rotMotion =
						v.velocity * eng.delta;
					rotMotion.x = Ogre::Math::Clamp(
						rotMotion.x, -0.04f, 0.04f);
					rotMotion.y = Ogre::Math::Clamp(
						rotMotion.y, -0.025f, 0.1f);
					rotMotion.z = Ogre::Math::Clamp(
						rotMotion.z, -0.04f, 0.04f);
					btVector3 currentPosition =
						body.mGhostObject
							->getWorldTransform()
							.getOrigin();
					is_on_floor =
						body.mController->isOnFloor();
					penetration = body.mController
							      ->isPenetrating();
					if (is_on_floor)
						v.gvelocity =
							Ogre::Vector3::ZERO;

					btTransform from(
						Ogre::Bullet::convert(
							ch.mBodyNode
								->getOrientation()),
						Ogre::Bullet::convert(
							ch.mBodyNode
								->getPosition()));
					ch.mBodyNode->_setDerivedPosition(
						ch.mBodyNode
							->_getDerivedPosition() +
						rotMotion);
				}
			}
#endif
		});
#if 0
	ecs.system<CharacterVelocity, CharacterBase>("HandleRootMotionEnd")
		.kind(flecs::OnUpdate)
		.each([this](flecs::entity e, CharacterVelocity &v,
			     CharacterBase &ch) {
			// zero the velocity;
			// v.velocity = Ogre::Vector3::ZERO;
			// ch.mBoneMotion = Ogre::Vector3::ZERO;
		});
#endif

	ecs.system<const Input, const CharacterBase, AnimationControl>(
		   "HandleNPCAnimations")
		.kind(flecs::OnUpdate)
		.with<Character>()
		.without<Player>()
		.each([](flecs::entity e, const Input &input,
			 const CharacterBase &ch, AnimationControl &anim) {
			if (!anim.configured)
				return;
            if (!anim.mAnimationSystem)
                return;
			AnimationNodeStateMachine *state_machine =
				anim.mAnimationSystem
					->get<AnimationNodeStateMachine>(
						"locomotion-state");
			Ogre::String current_state =
				state_machine->getCurrentState();
			Ogre::String next_state = "idle";
			if (current_state != "treading_water" &&
			    ch.is_submerged)
				next_state = "treading_water";
			if (current_state != "idle" && !ch.is_submerged)
				next_state = "idle";
			state_machine->setAnimation(next_state);
		});
	ecs.system<const CharacterBase, const CharacterInActuator,
		   AnimationControl>("HandlePlayerAnimationsActuator")
		.kind(flecs::OnUpdate)
		.with<Character>()
		.each([](flecs::entity e, const CharacterBase &ch,
			 const CharacterInActuator &inact,
			 AnimationControl &anim) {
			if (!anim.configured)
				return;
			AnimationNodeStateMachine *main_sm =
				anim.mAnimationSystem
					->get<AnimationNodeStateMachine>(
						"main");
			AnimationNodeStateMachine *actuator_sm =
				anim.mAnimationSystem
					->get<AnimationNodeStateMachine>(
						"actuator-state");
			Ogre::String current_state = main_sm->getCurrentState();
			if (current_state != "actuator")
				main_sm->setAnimation("actuator", true);
			actuator_sm->setAnimation(inact.animationState, true);
		});
	ecs.system<const CharacterBase, AnimationControl>(
		   "HandlePlayerAnimationsNoActuator")
		.kind(flecs::OnUpdate)
		.with<Character>()
		.without<CharacterInActuator>()
		.each([](flecs::entity e, const CharacterBase &ch,
			 AnimationControl &anim) {
			if (!anim.configured)
				return;
            if (!anim.mAnimationSystem)
                return;
			AnimationNodeStateMachine *main_sm =
				anim.mAnimationSystem
					->get<AnimationNodeStateMachine>(
						"main");
			Ogre::String current_state = main_sm->getCurrentState();
			if (current_state != "locomotion")
				main_sm->setAnimation("locomotion", true);
		});
	ecs.system<const Input, const CharacterBase, AnimationControl>(
		   "HandlePlayerAnimations")
		.kind(flecs::OnUpdate)
		.with<Character>()
		.with<Player>()
		.without<CharacterInActuator>()
		.each([](flecs::entity e, const Input &input,
			 const CharacterBase &ch, AnimationControl &anim) {
			if (!anim.configured)
				return;
			AnimationNodeStateMachine *state_machine =
				anim.mAnimationSystem
					->get<AnimationNodeStateMachine>(
						"locomotion-state");
			Ogre::String current_state =
				state_machine->getCurrentState();
			bool controls_idle = input.motion.zeroLength();
			bool anim_is_idle = current_state == "idle" ||
					    current_state == "treading_water";
			bool anim_is_walking = current_state == "walking";
			bool anim_is_running = current_state == "running";
			bool anim_is_swimming_slow = current_state ==
						     "swimming";
			bool anim_is_swimming_fast = current_state ==
						     "swimming-fast";
			bool anim_is_swimming = anim_is_swimming_slow ||
						anim_is_swimming_fast;
			bool anim_is_motion = anim_is_walking ||
					      anim_is_running ||
					      anim_is_swimming;
			bool start_motion = !controls_idle && anim_is_idle;
			bool end_motion = controls_idle && !anim_is_idle;
			Ogre::String next_state = current_state;
			if (controls_idle && anim_is_idle) {
				if (current_state != "treading_water" &&
				    ch.is_submerged)
					next_state = "treading_water";
				else if (current_state != "idle" &&
					 !ch.is_submerged)
					next_state = "idle";
				state_machine->setAnimation(next_state);
			} else if (start_motion) {
				if (ch.is_submerged) {
					if (input.fast)
						next_state = "swimming-fast";
					else
						next_state = "swimming";
				} else {
					if (input.fast)
						next_state = "running";
					else
						next_state = "walking";
				}
				state_machine->setAnimation(next_state, true);
			} else if (end_motion) {
				if (ch.is_submerged)
					state_machine->setAnimation(
						"treading_water");
				else
					state_machine->setAnimation("idle");
			} else {
				if (ch.is_submerged) {
					if (input.fast &&
					    !anim_is_swimming_fast) {
						next_state = "swimming-fast";
					} else if (!input.fast &&
						   !anim_is_swimming_slow) {
						next_state = "swimming";
					}
				} else {
					if (input.fast && !anim_is_running)
						next_state = "running";
					else if (!input.fast &&
						 !anim_is_walking)
						next_state = "walking";
				}
				if (current_state != next_state)
					state_machine->setAnimation(next_state);
			}
		});
	ecs.system<const Input, const CharacterBase, AnimationControl,
		   CharacterInActuator>("HandlePlayerAnimations2")
		.kind(flecs::OnUpdate)
		.with<Character>()
		.with<Player>()
		.each([](flecs::entity e, const Input &input,
			 const CharacterBase &ch, AnimationControl &anim,
			 CharacterInActuator &act) {
			bool controls_idle = input.motion.zeroLength();
			if (!controls_idle) {
				std::cout << "motion.z: "
					  << Ogre::Math::Abs(input.motion.z -
							     act.prevMotion.z)
					  << std::endl;
				bool trigger_event = false;
				e.each<InTrigger>([&](flecs::entity trig) {
					if (Ogre::Math::Abs(input.motion.z -
							    act.prevMotion.z) >
					    0.001f) {
						if (input.motion.z < 0) {
							trig.get_mut<EventData>()
								.add(e,
								     "actuator_forward",
								     trig, e);
							trig.modified<
								EventData>();
						}
						if (input.motion.z > 0) {
							trig.get_mut<EventData>()
								.add(e,
								     "actuator_backward",
								     trig, e);
							trig.modified<
								EventData>();
						}
					}
					if (input.act_pressed) {
						trig.get_mut<EventData>().add(
							e, "actuator_action",
							trig, e);
						trig.modified<EventData>();
					}
					//					ECS::get_mut<LuaData>().call_handler(
					//						"actuator_update", trig, e);
					trigger_event = true;
				});
				if (!trigger_event) {
					if (Ogre::Math::Abs(input.motion.z -
							    act.prevMotion.z) >
					    0.001f) {
						if (input.motion.z < 0) {
							e.get_mut<EventData>().add(
								e,
								"_in_actuator_forward",
								e, e);
						}
						if (input.motion.z > 0) {
							e.get_mut<EventData>().add(
								e,
								"_in_actuator_backward",
								e, e);
						}
					}
					if (input.act_pressed) {
						e.get_mut<EventData>().add(
							e,
							"_in_actuator_action",
							e, e);
					}
				}
				act.prevMotion.x = input.motion.x;
				act.prevMotion.y = input.motion.y;
				act.prevMotion.z = input.motion.z;
			}

#if 0
			if (!controls_idle) {
				if (Ogre::Math::Abs(input.motion.z - act.prevMotion.z) > 0.001f) {
					if (input.motion.z < 0)
						ECS::get_mut<LuaData>().call_handler("actuator_forward", e);
				}
				ECS::get_mut<LuaData>().call_handler("actuator_controls_update");
			}
#endif
			act.prevMotion = input.motion;
		});
	ecs.system<EventData>("UpdateEvents")
		.kind(flecs::OnUpdate)
		.with<Character>()
		.each([](flecs::entity e, EventData &evt) {
			for (auto ev : evt.events) {
				std::cout << "character event: " << ev.event
					  << std::endl;
				/* parse character events */
				e.each<InTrigger>([&](flecs::entity trig) {
					/* if triggered, dispatch events to trigger */
					trig.get_mut<EventData>().add(
						ev.sender, ev.event,
						trig, // it is easier this way to identify trigger entity
						ev.e2);
				});
			}
			evt.events.clear();
		});

#ifdef VDEBUG
	ecs.system<const CharacterBase>("CharacterGravityStatus")
		.kind(flecs::OnUpdate)
		.with<Character>()
		.with<Player>()
		.each([](flecs::entity e, const CharacterBase &ch) {
			if (e.has<CharacterGravity>())
				std::cout << "gravity\n";
			else
				std::cout << "no gravity\n";
			if (e.has<InWater>())
				std::cout << "in water\n";
			else
				std::cout << "out of water\n";
			std::cout
				<< "h=" << ch.mBodyNode->_getDerivedPosition().y
				<< std::endl;
		});
#endif
#if 0
	ecs.system<const EngineData, const CharacterBase, CharacterBody>(
		   "UpdateBodyCast")
		.kind(flecs::OnUpdate)
		.without<CharacterInActuator>()
		.each([](flecs::entity e, const EngineData &eng,
			 const CharacterBase &ch, CharacterBody &body) {
			struct ResultCallback
				: public btCollisionWorld::RayResultCallback {
				btCollisionObject *m_me;
				btVector3 m_from, m_to, m_hitNormalWorld,
					m_hitPointWorld;
				ResultCallback(btCollisionObject *me,
					       const btVector3 &from,
					       const btVector3 &to)
					: m_me(me)
					, m_from(from)
					, m_to(to)
				{
				}
				btScalar addSingleResult(
					btCollisionWorld::LocalRayResult
						&rayResult,
					bool normalInWorldSpace) override
				{
					if (rayResult.m_collisionObject == m_me)
						return 1.0f;
					if (!btPairCachingGhostObject::upcast(
						    rayResult.m_collisionObject))
						return 1.0f;
					if (!(rayResult.m_collisionObject
						      ->getCollisionFlags() &
					      btCollisionObject::
						      CF_CHARACTER_OBJECT))
						return 1.0f;
					m_closestHitFraction =
						rayResult.m_hitFraction;
					m_collisionObject =
						rayResult.m_collisionObject;
					if (normalInWorldSpace)
						m_hitNormalWorld =
							rayResult
								.m_hitNormalLocal;
					else
						m_hitNormalWorld =
							m_collisionObject
								->getWorldTransform()
								.getBasis() *
							rayResult
								.m_hitNormalLocal;
					m_hitPointWorld.setInterpolate3(
						m_from, m_to,
						rayResult.m_hitFraction);
					return rayResult.m_hitFraction;
				}
			};
			Ogre::Vector3 offset(0.0f, 0.5f, 0.0f);
			float dist = 0.5f;
			btVector3 a = Ogre::Bullet::convert(
					  ch.mBodyNode->getPosition() + offset),
				  b(Ogre::Bullet::convert(
					  ch.mBodyNode->getPosition() +
					  ch.mBodyNode->getOrientation() *
						  Ogre::Vector3(0, 0, dist) +
					  offset));
			ResultCallback result(body.mGhostObject, a, b);
			// body.mGhostObject->rayTest(a, b, result);
			eng.mWorld->getBtWorld()->rayTest(a, b, result);
			if (result.hasHit()) {
				std::cout << "Hit!!! " << result.m_hitPointWorld
					  << std::endl;
				e.set<CharacterInActuator>(
					{ "idle", { 0, 0, 0 } });
				ECS::get<LuaBase>().mLua->call_handler(
					"character_enter", e,
					ECS::get<Body2Entity>().entities.at(
						const_cast<btCollisionObject *>(
							result.m_collisionObject)));
			}
		});
#endif
	struct AnimationSetCommand : public GameWorld::Command {
		int operator()(const std::vector<GameWorld::Parameter *> &args)
			override
		{
			GameWorld::ValueParameter<flecs::entity> *param_e =
				static_cast<GameWorld::ValueParameter<
					flecs::entity> *>(args[0]);
			OgreAssert(param_e->get().is_valid(), "bad entity");
			GameWorld::ValueParameter<std::string> *param_node =
				static_cast<GameWorld::ValueParameter<
					std::string> *>(args[1]);
			GameWorld::ValueParameter<std::string> *param_state =
				static_cast<GameWorld::ValueParameter<
					std::string> *>(args[2]);
			if (param_e->get().has<AnimationControl>() &&
			    param_e->get().get<AnimationControl>().configured ==
				    true) {
				const AnimationControl &control =
					param_e->get().get<AnimationControl>();
				AnimationNodeStateMachine *sm =
					control.mAnimationSystem
						->get<AnimationNodeStateMachine>(
							param_node->get());
				bool reset = false;
				if (args.size() == 4) {
					GameWorld::ValueParameter<bool>
						*param_reset = static_cast<
							GameWorld::ValueParameter<
								bool> *>(
							args[3]);
					reset = param_reset->get();
				}
				sm->setAnimation(param_state->get(), reset);
				std::cout << "animation switch: "
					  << param_node->get() << " "
					  << param_state->get() << std::endl;
			}
			return 0;
		}
	};
	ECS::get_mut<GameWorld>().add_command<AnimationSetCommand>(
		"set_animation_state");
}
}
