// #pragma once

// #pragma once
#ifndef AnimationrigPhysics_H
#define AnimationrigPhysics_H

#include <AnimationrigMisc.h>
#include <AnimationrigWiggle.h>

using namespace std;

namespace Animrig {
	// typedef
	typedef Matrix4 PhysicsMatrix4;
	// vector
	struct PhysicsVector3
	{
		double x;
		double y;
		double z;

		inline double length()
		{
			return sqrt(x * x + y * y + z * z);
		}
		inline void normalize()
		{
			auto l = sqrt(x * x + y * y + z * z);
			if (l < 0.00001) return;

			l = 1.0 / l;
			x = x * l;
			y = y * l;
			z = z * l;
		}
		inline void setVector(double x_, double y_, double z_)
		{
			x = x_;
			y = y_;
			z = z_;
		}
		inline void setAdd(const PhysicsVector3 &v1, const PhysicsVector3 &v2)
		{
			x = v1.x + v2.x;
			y = v1.y + v2.y;
			z = v1.z + v2.z;
		}
		inline void setSub(const PhysicsVector3 &v1, const PhysicsVector3 &v2)
		{
			x = v1.x - v2.x;
			y = v1.y - v2.y;
			z = v1.z - v2.z;
		}
		inline void mul(double m)
		{
			x = x * m;
			y = y * m;
			z = z * m;
		}
		inline void transform(const PhysicsMatrix4 &m)
		{
			auto xx = x;
			auto yy = y;
			auto zz = z;
			x = xx * m._11 + yy * m._21 + zz * m._31 + m._41;
			y = xx * m._12 + yy * m._22 + zz * m._32 + m._42;
			z = xx * m._13 + yy * m._23 + zz * m._33 + m._43;
		}
		inline double dotProduct(const PhysicsVector3 &v1)
		{
			return v1.x * x + v1.x * y + v1.x * z;
		}
		inline bool Compare(const PhysicsVector3 &v1)
		{
			if ((x == v1.x) && (y == v1.y) && (z == v1.z)) return true;
			return false;
		}
	};
	// basic
	enum class PhysicsNodeType { Point, Bone, Angle };
	enum class PhysicsSpringModel { OneSide, Both };
	enum class PhysicsGroupType { Spring };

	class PhysicsGroup {
	public:
		wstring name = L"";
		wstring aliasName = L"";

		PhysicsGroupType groupType = PhysicsGroupType::Spring;

		virtual void update(double dt) { }
		virtual void apply() { }
		virtual void appease() { }
		virtual void addForce(const PhysicsVector3 &d) { }
	};
	class PhysicsNode {
	public:
		PhysicsNodeType nodeType = PhysicsNodeType::Point;
		void* userObject = nullptr;
		PhysicsVector3 worldPosition = {0,0,0};
		bool fixed = false;
		// public double Mass = 1.0;
	};
	class PhysicsSpring {
	public:
		PhysicsNodeType nodeType = PhysicsNodeType::Point;
		PhysicsGroup *ownerGroup;

		PhysicsSpring(PhysicsGroup *group)
		{
			ownerGroup = group;
		}
		virtual void update(double dt) {}
		virtual void setNatural(const PhysicsVector3 &v) {}
		virtual void appease() {}
	};
	// vector spring
	class PhysicsSpringGroup : public PhysicsGroup
	{
	public:
		double K = 100;
		double damping = 10;
		double gravity = 1.0;
		double mass = 1.0;
		PhysicsVector3 gravityVector = { 0.0, -1.0, 0.0 };
		double limitVelocity = 1000;
		double limitAngleMin = -0xff;
		double limitAngleMax = +0xff;
		double limitMoveRatio = 0xff;
	};
	class PhysicsSpringVector : public PhysicsSpring
	{
	protected:
		PhysicsNode *node1 = nullptr;
		PhysicsNode *node2 = nullptr;
		PhysicsSpringVector *fParent = nullptr;
	
