GHZMRSpace/Assets/SDK/Modules/Module_Slam/Scripts/GSXRManager.cs

using UnityEngine;
using UnityEngine.UI;
using UnityEngine.Events;
using System;
using System.Collections;
using System.Collections.Generic;
using System.Runtime.InteropServices;
using AOT;
using SC.XR.Unity;

public class GSXRManager : MonoBehaviour
{
    public static GSXRManager Instance;
    //{
    //    get
    //    {
    //        if (instance == null) instance = FindObjectOfType<GSXRManager>();
    //        if (instance == null) Debug.LogError("GSXRManager object component not found");
    //        return instance;
    //    }
    //}
    //private static GSXRManager instance;

    static public int EyeLayerMax = 8;
    static public int OverlayLayerMax = 8;
    static public int RenderLayersMax = 16;

    public static Action SlamInitializedCallBack;
    //public static Action SlamOnPauseCallback;
    //public static Action SlamOnResumeCallback;


    [SerializeField]
    private SpecificationType m_SpecificationType = SpecificationType.GSXR;
    public SpecificationType SpecificationType
    {
        get { return m_SpecificationType; }
        private set { m_SpecificationType = value; }
    }

    public enum slamEventType
    {
        kEventNone = 0,
        kEventSdkServiceStarting = 1,
        kEventSdkServiceStarted = 2,
        kEventSdkServiceStopped = 3,
        kEventControllerConnecting = 4,
        kEventControllerConnected = 5,
        kEventControllerDisconnected = 6,
        kEventThermal = 7,
        kEventVrModeStarted = 8,
        kEventVrModeStopping = 9,
        kEventVrModeStopped = 10,
        kEventSensorError = 11,
        kEventMagnometerUncalibrated = 12,
        kEventBoundarySystemCollision = 13,
        kEvent6dofRelocation = 14,
        kEvent6dofWarningFeatureCount = 15,
        kEvent6dofWarningLowLight = 16,
        kEvent6dofWarningBrightLight = 17,
        kEvent6dofWarningCameraCalibration = 18
    }

    public enum controllerBondState
    {
        No_Bond = 0,
        Left_Bond = 1,
        Right_Bond = 2,
        Left_UnBond = 3,
        Right_UnBond = 4,
        OverTime_Bond = 5,
        Unknown = 6
    }




    [Serializable]
    public class SlamSettings
    {
        public enum eAntiAliasing
        {
            k1 = 1,
            k2 = 2,
            k4 = 4,
        };

        public enum eDepth
        {
            k16 = 16,
            k24 = 24
        };

        public enum eChromaticAberrationCorrection
        {
            kDisable = 0,
            kEnable = 1
        };

        public enum eVSyncCount
        {
            k0 = 0,
            k1 = 1,
            k2 = 2,
        };

        public enum eMasterTextureLimit
        {
            k0 = 0, // full size
            k1 = 1, // half size
            k2 = 2, // quarter size
            k3 = 3, // ...
            k4 = 4  // ...
        };

        public enum ePerfLevel
        {
            Minimum = 1,
            Medium = 2,
            Maximum = 3
        };

        public enum eFrustumType
        {
            Camera = 0,
            Device = 1,
        }

        public enum eEyeBufferType
        {
            //Mono = 0,
            StereoSeperate = 1,
            //StereoSingle = 2,
            //Array = 3,
        }

        [Tooltip("If head tracking lost, fade the display")]
        public bool poseStatusFade = true;
        [Tooltip("Use eye tracking (if available)")]
        public bool trackEyes = true;
        [Tooltip("Use position tracking (if available)")]
        public bool trackPosition = true;
        [Tooltip("Track position conversion from meters")]
        public float trackPositionScale = 1;
        [Tooltip("Height of the eyes from base of head")]
        public float headHeight = 0.0750f;
        [Tooltip("Depth of the eyes from center of head")]
        public float headDepth = 0.0805f;
        [Tooltip("Distance between the eyes")]
        public float interPupilDistance = 0.064f;
        //[Tooltip("Distance of line-of-sight convergence (0 disabled)")]
        //public float stereoConvergence = 0;
        //[Tooltip("Pitch angle to the horizon in degrees")]
        //public float horizonElevation = 0;
        [Tooltip("Eye field of view render target reprojection margin (% of fov) [0..]")]
        public float eyeFovMargin = 0.0f;
        [Tooltip("Eye render target scale factor")]
        public float eyeResolutionScaleFactor = 1.0f;
        [Tooltip("Eye render target depth buffer")]
        public eDepth eyeDepth = eDepth.k24;
        [Tooltip("Eye render target MSAA value")]
        public eAntiAliasing eyeAntiAliasing = eAntiAliasing.k2;
        [Tooltip("Overlay render target scale factor")]
        public float overlayResolutionScaleFactor = 1.0f;
        [Tooltip("Overlay render target depth buffer")]
        public eDepth overlayDepth = eDepth.k16;
        [Tooltip("Overlay render target MSAA value")]
        public eAntiAliasing overlayAntiAliasing = eAntiAliasing.k1;
        [Tooltip("Limit refresh rate")]
        public eVSyncCount vSyncCount = eVSyncCount.k1;
        [Tooltip("Chromatic Aberration Correction")]
        public eChromaticAberrationCorrection chromaticAberationCorrection = eChromaticAberrationCorrection.kEnable;
        [Tooltip("QualitySettings TextureQuality FullRes, HalfRes, etc.")]
        public eMasterTextureLimit masterTextureLimit = eMasterTextureLimit.k0;
        [Tooltip("CPU performance level")]
        public ePerfLevel cpuPerfLevel = ePerfLevel.Medium;
        [Tooltip("GPU performance level")]
        public ePerfLevel gpuPerfLevel = ePerfLevel.Medium;
        [Tooltip("Foveated render gain [1..], 0/feature disabled")]
        public Vector2 foveationGain = new Vector2(0.0f, 0.0f);
        [Tooltip("Foveated render hires area [0..]")]
        public float foveationArea = 0.0f;
        [Tooltip("Foveated render min pixel density [1/16..1]")]
        public float foveationMinimum = 0.25f;
        [Tooltip("Use perspective of unity camera or device frustum data")]
        public eFrustumType frustumType = eFrustumType.Camera;
        [Tooltip("Display buffer type (default stereo seperate)")]
        public eEyeBufferType displayType = eEyeBufferType.StereoSeperate;
    }
    [SerializeField]
    public SlamSettings settings;

    [Serializable]
    public class SlamStatus
    {
        [Tooltip("SnapdragonVR SDK Initialized")]
        public bool initialized = false;
        [Tooltip("SnapdragonVR SDK Running")]
        public bool running = false;
        [Tooltip("SnapdragonVR SDK Pose Status: 0/None, 1/Rotation, 2/Position, 3/RotationAndPosition")]
        public int pose = 0;
    }
    [SerializeField]
    public SlamStatus status;

    public enum eFadeState { FadeIn, FadeOut }
    [NonSerialized]
    public eFadeState fadeState = eFadeState.FadeIn;
    [NonSerialized]
    public float fadeDuration = 0.5f;

    [Header("Camera Rig")]
    public Transform head;
    public Transform gaze;
    public Camera monoCamera;
    public Camera leftCamera;
    public Camera rightCamera;
    public Camera leftOverlay;
    public Camera rightOverlay;
    public Camera monoOverlay;
    public GSXROverlay fadeOverlay;
    public GSXROverlay reticleOverlay;

    private bool IsUseOpticsCalibration = true;
    private bool IsUseCommonEvent = false;

    public Vector2 FocalPoint { get; set; } // Foveated Rendering Focal Point

    public int FrameCount { get { return frameCount; } }

    private int frameCount = 0;
    private static WaitForEndOfFrame waitForEndOfFrame = new WaitForEndOfFrame();
    public GSXRPlugin plugin = null;
    private float sensorWarmupDuration = 0.25f;
    private List<GSXREye> eyes = new List<GSXREye>(EyeLayerMax);
    private List<GSXROverlay> overlays = new List<GSXROverlay>(OverlayLayerMax);
    private bool disableInput = false;
    private Coroutine onResume = null;
    private Coroutine submitFrame = null;

    private Matrix4x4 headDeltaInitLocalToWorld = Matrix4x4.identity;

    public Material DepthM;
    public bool IsCreateDepthRT = false;
    private bool IsDontDestroyOnLoad = true;

    private bool isInitRecenter = false;

    /// <summary>
    /// Slam event listener.
    /// </summary>
    public interface SlamEventListener
    {
        /// <summary>
        /// Raises the slam event event.
        /// </summary>
        /// <param name="ev">Ev.</param>
        void OnSlamEvent(SlamEvent ev);
    };

    public enum slamThermalLevel
    {
        kSafe,
        kLevel1,
        kLevel2,
        kLevel3,
        kCritical,
        kNumThermalLevels
    };

    public enum slamThermalZone
    {
        kCpu,
        kGpu,
        kSkin,
        kNumThermalZones
    };

    public struct slamEventData_Thermal
    {
        public slamThermalZone zone;               //!< Thermal zone
        public slamThermalLevel level;             //!< Indication of the current zone thermal level
    };

