using System;
using System.Collections.Generic;

namespace Animrig
{
    // typedef
    using PhysicsMatrix4 = Matrix4;
    // vector
    public struct PhysicsVector3
    {
        public double x;
        public double y;
        public double z;
        
        public PhysicsVector3(double x_, double y_, double z_)  // contructor
        {
            x = x_;
            y = y_;
            z = z_;
        }
        public double Length()
        {
            return Math.Sqrt(x * x + y * y + z * z);
        }
        public void Normalize()
        {
            var l = Math.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;
        }
        public void SetVector(double x_, double y_, double z_)
        {
            x = x_;
            y = y_;
            z = z_;
        }
        public void SetAdd(ref PhysicsVector3 v1, ref PhysicsVector3 v2)
        {
            x = v1.x + v2.x;
            y = v1.y + v2.y;
            z = v1.z + v2.z;
        }
        public void SetSub(ref PhysicsVector3 v1, ref PhysicsVector3 v2)
        {
            x = v1.x - v2.x;
            y = v1.y - v2.y;
            z = v1.z - v2.z;
        }
        public void Mul(double m)
        {
            x = x * m;
            y = y * m;
            z = z * m;
        }
        public void Transform(ref PhysicsMatrix4 m)
        {
            var xx = x;
            var yy = y;
            var 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;
        }
        public double DotProduct(ref PhysicsVector3 v1)
        {
            return v1.x * x + v1.x * y + v1.x * z;
        }
        public bool Compare(ref PhysicsVector3 v1)
        {
            if ((x == v1.x) && (y == v1.y) && (z == v1.z)) return true;
            return false;
        }
    }
    // base
    public enum PhysicsNodeType { Point,Bone,Angle };
    public enum PhysicsSpringModel { OneSide, Both };
    public enum PhysicsGroupType { Spring };
    public abstract class PhysicsGroup
    {
        public string Name = "";
        public string AliasName = "";

        public PhysicsGroupType GroupType { get; protected set; } = PhysicsGroupType.Spring;
        public abstract void Update(double dt);
        public abstract void Apply();
        public abstract void Appease();
        public abstract void AddForce(ref PhysicsVector3 d);
    }
    public class PhysicsNode
    {
        public PhysicsNodeType NodeType { get; protected set; }  = PhysicsNodeType.Point;
        public Object UserObject = null;
        public PhysicsVector3 WorldPosition = new PhysicsVector3(0.0,0.0,0.0);
        public bool Fixed = false;
        // public double Mass = 1.0;
    }
    public abstract class PhysicsSpring
    {
        public PhysicsNodeType NodeType { get; protected set; } = PhysicsNodeType.Point;
        public PhysicsGroup OwnerGroup { get; protected set; } = null;

        public PhysicsSpring(PhysicsGroup group)
        {
            OwnerGroup = group;
        }
        public abstract void Update(double dt);
        public abstract void SetNatural(ref PhysicsVector3 v);
        public abstract void Appease();
    }
    // vector spring
    public abstract class PhysicsSpringGroup : PhysicsGroup
    {
        public double K = 100;
        public double Damping = 10;
        public double Gravity = 1.0;
        public double Mass = 1.0;
        public PhysicsVector3 GravityVector = new PhysicsVector3(0.0,-1.0,0.0);
        public double LimitVelocity = 1000;
        public double LimitAngleMin = -0xff;
        public double LimitAngleMax = +0xff;
        public double LimitMoveRatio = 0xff;
    }
    public class PhysicsSpringVector : PhysicsSpring
    {
        protected PhysicsNode Node1 = null;
        protected PhysicsNode Node2 = null;

        public PhysicsVector3 Natural  = new PhysicsVector3(0, 0, 0);
        public double NaturalAngle { get; protected set; } = 0.0;
        public PhysicsVector3 Velocity = new PhysicsVector3(0, 0, 0);

        public bool FixedLength = false;
        public PhysicsSpringModel SpringMode = PhysicsSpringModel.OneSide;