	public:
		PhysicsVector3 natural = { 0,0,0 };
		PhysicsVector3 velocity = { 0,0,0 };
		double naturalAngle = 0.0;
		double appliedAngle = 0.0;

		bool fixedLength = false;
		PhysicsSpringModel springMode = PhysicsSpringModel::OneSide;

		PhysicsSpringVector(PhysicsGroup *group, PhysicsSpringVector *parent_)
			: PhysicsSpring(group)
		{
			nodeType = PhysicsNodeType::Point;
			fParent = parent_;
		}
		void update(double dt) override
		{
			auto sg = (PhysicsSpringGroup*)ownerGroup;
			// mass
			auto mass = fmax(sg->mass, 0.001);
			auto imass = 1.0 / mass;
			// spring param
			auto k = sg->K;
			// gravity
			/*
			{
				PhysicsVector3 v;
				v.setVector(1, 0, 0);
				double gx = v.dotProduct(sg->gravityVector) * sg->gravity;
				v.setVector(0, 1, 0);
				double gy = v.dotProduct(sg->gravityVector) * sg->gravity;
				v.setVector(0, 0, 1);
				double gz = v.dotProduct(sg->gravityVector) * sg->gravity;
				
			}
			*/
			double gx = 0.0;
			double gy = 0.0;
			double gz = 0.0;

			PhysicsVector3 d;
			d.setSub(node2->worldPosition, node1->worldPosition);
			
			double force = -k * (d.x - natural.x);
			double damping = velocity.x * sg->damping;
			force += sg->mass * gx - damping;
			double accel = force * imass;
			velocity.x += accel * dt;

			force = -k * (d.y - natural.y);
			damping = velocity.y * sg->damping;
			force += sg->mass * gy - damping;
			accel = force * imass;
			velocity.y += accel * dt;

			force = -k * (d.z - natural.z);
			damping = velocity.z * sg->damping;
			force += sg->mass * gz - damping;
			accel = force * imass;
			velocity.z += accel * dt;

			// velocity limitter
			if (velocity.length() > sg->limitVelocity * 100)   // 100 = 100fps
			{
				velocity.normalize();
				velocity.mul(sg->limitVelocity);
			}

			// pos = pos + velocity * dt
			d = velocity;
			d.mul(dt);

			if (springMode == PhysicsSpringModel::Both)
			{
				if (!node1->fixed) node1->worldPosition.setSub(node1->worldPosition, d);
				if (!node2->fixed) node2->worldPosition.setAdd(node2->worldPosition, d);
			}
			else
			{
				node2->worldPosition.setAdd(node2->worldPosition, d);
			}


			// limitter
			if ((sg->limitAngleMin > -1.0) || (sg->limitAngleMax < +1.0))
			{
				// parent
				double pdr = 0.0;
				if (parent() != nullptr) pdr = parent()->appliedAngle - parent()->naturalAngle;
				auto targetangle = naturalAngle + pdr;
				// self
				d.setSub(node2->worldPosition, node1->worldPosition);
				double pr = atan2(d.y, d.x) * Misc::RadianToAngle;
				double dr = pr - targetangle;
				// bitween -0.5 to +0.5
				dr = dr + 0xff;
				dr = dr - floor(dr);
				if (dr > 0.5) dr = dr - 1.0;
				if ((dr < sg->limitAngleMin) || (dr > sg->limitAngleMax))
				{
					dr = min(max(dr, sg->limitAngleMin), sg->limitAngleMax);
					d.setVector(d.length(), 0, 0);
					auto m = Misc::IdentityMatrix4;
					Misc::m4PushRotateZ(m, (float)(targetangle + dr));
					d.transform(m);
					node2->worldPosition.setAdd(node1->worldPosition, d);
				}
			}

			if (sg->limitMoveRatio < 0xff)
			{
				double len = natural.length() * sg->limitMoveRatio;  // ratio to world
				d.setSub(node2->worldPosition, node1->worldPosition);
				d.setSub(d, natural);
				if (d.length() > len)
				{
					d.normalize();
					d.mul(len);
					d.setAdd(d, natural);
					node2->worldPosition.setAdd(node1->worldPosition, d);
				}
			}

			// fixed length
			if (fixedLength)
			{
				double len = natural.length();
				d.setSub(node2->worldPosition, node1->worldPosition);
				d.normalize();
				d.mul(len);
				node2->worldPosition.setAdd(node1->worldPosition, d);
			}

			// applied angle
			d.setSub(node2->worldPosition, node1->worldPosition);
			appliedAngle = atan2(d.y, d.x) * Misc::RadianToAngle;
		}
		void appease() override
		{
			velocity.setVector(0, 0, 0);
			if (node1 == nullptr) return;
			if (node2 == nullptr) return;

			node2->worldPosition.x = node1->worldPosition.x + natural.x;
			node2->worldPosition.y = node1->worldPosition.y + natural.y;
			node2->worldPosition.z = node1->worldPosition.z + natural.z;
			appliedAngle = naturalAngle;
		}
		void setNatural(const PhysicsVector3 &v)
		{
			natural = v;
			naturalAngle = atan2(natural.y, natural.x) * Misc::RadianToAngle;
			appliedAngle = naturalAngle;
		}
		PhysicsSpringVector* parent() { return fParent; }
	};
	// bone
	class PhysicsNodeBone : public PhysicsNode
	{
	protected:
		PhysicsNodeBone *fParent = nullptr;
	public:
		// original
		double localLength = 100;
		PhysicsVector3 localPosition = { 0,0,0 };
		double localAngle = 0;
		double localScaleX = 1.0;
		double localScaleY = 1.0;
		// applied spring physics
		PhysicsVector3 aLocalPosition = { 0,0,0 };
		double aLocalAngle = 0;

