using System.Collections;
using System.Collections.Generic;
using Assets.NXR.Scripts.Slam;
using UnityEngine;
using Nxr.Internal;
using NibiruTask;
using System;
using System.Runtime.InteropServices;
using System.Threading;

public class SlamApi : MonoBehaviour
{
    enum TestMode
    {
        TM_Map,
        TM_Camera,
        TM_PlaneDetection
    }

    public struct MapStatus
    {
        public bool refresh;
        public int status;
        public int quality; // 0~100
        public float percent; // map match
    }

    public TextMesh CameraTextTip;
    public TextMesh MapTextTip;
    public TextMesh PlaneDetectionTip;

    public MeshRenderer ThermalCameraPreview;
    public MeshRenderer RgbCameraPreview;
    public MeshRenderer TofCameraPreview;
    public MeshRenderer FishEyeLeftCameraPreview;
    public MeshRenderer FishEyeRightCameraPreview;

    Texture2D RgbCamTexture2D_RGB;
    IntPtr[] RgbCameraDataPtrArray = new IntPtr[2] {IntPtr.Zero, IntPtr.Zero};

    public GameObject MapTestObj;
    public GameObject CameraTestObj;
    public GameObject PlaneDetectionTestObj;

    Texture2D ThermalCamTexture2D_Y;
    Texture2D ThermalCamTexture2D_U;
    Texture2D ThermalCamTexture2D_V;

    Texture2D TofCamTexture2D;

    Texture2D FishEyeLeftCamTexture2D;
    Texture2D FishEyeRightCamTexture2D;

    Color[] FishEyeLeftColors;
    Color[] FishEyeRightColors;

    TestMode CurTestMode = TestMode.TM_Map;

    Dictionary<NxrSlam.RawDataType, Queue<NxrSlam.NXRStreamData>> StreamDataDict =
        new Dictionary<NxrSlam.RawDataType, Queue<NxrSlam.NXRStreamData>>();

    Queue<NxrSlam.NXRPlaneData> PlaneDataQueue = new Queue<NxrSlam.NXRPlaneData>();
    Dictionary<int, NxrSlamPlane> PlaneDict = new Dictionary<int, NxrSlamPlane>();

    MapStatus mapStatus;

    public GameObject MapObjects;

    public void ShowMapTest()
    {
        CurTestMode = TestMode.TM_Map;

        MapTestObj.SetActive(true);
        CameraTestObj.SetActive(false);
        PlaneDetectionTestObj.SetActive(false);
    }

    public void ShowCameraTest()
    {
        CurTestMode = TestMode.TM_Camera;

        MapTestObj.SetActive(false);
        CameraTestObj.SetActive(true);
        PlaneDetectionTestObj.SetActive(false);
    }

    public void ShowPlaneDetectionTest()
    {
        CurTestMode = TestMode.TM_PlaneDetection;

        MapTestObj.SetActive(false);
        CameraTestObj.SetActive(false);
        PlaneDetectionTestObj.SetActive(true);
    }

    public void StartPlaneDetection()
    {
        PlaneDetectionTip.text = "等待检测结果...";
        Debug.Log("StartPlaneDetection");
        NxrSlam.Instance.StartPlaneDetection();
    }

    public void StopPlaneDetection()
    {
        PlaneDetectionTip.text = "请开始平面检测...";
        Debug.Log("StopPlaneDetection");
        NxrSlam.Instance.StopPlaneDetection();
    }

    string MapBinPath;

    public void BuildMap()
    {
        MapBinPath = Nxr.Internal.NxrViewer.Instance.GetStoragePath() + "/unity_map.bin";
        Debug.Log("BuildMap." + MapBinPath);
        NxrSlam.Instance.StartBuildMap();
    }