        public PhysicsSpringVector(PhysicsGroup group)
            : base(group)
        {
            NodeType = PhysicsNodeType.Point;
        }
        public override void Update(double dt)
        {
            var sg = OwnerGroup as PhysicsSpringGroup;
            // mass
            var mass = Math.Max(sg.Mass, 0.001);
            var imass = 1.0 / mass;
            // spring param
            var k = sg.K;
            // gravity
            var v = new PhysicsVector3(1, 0, 0);
            var gx = v.DotProduct(ref sg.GravityVector) * sg.Gravity;
            v.SetVector(0, 1, 0);
            var gy = v.DotProduct(ref sg.GravityVector) * sg.Gravity;
            v.SetVector(0, 0, 1);
            var gz = v.DotProduct(ref sg.GravityVector) * sg.Gravity;

            var d = new PhysicsVector3();
            d.SetSub(ref Node2.WorldPosition, ref Node1.WorldPosition);

            var force = -k * (d.x - Natural.x);
            var damping = Velocity.x * sg.Damping;
            force += sg.Mass * gx - damping;
            var 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(ref Node1.WorldPosition, ref d);
                if (!Node2.Fixed) Node2.WorldPosition.SetAdd(ref Node2.WorldPosition, ref d);
            }
            else
            {
                Node2.WorldPosition.SetAdd(ref Node2.WorldPosition, ref d);
            }


            // limitter
            if ((sg.LimitAngleMin > -1.0) || (sg.LimitAngleMax < +1.0))
            {
                d.SetSub(ref Node2.WorldPosition, ref Node1.WorldPosition);
                var pr = Math.Atan2(d.y, d.x) * Misc.RadianToAngle;
                var dr = pr - NaturalAngle;
                // bitween -0.5 to +0.5
                dr = dr + 0xff;
                dr = dr - Math.Floor(dr);
                if (dr > 0.5) dr = dr - 1.0;
                if ((dr < sg.LimitAngleMin) ||(dr > sg.LimitAngleMax))
                {
                    dr = Math.Min(Math.Max(dr, sg.LimitAngleMin), sg.LimitAngleMax);
                    d.SetVector(d.Length(), 0, 0);
                    var m = Misc.IdentityMatrix4;
                    Misc.m4PushRotateZ(ref m, (float)(NaturalAngle + dr));
                    d.Transform(ref m);
                    Node2.WorldPosition.SetAdd(ref Node1.WorldPosition, ref d);
                }
            }

            if (sg.LimitMoveRatio < 0xff)
            {
                var len = Natural.Length() * sg.LimitMoveRatio;  // ratio to world
                d.SetSub(ref Node2.WorldPosition, ref Node1.WorldPosition);
                d.SetSub(ref d, ref Natural);
                if (d.Length() > len)
                {
                    d.Normalize();
                    d.Mul(len);
                    d.SetAdd(ref d, ref Natural);
                    Node2.WorldPosition.SetAdd(ref Node1.WorldPosition, ref d);
                }
            }
            