		PhysicsMatrix4 worldMatrix;

		PhysicsNodeBone *getParent() { return fParent; }
		vector<PhysicsNodeBone*> children;
		vector<PhysicsSpring*> springs;

		PhysicsNodeBone(PhysicsNodeBone *parentbone)
		{
			fParent = parentbone;
			if (fParent != nullptr) fParent->children.emplace_back(this);

			fixed = fParent == nullptr;   // root bone is fixed

			worldMatrix = Misc::IdentityMatrix4;
		}

		void updateWorldMatrix(const PhysicsMatrix4 &parentmatrix, bool setupworldposition)
		{
			if (setupworldposition)
			{
				aLocalPosition = localPosition;
				aLocalAngle = localAngle;

				worldMatrix = getAppliedLocalMatrix();
				Misc::m4PushMatrix(worldMatrix, parentmatrix);

				PhysicsVector3 v;
				v.setVector(0, 0, 0);
				v.transform(worldMatrix);
				worldPosition = v;
			}
			else
			{
				worldMatrix = getAppliedLocalMatrix();
				Misc::m4PushMatrix(worldMatrix, parentmatrix);
			}

			for(auto b : children)
			{
				b->updateWorldMatrix(worldMatrix, setupworldposition);
			}
		}
		PhysicsNodeBone *getRoot()
		{
			if (fParent != nullptr)
				return fParent->getRoot();
			else
				return this;
		}
		PhysicsMatrix4 getLocalMatrix()
		{
			auto m = Misc::IdentityMatrix4;
			Misc::m4PushScale(m, (float)localScaleX, (float)localScaleY, 1.0f);
			Misc::m4PushRotateZ(m, (float)localAngle);
			Misc::m4PushMove(m, (float)localPosition.x, (float)localPosition.y, (float)localPosition.z);
			return m;
		}
		PhysicsMatrix4 getAppliedLocalMatrix()
		{
			auto m = Misc::IdentityMatrix4;
			Misc::m4PushScale(m, (float)localScaleX, (float)localScaleY, 1.0f);
			Misc::m4PushRotateZ(m, (float)aLocalAngle);
			Misc::m4PushMove(m, (float)aLocalPosition.x, (float)aLocalPosition.y, (float)aLocalPosition.z);
			return m;
		}
	};
	class PhysicsSpringBone : public PhysicsSpringVector
	{
	protected:
		double offsetAngle = 0;
		PhysicsNodeBone *fOwner = nullptr;
		PhysicsNodeBone *fChild = nullptr;
	public:
		PhysicsNodeBone *getOwner() { return fOwner; }
		PhysicsNodeBone *getChild() { return fChild; }