    public void FinishMap()
    {
        Debug.Log("FinishMap");
        // 记录地图原点
        Vector3 orginalVec = Camera.main.transform.position;
        PlayerPrefs.SetFloat("map_original_x", orginalVec.x);
        PlayerPrefs.SetFloat("map_original_y", orginalVec.y);
        PlayerPrefs.SetFloat("map_original_z", orginalVec.z);
        Debug.Log("Save Map Original Position : " + orginalVec.x + "," + orginalVec.y + "," + orginalVec.z);
        PlayerPrefs.Save();

        NxrSlam.Instance.StopBuildMap(MapBinPath);
    }

    public void ReadMap()
    {
        MapBinPath = Nxr.Internal.NxrViewer.Instance.GetStoragePath() + "/unity_map.bin";
        Debug.Log("ReadMap." + MapBinPath);
        // NxrSlam.Instance.StopSlam();
        NxrSlam.Instance.StartSlamWithMap(MapBinPath);
    }

    bool MapObjectsShowing = false;

    public void PutObjectInMap()
    {
        MapObjectsShowing = !MapObjectsShowing;
        Debug.Log("MapObjectsShowing." + MapObjectsShowing);
        MapObjects.SetActive(MapObjectsShowing);

        Vector3 mapOriginalVec;
        mapOriginalVec.x = PlayerPrefs.GetFloat("map_original_x");
        mapOriginalVec.y = PlayerPrefs.GetFloat("map_original_y");
        mapOriginalVec.z = PlayerPrefs.GetFloat("map_original_z");
        Debug.Log("Load Map Original Position : " + mapOriginalVec.x + "," + mapOriginalVec.y + "," + mapOriginalVec.z);
        MapObjects.transform.position = mapOriginalVec;
    }

    public void THERMALCamera()
    {
        Debug.Log("THERMALCamera");
        NxrSlam.Instance.StartStreaming(NxrSlam.RawDataType.THERMAL);
    }


    public void RGBCamera()
    {
        Debug.Log("RGBCamera");
        NxrSlam.Instance.StartStreaming(NxrSlam.RawDataType.RGB);
    }

    public void TOFCamera()
    {
        Debug.Log("TOFCamera");
        NxrSlam.Instance.SetTofStreamMode(NxrSlam.TOFStreamMode.ONLY_DEPTH);
        NxrSlam.Instance.StartStreaming(NxrSlam.RawDataType.TOF);
    }

    public void FISHEYECamera()
    {
        Debug.Log("FISHEYECamera");
        NxrSlam.Instance.StartStreaming(NxrSlam.RawDataType.FISHEYE);
    }

    public void StopCamera()
    {
        Debug.Log("StopCamera");
        NxrSlam.Instance.StopStreaming();
    }

    // Start is called before the first frame update
    void Start()
    {
        NxrSlam.Instance.Init();
        NxrSlam.Instance.EnableNativeLog(false);
        NxrSlam.Instance.MapStatusAndQualityEvent = OnMapStatusAndQualityEvent;
        NxrSlam.Instance.MapPercentEvent = OnMapPercent;
        NxrSlam.Instance.StreamDataEvent = OnStreamDataEvent;
        NxrSlam.Instance.PlaneDetectionEvent = OnPlaneDetectionEvent;
        mapStatus.refresh = false;

        StreamDataDict[NxrSlam.RawDataType.EYETRACKING] = new Queue<NxrSlam.NXRStreamData>();
        StreamDataDict[NxrSlam.RawDataType.RGB] = new Queue<NxrSlam.NXRStreamData>();
        StreamDataDict[NxrSlam.RawDataType.TOF] = new Queue<NxrSlam.NXRStreamData>();
        StreamDataDict[NxrSlam.RawDataType.FISHEYE] = new Queue<NxrSlam.NXRStreamData>();
        StreamDataDict[NxrSlam.RawDataType.THERMAL] = new Queue<NxrSlam.NXRStreamData>();
        Thread thread = new Thread(ShowFishEyeImage);
        thread.Start();
    }