    [StructLayout(LayoutKind.Explicit)]
    public struct slamEventData
    {
        [FieldOffset(0)]
        public slamEventData_Thermal thermal;
        //[FieldOffset(0)]
        //public slamEventData_New newData;
    }

    public struct SlamEvent
    {
        public slamEventType eventType;      //!< Type of event
        public uint deviceId;               //!< An identifier for the device that generated the event (0 == HMD)
        public float eventTimeStamp;        //!< Time stamp for the event in seconds since the last BeginVr call
        public slamEventData eventData;      //!< Event specific data payload
    };

    // david start
    public struct SCSingleLayerData
    {
        public UInt32 layerId;
        public UInt32 parentLayerId;
        public UInt32 layerTextureId;
        [MarshalAsAttribute(UnmanagedType.ByValArray, SizeConst = 16)]
        public float[] modelMatrixData;
        public UInt32 editFlag;
        public int z;
        [MarshalAsAttribute(UnmanagedType.ByValArray, SizeConst = 12)]
        public float[] vertexPosition;
        [MarshalAsAttribute(UnmanagedType.ByValArray, SizeConst = 8)]
        public float[] vertexUV;
        public float alpha;
        public byte bOpaque;
        public UInt32 taskId;
        public UInt32 recentTaskId;
    };
    public struct SCAllLayers
    {
        public UInt32 layerNum;
        [MarshalAsAttribute(UnmanagedType.ByValArray, SizeConst = 16)]
        public float[] viewMatrixData;
        public IntPtr layerData;
    };
    // david end
    private List<SlamEventListener> eventListeners = new List<SlamEventListener>();

    public bool Initialized
    {
        get { return status.initialized; }
    }

    public bool IsRunning
    {
        get { return status.running; }
    }

    public bool DisableInput
    {
        get { return disableInput; }
        set { disableInput = value; }
    }

    void Awake()
    {
        if (Instance)
        {
            DestroyImmediate(gameObject);
            return;
        }
        Instance = this;


        if (!ValidateReferencedComponents())
        {
            enabled = false;
            return;
        }
        RegisterListeners();
        Input.backButtonLeavesApp = true;
        Screen.sleepTimeout = SleepTimeout.NeverSleep;
        Application.targetFrameRate = -1;

        if (API_Module_SDKConfiguration.HasKey("Module_Slam", "IsCreateDepthRT"))
        {
            IsCreateDepthRT = API_Module_SDKConfiguration.GetBool("Module_Slam", "IsCreateDepthRT", 0);
            DebugMy.Log("IsCreateDepthRT:" + IsCreateDepthRT, this, true);
        }

    }

    //void OnGUI() { 

    //}
    bool ValidateReferencedComponents()
    {
        plugin = GSXRPlugin.Instance;
        if (plugin == null)
        {
            Debug.LogError("slam Plugin failed to load. Disabling...");
            return false;
        }

        if (head == null)
        {
            Debug.LogError("Required head gameobject not found! Disabling...");
            return false;
        }

        if (monoCamera == null && (leftCamera == null || rightCamera == null))
        {
            Debug.LogError("Required eye components are missing! Disabling...");
            return false;
        }

        return true;
    }
#if UNITY_2018
    void OnEnable()
    {
        if (UnityEngine.Rendering.GraphicsSettings.renderPipelineAsset)
        {
            UnityEngine.Experimental.Rendering.RenderPipeline.beginCameraRendering += OnPreRenderEvent;
        }
    }

    void OnDisable()
    {
        if (UnityEngine.Rendering.GraphicsSettings.renderPipelineAsset)
        {
            UnityEngine.Experimental.Rendering.RenderPipeline.beginCameraRendering -= OnPreRenderEvent;
        }
    }

    private void OnPreRenderEvent(Camera camera)
    {
        camera.SendMessage("OnPreRenderEvent", SendMessageOptions.DontRequireReceiver);
    }
#endif


    IEnumerator Start()
    {
        yield return StartCoroutine(Initialize());
        status.initialized = plugin.IsInitialized();

        SetOverlayFade(eFadeState.FadeIn);

        yield return StartCoroutine(plugin.BeginVr((int)settings.cpuPerfLevel, (int)settings.gpuPerfLevel));

        /// for some platform plugin.BeginVr return so fast,so need check plugin.IsRunning() and Wait
        int waitFrameCount = 200;
        while (!plugin.IsRunning() && waitFrameCount >= 0)
        {
            yield return null;
            waitFrameCount--;
        }
        ///

        if (!plugin.IsRunning())
        {
            Debug.LogError("Slam failed!");
            Application.Quit();
            yield return null;  // Wait one frame
        }

        //float recenterTimeout = 8f;
        //while (!plugin.RecenterTracking() && recenterTimeout > 0f) {
        //    yield return null;  // Wait one frame
        //    recenterTimeout -= Time.deltaTime;
        //}

        yield return new WaitForSecondsRealtime(sensorWarmupDuration);

        submitFrame = StartCoroutine(SubmitFrame());

        status.running = true;

        SlamInitializedCallBack?.Invoke();  //Slam initialization complete callback

        plugin.GSXR_Set_GlassDisconnectedCallBack(GlassDisconnetedCallBack);  //Register glasses disconnection callback


        Debug.Log("Slam initialized!");

        if (API_Module_SDKConfiguration.HasKey("Module_Slam", "IsUseCommonEvent"))
        {
            IsUseCommonEvent = API_Module_SDKConfiguration.GetBool("Module_Slam", "IsUseCommonEvent", 1);
            DebugMy.Log("IsUseCommonEvent:" + IsUseCommonEvent, this, true);
        }
        if (IsUseCommonEvent)
        {
            gameObject.AddComponent<Module_CommonEvent>();
        }

     //   DebugMy.Log("SDK SpecificationType:" + API_GSXR_Slam.SlamManager.SpecificationType + ":" + plugin.GSXR_Get_Version(), this, true);

        if (API_Module_SDKConfiguration.HasKey("Module_Slam", "IsUseOpticsCalibration"))
        {
            IsUseOpticsCalibration = API_Module_SDKConfiguration.GetBool("Module_Slam", "IsUseOpticsCalibration", 1);
            DebugMy.Log("IsUseOpticsCalibration:" + IsUseOpticsCalibration, this, true);
        }

        if (API_Module_SDKConfiguration.HasKey("Module_Slam", "trackPositionScale"))
        {
            settings.trackPositionScale = API_Module_SDKConfiguration.GetFloat("Module_Slam", "trackPositionScale", 1);
            DebugMy.Log("settings.trackPositionScale:" + settings.trackPositionScale, this, true);
        }

        Vector3 localLeftShoulder = Vector3.zero;
        if (API_Module_SDKConfiguration.HasKey("Module_Slam", "LeftShoulderX") && API_Module_SDKConfiguration.HasKey("Module_Slam", "LeftShoulderY") && API_Module_SDKConfiguration.HasKey("Module_Slam", "LeftShoulderZ"))
        {
            localLeftShoulder = new Vector3(API_Module_SDKConfiguration.GetFloat("Module_Slam", "LeftShoulderX", 0.0f), API_Module_SDKConfiguration.GetFloat("Module_Slam", "LeftShoulderY", 0.0f), API_Module_SDKConfiguration.GetFloat("Module_Slam", "LeftShoulderZ", 0.0f));
        }

        if (API_Module_SDKConfiguration.HasKey("Module_SDKSystem", "IsDontDestroyOnLoad"))
        {
            IsDontDestroyOnLoad = API_Module_SDKConfiguration.GetBool("Module_SDKSystem", "IsDontDestroyOnLoad", 1);
        }
        shoulder = new Shoulder(head, localLeftShoulder == Vector3.zero ? new Vector3(-0.15f, -0.08f, -0.1f) : localLeftShoulder);
        isInit = true;
    }

    [MonoPInvokeCallback(typeof(Action))]
    private static void GlassDisconnetedCallBack()
    {
        Debug.Log("GlassDisconnetedCallBack");
        Application.Quit();
    }



    IEnumerator Initialize()
    {
        // Plugin must be initialized OnStart in order to properly
        // get a valid surface
        //        GameObject mainCameraGo = GameObject.FindWithTag("MainCamera");
        //        if (mainCameraGo)
        //        {
        //            mainCameraGo.SetActive(false);
        //
        //            Debug.Log("Camera with MainCamera tag found.");
        //            if (!disableInput)
        //            {
        //                Debug.Log("Will use translation and orientation from the MainCamera.");
        //                transform.position = mainCameraGo.transform.position;
        //                transform.rotation = mainCameraGo.transform.rotation;
        //            }
        //
        //            Debug.Log("Disabling Camera with MainCamera tag");
        //        }

        GL.Clear(false, true, Color.black);

        yield return StartCoroutine(plugin.Initialize());
        InitializeCameras();
        InitializeEyes();
        InitializeOverlays();

        int trackingMode = (int)GSXRPlugin.TrackingMode.kTrackingOrientation;

        if (API_Module_SDKConfiguration.HasKey("Module_Slam", "IsSlamUse3Dof"))
        {
            if (API_Module_SDKConfiguration.GetBool("Module_Slam", "IsSlamUse3Dof", 0))
            {
                DebugMy.Log("Slam Use3Dof", this, true);
                settings.trackPosition = false;
            }
        }

        if (settings.trackPosition)
            trackingMode |= (int)GSXRPlugin.TrackingMode.kTrackingPosition;
        if (settings.trackEyes)
            trackingMode |= (int)GSXRPlugin.TrackingMode.kTrackingEye;

        plugin.SetTrackingMode(trackingMode);

        plugin.SetVSyncCount((int)settings.vSyncCount);
        QualitySettings.vSyncCount = (int)settings.vSyncCount;
    }

