using System;
using System.Collections.Generic;
using System.Linq;
using System.Numerics;
using System.Text;
using System.Threading.Tasks;
using MagicPhysX; // for enable Extension Methods.
using MagicPhysX.Toolkit;
using static System.Formats.Asn1.AsnWriter;
using static MagicPhysX.NativeMethods; // recommend to use C API.

namespace Physx
{

    using PxI64 = Int64;
    using PxU64 = UInt64;
    using PxI32 = Int32;
    using PxU32 = UInt32;
    using PxI16 = Int16;
    using PxU16 = UInt16;
    using PxI8 = Byte;
    using PxU8 = SByte;
    //using  PxF32 = System.Runtime.InteropServices.NFloat;
    //using  PxF64 =double;
    //using  PxReal = float;


    public unsafe class PhysxCommon : IDisposable
    {

        public const float PxPi = 3.141592653589793f;
        public const float PxHalfPi = 1.57079632679489661923f;
        public const float PxTwoPi = 6.28318530717958647692f;
        public const float PxInvPi = 0.31830988618379067154f;
        public const float PxInvTwoPi = 0.15915494309189533577f;
        public const float PxPiDivTwo = 1.57079632679489661923f;
        public const float PxPiDivFour = 0.78539816339744830962f;


        public static PxFoundation* _Foundation;
        public static PxPhysics* _Physics;
        public static PxMaterial* _Material;
        public static PxDefaultCpuDispatcher* _Dispatcher;
        public static PxPvd* _Pvd;

        public static PxAllocatorCallback _Allocator;
        public static PxDefaultErrorCallback _ErrorCallback;

        bool disposedValue;


        public PhysxCommon() { }
        public static PhysxCommon Instance { get { return instance; } }
        private static readonly PhysxCommon instance = new PhysxCommon();


        //系统自动调用
        ~PhysxCommon()
        {
            Dispose(disposing: false);
        }

        private void Dispose(bool disposing)
        {
            if (!disposedValue)
            {
                if (disposing)
                {
                    // cleanup managed code

                }

                // cleanup unmanaged resource
                disposedValue = true;
            }
        }

        //手动调用
        public void Dispose()
        {
            Dispose(disposing: true);
            GC.SuppressFinalize(this);
        }

        //physx_common::~physx_common() { Destory(); }

        public bool InitPhyxsSDK()
        {

            // create physics
            uint PX_PHYSICS_VERSION_MAJOR = 5;
            uint PX_PHYSICS_VERSION_MINOR = 1;
            uint PX_PHYSICS_VERSION_BUGFIX = 3;
            uint versionNumber = (PX_PHYSICS_VERSION_MAJOR << 24) + (PX_PHYSICS_VERSION_MINOR << 16) + (PX_PHYSICS_VERSION_BUGFIX << 8);
            if (_Foundation == null)
            {
                fixed (PxAllocatorCallback* allocPtr = &_Allocator)
                {
                    fixed (PxDefaultErrorCallback* errorPtr = &_ErrorCallback)
                    {
                        //_Foundation = phys_PxCreateFoundation(versionNumber, allocPtr,
                        //errorPtr);
                        // _Foundation =
                    }
                }

            }
            if (_Foundation == null)
            {
                //LogMgr::GetInstance()->Error(
                //    "physx_common::InitPhyxsSDK() error:px_foundation_ is null");
                return false;
            }


            //if (!px_pvd_)
            //{
            //    px_pvd_ = PxCreatePvd(*px_foundation_);
            //}
            //if (px_pvd_ == nullptr) return false;


            if (_Physics == null)
            {

                var tolerancesScale = new PxTolerancesScale { length = 1, speed = 10 };
                _Physics = phys_PxCreatePhysics(versionNumber, _Foundation, &tolerancesScale, true, _Pvd, null);

            }
            if (_Physics == null)
            {
                //LogMgr::GetInstance()->Error(
                //    "physx_common::InitPhyxsSDK() error: px_physics_ is null");
                return false;
            }

            if (_Dispatcher == null)
                //_Dispatcher = phys_PxDefaultCpuDispatcherCreate(0);
                phys_PxDefaultCpuDispatcherCreate(1, null, PxDefaultCpuDispatcherWaitForWorkMode.WaitForWork, 0);
            if (_Material == null)
                // _Material = _Physics->createMaterial(0.0f, 0.0f, 0.0f);
                _Material = PxPhysics_createMaterial_mut(_Physics, 0.0f, 0.0f, 0.0f);

            return true;
        }