		bool lockBoneAngle = false;
		bool lockBonePosition = false;

		PhysicsSpringBone(PhysicsGroup *group, PhysicsSpringBone *parent_,PhysicsNodeBone *ownerbone, PhysicsNodeBone *childbone)
			: PhysicsSpringVector(group,parent_)
		{
			nodeType = PhysicsNodeType::Bone;
			springMode = PhysicsSpringModel::OneSide;

			fOwner = ownerbone;
			fChild = childbone;
			fOwner->springs.emplace_back(this);

			fixedLength = true;
			lockBoneAngle = false;
			lockBonePosition = true;

			node1 = fOwner;
			node2 = fChild;
		}
		void update(double dt) override         // need call parent
		{
			PhysicsSpringVector::update(dt);

			for(auto s : fChild->springs)
			{
				s->update(dt);
			}
		}
		void setupNatural(const PhysicsMatrix4 &parentmatrix)
		{
			auto m = fOwner->getLocalMatrix();
			Misc::m4PushMatrix(m, parentmatrix);
			PhysicsVector3 v1 = { 0,0,0 };
			v1.transform(m);

			if (fOwner->fixed)
			{
				fOwner->worldPosition = v1;
			}

			if (fChild == nullptr)
			{
				PhysicsVector3 d = { 100,0,0 };
				setNatural(d);
			}
			else
			{
				// next bone position
				auto v2 = fChild->localPosition;
				v2.transform(m);
				PhysicsVector3 d;
				d.setSub(v2,v1);
				setNatural(d);
				auto r = atan2(d.y, d.x) * Misc::RadianToAngle;
				// angle target position (offset)
				PhysicsVector3 v3 = { 100,0,0 };
				v3.transform(m);
				d.setSub(v3,v1);
				offsetAngle = atan2(d.y, d.x) * Misc::RadianToAngle - r;
			}

			for(auto s : fChild->springs)
			{
				static_cast<PhysicsSpringBone*>(s)->setupNatural(m);
			}
		}
		void updateBoneLocal(const PhysicsMatrix4 &invparentmatrix)
		{
			if (fOwner == nullptr) return;
			if (fChild == nullptr) return;

			auto im = invparentmatrix;
			Misc::m4PushMove(im, (float)-fOwner->aLocalPosition.x, (float)-fOwner->aLocalPosition.y, (float)-fOwner->aLocalPosition.z);
			auto v1 = fOwner->worldPosition;
			auto v2 = fChild->worldPosition;
			v1.transform(im);
			v2.transform(im);

			PhysicsVector3 d;
			d.setSub(v2,v1);

			// angle
			if (!lockBoneAngle)
			{
				double dr = atan2(d.y, d.x) * Misc::RadianToAngle + offsetAngle;
				// dr = dr + 0xff;
				// dr = dr - floor(dr);
				// if (dr > +0.5) dr = dr - 1.0;
				// if (dr < -0.5) dr = dr + 1.0;
				fOwner->aLocalAngle = dr;
			}
			else
			{
				fOwner->aLocalAngle = fOwner->localAngle;
			}

			Misc::m4PushRotateZ(im, (float)-fOwner->aLocalAngle);

			// position
			if (!lockBonePosition)
			{
				v2 = fChild->worldPosition;
				v2.transform(im);
				fChild->aLocalPosition = v2;
			}
			else
			{
				fChild->aLocalPosition = fChild->localPosition;
			}

			for(auto s : fChild->springs)
			{
				static_cast<PhysicsSpringBone*>(s)->updateBoneLocal(im);
			}
		}
	};
	// group
	enum class PhysicsGroupSpringBoneMode { Bone, StrBone, Point, StrPoint };
	class PhysicsGroupSpringBone : public PhysicsSpringGroup
	{
	protected:
		PhysicsNodeBone *fRootBone = nullptr;
		// force
		bool cacheInitFlag;
		PhysicsVector3 cacheRootPosition;
		double cacheRootAngle;