    Matrix4x4 invertZ = new Matrix4x4(
            new Vector4(1, 0, 0, 0),
            new Vector4(0, 1, 0, 0),
            new Vector4(0, 0, -1, 0),
            new Vector4(0, 0, 0, 1));

    private void InitializeCameras()
    {
        float stereoConvergence = plugin.deviceInfo.targetFrustumConvergence; //settings.stereoConvergence
        float horizonElevation = plugin.deviceInfo.targetFrustumPitch; //settings.horizonElevation
        float convergenceAngle = 0f;
        if (stereoConvergence > Mathf.Epsilon) convergenceAngle = Mathf.Rad2Deg * Mathf.Atan2(0.5f * settings.interPupilDistance, stereoConvergence);
        else if (stereoConvergence < -Mathf.Epsilon) convergenceAngle = -Mathf.Rad2Deg * Mathf.Atan2(0.5f * settings.interPupilDistance, -stereoConvergence);

        // left
        Vector3 eyePos;
        eyePos.x = -0.5f * settings.interPupilDistance;
        eyePos.y = (!settings.trackPosition ? settings.headHeight : 0);
        eyePos.z = (!settings.trackPosition ? settings.headDepth : 0);
        eyePos += head.transform.localPosition;

        Quaternion eyeRot;
        eyeRot = Quaternion.Euler(horizonElevation, convergenceAngle, 0);

        if (leftCamera != null)
        {
            leftCamera.transform.localPosition = eyePos;
            leftCamera.transform.localRotation = eyeRot;
        }
        if (leftOverlay != null)
        {
            leftOverlay.transform.localPosition = eyePos;
            leftOverlay.transform.localRotation = eyeRot;
        }

        // right
        eyePos.x *= -1;
        eyeRot = Quaternion.Euler(horizonElevation, -convergenceAngle, 0);

        if (rightCamera != null)
        {
            rightCamera.transform.localPosition = eyePos;
            rightCamera.transform.localRotation = eyeRot;
        }
        if (rightOverlay != null)
        {
            rightOverlay.transform.localPosition = eyePos;
            rightOverlay.transform.localRotation = eyeRot;
        }

        // mono
        eyePos.x = 0.0f;
        eyeRot = Quaternion.Euler(horizonElevation, 0, 0);
        if (monoCamera != null)
        {
            monoCamera.transform.localPosition = eyePos;
            monoCamera.transform.localRotation = eyeRot;
        }
        if (monoOverlay != null)
        {
            monoOverlay.transform.localPosition = eyePos;
            monoOverlay.transform.localRotation = eyeRot;
        }
    }

    private void AddEyes(Camera cam, GSXREye.eSide side)
    {
        bool enableCamera = false;
        var eyesFound = cam.gameObject.GetComponents<GSXREye>();
        for (int i = 0; i < eyesFound.Length; i++)
        {
            eyesFound[i].Side = side;
            if (eyesFound[i].imageType == GSXREye.eType.RenderTexture) enableCamera = true;
        }
        eyes.AddRange(eyesFound);
        if (eyesFound.Length == 0)
        {
            var eye = cam.gameObject.AddComponent<GSXREye>();
            eye.Side = side;
            eyes.Add(eye);
            enableCamera = true;
        }
#if UNITY_5_4 || UNITY_5_5
        cam.hdr = false;
#else // UNITY_5_6 plus
        cam.allowHDR = false;
        cam.allowMSAA = false;
#endif
        //cam.enabled = enableCamera;
    }

    private void InitializeEyes()
    {
        eyes.Clear();
        if (monoCamera != null && monoCamera.gameObject.activeSelf)
        {
            AddEyes(monoCamera, GSXREye.eSide.Both);
        }
        if (leftCamera != null && leftCamera.gameObject.activeSelf)
        {
            AddEyes(leftCamera, GSXREye.eSide.Left);
        }
        if (rightCamera != null && rightCamera.gameObject.activeSelf)
        {
            AddEyes(rightCamera, GSXREye.eSide.Right);
        }
        for (int i = 0; i < GSXREye.Instances.Count; i++)
        {
            var eye = GSXREye.Instances[i];
            if (!eyes.Contains(eye))
            {
                eyes.Add(eye); // Add eyes found outside of slam camera hierarchy
            }
        }

        GSXRPlugin.DeviceInfo info = plugin.deviceInfo;

        foreach (GSXREye eye in eyes)
        {
            if (eye == null) continue;

            eye.FovMargin = settings.eyeFovMargin;
            eye.Format = RenderTextureFormat.Default;
            eye.Resolution = new Vector2(info.targetEyeWidthPixels, info.targetEyeHeightPixels);
            eye.ResolutionScaleFactor = settings.eyeResolutionScaleFactor;
            eye.Depth = (int)settings.eyeDepth;
            eye.AntiAliasing = (int)settings.eyeAntiAliasing;	// hdr not supported with antialiasing
            eye.FrustumType = (int)settings.frustumType;
            eye.OnPreRenderListener = OnPreRenderListener;
            eye.OnPostRenderListener = OnPostRenderListener;

            eye.Initialize();
        }
    }

    private void AddOverlays(Camera cam, GSXROverlay.eSide side)
    {
        bool enableCamera = false;
        var overlaysFound = cam.gameObject.GetComponents<GSXROverlay>();
        for (int i = 0; i < overlaysFound.Length; i++)
        {
            overlaysFound[i].Side = side;
            if (overlaysFound[i].imageType == GSXROverlay.eType.RenderTexture) enableCamera = true;
        }
        overlays.AddRange(overlaysFound);
        if (overlaysFound.Length == 0)
        {
            var overlay = cam.gameObject.AddComponent<GSXROverlay>();
            overlay.Side = side;
            overlays.Add(overlay);
            enableCamera = true;
        }
#if UNITY_5_4 || UNITY_5_5
        cam.hdr = false;
#else // UNITY_5_6 plus
        cam.allowHDR = false;
        cam.allowMSAA = false;
#endif
        cam.enabled = enableCamera;
    }

    private void InitializeOverlays()
    {
        overlays.Clear();
        if (leftOverlay != null && leftOverlay.gameObject.activeSelf)
        {
            AddOverlays(leftOverlay, GSXROverlay.eSide.Left);
        }
        if (rightOverlay != null && rightOverlay.gameObject.activeSelf)
        {
            AddOverlays(rightOverlay, GSXROverlay.eSide.Right);
        }
        if (monoOverlay != null && monoOverlay.gameObject.activeSelf)
        {
            AddOverlays(monoOverlay, GSXROverlay.eSide.Both);
        }
        for (int i = 0; i < GSXROverlay.Instances.Count; i++)
        {
            var overlay = GSXROverlay.Instances[i];
            if (!overlays.Contains(overlay))
            {
                overlays.Add(overlay); // Add overlays found outside of slam camera hierarchy
            }
        }

        GSXRPlugin.DeviceInfo info = plugin.deviceInfo;

        foreach (GSXROverlay overlay in overlays)
        {
            if (overlay == null) continue;

            overlay.Format = RenderTextureFormat.Default;
            overlay.Resolution = new Vector2(info.targetEyeWidthPixels, info.targetEyeHeightPixels);
            overlay.ResolutionScaleFactor = settings.overlayResolutionScaleFactor;
            overlay.Depth = (int)settings.overlayDepth;
            overlay.AntiAliasing = (int)settings.overlayAntiAliasing;  // hdr not supported with antialiasing
            overlay.FrustumType = (int)settings.frustumType;
            overlay.OnPreRenderListener = OnPreRenderListener;
            overlay.OnPostRenderListener = OnPostRenderListener;

            overlay.Initialize();
        }
    }

    public void SetOverlayFade(eFadeState fadeValue)
    {
        fadeState = fadeValue;
        var startAlpha = fadeState == eFadeState.FadeIn ? 1f : 0f;
        UpdateOverlayFade(startAlpha);
    }

    public bool IsOverlayFading()
    {
        return !Mathf.Approximately((float)fadeState, fadeAlpha);
    }

    private float fadeAlpha = 0f;
    private void UpdateOverlayFade(float targetAlpha, float rate = 0)
    {
        if (fadeOverlay == null) return;

        fadeAlpha = rate > 0 ? Mathf.MoveTowards(fadeAlpha, targetAlpha, rate) : targetAlpha;

        var fadeTexture = fadeOverlay.imageTexture as Texture2D;
        if (fadeTexture != null)
        {
            var fadeColors = fadeTexture.GetPixels();
            for (int i = 0; i < fadeColors.Length; ++i)
            {
                fadeColors[i].a = fadeAlpha;
            }
            fadeTexture.SetPixels(fadeColors);
            fadeTexture.Apply(false);
        }

        var isActive = fadeAlpha > 0.0f;
        if (fadeOverlay.enabled != isActive)
        {
            fadeOverlay.enabled = isActive;
        }
    }