        //public void Destory()
        //{
        //if (instance_ != nullptr)
        //{
        //    ReleaseSDK();
        //    delete instance_;
        //    instance_ = nullptr;
        //}
        //}


        bool IsConnectedPvd()
        {
            // return _Pvd !=null ? _Pvd->PxPvd_isConnected_mut() : false;
            return _Pvd != null ? PxPvd_isConnected_mut(_Pvd, true) : false;
        }

        public void ConnectPvdServer()
        {
            if (IsConnectedPvd() == false)
                return;
            if (_Pvd == null)
                return;

            //PxPvdTransport* transport = PxDefaultPvdSocketTransportCreate(g_pGameWorld->m_Settings.Ip_Pvd, 5425, 10);
            byte hostip = 10;
            PxPvdTransport* transport = phys_PxDefaultPvdSocketTransportCreate(&hostip, 5425, 10);

            if (transport == null)
                return;

            bool is_connected = _Pvd->ConnectMut(transport, PxPvdInstrumentationFlags.All);
            if (!is_connected)
            {
                //LogMgr::GetInstance()->Error(
                //    "physx_common::InitPhyxsSDK() error:pvd connect error.");
            }
        }

        public void DisconnectPvdServer()
        {
            if (!IsConnectedPvd())
                return;
            if (_Pvd == null) return;

            _Pvd->DisconnectMut();
        }

        public void ReleaseSDK()
        {
            if (_Physics != null)
            {
                _Dispatcher->ReleaseMut();
                _Physics->ReleaseMut();
                if (_Pvd != null)
                {
                    PxPvdTransport* transport = _Pvd->GetTransportMut();
                    if (transport != null)
                    {
                        transport->ReleaseMut();
                        transport = null;
                    }
                    _Pvd->ReleaseMut();
                    _Pvd = null;
                }

                _Foundation->ReleaseMut();
                _Dispatcher = null;
                _Physics = null;
                _Foundation = null;
            }
        }

        public PxPhysics* GetPhysics() { return _Physics; }
        public PxDefaultCpuDispatcher* GetCpuDispatcher() { return _Dispatcher; }
        public PxMaterial* GetDefaultMaterial() { return _Material; }

        /////////////////////////////////////////////////////////////////////////
        // maths tools
        /////////////////////////////////////////////////////////////////////////
        void SetEulerYXZ(ref float yaw, ref float pitch,
                          ref float roll, ref PxQuat quat)
        {
            float halfYaw = yaw * 0.5f;
            float halfPitch = pitch * 0.5f;
            float halfRoll = roll * 0.5f;
            float cosYaw = MathF.Cos(halfYaw);
            float sinYaw = MathF.Sin(halfYaw);
            float cosPitch = MathF.Cos(halfPitch);
            float sinPitch = MathF.Sin(halfPitch);
            float cosRoll = MathF.Cos(halfRoll);
            float sinRoll = MathF.Sin(halfRoll);

            //效率更高
            quat.x = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
            quat.y = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;
            quat.z = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw;
            quat.w = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw;
        }

        //PxVec3 GetQuatRotate(KVector3D& dir, KVector3D& velocity)
        //{
        //    PxQuat px_qua;
        //    SetEulerYXZ((float)((PI_F * dir.nY) / (1000 * 180)), (float)((PI_F * dir.nX) / (1000 * 180)),
        //                (float)((PI_F * dir.nZ) / (1000 * 180)), px_qua);

        //    return px_qua.rotate(PxVec3((float)velocity.nX, (float)velocity.nY,
        //                                       (float)velocity.nZ));
        //}

        PxVec3 QuatToEuler(PxQuat* q)
        {
            //PxVec3 dir = -PxMat33(q)[2];
            PxVec3 dir = PxVec3_new_3(-q->Mat33New6().column2.x, -q->Mat33New6().column2.y, -q->Mat33New6().column2.z);
            float r = MathF.Sqrt(dir.x * dir.x + dir.z * dir.z);
            PxVec3 rot = PxVec3_new_3(0.0f, PxHalfPi, 0.0f);
            if (r != 0.0f)
            {
                rot.x = -MathF.Atan(dir.y / r);
                rot.y = MathF.Asin(dir.x / r);
                if (dir.z > 0.0f) rot.y = PxPi - rot.y;
            }
            return rot;
        }