		void updateTransformForce()
		{
			if (fRootBone == nullptr) return;

			// optimize
			auto m = rootMatrix;
			Misc::m4PushMatrix(m,externalMatrix);

			auto v1 = fRootBone->localPosition;
			v1.mul(forceRotateRatio);     // Value adjustment during rotation
			v1.transform(m);
			double r1 = 0.0;
			if (cacheInitFlag)
			{
				cacheRootPosition = v1;
				cacheRootAngle = r1;
				cacheInitFlag = false;
			}

			if (v1.Compare(cacheRootPosition) && (r1 == cacheRootAngle)) return;

			// world position diff
			PhysicsVector3 d;
			d.setSub(v1,cacheRootPosition);
			d.mul(forceRatio);        // Value adjustment during movement

			// convert root local space (physics group space)
			auto im = invMatrix2D(m);
			im._41 = 0.0f;    // disable move
			im._42 = 0.0f;
			im._43 = 0.0f;
			d.transform(im);
			slidSpring(d.x, d.y, fRootBone);

			cacheRootPosition = v1;
			cacheRootAngle = r1;
		}
		PhysicsMatrix4 invMatrix2D(const PhysicsMatrix4 &m)
		{
			PhysicsMatrix4 r = Misc::IdentityMatrix4;

			auto d = m._11 * m._22 - m._12 * m._21;
			if (abs(d) < 0.000001f) d = 0.000001f;

			float rd = (float)(1.0 / d);
			r._11 = +m._22 * rd;
			r._12 = -m._12 * rd;
			r._21 = -m._21 * rd;
			r._22 = +m._11 * rd;

			r._41 = -m._41;
			r._42 = -m._42;
			r._43 = -m._43;

			return r;
		}
		void slidSpring(double x, double y, PhysicsNodeBone *bone)
		{
			struct InnerFunc {
				static void inSlide(double x, double y, PhysicsNodeBone *bone) {
					if (bone == nullptr) return;

					if (!bone->fixed)
					{
						bone->worldPosition.x -= x;
						bone->worldPosition.y -= y;
					}

					for (auto b : bone->children) {
						inSlide(x, y, b);
					}
				}
			};
			auto w = Wiggle::random(elapsedTime * 2, WiggleType::Linear, 273) * forceWiggle + (1.0 - forceWiggle * 0.5);
			x *= w;
			y *= w;

			InnerFunc::inSlide(x, y, bone);
		}
	public:
		vector<unique_ptr<PhysicsNode>> nodes;
		vector<unique_ptr<PhysicsSpring>> springs;
		PhysicsNodeBone *getRootBone() { return fRootBone; }
		
		PhysicsMatrix4 rootMatrix;
		PhysicsMatrix4 externalMatrix;

		void *rootUserObject = nullptr;

		double elapsedTime;
		double forceRotateRatio;
		double forceRatio;
		double alpha;
		double forceWiggle;
		PhysicsGroupSpringBoneMode mode = PhysicsGroupSpringBoneMode::Bone;