    IEnumerator SubmitFrame()
    {
        Vector3 frustumSize = Vector3.zero;
        frustumSize.x = 0.5f * (plugin.deviceInfo.targetFrustumLeft.right - plugin.deviceInfo.targetFrustumLeft.left);
        frustumSize.y = 0.5f * (plugin.deviceInfo.targetFrustumLeft.top - plugin.deviceInfo.targetFrustumLeft.bottom);
        frustumSize.z = plugin.deviceInfo.targetFrustumLeft.near;
        //Debug.LogFormat("SubmitFrame: Frustum Size ({0:F2}, {1:F2}, {2:F2})", frustumSize.x, frustumSize.y, frustumSize.z);

        while (true)
        {
            yield return waitForEndOfFrame;//固定帧测试

            if ((plugin.GetTrackingMode() & (int)GSXRPlugin.TrackingMode.kTrackingEye) != 0) // Request eye pose
            {
                status.pose |= plugin.GSXR_Get_EyePose(ref eyePose);
            }
            var eyePoint = Vector3.zero;
            if ((status.pose & (int)GSXRPlugin.TrackingMode.kTrackingEye) != 0) // Valid eye pose
            {
                //Debug.LogFormat("Left Eye Position: {0}, Direction: {1}", eyePose.leftPosition.ToString(), eyePose.leftDirection.ToString());
                //Debug.LogFormat("Right Eye Position: {0}, Direction: {1}", eyePose.rightPosition.ToString(), eyePose.rightDirection.ToString());
                //Debug.LogFormat("Combined Eye Position: {0}, Direction: {1}", eyePose.combinedPosition.ToString(), eyePose.combinedDirection.ToString());
                var combinedDirection = Vector3.zero;
                if ((eyePose.leftStatus & (int)GSXRPlugin.EyePoseStatus.kGazeVectorValid) != 0) combinedDirection += eyePose.leftDirection;
                if ((eyePose.rightStatus & (int)GSXRPlugin.EyePoseStatus.kGazeVectorValid) != 0) combinedDirection += eyePose.rightDirection;
                //if ((eyePose.combinedStatus & (int)GSXRPlugin.EyePoseStatus.kGazeVectorValid) != 0) combinedDirection += eyePose.combinedDirection;
                if (combinedDirection.sqrMagnitude > 0f)
                {
                    combinedDirection.Normalize();
                    //Debug.LogFormat("Eye Direction: ({0:F2}, {1:F2}, {2:F2})", combinedDirection.x, combinedDirection.y, combinedDirection.z);

                    float denominator = Vector3.Dot(combinedDirection, Vector3.forward);
                    if (denominator > float.Epsilon)
                    {
                        // eye direction intersection with frustum near plane (left)
                        eyePoint = combinedDirection * frustumSize.z / denominator;
                        eyePoint.x /= frustumSize.x; // [-1..1]
                        eyePoint.y /= frustumSize.y; // [-1..1]
                        //Debug.LogFormat("Eye Point: ({0:F2}, {1:F2})", eyePoint.x, eyePoint.y);
                    }
                }
            }
            var currentGain = GSXROverrideSettings.FoveateGain == Vector2.zero ?
                settings.foveationGain : GSXROverrideSettings.FoveateGain;
            var currentArea = GSXROverrideSettings.FoveateArea == 0f ?
                settings.foveationArea : GSXROverrideSettings.FoveateArea;
            var currentMinimum = GSXROverrideSettings.FoveateMinimum == 0f ?
                settings.foveationMinimum : GSXROverrideSettings.FoveateMinimum;
            for (int i = 0; i < eyes.Count; i++)
            {
                var eye = eyes[i];
                if (eye.TextureId > 0 && eye.ImageType == GSXREye.eType.RenderTexture)
                {
                    plugin.SetFoveationParameters(eye.TextureId, eye.PreviousId, eyePoint.x, eyePoint.y, currentGain.x, currentGain.y, currentArea, currentMinimum);
                    plugin.ApplyFoveation();
                }
            }

            var horizontalFieldOfView = 0f;
            if (settings.eyeFovMargin > 0f)
            {
                horizontalFieldOfView = (monoCamera.enabled ? monoCamera.fieldOfView / monoCamera.aspect : leftCamera.fieldOfView / leftCamera.aspect) * Mathf.Deg2Rad;
            }
            plugin.SubmitFrame(frameCount, horizontalFieldOfView, (int)settings.displayType);
            frameCount++;
        }
    }

    public bool RecenterTracking()
    {
        if (!Initialized)
            return false;

        return plugin.RecenterTracking();
    }

    void OnPreRenderListener(int sideMask, int textureId, int previousId)
    {
        if (!IsRunning)
            return;

        var currentGain = GSXROverrideSettings.FoveateGain == Vector2.zero ?
            settings.foveationGain : GSXROverrideSettings.FoveateGain;
        var currentArea = GSXROverrideSettings.FoveateArea == 0f ?
            settings.foveationArea : GSXROverrideSettings.FoveateArea;
        var currentMinimum = GSXROverrideSettings.FoveateMinimum == 0f ?
            settings.foveationMinimum : GSXROverrideSettings.FoveateMinimum;
        plugin.SetFoveationParameters(textureId, previousId, FocalPoint.x, FocalPoint.y, currentGain.x, currentGain.y, currentArea, currentMinimum);

        plugin.BeginEye(sideMask, frameDelta);
    }

    void OnPostRenderListener(int sideMask, int layerMask)
    {
        if (!IsRunning)
            return;

        plugin.EndEye(sideMask, layerMask);
    }

    public void SetPause(bool pause)
    {
        if (!Initialized || !isInit)
            return;
        if (pause)
        {
            OnPause();
        }
        else
        {
            onResume = StartCoroutine(OnResume());
        }
    }

    void OnPause()
    {
        //Debug.Log("GSXRManager.OnPause()");

        status.running = false;

        if (onResume != null)
        {
            StopCoroutine(onResume);
            onResume = null;
        }

        if (submitFrame != null) { StopCoroutine(submitFrame); submitFrame = null; }

        if (plugin.IsRunning()) plugin.EndVr();


    }

    IEnumerator OnResume()
    {
        //Debug.Log("GSXRManager.OnResume()");

        SetOverlayFade(eFadeState.FadeIn);

        yield return StartCoroutine(plugin.BeginVr((int)settings.cpuPerfLevel, (int)settings.gpuPerfLevel));

        float recenterTimeout = 1f;
        while (!plugin.RecenterTracking() && recenterTimeout > 0f)
        {
            yield return null;  // Wait one frame
            recenterTimeout -= Time.deltaTime;
        }

        yield return new WaitForSecondsRealtime(sensorWarmupDuration);

        submitFrame = StartCoroutine(SubmitFrame());

        status.running = plugin.IsRunning();
        onResume = null;
        onApplicationPauseDele?.Invoke(false);
    }

    [NonSerialized]
    public GSXRPlugin.HeadPose headPose;
    [NonSerialized]
    public GSXRPlugin.EyePose eyePose;
    [NonSerialized]
    public Vector3 eyeDirection = Vector3.forward;
    [NonSerialized]
    public Vector2 eyeFocus = Vector2.zero;
    [NonSerialized]
    public float eyeSmoother = 0.2f;
    [NonSerialized]
    public float[] frameDelta = new float[9];

    public Vector3 modifyPosition = Vector3.zero;
    public Quaternion modifyOrientation = Quaternion.identity;

    bool outBload = false;
    float[] outTransformArray = new float[32];
    Matrix4x4 leftcalib = Matrix4x4.identity;
    Matrix4x4 rightCalib = Matrix4x4.identity;