    void OnPlaneDetectionEvent(NxrSlam.NXRPlaneData data)
    {
        //Debug.Log("plane data id=" + data.id + ",normal : x=" + data.normal.x + ",y=" + data.normal.y + ",z=" + data.normal.z
        //       + ",pointes size=" + data.size);
        //for (int i = 0; i < data.size; i++)
        //{
        //    Debug.Log(data.id +"," + i + ".Point=" + data.points[i].x + "," + data.points[i].y + "," + data.points[i].z);
        //}

        while (PlaneDataQueue.Count > 10)
        {
            PlaneDataQueue.Dequeue();
        }

        PlaneDataQueue.Enqueue(data);
    }

    void OnStreamDataEvent(NxrSlam.NXRStreamData data)
    {
        // data=RGB,type=0,1280X720,size=1382400,format=YUV420p
        // data=TOF,type=3,224X172,size=462336,format=YUYV
        // data=FISHEYE,type=0,640X400,size = 0,format = YUYV,left = 256000,right = 256000
        //Debug.Log("stream data=" + data.rawDataType.ToString() + ",type=" + data.type + ",size=" + data.width + "X" + data.height
        //     + ",size=" + data.size + ",format=" + data.rgbCodecFormat + ",left=" + data.leftSize + ",right=" + data.rightSize);
        if (data.size > 0 || data.leftSize > 0 || data.rightSize > 0)
        {
            if (StreamDataDict[data.rawDataType].Count > 30)
            {
                StreamDataDict[data.rawDataType].Clear();
            }

            if (data.rawDataType == NxrSlam.RawDataType.RGB)
            {
                lock (rgbCameraDataLock)
                {
                    StreamDataDict[data.rawDataType].Enqueue(data);
                }

                RgbFpsCheck();
            }
            else
            {
                StreamDataDict[data.rawDataType].Enqueue(data);
            }
        }
    }

    void OnMapStatusAndQualityEvent(int status, int quality)
    {
        mapStatus.refresh = true;
        mapStatus.status = status;
        mapStatus.quality = quality;
    }

    void OnMapPercent(float percentage)
    {
        mapStatus.refresh = true;
        mapStatus.percent = percentage;
    }

    public bool TestPoints = false;
    DateTime RgbFpsPrintTime = DateTime.Now;
    private int RgbFpsCount = 0;

    private void RgbFpsCheck()
    {
        TimeSpan diffSpan = new TimeSpan(DateTime.Now.Ticks - RgbFpsPrintTime.Ticks);
        if (diffSpan.TotalMilliseconds > 900)
        {
            RgbFpsPrintTime = DateTime.Now;
            Debug.LogError("Rgb Camera Frame Rate : " + RgbFpsCount);
            RgbFpsCount = 0;
        }

        RgbFpsCount++;
    }


    private readonly object rgbCameraDataLock = new object();

    void OnEnable()
    {
        StartCoroutine("EndOfFrame");
    }

    void OnDisable()
    {
        StopCoroutine("EndOfFrame");
    }

    IEnumerator EndOfFrame()
    {
        while (true)
        {
            yield return new WaitForEndOfFrame();
            //
            // ProcessRGBCameraData();
        }
    }