        PxVec3 GetEuler(PxVec3 forward)
        {
            PxVec3 euler = PxVec3_new_3(0, 0, 0);
            PxVec3 tmp_forward = forward;
            tmp_forward.y = 0;
            //tmp_forward = BaseBehavior::SafeNormalized(tmp_forward);

            // xz平面
            //float angle = BaseBehavior::PxVec3Angle(PxVec3(0, 0, 1), tmp_forward) /
            //              PxPi * 180;  // 角度值
            //TODO:需要重写
            float angle = 1.0f;
            if (tmp_forward.x >= 0)
            {
                euler.y = angle;
            }
            else
            {
                euler.y = -angle;
            }

            // pitch角度
            tmp_forward = forward;
            tmp_forward.z = MathF.Sqrt(tmp_forward.x * tmp_forward.x + tmp_forward.z * tmp_forward.z);
            tmp_forward.x = 0;
            //tmp_forward = BaseBehavior::SafeNormalized(tmp_forward);
            //angle = BaseBehavior::PxVec3Angle(PxVec3(0, 0, 1), tmp_forward) /
            //        PxPi * 180;  // 角度值
            if (tmp_forward.y >= 0)
            {
                euler.x = -angle;
            }
            else
            {
                euler.x = angle;
            }
            return euler;
        }

        float GetAngle(PxVec3* v0, PxVec3* v1)
        {
            float m = v0->MagnitudeSquared() * v1->MagnitudeSquared();
            float s = MathF.Sqrt(m);

            if (s < (float)(1e-20f))
            {
                return 0.0f;
            }

            s = 1.0f / s;
            return MathF.Acos(v0->Dot(v1) * s);
        }

        void QuatToMatrix(float* quat, float* matrix)
        {
            float xx = quat[0] * quat[0];
            float yy = quat[1] * quat[1];
            float zz = quat[2] * quat[2];
            float xy = quat[0] * quat[1];
            float xz = quat[0] * quat[2];
            float yz = quat[1] * quat[2];
            float wx = quat[3] * quat[0];
            float wy = quat[3] * quat[1];
            float wz = quat[3] * quat[2];

            matrix[0 * 4 + 0] = 1 - 2 * (yy + zz);
            matrix[1 * 4 + 0] = 2 * (xy - wz);
            matrix[2 * 4 + 0] = 2 * (xz + wy);

            matrix[0 * 4 + 1] = 2 * (xy + wz);
            matrix[1 * 4 + 1] = 1 - 2 * (xx + zz);
            matrix[2 * 4 + 1] = 2 * (yz - wx);

            matrix[0 * 4 + 2] = 2 * (xz - wy);
            matrix[1 * 4 + 2] = 2 * (yz + wx);
            matrix[2 * 4 + 2] = 1 - 2 * (xx + yy);

            matrix[3 * 4 + 0] = matrix[3 * 4 + 1] = matrix[3 * 4 + 2] =
                (float)0.0f;
            matrix[0 * 4 + 3] = matrix[1 * 4 + 3] = matrix[2 * 4 + 3] =
                (float)0.0f;
            matrix[3 * 4 + 3] = (float)1.0f;
        }

        PxVec3 ToEulerAngle(float* quat)
        {
            // float matrix[16];

            float* matrix = stackalloc float[16];
            QuatToMatrix(quat, matrix);

            float m11 = matrix[0], m12 = matrix[4], m13 = matrix[8];
            float m21 = matrix[1], m22 = matrix[5], m23 = matrix[9];
            float m31 = matrix[2], m32 = matrix[6], m33 = matrix[10];

            if (m23 < -1)
            {
                m23 = -1;
            }

            if (m23 > 1)
            {
                m23 = 1;
            }

            float _x, _y, _z;
            _x = MathF.Asin(-m23);

            if (MathF.Abs(m23) < 0.99999)
            {
                _y = MathF.Atan2(m13, m33);
                _z = MathF.Atan2(m21, m22);
            }
            else
            {
                _y = MathF.Atan2(-m31, m11);
                _z = 0;
            }

            return PxVec3_new_3(_x, _y, _z);
        }

        public double Distance2(PxVec3* a, PxVec3* b)
        {
            PxVec3 tmp = a->Multiply(b);
            return tmp.Dot(&tmp);
        }

        public double Distane(PxVec3* a, ref PxVec3* b)
        {
            return Math.Sqrt(Distance2(a, b));
        }

    }
}