    bool isInit;
    bool isRotNan = false;
    bool isPosNan = false;
    void LateUpdate()
    {
        if (!IsRunning)
        {
            return;
        }

        int trackingMode = plugin.GetTrackingMode();
        var prevOrientation = headPose.orientation;

        status.pose = plugin.GetHeadPose(ref headPose, frameCount);
      //  if (((API_GSXR_Slam.SlamManager.status.pose & (int)GSXRPlugin.TrackingMode.kNotInit) != 0) || ((API_GSXR_Slam.SlamManager.status.pose & (int)GSXRPlugin.TrackingMode.kLost) != 0))
    //    {
         //   DebugMy.Log("Tracking state ===================================== NotInit or Lost !!!! " + API_GSXR_Slam.SlamManager.status.pose, this, true);
      //  }

        if (float.IsNaN(headPose.position.x) || float.IsNaN(headPose.position.y) || float.IsNaN(headPose.position.z))
        {
            DebugMy.Log("Position is " + headPose.position + " Error!!!!!!!!!!!!!!!!!!!!!!!!!!!", this, true);
            if (!isPosNan)
            {
                isPosNan = true;
                headPose.position = Vector3.zero;
                API_Module_DetectorSystem_Notice.SetNotice("The device position is NAN", "Please Reboot recovery", NoticeType.Warning, 0.8f, AlignmentType.Center, FollowType.False);
                API_Module_DetectorSystem_Notice.Show(1.5f);
            }
        }
        else
        {
            if (isPosNan)
            {
                isPosNan = false;
                DebugMy.Log("Position is " + headPose.position + " Success", this, true);
            }

        }
        if (float.IsNaN(headPose.orientation.x) || float.IsNaN(headPose.orientation.y) || float.IsNaN(headPose.orientation.z) || float.IsNaN(headPose.orientation.w))
        {

            DebugMy.Log("rotation is " + headPose.orientation + " Error!!!!!!!!!!!!!!!!!!!!!!!!!!!", this, true);
            if (!isRotNan)
            {
                isRotNan = true;
                headPose.orientation = Quaternion.identity;
                API_Module_DetectorSystem_Notice.SetNotice("The device rotation is NAN", "Please Reboot recovery", NoticeType.Warning, 0.8f, AlignmentType.Center, FollowType.False);
                API_Module_DetectorSystem_Notice.Show(1.5f);
            }

        }
        else
        {
            if (isRotNan)
            {
                isRotNan = false;
                DebugMy.Log("rotation is " + headPose.orientation + "Success", this, true);
            }
        }


        if ((trackingMode & (int)GSXRPlugin.TrackingMode.kTrackingEye) != 0)
        {
            status.pose |= plugin.GSXR_Get_EyePose(ref eyePose, frameCount);
        }

        if (!disableInput)
        {
            if ((status.pose & (int)GSXRPlugin.TrackingMode.kTrackingOrientation) != 0)
            {
                head.transform.localRotation = headPose.orientation;

                // delta orientation screen space x, y offset for foveated rendering
                var deltaOrientation = Quaternion.Inverse(prevOrientation) * headPose.orientation;
                var lookDirection = deltaOrientation * Vector3.forward;
                //Debug.LogFormat("Look Direction: {0}", lookDirection.ToString());
                lookDirection *= plugin.deviceInfo.targetFrustumLeft.near / lookDirection.z;
                float xTotal = 0.5f * (plugin.deviceInfo.targetFrustumLeft.right - plugin.deviceInfo.targetFrustumLeft.left);
                float xAdjust = (xTotal != 0.0f) ? lookDirection.x / xTotal : 0.0f;
                float yTotal = 0.5f * (plugin.deviceInfo.targetFrustumLeft.top - plugin.deviceInfo.targetFrustumLeft.bottom);
                float yAdjust = (yTotal != 0.0f) ? lookDirection.y / yTotal : 0.0f;
                //xAdjust *= 0.5f * plugin.deviceInfo.targetEyeWidthPixels;
                //yAdjust *= 0.5f * plugin.deviceInfo.targetEyeHeightPixels;

                // rotation around z [cos(z), sin(z), 0], [-sin(z), cos(z), 0], [0, 0, 1]
                Vector3 deltaEulers = deltaOrientation.eulerAngles;
                float cosZ = Mathf.Cos(deltaEulers.z * Mathf.Deg2Rad);
                float sinZ = Mathf.Sin(deltaEulers.z * Mathf.Deg2Rad);

                // Output rotation and translation
                frameDelta[0] = cosZ;
                frameDelta[1] = sinZ;
                frameDelta[2] = 0.0f;

                frameDelta[3] = -sinZ;
                frameDelta[4] = cosZ;
                frameDelta[5] = 0.0f;

                frameDelta[6] = xAdjust;
                frameDelta[7] = yAdjust;
                frameDelta[8] = 1.0f;
            }
            if (settings.trackPosition && (status.pose & (int)GSXRPlugin.TrackingMode.kTrackingPosition) != 0)
            {

                if (isInitRecenter == false && headPose.position != Vector3.zero)
                {
                    plugin.RecenterTracking();
                    isInitRecenter = true;
                    Debug.Log("isInitRecenter == true");
                }

                head.transform.localPosition = headPose.position * settings.trackPositionScale;
            }
            if ((status.pose & (int)GSXRPlugin.TrackingMode.kTrackingEye) != 0)
            {
                //Debug.LogFormat("Left Eye Position: {0}, Direction: {1}", eyePose.leftPosition.ToString(), eyePose.leftDirection.ToString());
                //Debug.LogFormat("Right Eye Position: {0}, Direction: {1}", eyePose.rightPosition.ToString(), eyePose.rightDirection.ToString());
                //Debug.LogFormat("Combined Eye Position: {0}, Direction: {1}", eyePose.combinedPosition.ToString(), eyePose.combinedDirection.ToString());
                var combinedDirection = Vector3.zero;
                if ((eyePose.leftStatus & (int)GSXRPlugin.EyePoseStatus.kGazeVectorValid) != 0) combinedDirection += eyePose.leftDirection;
                if ((eyePose.rightStatus & (int)GSXRPlugin.EyePoseStatus.kGazeVectorValid) != 0) combinedDirection += eyePose.rightDirection;
                //if ((eyePose.combinedStatus & (int)GSXRPlugin.EyePoseStatus.kGazeVectorValid) != 0) combinedDirection += eyePose.combinedDirection;
                if (combinedDirection.sqrMagnitude > 0f)
                {
                    combinedDirection.Normalize();
                    //Debug.LogFormat("Eye Direction: ({0:F2}, {1:F2}, {2:F2})", combinedDirection.x, combinedDirection.y, combinedDirection.z);

                    eyeDirection = eyeSmoother > 0.001f ? Vector3.Lerp(eyeDirection, combinedDirection, eyeSmoother) : combinedDirection;

                    //var combinedPosition = Vector3.zero;
                    //if ((eyePose.leftStatus & (int)GSXRPlugin.EyePoseStatus.kGazePointValid) != 0) combinedPosition += eyePose.leftPosition;
                    //if ((eyePose.rightStatus & (int)GSXRPlugin.EyePoseStatus.kGazePointValid) != 0) combinedPosition += eyePose.rightPosition;
                    ////if ((eyePose.combinedStatus & (int)GSXRPlugin.EyePoseStatus.kGazePointValid) != 0) combinedPosition += eyePose.combinedPosition;

                    gaze.localPosition = monoCamera.transform.localPosition;
                    gaze.localRotation = Quaternion.LookRotation(eyeDirection, Vector3.up);

                    float denominator = Vector3.Dot(eyeDirection, Vector3.forward);
                    if (denominator > float.Epsilon)
                    {
                        // eye direction intersection with frustum near plane (left)
                        var eyePoint = eyeDirection * plugin.deviceInfo.targetFrustumLeft.near / denominator;

                        // size of the frustum near plane (left)
                        var nearSize = new Vector2(0.5f * (plugin.deviceInfo.targetFrustumLeft.right - plugin.deviceInfo.targetFrustumLeft.left),
                            0.5f * (plugin.deviceInfo.targetFrustumLeft.top - plugin.deviceInfo.targetFrustumLeft.bottom));

                        eyeFocus.Set(eyePoint.x / nearSize.x, eyePoint.y / nearSize.y);   // Normalized [-1,1]
                        //Debug.LogFormat("Eye Focus: {0}", eyeFocus.ToString());

                        FocalPoint = eyeFocus;  // Cache for foveated rendering
                    }
                }
            }
        }

        //var isValid = true;
        //if (settings.poseStatusFade)
        //{
        //    isValid = !settings.trackPosition
        //        || ((trackingMode & (int)GSXRPlugin.TrackingMode.kTrackingPosition) == 0)
        //        || (status.pose & (int)GSXRPlugin.TrackingMode.kTrackingPosition) != 0;
        //}

        //var targetAlpha = fadeState == eFadeState.FadeOut || !isValid ? 1f : 0f;

        var targetAlpha = fadeState == eFadeState.FadeOut ? 1f : 0f;
        UpdateOverlayFade(targetAlpha, Time.deltaTime / fadeDuration);


        if (plugin.GSXR_Is_SupportOpticsCalibration())
        {
            if (leftcalib == Matrix4x4.identity || rightCalib == Matrix4x4.identity)
            {
                plugin.GSXR_Get_TransformMatrix(ref outBload, outTransformArray);

                if (outBload)
                {
                    leftcalib.SetColumn(0, new Vector4(outTransformArray[0], outTransformArray[1], outTransformArray[2], outTransformArray[3]));
                    leftcalib.SetColumn(1, new Vector4(outTransformArray[4], outTransformArray[5], outTransformArray[6], outTransformArray[7]));
                    leftcalib.SetColumn(2, new Vector4(outTransformArray[8], outTransformArray[9], outTransformArray[10], outTransformArray[11]));
                    leftcalib.SetColumn(3, new Vector4(outTransformArray[12], outTransformArray[13], outTransformArray[14], outTransformArray[15]));

                    rightCalib.SetColumn(0, new Vector4(outTransformArray[16], outTransformArray[17], outTransformArray[18], outTransformArray[19]));
                    rightCalib.SetColumn(1, new Vector4(outTransformArray[20], outTransformArray[21], outTransformArray[22], outTransformArray[23]));
                    rightCalib.SetColumn(2, new Vector4(outTransformArray[24], outTransformArray[25], outTransformArray[26], outTransformArray[27]));
                    rightCalib.SetColumn(3, new Vector4(outTransformArray[28], outTransformArray[29], outTransformArray[30], outTransformArray[31]));

                    Debug.Log($"leftcalib [{leftcalib.m00}, {leftcalib.m01}, {leftcalib.m02}, {leftcalib.m03};" +
            $"{leftcalib.m10}, {leftcalib.m11}, {leftcalib.m12}, {leftcalib.m13};" +
            $"{leftcalib.m20}, {leftcalib.m21}, {leftcalib.m22}, {leftcalib.m23};" +
            $"{leftcalib.m30}, {leftcalib.m31}, {leftcalib.m32}, {leftcalib.m33}]");

                    Debug.Log($"rightCalib [{rightCalib.m00}, {rightCalib.m01}, {rightCalib.m02}, {rightCalib.m03};" +
            $"{rightCalib.m10}, {rightCalib.m11}, {rightCalib.m12}, {rightCalib.m13};" +
            $"{rightCalib.m20}, {rightCalib.m21}, {rightCalib.m22}, {rightCalib.m23};" +
            $"{rightCalib.m30}, {rightCalib.m31}, {rightCalib.m32}, {rightCalib.m33}]");
                }
            }

            //if (outBload) {
            //    Matrix4x4 xx = leftcalib * invertZ * Matrix4x4.Inverse(Matrix4x4.TRS(leftCamera.transform.position, leftCamera.transform.rotation, leftCamera.transform.lossyScale));
            //    Matrix4x4 yy = rightCalib * invertZ * Matrix4x4.Inverse(Matrix4x4.TRS(rightCamera.transform.position, rightCamera.transform.rotation, rightCamera.transform.lossyScale));
            //    Debug.Log("SlamManager: OpticsCalibration leftCamera.worldToCameraMatrix：" + xx);
            //    Debug.Log("SlamManager: OpticsCalibration rightCamera.worldToCameraMatrix：" + yy);
            //}

            if (head.position != head.localPosition || head.rotation != head.localRotation)
            {
                ///for If Head parent not at origin,must mul the delta Matirx for Camera ViewMatrix
                //MD*Ml*P = Mw*P ==> MD == Mw * (M1 -1 ni)
                headDeltaInitLocalToWorld = Matrix4x4.TRS(head.position, head.rotation, Vector3.one) * Matrix4x4.Inverse(Matrix4x4.TRS(head.localPosition, head.localRotation, Vector3.one));
            }

            leftCamera.worldToCameraMatrix = plugin.leftViewMatrix * invertZ;
            leftCamera.worldToCameraMatrix = Matrix4x4.Inverse(headDeltaInitLocalToWorld * leftCamera.cameraToWorldMatrix/* not worldToCameraMatrix*/);

            rightCamera.worldToCameraMatrix = plugin.rightViewMatrix * invertZ;
            rightCamera.worldToCameraMatrix = Matrix4x4.Inverse(headDeltaInitLocalToWorld * rightCamera.cameraToWorldMatrix/* not worldToCameraMatrix*/);

            //Debug.Log("SlamManager: leftCamera.worldToCameraMatrix：" + leftCamera.worldToCameraMatrix);
            //Debug.Log("SlamManager: rightCamera.worldToCameraMatrix：" + rightCamera.worldToCameraMatrix);
        }
    }