    private void ProcessRGBCameraData()
    {
        Queue<NxrSlam.NXRStreamData> rgbQueue = null;
        lock (rgbCameraDataLock)
        {
            rgbQueue = StreamDataDict[NxrSlam.RawDataType.RGB];
        }

        if (rgbQueue != null && rgbQueue.Count > 0)
        {
            // tail
            NxrSlam.NXRStreamData data = rgbQueue.ToArray()[rgbQueue.Count - 1];
            if (data.size > 0)
            {
                if (RgbCamTexture2D_RGB == null)
                {
                    Debug.Log("Create Texture2D : data=" + data.size + "," + data.width + "," + data.height + "," +
                              data.data.Length);
                    RgbCamTexture2D_RGB = new Texture2D(data.width, data.height, TextureFormat.RGB24, false);
                    RgbCameraPreview.material.SetTexture("_MainTex", RgbCamTexture2D_RGB);
                }

                //  Debug.Log("data=" + data.size + "," + data.width + "," + data.height + "," + data.data.Length);

                RgbCamTexture2D_RGB.LoadRawTextureData(data.data);
                RgbCamTexture2D_RGB.Apply();
            }
        }

        if (NxrSlam.Instance.IsRGBCameraSteaming)
        {
            int size = NxrSlam.getNxrStreamSize((int) NxrSlam.RawDataType.RGB);
            if (size > 0 && RgbCameraDataPtrArray[0] == IntPtr.Zero)
            {
                RgbCameraDataPtrArray[0] = Marshal.AllocHGlobal(size);
                RgbCameraDataPtrArray[1] = Marshal.AllocHGlobal(size);
                bool success = NxrSlam.initPtr((int) NxrSlam.RawDataType.RGB, RgbCameraDataPtrArray[0],
                    RgbCameraDataPtrArray[1]);
                if (!success)
                {
                    Debug.LogError("NxrSlam.initPtr Failed !!!");
                }
            }

            if (size > 0)
            {
                int width = 0;
                int height = 0;

                //DateTime startTime = DateTime.Now;
                int validPtrIndex = NxrSlam.getNxrStreamData((int) NxrSlam.RawDataType.RGB, ref width, ref height);
                //TimeSpan diffSpan = DateTime.Now.Subtract(startTime);
                //Debug.LogError("getNxrStreamData:" + width + "X" + height + "," + size + "," + validPtrIndex + "," + diffSpan.TotalMilliseconds);

                if (width > 0 && height > 0 && RgbCamTexture2D_RGB == null)
                {
                    RgbCamTexture2D_RGB = new Texture2D(width, height, TextureFormat.RGB24, false);
                    RgbCameraPreview.material.SetTexture("_MainTex", RgbCamTexture2D_RGB);
                }

                if (validPtrIndex >= 0 && RgbCamTexture2D_RGB != null)
                {
                    RgbCamTexture2D_RGB.LoadRawTextureData(RgbCameraDataPtrArray[validPtrIndex], size);
                    RgbCamTexture2D_RGB.Apply();
                    NxrSlam.unlockStream(validPtrIndex);
                }
            }
        }
    }