            // fixed length
            if (FixedLength)
            {
                var len = Natural.Length();
                d.SetSub(ref Node2.WorldPosition, ref Node1.WorldPosition);
                d.Normalize();
                d.Mul(len);
                Node2.WorldPosition.SetAdd(ref Node1.WorldPosition, ref d);
            }
        }
        public override void Appease()
        {
            Velocity.SetVector(0, 0, 0);
        }
        public override void SetNatural(ref PhysicsVector3 v)
        {
            Natural = v;
            NaturalAngle = Math.Atan2(Natural.y, Natural.x) * Misc.RadianToAngle;
        }
    }
    // bone
    public class PhysicsNodeBone : PhysicsNode
    {
        // original
        public double LocalLength = 100;
        public PhysicsVector3 LocalPosition = new PhysicsVector3(0,0,0);
        public double LocalAngle = 0;
        // applied spring physics
        public PhysicsVector3 aLocalPosition = new PhysicsVector3(0,0,0);
        public double aLocalAngle = 0;

        public PhysicsMatrix4 WorldMatrix;

        public PhysicsNodeBone Parent { get; protected set; } = null;
        public List<PhysicsNodeBone> Children = new List<PhysicsNodeBone>();
        public List<PhysicsSpring> Springs = new List<PhysicsSpring>();

        public PhysicsNodeBone(PhysicsNodeBone parentbone)
        {   
            Parent = parentbone;
            if (Parent != null) Parent.Children.Add(this);

            Fixed = Parent == null;   // root bone is fixed

            WorldMatrix = Misc.IdentityMatrix4;
        }

        public void UpdateWorldMatrix(ref PhysicsMatrix4 parentmatrix, bool setupworldposition)
        {
            if (setupworldposition)
            {
                aLocalPosition = LocalPosition;
                aLocalAngle = LocalAngle;

                WorldMatrix = GetAppliedLocalMatrix();
                Misc.m4PushMatrix(ref WorldMatrix, ref parentmatrix);

                var v = new PhysicsVector3(0, 0, 0);
                v.Transform(ref WorldMatrix);
                WorldPosition = v;
            }
            else
            {
                WorldMatrix = GetAppliedLocalMatrix();
                Misc.m4PushMatrix(ref WorldMatrix, ref parentmatrix);
            }
            
            foreach (var b in Children)
            {
                b.UpdateWorldMatrix(ref WorldMatrix, setupworldposition);
            }
        }

        public PhysicsNodeBone GetRoot()
        {
            if (Parent != null)
                return Parent.GetRoot();
            else
                return this;
        }
        public PhysicsMatrix4 GetLocalMatrix()
        {
            var m = Misc.IdentityMatrix4;
            Misc.m4PushRotateZ(ref m, (float)LocalAngle);
            Misc.m4PushMove(ref m, (float)LocalPosition.x, (float)LocalPosition.y, (float)LocalPosition.z);
            return m;
        }
        public PhysicsMatrix4 GetAppliedLocalMatrix()
        {
            var m = Misc.IdentityMatrix4;
            Misc.m4PushRotateZ(ref m, (float)aLocalAngle);
            Misc.m4PushMove(ref m, (float)aLocalPosition.x, (float)aLocalPosition.y, (float)aLocalPosition.z);
            return m;
        }
    }
    public class PhysicsSpringBone : PhysicsSpringVector
    {
        protected double OffsetAngle = 0;
        public PhysicsNodeBone Owner { get; protected set; } = null;
        public PhysicsNodeBone Child { get; protected set; } = null;

        public bool LockBoneAngle = false;
        public bool LockBonePosition = false;

        public PhysicsSpringBone(PhysicsGroup group, PhysicsNodeBone ownerbone, PhysicsNodeBone childbone)
            : base(group)
        {
            NodeType = PhysicsNodeType.Bone;
            //SpringMode = smBoth;
            SpringMode = PhysicsSpringModel.OneSide;

            Owner = ownerbone;
            Child = childbone;
            Owner.Springs.Add(this);

            FixedLength = true;
            LockBoneAngle = false;
            LockBonePosition = true;

            Node1 = Owner;
            Node2 = Child;
        }
        public override void Update(double dt) 
        {
            base.Update(dt);

            foreach (var s in Child.Springs)
            {
                s.Update(dt);
            }
        }
        public void SetupNatural(ref PhysicsMatrix4 parentmatrix)
        {
            var m = Owner.GetLocalMatrix();
            Misc.m4PushMatrix(ref m, ref parentmatrix);
            var v1 = new PhysicsVector3(0, 0, 0);
            v1.Transform(ref m);

            if (Owner.Fixed)
            {
                Owner.WorldPosition = v1;
            }

            if (Child == null)
            {
                var d = new PhysicsVector3(100,0,0);
                SetNatural(ref d);
            }
            else
            {
                // next bone position
                var v2 = Child.LocalPosition;
                v2.Transform(ref m);
                var d = new PhysicsVector3();
                d.SetSub(ref v2, ref v1);
                SetNatural(ref d);
                var r = Math.Atan2(d.y, d.x) * Misc.RadianToAngle;
                // angle target position (offset)
                var v3 = new PhysicsVector3(100, 0, 0);
                v3.Transform(ref m);
                d.SetSub(ref v3, ref v1);
                OffsetAngle = Math.Atan2(d.y, d.x) * Misc.RadianToAngle - r;
            }

            foreach (PhysicsSpringBone s in Child.Springs)
            {
                s.SetupNatural(ref m);
            }
        }
        public void UpdateBoneLocal(ref PhysicsMatrix4 invparentmatrix)
        {
            if (Owner == null) return;
            if (Child == null) return;

            var im = invparentmatrix;
            Misc.m4PushMove(ref im, (float)-Owner.aLocalPosition.x, (float)-Owner.aLocalPosition.y, (float)-Owner.aLocalPosition.z);
            var v1 = Owner.WorldPosition;
            var v2 = Child.WorldPosition;
            v1.Transform(ref im);
            v2.Transform(ref im);

            var d = new PhysicsVector3(0, 0, 0);
            d.SetSub(ref v2, ref v1);

            // angle
            if (!LockBoneAngle)
            {
                double dr = Math.Atan2(d.y, d.x) * Misc.RadianToAngle + OffsetAngle;
                dr = dr + 0xff;
                dr = dr - Math.Floor(dr);
                if (dr > +0.5) dr = dr - 1.0;
                if (dr < -0.5) dr = dr + 1.0;
                Owner.aLocalAngle = dr;
            }
            else
            {
                Owner.aLocalAngle = Owner.LocalAngle;
            }

            Misc.m4PushRotateZ(ref im, (float)-Owner.aLocalAngle);

            // position
            if (!LockBonePosition)
            {
                v2 = Child.WorldPosition;
                v2.Transform(ref im);
                Child.aLocalPosition = v2;
            }
            else
            {
                Child.aLocalPosition = Child.LocalPosition;
            }

            foreach (PhysicsSpringBone s in Child.Springs)
            {
                s.UpdateBoneLocal(ref im);
            }
        }
    }

    // group
    public enum PhysicsGroupSpringBoneMode { Bone, StrBone, Point, StrPoint };
    public class PhysicsGroupSpringBone : PhysicsSpringGroup
    {
        public List<PhysicsNode> Nodes { get; protected set; } = new List<PhysicsNode>();
        public List<PhysicsSpring> Springs { get; protected set; } = new List<PhysicsSpring>();
        public PhysicsNodeBone RootBone { get; protected set; } = null;

        public PhysicsMatrix4 ExternalMatrix;
        public PhysicsMatrix4 RootMatrix;
        public Object RootUserObject = null;

        public double ForceRotateRatio = 1.0;
        public double ForceRatio = 1.0;

        public PhysicsGroupSpringBoneMode Mode = PhysicsGroupSpringBoneMode.Bone;

        public PhysicsGroupSpringBone()
        {
            GroupType = PhysicsGroupType.Spring;
            RootMatrix = Misc.IdentityMatrix4;
            ExternalMatrix = Misc.IdentityMatrix4;

            RootUserObject = null;

            ForceRotateRatio = 1.0;
            ForceRatio = 1.0;

            CacheInitFlag = true;
            CacheRootPosition.SetVector(0, 0, 0);
            CacheRootAngle = 0.0;
        }

        public override void Update(double dt)
        {
            UpdateTransformForce();

            // safe
            K = Math.Min(Math.Max(K, 0.1), 100 * 100);
            Mass = Math.Min(Math.Max(Mass, 0.001), 100 * 100);
            Damping = Math.Min(Math.Max(Damping, 0.1), 100 * 100);
            Gravity = Math.Min(Math.Max(Gravity, 0.0), 100 * 100);
            if (GravityVector.Length() > 1.0) GravityVector.Normalize();
            LimitVelocity = Math.Min(Math.Max(LimitVelocity, 1), 0xffff);

            // 100 fps
            while (dt > 0.0)
            {
                var t = Math.Min(dt, 1.0 / 100.0);
                foreach (var s in RootBone.Springs) {
                    s.Update(t);
                }
                dt -= 1.0 / 100.0;
            }
        }
        public override void Apply()
        {
            var m = Misc.IdentityMatrix4;
            foreach (PhysicsSpringBone s in RootBone.Springs)
            {
                s.UpdateBoneLocal(ref m);
            }
        }
        public override void Appease()
        {
            var m = Misc.IdentityMatrix4;
            RootBone.UpdateWorldMatrix(ref m, true);
            foreach(PhysicsSpringBone s in RootBone.Springs)
            {
                s.Appease();
            }
        }
        public override void AddForce(ref PhysicsVector3 d)
        {
            var m = InvMatrix2D(ref RootMatrix);
            m._41 = 0.0f;  // disable move
            m._42 = 0.0f;
            m._43 = 0.0f;
            var v = d;
            v.Transform(ref m);

            SlidSpring(v.x, v.y, RootBone);
        }

        public void AddBone(PhysicsNodeBone b)
        {
            Nodes.Add(b);

            if (b.Parent == null)
            {
                RootBone = b;
            }
        }
        public 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;
            }

            Springs.Add(s);
        }
        public void UpdateWorldMatrix()
        {
            var m = Misc.IdentityMatrix4;
            RootBone.UpdateWorldMatrix(ref m, false);
        }
        public void SetupNatural()
        {
            var m = Misc.IdentityMatrix4;
            // build worldposition. but worldmatrix not used in SetupNatural.
            RootBone.UpdateWorldMatrix(ref m, true); 
            // build spring natual
            foreach(PhysicsSpringBone s in RootBone.Springs)
            {
                s.SetupNatural(ref m);
            }

            // clear force
            CacheInitFlag = true;
        }
        // force
        protected bool CacheInitFlag = false;
        protected PhysicsVector3 CacheRootPosition;
        protected double CacheRootAngle;
        // protected
        protected void UpdateTransformForce()
        {
            if (RootBone == null) return;

            // optimize
            var m = RootMatrix;
            Misc.m4PushMatrix(ref m, ref ExternalMatrix);

            var v1 = RootBone.LocalPosition;
            v1.Mul(ForceRotateRatio);     // Value adjustment during rotation
            v1.Transform(ref m);
            double r1 = 0.0;
            if (CacheInitFlag)
            {
                CacheRootPosition = v1;
                CacheRootAngle = r1;
                CacheInitFlag = false;
            }

            if (v1.Compare(ref CacheRootPosition) && (r1 == CacheRootAngle)) return;

            // world position diff
            var d = new PhysicsVector3();
            d.SetSub(ref v1, ref CacheRootPosition);
            d.Mul(ForceRatio);        // Value adjustment during movement

            // convert root local space (physics group space)
            var im = InvMatrix2D(ref m);
            im._41 = 0.0f;    // disable move
            im._42 = 0.0f;
            im._43 = 0.0f;
            d.Transform(ref im);
            SlidSpring(d.x, d.y, RootBone);

            CacheRootPosition = v1;
            CacheRootAngle = r1;
        }
        protected PhysicsMatrix4 InvMatrix2D(ref PhysicsMatrix4 m)
        {
            PhysicsMatrix4 r = Misc.IdentityMatrix4;

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

            r._11 = +m._22 / d;
            r._12 = -m._12 / d;
            r._21 = -m._21 / d;
            r._22 = +m._11 / d;

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

            return r;
        }
        protected void SlidSpring(double x, double y, PhysicsNodeBone bone)
        {
            if (bone == null) return;

            if (!bone.Fixed)
            {
                bone.WorldPosition.x -= x;
                bone.WorldPosition.y -= y;
            }

            foreach (var p in bone.Children)
            {
                SlidSpring(x, y, p);
            }
        }
    }
}