    public void Shutdown()
    {
        Debug.Log("GSXRManager.Shutdown()");

        status.running = false;

        if (submitFrame != null) { StopCoroutine(submitFrame); submitFrame = null; }

        if (plugin.IsRunning()) plugin.EndVr();
        if (IsDontDestroyOnLoad)
        {
            if (plugin.IsInitialized()) plugin.Shutdown();
        }

        status.initialized = false;
    }

    void OnDestroy()
    {
        if (Instance != this)
            return;

        UnregisterListeners();
        // plugin.GSXR_Set_ControllerBondEventCallback(null);  //UnRegister Controller binding callback
        Shutdown();
    }

    public delegate void OnApplicationPauseDele(bool pause);
    public static event OnApplicationPauseDele onApplicationPauseDele;
    void OnApplicationPause(bool pause)
    {
        return;
      //  ScenceMain.isEnterApp = false;
        Debug.LogFormat("GSXRManager.OnApplicationPause({0})", pause);
        if (pause)
        {
            onApplicationPauseDele?.Invoke(true);
        }
        SetPause(pause);
    }

    void OnApplicationQuit()
    {
        Debug.Log("GSXRManager.OnApplicationQuit()");
        if (!IsDontDestroyOnLoad)
        {
            Shutdown();
            if (plugin.IsInitialized()) plugin.Shutdown();
        }

    }

    static public Matrix4x4 Perspective(float left, float right, float bottom, float top, float near, float far)
    {
        float x = 2.0F * near / (right - left);
        float y = 2.0F * near / (top - bottom);
        float a = (right + left) / (right - left);
        float b = (top + bottom) / (top - bottom);
        float c = -(far + near) / (far - near);
        float d = -(2.0F * far * near) / (far - near);
        float e = -1.0F;
        Matrix4x4 m = new Matrix4x4();
        m[0, 0] = x;
        m[0, 1] = 0;
        m[0, 2] = a;
        m[0, 3] = 0;
        m[1, 0] = 0;
        m[1, 1] = y;
        m[1, 2] = b;
        m[1, 3] = 0;
        m[2, 0] = 0;
        m[2, 1] = 0;
        m[2, 2] = c;
        m[2, 3] = d;
        m[3, 0] = 0;
        m[3, 1] = 0;
        m[3, 2] = e;
        m[3, 3] = 0;
        return m;
    }

    void RegisterListeners()
    {
        GSXROverrideSettings.OnEyeAntiAliasingChangedEvent += OnEyeAntiAliasingChanged;
        GSXROverrideSettings.OnEyeDepthChangedEvent += OnEyeDepthChanged;
        GSXROverrideSettings.OnEyeResolutionScaleFactorChangedEvent += OnEyeResolutionScaleFactorChanged;
        GSXROverrideSettings.OnOverlayAntiAliasingChangedEvent += OnOverlayAntiAliasingChanged;
        GSXROverrideSettings.OnOverlayDepthChangedEvent += OnOverlayDepthChanged;
        GSXROverrideSettings.OnOverlayResolutionScaleFactorChangedEvent += OnOverlayResolutionScaleFactorChanged;
        GSXROverrideSettings.OnChromaticAberrationCorrectionChangedEvent += OnChromaticAberrationCorrectionChanged;
        GSXROverrideSettings.OnVSyncCountChangedEvent += OnVSyncCountChanged;
        GSXROverrideSettings.OnMasterTextureLimitChangedEvent += OnMasterTextureLimitChanged;
        GSXROverrideSettings.OnPerfLevelChangedEvent += OnPerfLevelChanged;
        GSXROverrideSettings.OnFoveateChangedEvent += OnFoveateChanged;
    }

    void UnregisterListeners()
    {
        GSXROverrideSettings.OnEyeAntiAliasingChangedEvent -= OnEyeAntiAliasingChanged;
        GSXROverrideSettings.OnEyeDepthChangedEvent -= OnEyeDepthChanged;
        GSXROverrideSettings.OnEyeResolutionScaleFactorChangedEvent -= OnEyeResolutionScaleFactorChanged;
        GSXROverrideSettings.OnOverlayAntiAliasingChangedEvent -= OnOverlayAntiAliasingChanged;
        GSXROverrideSettings.OnOverlayDepthChangedEvent -= OnOverlayDepthChanged;
        GSXROverrideSettings.OnOverlayResolutionScaleFactorChangedEvent -= OnOverlayResolutionScaleFactorChanged;
        GSXROverrideSettings.OnChromaticAberrationCorrectionChangedEvent -= OnChromaticAberrationCorrectionChanged;
        GSXROverrideSettings.OnVSyncCountChangedEvent -= OnVSyncCountChanged;
        GSXROverrideSettings.OnMasterTextureLimitChangedEvent -= OnMasterTextureLimitChanged;
        GSXROverrideSettings.OnPerfLevelChangedEvent -= OnPerfLevelChanged;
        GSXROverrideSettings.OnFoveateChangedEvent -= OnFoveateChanged;
    }

    void OnEyeAntiAliasingChanged(GSXROverrideSettings.eAntiAliasing antiAliasing)
    {
        foreach (GSXREye eye in eyes)
        {
            eye.AntiAliasing = antiAliasing == GSXROverrideSettings.eAntiAliasing.NoOverride ?
                (int)settings.eyeAntiAliasing : (int)antiAliasing;
        }
    }

    void OnEyeDepthChanged(GSXROverrideSettings.eDepth depth)
    {
        foreach (GSXREye eye in eyes)
        {
            eye.Depth = depth == GSXROverrideSettings.eDepth.NoOverride ?
                (int)settings.eyeDepth : (int)depth;
        }
    }