    // Update is called once per frame
    void Update()
    {
        if (TestPoints)
        {
            TestPoints = false;
            DebugPoints();
        }

        if (mapStatus.refresh)
        {
            mapStatus.refresh = false;
            MapTextTip.text = "地图质量:" + mapStatus.quality + ",地图匹配度(0~100):" + (int) (mapStatus.percent * 100);
        }

        if (NxrInput.GetKeyUp(CKeyEvent.KEYCODE_BACK) ||
            NxrInput.GetControllerKeyUp(CKeyEvent.KEYCODE_CONTROLLER_MENU) ||
            NxrInput.GetControllerKeyUp(CKeyEvent.KEYCODE_CONTROLLER_MENU,
                InteractionManager.NACTION_HAND_TYPE.HAND_LEFT) ||
            NxrInput.GetControllerKeyUp(CKeyEvent.KEYCODE_CONTROLLER_MENU,
                InteractionManager.NACTION_HAND_TYPE.HAND_RIGHT))
        {
            // 返回键
            if (Application.isMobilePlatform) Application.Quit();
        }

        // THERMAL
        Queue<NxrSlam.NXRStreamData> thermalQueue = StreamDataDict[NxrSlam.RawDataType.THERMAL];
        if (thermalQueue.Count > 0)
        {
            NxrSlam.NXRStreamData data = thermalQueue.Dequeue();
            if (data.size > 0)
            {
                if (ThermalCamTexture2D_Y == null)
                {
                    //yuv422
                    ThermalCamTexture2D_Y = new Texture2D(data.width, data.height, TextureFormat.Alpha8, false);
                    ThermalCamTexture2D_U = new Texture2D(data.width >> 1, data.height, TextureFormat.Alpha8, false);
                    ThermalCamTexture2D_V = new Texture2D(data.width >> 1, data.height, TextureFormat.Alpha8, false);
                    ThermalCameraPreview.material.SetTexture("_MainTex", ThermalCamTexture2D_Y);
                    ThermalCameraPreview.material.SetTexture("_UTex", ThermalCamTexture2D_U);
                    ThermalCameraPreview.material.SetTexture("_VTex", ThermalCamTexture2D_V);
                }

                byte[] dataY = null;
                byte[] dataU = null;
                byte[] dataV = null;
                NxrSlam.Instance.LoadYUV(data.data, data.width, data.height, ref dataY, ref dataU, ref dataV,
                    NxrSlam.Codec.YUYV);

                ThermalCamTexture2D_Y.LoadRawTextureData(dataY);
                ThermalCamTexture2D_U.LoadRawTextureData(dataU);
                ThermalCamTexture2D_V.LoadRawTextureData(dataV);
                ThermalCamTexture2D_Y.Apply();
                ThermalCamTexture2D_U.Apply();
                ThermalCamTexture2D_V.Apply();
            }
        }

        // RGB
        ProcessRGBCameraData();

        // TOF
        Queue<NxrSlam.NXRStreamData> tofQueue = StreamDataDict[NxrSlam.RawDataType.TOF];
        if (tofQueue.Count > 0)
        {
            NxrSlam.NXRStreamData data = tofQueue.Dequeue();
            // Debug.Log("tofQueue=" + tofQueue.Count + ",size=" + data.size + ",w=" + data.width + ",h=" + data.height);
            if (data.size > 0)
            {
                if (TofCamTexture2D == null)
                {
                    TofCamTexture2D = new Texture2D(data.width, data.height, TextureFormat.RGB565, false);
                    TofCameraPreview.material.SetTexture("_MainTex", TofCamTexture2D);
                }

                // 224*172*3*4
                float dmax = 7.5f;
                //Color[] colors = new Color[data.width* data.height];
                int dIndex = 0;
                for (int i = 0; i < data.size; i++)
                {
                    if (i % 4 == 0)
                    {
                        float d = System.BitConverter.ToSingle(data.data, i);
                        int u = (int) Mathf.Max(0, Mathf.Min(255.0f, d * 255.0f / dmax));
                        // RGB = R + G * 256 + B * 256 * 256
                        Color pixelsColor = NxrSlam.Instance.Int_To_RGB(u);
                        //colors[dIndex] = pixelsColor;

                        int x = dIndex % data.width;
                        int y = dIndex / data.width;
                        // 上下翻转
                        TofCamTexture2D.SetPixel(x, data.height - y, pixelsColor);

                        dIndex++;
                    }
                }

                //TofCamTexture2D.SetPixels(0, 0, data.width, data.height, colors);
                TofCamTexture2D.Apply();
            }
        }

        // FishEye
        Queue<NxrSlam.NXRStreamData> fishEyeQueue = StreamDataDict[NxrSlam.RawDataType.FISHEYE];
        if (fishEyeQueue.Count > 0)
        {
            if (!isStartConvert)
            {
                NxrSlam.NXRStreamData data = fishEyeQueue.Dequeue();
                _nxrStreamData = data;
                if (data.leftSize > 0 && data.rightSize > 0)
                {
                    if (FishEyeLeftCamTexture2D == null)
                    {
                        FishEyeLeftCamTexture2D = new Texture2D(data.width, data.height, TextureFormat.RGB24, false);
                        FishEyeRightCamTexture2D = new Texture2D(data.width, data.height, TextureFormat.RGB24, false);
                        FishEyeLeftCameraPreview.material.SetTexture("_MainTex", FishEyeLeftCamTexture2D);
                        FishEyeRightCameraPreview.material.SetTexture("_MainTex", FishEyeRightCamTexture2D);
                        FishEyeLeftColors = new Color[data.leftSize];
                        FishEyeRightColors = new Color[data.rightSize];
                    }
                }
                isStartConvert = true;
            }
        }
        
        if (isConverted)
        {
            FishEyeLeftCamTexture2D.SetPixels(FishEyeLeftColors);
            FishEyeLeftCamTexture2D.Apply();

            FishEyeRightCamTexture2D.SetPixels(FishEyeRightColors);
            FishEyeRightCamTexture2D.Apply();
            isConverted = false;
        }

        // Plane Detection
        if (PlaneDataQueue.Count > 0)
        {
            NxrSlam.NXRPlaneData data = PlaneDataQueue.Dequeue();
            if (PlaneDict.ContainsKey(data.id))
            {
                PlaneDict[data.id].MakePlane(data);
            }
            else
            {
                PlaneDict[data.id] = NxrSlam.Instance.GeneratePlaneObject(data);
            }

            string planeTip = "平面检测成功: \n";
            foreach (NxrSlamPlane plane in PlaneDict.Values)
            {
                planeTip += "[Id." + plane.PlaneId + "] 共 " + plane.PlanePointsCount + "个点\n";
            }

            PlaneDetectionTip.text = planeTip;
        }
    }