		PhysicsGroupSpringBone()
			: PhysicsSpringGroup()
		{
			groupType = PhysicsGroupType::Spring;
			rootMatrix = Misc::IdentityMatrix4;
			externalMatrix = Misc::IdentityMatrix4;

			rootUserObject = nullptr;

			forceRotateRatio = 1.0;
			forceRatio = 1.0;
			alpha = 1.0;
			forceWiggle = 0.0;

			cacheInitFlag = true;
			cacheRootPosition.setVector(0, 0, 0);
			cacheRootAngle = 0.0;
			elapsedTime = rand() % 1024;
		}

		void update(double dt) override
		{
			elapsedTime += dt;

			updateTransformForce();

			// safe
			K = fmin(fmax(K, 0.001), 100 * 100);
			mass = fmin(fmax(mass, 0.001), 100 * 100);
			damping = fmin(fmax(damping, 0.1), 100 * 100);
			gravity = fmin(fmax(gravity, 0.0), 100 * 100);
			if (gravityVector.length() > 1.0) gravityVector.normalize();
			limitVelocity = fmin(fmax(limitVelocity, 1), 0xffff);

			// 100 fps
			while (dt > 0.0)
			{
				auto t = fmin(dt, 1.0 / 100.0);
				for(auto s : fRootBone->springs) {
					s->update(t);
				}
				dt -= 1.0 / 100.0;
			}
		}
		void apply() override
		{
			for(auto s : fRootBone->springs)
			{
				static_cast<PhysicsSpringBone*>(s)->updateBoneLocal(Misc::IdentityMatrix4);
			}
		}
		void appease() override
		{
			fRootBone->updateWorldMatrix(Misc::IdentityMatrix4, true);
			for(auto s : fRootBone->springs)
			{
				static_cast<PhysicsSpringBone*>(s)->appease();
			}
		}
		void addForce(const PhysicsVector3 &d) override {
			auto m = invMatrix2D(rootMatrix);
			m._41 = 0.0f;  // disable move
			m._42 = 0.0f;
			m._43 = 0.0f;
			auto v = d;
			v.transform(m);

			slidSpring(-v.x, -v.y, fRootBone);
		}


		void addBone(PhysicsNodeBone *b)
		{
			//auto o = new unique_ptr<PhysicsNodeBone>(b);
			nodes.emplace_back(b);

			if (b->getParent() == nullptr)
			{
				fRootBone = b;
			}
		}
		void addSpring(PhysicsSpringBone *s)
		{
			switch (mode)
			{
			case PhysicsGroupSpringBoneMode::StrPoint:
				s->fixedLength = false;
				s->lockBoneAngle = true;
				s->lockBonePosition = false;
				break;
			case PhysicsGroupSpringBoneMode::Point:
				s->fixedLength = true;
				s->lockBoneAngle = true;
				s->lockBonePosition = false;
				break;
			case PhysicsGroupSpringBoneMode::StrBone:
				s->fixedLength = false;
				s->lockBoneAngle = false;
				s->lockBonePosition = false;
				break;
			case PhysicsGroupSpringBoneMode::Bone:
				s->fixedLength = true;
				s->lockBoneAngle = false;
				s->lockBonePosition = true;
				break;
			}

			//auto o = new unique_ptr<PhysicsSpringBone>(s);
			springs.emplace_back(s);
		}
		void updateWorldMatrix()
		{
			fRootBone->updateWorldMatrix(Misc::IdentityMatrix4, false);
		}
		void setupNatural()
		{
			// build worldposition. but worldmatrix not used in SetupNatural.
			fRootBone->updateWorldMatrix(Misc::IdentityMatrix4, true);
			// build spring natual
			for(auto s : fRootBone->springs)
			{
				static_cast<PhysicsSpringBone*>(s)->setupNatural(Misc::IdentityMatrix4);
			}

			// clear force
			cacheInitFlag = true;
		}
	};
}

#endif