    void OnEyeResolutionScaleFactorChanged(float scaleFactor)
    {
        foreach (GSXREye eye in eyes)
        {
            eye.ResolutionScaleFactor = scaleFactor <= 0 ? settings.eyeResolutionScaleFactor : scaleFactor;
        }
    }

    void OnOverlayAntiAliasingChanged(GSXROverrideSettings.eAntiAliasing antiAliasing)
    {
        foreach (GSXROverlay overlay in overlays)
        {
            overlay.AntiAliasing = antiAliasing == GSXROverrideSettings.eAntiAliasing.NoOverride ?
                (int)settings.overlayAntiAliasing : (int)antiAliasing;
        }
    }

    void OnOverlayDepthChanged(GSXROverrideSettings.eDepth depth)
    {
        foreach (GSXROverlay overlay in overlays)
        {
            overlay.Depth = depth == GSXROverrideSettings.eDepth.NoOverride ?
                (int)settings.overlayDepth : (int)depth;
        }
    }

    void OnOverlayResolutionScaleFactorChanged(float scaleFactor)
    {
        foreach (GSXROverlay overlay in overlays)
        {
            overlay.ResolutionScaleFactor = scaleFactor <= 0 ? settings.overlayResolutionScaleFactor : scaleFactor;
        }
    }

    void OnChromaticAberrationCorrectionChanged(GSXROverrideSettings.eChromaticAberrationCorrection aberrationCorrection)
    {
        if (aberrationCorrection == GSXROverrideSettings.eChromaticAberrationCorrection.kDisable)
        {
            plugin.SetFrameOption(GSXRPlugin.FrameOption.kDisableChromaticCorrection);
        }
        else
        {
            plugin.UnsetFrameOption(GSXRPlugin.FrameOption.kDisableChromaticCorrection);
        }
    }

    void OnVSyncCountChanged(GSXROverrideSettings.eVSyncCount vSyncCount)
    {
        if (vSyncCount == GSXROverrideSettings.eVSyncCount.NoOverride)
        {
            plugin.SetVSyncCount((int)settings.vSyncCount);
            QualitySettings.vSyncCount = (int)settings.vSyncCount;
        }
        else
        {
            plugin.SetVSyncCount((int)vSyncCount);
            QualitySettings.vSyncCount = (int)settings.vSyncCount;
        }
    }

    void OnMasterTextureLimitChanged(GSXROverrideSettings.eMasterTextureLimit masterTextureLimit)
    {
        QualitySettings.masterTextureLimit = masterTextureLimit == GSXROverrideSettings.eMasterTextureLimit.NoOverride ?
            (int)settings.masterTextureLimit : (int)masterTextureLimit;
    }

    void OnPerfLevelChanged(GSXROverrideSettings.ePerfLevel cpuPerfLevel, GSXROverrideSettings.ePerfLevel gpuPerfLevel)
    {
        int currentCpuPerfLevel = cpuPerfLevel == GSXROverrideSettings.ePerfLevel.NoOverride ?
            (int)settings.cpuPerfLevel : (int)GSXROverrideSettings.CpuPerfLevel;
        int currentGpuPerfLevel = gpuPerfLevel == GSXROverrideSettings.ePerfLevel.NoOverride ?
            (int)settings.gpuPerfLevel : (int)GSXROverrideSettings.GpuPerfLevel;
        plugin.SetPerformanceLevels(currentCpuPerfLevel, currentGpuPerfLevel);
    }

    void OnFoveateChanged(Vector2 gain, float area, float minPixelDensity)
    {
        var point = Vector2.zero;
        var currentGain = gain == Vector2.zero ?
            settings.foveationGain : GSXROverrideSettings.FoveateGain;
        var currentArea = area == 0f ?
            settings.foveationArea : GSXROverrideSettings.FoveateArea;
        var currentMinimum = minPixelDensity == 0f ?
            settings.foveationMinimum : GSXROverrideSettings.FoveateMinimum;
        plugin.SetFoveationParameters(0, 0, point.x, point.y, currentGain.x, currentGain.y, currentArea, currentMinimum);
    }

    /// <summary>
    /// Update this instance.
    /// </summary>
    //---------------------------------------------------------------------------------------------
    void Update()
    {
        SlamEvent frameEvent = new SlamEvent();

        while (plugin.PollEvent(ref frameEvent))
        {
            //Debug.LogFormat("SlamEvent: {0}", frameEvent.eventType.ToString());
            for (int i = 0; i < eventListeners.Count; i++)
            {
                eventListeners[i].OnSlamEvent(frameEvent);
            }
        }
    }

    /// <summary>
    /// Adds the event listener.
    /// </summary>
    /// <param name="listener">Listener.</param>
    //---------------------------------------------------------------------------------------------
    public void AddEventListener(SlamEventListener listener)
    {
        if (listener != null)
        {
            eventListeners.Insert(0, listener);
        }
    }

    /// <summary>
    /// Start Tracking
    /// </summary>
    /// <returns>Handle to the controller</returns>
    /// <param name="desc">Desc.</param>
    //---------------------------------------------------------------------------------------------
    public int ControllerStartTracking(string desc)
    {
        return plugin.ControllerStartTracking(desc);
    }

    /// <summary>
    /// Stop Tracking
    /// </summary>
    /// <param name="handle">Handle.</param>
    //---------------------------------------------------------------------------------------------
    public void ControllerStopTracking(int handle)
    {
        plugin.ControllerStopTracking(handle);
    }

    /// <summary>
    /// Get current state
    /// </summary>
    /// <returns>Controller State.</returns>
    /// <param name="handle">Handle.</param>
    //---------------------------------------------------------------------------------------------
    public GSXRControllerState ControllerGetState(int handle, int space = 0)
    {
        return plugin.ControllerGetState(handle, space);
    }

    /// <summary>
    /// Send an event to the controller
    /// </summary>
    /// <param name="handle">Handle.</param>
    /// <param name="what">What.</param>
    /// <param name="arg1">Arg1.</param>
    /// <param name="arg2">Arg2.</param>
    //---------------------------------------------------------------------------------------------
    public void ControllerSendMessage(int handle, GSXRController.slamControllerMessageType what, int arg1, int arg2)
    {
        plugin.ControllerSendMessage(handle, what, arg1, arg2);
    }

    /// <summary>
    /// Controllers the query.
    /// </summary>
    /// <returns>The query.</returns>
    /// <param name="handle">Handle.</param>
    /// <param name="what">What.</param>
    /// <param name="mem">Mem.</param>
    /// <param name="size">Size.</param>
    //---------------------------------------------------------------------------------------------
    public object ControllerQuery(int handle, GSXRController.slamControllerQueryType what)
    {
        return plugin.ControllerQuery(handle, what);
    }




    Coroutine startSlameCor;
    public void StartSlam()
    {
        if (startSlameCor == null)
        {
            Debug.Log("StartSlam Coroutine Start");
            startSlameCor = StartCoroutine(startSlam());
        }
        else
        {
            Debug.Log("startSlame Coroutine Not Null");
        }
    }

    IEnumerator startSlam()
    {
        yield return new WaitUntil(() => GSXRManager.Instance != null);

        yield return new WaitUntil(() => !GSXRManager.Instance.IsRunning);
        GSXRManager.Instance.SetPause(false);
        yield return new WaitUntil(() => IsTrackingValid);

        startSlameCor = null;
    }


    Coroutine stopSlameCor;

    //when stopSlam you must be sure no StartCorutine run before!
    public void StopSlam()
    {
        if (stopSlameCor == null && startSlameCor == null)
        {
            Debug.Log("stopSlame Coroutine Start");
            stopSlameCor = StartCoroutine(stopSlam());
        }
        else if (stopSlameCor != null)
        {
            Debug.Log("stopSlame Coroutine Not Null");
        }
        else if (startSlameCor != null)
        {
            Debug.Log("stopSlame: startSlame Coroutine Not Null");
        }
    }

    IEnumerator stopSlam()
    {
        yield return new WaitUntil(() => GSXRManager.Instance != null);
        yield return new WaitUntil(() => GSXRManager.Instance.IsRunning);

        GSXRManager.Instance.SetPause(true);
        yield return new WaitUntil(() => !GSXRManager.Instance.IsRunning);

        stopSlameCor = null;
    }



    Coroutine resetSlameCor;
    public void ResetSlam()
    {
        if (resetSlameCor == null)
        {
            Debug.Log("resetSlameCor Coroutine Start");
            resetSlameCor = StartCoroutine(resetSlam());
        }
        else
        {
            Debug.Log("resetSlameCor Coroutine Not Null");
        }
    }

    IEnumerator resetSlam()
    {

        Debug.Log("ResetSlam Coroutine Step 0: CheckFlag...");

        yield return new WaitUntil(() => GSXRManager.Instance != null);
        yield return new WaitUntil(() => GSXRManager.Instance.IsRunning);
        Debug.Log("ResetSlam Coroutine Step 1: StopSlam");
        GSXRManager.Instance.SetPause(true);

        yield return new WaitUntil(() => !GSXRManager.Instance.IsRunning);
        Debug.Log("ResetSlam Coroutine Step 2: StartSlam");
        GSXRManager.Instance.SetPause(false);
        yield return new WaitUntil(() => IsTrackingValid);

        Debug.Log("ResetSlam Coroutine Finish");
        resetSlameCor = null;
    }