    private bool isRunning = true;
    private bool isStartConvert = false;
    private bool isConverted = false;
    private NxrSlam.NXRStreamData _nxrStreamData;

    void ShowFishEyeImage()
    {
        while (isRunning)
        {
            if (isStartConvert && !isConverted)
            {
                if (_nxrStreamData != null && _nxrStreamData.leftSize > 0 && _nxrStreamData.rightSize > 0)
                {
                    var data = _nxrStreamData;
                    for (int i = 0; i < data.leftSize; i++)
                    {
                        var dataLeft = data.left[i] / 255.0f;
                        var dataRight = data.right[i] / 255.0f;
                        FishEyeLeftColors[i].r = dataLeft;
                        FishEyeLeftColors[i].g = dataLeft;
                        FishEyeLeftColors[i].b = dataLeft;
                        FishEyeRightColors[i].r = dataRight;
                        FishEyeRightColors[i].g = dataRight;
                        FishEyeRightColors[i].b = dataRight;
                    }

                    isStartConvert = false;
                    isConverted = true;
                }
            }
            Thread.Sleep(10);
        }
    }

    private void OnDestroy()
    {
        if (RgbCameraDataPtrArray[0] != IntPtr.Zero)
        {
            Marshal.FreeHGlobal(RgbCameraDataPtrArray[0]);
            Marshal.FreeHGlobal(RgbCameraDataPtrArray[1]);
        }

        NxrSlam.Instance.UnInit();
        Debug.Log("OnDestroy");
        isRunning = false;
        isStartConvert = false;
        isConverted = false;
    }

    private void OnApplicationPause(bool pause)
    {
        Debug.Log("OnApplicationPause=" + pause);
        if (pause) NxrSlam.Instance.Pause();
    }

    private void OnApplicationQuit()
    {
        NxrSlam.Instance.UnInit();
        Debug.Log("OnApplicationQuit");
    }