    public bool IsTrackingValid
    {
        get
        {
            if (GSXRManager.Instance && GSXRManager.Instance.IsRunning)
            {
                return !GSXRManager.Instance.settings.trackPosition
                || ((GSXRManager.Instance.plugin.GetTrackingMode() & (int)GSXRPlugin.TrackingMode.kTrackingPosition) == 0)
                || (GSXRManager.Instance.status.pose & (int)GSXRPlugin.TrackingMode.kTrackingPosition) != 0;
            }
            return false;
        }
    }

    public Shoulder shoulder;

    public class Shoulder
    {

        public Transform Left { get; private set; }
        public Transform Right { get; private set; }

        private Transform head;
        private Vector3 leftOffset;
        public Shoulder(Transform head, Vector3 leftOffset)
        {
            this.head = head;
            this.leftOffset = leftOffset;

            if (head)
            {
                Left = new GameObject("LeftShoulder").transform;
                Left.SetParent(head, false);
                Left.localPosition = leftOffset;

                Right = new GameObject("RightShoulder").transform;
                Right.SetParent(head, false);
                Right.localPosition = new Vector3(-leftOffset.x, leftOffset.y, leftOffset.z);
            }

        }

    }

    public void GSXR_Set_OnFloorOrOnHead(int type)
    {
        if (type != 0 && type != 1)
        {
            DebugMy.Log("GSXR_Set_OnFloorOrOnHead: index Must 1 or 0", this, true);
            return;
        }
        plugin.GSXR_Set_OnFloorOrOnHead(type);
    }



    Coroutine VibrateCoroutine0, VibrateCoroutine1;
    public void GSXR_Set_ControllerVibrate(int index, bool isOn, VibrateOnType vibrateOnType, float time)
    {
        if (index != 0 && index != 1)
        {
            DebugMy.Log("GSXR_Set_ControllerVibrate: index Must 1 or 0", this, true);
            return;
        }

        if (vibrateOnType == VibrateOnType.Null)
        {
            //DebugMy.Log("GSXR_Set_ControllerVibrate: vibrateOnType Null", this, true);
            return;
        }

        //DebugMy.Log("GSXR_Set_ControllerVibrate:" + index + " " + isOn + " " + vibrateOnType + " " + time, this, true);

        if (index == 0 && VibrateCoroutine0 != null)
        {
            StopCoroutine(VibrateCoroutine0);
            VibrateCoroutine0 = null;
        }
        else if (index == 1 && VibrateCoroutine1 != null)
        {
            StopCoroutine(VibrateCoroutine1);
            VibrateCoroutine1 = null;
        }

        if (isOn)
        {
            if (index == 0)
            {
                VibrateCoroutine0 = StartCoroutine(VibrateRun(index, vibrateOnType, time));
            }
            else if (index == 1)
            {
                VibrateCoroutine1 = StartCoroutine(VibrateRun(index, vibrateOnType, time));
            }
        }
        else
        {
            plugin.GSXR_Set_ControllerVibrate(index, false);
        }
    }


    public int GSXR_Set_ControllerVibrate(int index, bool isOn, float amplitude, float frequency, float time)
    {
        if (index != 0 && index != 1 && index != 2)
        {
            DebugMy.Log("GSXR_Set_ControllerVibrateStatus: index Must 2 or 1 or 0", this, true);
            return -1;
        }
        int amplitude1 = Convert.ToInt32(Math.Ceiling(amplitude * 16));
        int frequency1 = Convert.ToInt32(Math.Ceiling(frequency * 16));
        int time1 = Convert.ToInt32(Math.Round(time * 10));
        //Debug.Log("KMSet_ControllerVibrate: "+index+","+isOn+","+amplitude1+","+ frequency1+","+ time1);
        return plugin.GSXR_Set_ControllerVibrate(index, isOn, amplitude1, frequency1, time1);
    }

    public bool GSXR_Get_ControllerVibrateStatus(int index)
    {
        if (index != 0 && index != 1)
        {
            DebugMy.Log("GSXR_Get_ControllerVibrateStatus: index Must 1 or 0", this, true);
            return false;
        }
        return plugin.GSXR_Get_ControllerVibrateStatus(index);
    }


    public int GSXR_Get_ControllerVibrateState(int index)
    {
        if (index != 0 && index != 1 && index != 2)
        {
            DebugMy.Log("GSXR_Get_ControllerVibrateState: index Must 2 or 1 or 0", this, true);
            return -1;
        }
        return ((plugin.GSXR_Get_ControllerVibrateState(index) >> 2) & 1);
    }
    public enum VibrateOnType
    {
        Shutdown,
        OneShot,
        Continuous,
        OneShotContinuous,
        Null,
    }

    IEnumerator VibrateRun(int index, VibrateOnType vibrateOnType, float time)
    {
        yield return null;

        if (vibrateOnType == VibrateOnType.OneShot)
        {

            plugin.GSXR_Set_ControllerVibrate(index, true);
            yield return new WaitForSeconds(time);
            plugin.GSXR_Set_ControllerVibrate(index, false);

        }
        else if (vibrateOnType == VibrateOnType.Continuous)
        {
            plugin.GSXR_Set_ControllerVibrate(index, true);
            while (true)
            {
                yield return null;
            }
        }
        else if (vibrateOnType == VibrateOnType.OneShotContinuous)
        {
            while (true)
            {
                plugin.GSXR_Set_ControllerVibrate(index, true);
                yield return new WaitForSeconds(time);
                plugin.GSXR_Set_ControllerVibrate(index, false);
                yield return new WaitForSeconds(time / 2);
            }
        }
        else if (vibrateOnType == VibrateOnType.Shutdown)
        {
            plugin.GSXR_Set_ControllerVibrate(index, false);
        }
        if (index == 0)
        {
            VibrateCoroutine0 = null;
        }
        else if (index == 1)
        {
            VibrateCoroutine1 = null;
        }
    }

    public int GSXR_Get_ControllerAccelerometer(float[] orientationArray, int index)
    {
        if (index != 0 && index != 1)
        {
            DebugMy.Log("GSXR_Get_ControllerAccelerometer: index Must 1 or 0", this, true);
            return -4;
        }
        return plugin.GSXR_Get_ControllerAccelerometer(orientationArray, index);
    }

    public int GSXR_Get_ControllerGyroscope(float[] orientationArray, int index)
    {
        if (index != 0 && index != 1)
        {
            DebugMy.Log("GSXR_Get_ControllerGyroscope: index Must 1 or 0", this, true);
            return -4;
        }
        return GSXR_Get_ControllerGyroscope(orientationArray, index);
    }
    public int GSXR_Get_ControllerAngleVelocity(float[] orientationArray, int index)
    {
        if (index != 0 && index != 1)
        {
            DebugMy.Log("GSXR_Get_ControllerAngleVelocity: index Must 1 or 0", this, true);
            return -4;
        }
        return plugin.GSXR_Get_ControllerAngleVelocity(orientationArray, index);
    }

    public int GSXR_Get_ControllerLinearVelocity(float[] orientationArray, int index)
    {
        if (index != 0 && index != 1)
        {
            DebugMy.Log("GSXR_Get_ControllerLinearVelocity: index Must 1 or 0", this, true);
            return -4;
        }
        return plugin.GSXR_Get_ControllerLinearVelocity(orientationArray, index);
    }
    public void GSXR_Bind_Controller(int index, int type)
    {
        if (index != 0 && index != 1)
        {
            DebugMy.Log("GSXR_Bind_Controller: index Must 1 or 0", this, true);
            return;
        }
        plugin.GSXR_Bind_Controller(index, type);
    }

    public void GSXR_Unbind_Controller(int index, int type)
    {
        if (index != 0 && index != 1)
        {
            DebugMy.Log("GSXR_Unbind_Controller: index Must 1 or 0", this, true);
            return;
        }

        Debug.Log("GSXR_Unbind_Controller===>normal execute");
        plugin.GSXR_Unbind_Controller(index, type);
    }

    public int GSXR_Get_ControllerBondState()
    {
        return plugin.GSXR_Get_ControllerBondState();
    }

    public void GSXR_Set_ControllerBondEventCallback(bool isEnable)
    {
        if (isEnable)
        {
            plugin.GSXR_Set_ControllerBondEventCallback(ControllerBondStateCallBack);
        }
        else
        {
            plugin.GSXR_Set_ControllerBondEventCallback(null);
        }

    }
    [MonoPInvokeCallback(typeof(Action<int>))]
    private static void ControllerBondStateCallBack(int bondState)
    {
        if (OnBondStateEvent != null && OnBondStateEvent.GetInvocationList().Length != 0)
        {
            OnBondStateEvent(bondState);
        }
    }

    private static Action<int> OnBondStateEvent;
    public void GSXR_Add_ControllerBondEventCallback(Action<int> callback)   //Register the Controller binding status callback
    {
        OnBondStateEvent += callback;
    }

    public void GSXR_Remove_ControllerBondEventCallback(Action<int> callback)  //UnRegister the Controller binding status callback
    {
        OnBondStateEvent -= callback;
    }


}