    void DebugPoints()
    {
        NxrSlam.NXRPlaneData testdata = new NxrSlam.NXRPlaneData(null);

        testdata.id = 1;
        testdata.points = new NxrSlam.NXRPlaneData.Point3D[]
        {
            /*
            new NxrSlam.NXRPlaneData.Point3D(0.597963809967041,-0.483312696218491,0.43642520904541),
            new NxrSlam.NXRPlaneData.Point3D(0.448058247566223,-0.483312696218491,0.369752883911133),
            new NxrSlam.NXRPlaneData.Point3D(0.32343664765358,-0.483312696218491,0.323436379432678),
            new NxrSlam.NXRPlaneData.Point3D(0.3141910135746,-0.483312696218491,0.323615431785584),
            new NxrSlam.NXRPlaneData.Point3D(0.0990831553936005,-0.483312696218491,0.33366933465004),
            new NxrSlam.NXRPlaneData.Point3D(0.0974671021103859,-0.483312696218491,0.333752363920212),
            new NxrSlam.NXRPlaneData.Point3D(0.0561475902795792,-0.483312696218491,0.337889224290848),
            new NxrSlam.NXRPlaneData.Point3D(-0.465384006500244,-0.483312696218491,0.51269006729126),
            new NxrSlam.NXRPlaneData.Point3D(-0.232375964522362,-0.483312696218491,0.975189507007599),
            new NxrSlam.NXRPlaneData.Point3D(0.267775535583496,-0.483312696218491,0.737699329853058),
            new NxrSlam.NXRPlaneData.Point3D(0.360924899578094,-0.483312696218491,0.65373432636261),
            new NxrSlam.NXRPlaneData.Point3D(0.597963809967041,-0.483312696218491,0.43642520904541)
            */

// II
            new NxrSlam.NXRPlaneData.Point3D(1.0432962179184, -0.302729845046997, 0.245670855045319),
            new NxrSlam.NXRPlaneData.Point3D(0.599149584770203, -0.302729845046997, 0.168545603752136),
            new NxrSlam.NXRPlaneData.Point3D(-0.248820841312408, -0.302729845046997, 0.573924124240875),
            new NxrSlam.NXRPlaneData.Point3D(-0.401118278503418, -0.302729845046997, 0.789052367210388),
            new NxrSlam.NXRPlaneData.Point3D(-0.450264513492584, -0.302729845046997, 0.886043250560761),
            new NxrSlam.NXRPlaneData.Point3D(-0.450264513492584, -0.302729845046997, 0.886043250560761),
            new NxrSlam.NXRPlaneData.Point3D(-0.0576245971024036, -0.302729845046997, 1.1546128988266),
            new NxrSlam.NXRPlaneData.Point3D(0.103346958756447, -0.302729845046997, 1.081174492836),
            new NxrSlam.NXRPlaneData.Point3D(0.836297452449799, -0.302729845046997, 0.65020763874054),
            new NxrSlam.NXRPlaneData.Point3D(1.04192793369293, -0.302729845046997, 0.37995707988739),
            new NxrSlam.NXRPlaneData.Point3D(1.0432962179184, -0.302729845046997, 0.245670855045319)

            //III
//new NxrSlam.NXRPlaneData.Point3D(1.3766975402832,-0.298782527446747,-0.534084439277649),
//new NxrSlam.NXRPlaneData.Point3D(-0.277698934078217,-0.298782527446747,-0.0697378441691399),
//new NxrSlam.NXRPlaneData.Point3D(-0.531768798828125,-0.298782527446747,0.118963070213795),
//new NxrSlam.NXRPlaneData.Point3D(-0.630833983421326,-0.298782527446747,0.548060476779938),
//new NxrSlam.NXRPlaneData.Point3D(-0.165737017989159,-0.298782527446747,0.634526371955872),
//new NxrSlam.NXRPlaneData.Point3D(0.930005371570587,-0.298782527446747,0.364291131496429),
//new NxrSlam.NXRPlaneData.Point3D(0.97565633058548,-0.298782527446747,0.314914882183075),
//new NxrSlam.NXRPlaneData.Point3D(1.14742374420166,-0.298782527446747,0.0324256010353565),
//new NxrSlam.NXRPlaneData.Point3D(1.34535908699036,-0.298782527446747,-0.367374181747437)
        };
        testdata.size = testdata.points.Length;
        testdata.Refresh();
        PlaneDataQueue.Enqueue(testdata);
    }
}