using System.Collections.Generic;
using UnityEngine;

namespace Assets.NXR.Scripts.Slam
{
    public class NxrSlam
    {
        [System.Runtime.InteropServices.DllImport("nxr_slam")]
        public static extern bool initPtr(int streamType, System.IntPtr dataPtr0, System.IntPtr dataPtr1);

        /// <summary>
        /// 
        /// </summary>
        /// <param name="streamType"></param>
        /// <param name="width"></param>
        /// <param name="height"></param>
        /// <returns>0/1 has valid data, -1 no data</returns>
        [System.Runtime.InteropServices.DllImport("nxr_slam")]
        public static extern int getNxrStreamData(int streamType, ref int width, ref int height);

        [System.Runtime.InteropServices.DllImport("nxr_slam")]
        public static extern int getNxrStreamSize(int streamType);

        [System.Runtime.InteropServices.DllImport("nxr_slam")]
        public static extern void unlockStream(int idx);

        public enum TOFStreamMode
        {
            ONLY_DEPTH, // w*h*4
            ONLY_CLOUD,
            DEPTH_CLOUD,// w*h*12
            NONE,
            CLOUD_ON_LEFTHAND_SLAM
        }

        public enum TOFFPS
        {
            TOF_FPS_5, TOF_FPS_10, TOF_FPS_15, TOF_FPS_20, TOF_FPS_25, TOF_FPS_30
        }

        public enum RgbResolution
        {
            RGB_1920x1080, RGB_1280x720, RGB_640x480, RGB_320x240, RGB_2560x1920, TOF
        }

        public enum Mode
        {
            MODE_3DOF, MODE_6DOF
        }

        public enum RawDataType
        {
            NONE,
            EYETRACKING,
            RGB,
            TOF,
            FISHEYE,
            THERMAL
        }

        public enum Codec
        {
            YUYV,
            YUV420p,
            JPEG,
            NV12,
            BITSTREAM
        }

        public enum RgbType
        {
            Preview,
            Capture
        }
        public enum RgbFormat
        {
            FORMAT_NONE, 
            FORMAT_YUYV, 
            FORMAT_YUV420, 
            FORMAT_RGB, 
            FORMAT_RGBA, 
            FORMAT_H264
        }

        public enum TofType
        {
            Depth_16,
            Depth_32,
            IR,
            Cloud,
            Raw,
            Eeprom
        }

        public class NXRStreamData
        {
            public RawDataType rawDataType;
            public byte[] data;//rgb or tof camera data
            public byte[] left; // fish eye left camera data
            public byte[] right;// fish eye right camera data
            public int type;//数据类型:TofType/RgbType
            // 224*172*3*4 bytes
            public TofType tofType;
            public RgbType rgbType;
            public Codec rgbCodecFormat;// only for rgb
            public int width;
            public int height;
            public int size; // rgb or tof size
            public int leftSize; // fish eye left size
            public int rightSize;// fish eye right size
            public long timestamp;
            public TOFStreamMode tofStreamMode;

            public NXRStreamData(RawDataType dataType, AndroidJavaObject javaObject)
            {
                rawDataType = dataType;
                data = AndroidJNIHelper.ConvertFromJNIArray<byte[]>(javaObject.Get<AndroidJavaObject>("data").GetRawObject());

                if (rawDataType == RawDataType.FISHEYE)
                {
                    left = AndroidJNIHelper.ConvertFromJNIArray<byte[]>(javaObject.Get<AndroidJavaObject>("left").GetRawObject());
                    right = AndroidJNIHelper.ConvertFromJNIArray<byte[]>(javaObject.Get<AndroidJavaObject>("right").GetRawObject());
                }

                type = javaObject.Get<int>("type");
                rgbCodecFormat = (Codec)javaObject.Get<int>("format");
                width = javaObject.Get<int>("width");
                height = javaObject.Get<int>("height");
                size = javaObject.Get<int>("size");
                leftSize = javaObject.Get<int>("leftSize");
                rightSize = javaObject.Get<int>("rightSize");
                timestamp = javaObject.Get<long>("timestamp");
                if (rawDataType == RawDataType.RGB)
                {
                    rgbType = (RgbType)type;
                }
                else if (rawDataType == RawDataType.TOF)
                {
                    tofType = (TofType)type;
                }
            }
        }

        public class NXRPlaneData
        {
            public long hostTimestamp = 0L;
            public long deviceTimestamp = 0L;
            public NXRPlaneData.Point3D[] points;
            public NXRPlaneData.Point3D normal = new Point3D();
            public int size;
            public int id = -1;

            public void Refresh()
            {
                if (
                             Mathf.Abs((float)(points[0].x - points[size - 1].x)) < Mathf.Epsilon
                             && Mathf.Abs((float)(points[0].y - points[size - 1].y)) < Mathf.Epsilon
                             && Mathf.Abs((float)(points[0].z - points[size - 1].z)) < Mathf.Epsilon
                             )
                {  
                    // 首尾相同
                    size = size - 1;
                }
          
            }
 
            public NXRPlaneData(AndroidJavaObject javaObject)
            {
                if(javaObject != null)
                {
                    hostTimestamp = javaObject.Get<long>("hostTimestamp");
                    deviceTimestamp = javaObject.Get<long>("deviceTimestamp");
                    size = javaObject.Get<int>("size");
                    id = javaObject.Get<int>("id");

                    AndroidJavaObject normalObj = javaObject.Get<AndroidJavaObject>("normal");
                    if (normalObj != null)
                    {
                        normal.x = normalObj.Get<double>("x");
                        normal.y = normalObj.Get<double>("y");
                        normal.z = normalObj.Get<double>("z");
                        normalObj.Dispose();
                    }

                    AndroidJavaObject pointsObj = javaObject.Get<AndroidJavaObject>("points");
                    if (pointsObj != null)
                    {
                        AndroidJavaObject[] pointsArray = AndroidJNIHelper.ConvertFromJNIArray<AndroidJavaObject[]>(pointsObj.GetRawObject());
                        if (pointsArray != null)
                        {
                            if (
                                Mathf.Abs((float) (pointsArray[0].Get<double>("x") - pointsArray[size-1].Get<double>("x"))) < Mathf.Epsilon
                                && Mathf.Abs((float)(pointsArray[0].Get<double>("y") - pointsArray[size - 1].Get<double>("y"))) < Mathf.Epsilon
                                && Mathf.Abs((float)(pointsArray[0].Get<double>("z") - pointsArray[size - 1].Get<double>("z"))) < Mathf.Epsilon
                                )
                            {
                                // 首尾相同
                                size = size - 1;
                            }

                            points = new Point3D[size];
                            for (int i = 0; i < size; i++)
                            {
                                Point3D point3D = new Point3D();
                                point3D.x = pointsArray[i].Get<double>("x");
                                point3D.y = pointsArray[i].Get<double>("y");
                                point3D.z = pointsArray[i].Get<double>("z");
                                // 右手坐标系转换，绕x轴旋转-180度
                                point3D.y = point3D.y * (-1);
                                points[i] = point3D;
                                pointsArray[i].Dispose();
                            }
                        }
                        pointsObj.Dispose();
                    }
                }
            }

            public class Point3D
            {
                public double x;
                public double y;
                public double z;

                public Point3D()
                {
                }

                public Point3D(double x, double y, double z)
                {
                    this.x = x;
                    this.y = y;
                    this.z = z;
                }
            }
        }

        private static object syncRoot = new object();

        private static NxrSlam _instance = null;

        public static NxrSlam Instance
        {
            get
            {
                if (_instance == null) //第一重判断，先判断实例是否存在，不存在再加锁处理
                {
                    lock (syncRoot) //加锁，在某一时刻只允许一个线程访问
                    {
                        if (_instance == null) //第二重判断: 第一个线程进入Lock中执行创建代码，第二个线程处于排队等待状态，当第二个线程进入Lock后并不知道实例已创建，将会继续创建新的实例
                        {
                            _instance = new NxrSlam();
                        }
                    }
                }

                return _instance;
            }
        }

        public bool IsRGBCameraSteaming { get; set; }
        private NxrSlam()
        {
#if !UNITY_EDITOR
            GetNxrSlamInstance();
#endif
        }

        AndroidJavaObject nxrSlamInstance;
        AndroidJavaClass nxrSlamClass;
#if !UNITY_EDITOR
        public AndroidJavaObject GetNxrSlamInstance()
        {
            if (nxrSlamClass == null)
            {
                nxrSlamClass = new AndroidJavaClass("ruiyue.nxr.slam.NXRSlam");
            }

            if (nxrSlamClass != null && nxrSlamInstance == null)
            {
                nxrSlamInstance = nxrSlamClass.CallStatic<AndroidJavaObject>("getInstance");
                Debug.Log("NXRSlam getInstance");
            }

            return nxrSlamInstance;
        }
#else
        private AndroidJavaObject GetJavaObject() { return null;}
#endif

        public void EnableNativeLog(bool enable)
        {
            if (nxrSlamInstance != null)
            {
                Debug.Log("NXRSlam enableNativeLog");
                nxrSlamInstance.Call("enableNativeLog", enable);
            }
        }

        bool Inited = false;
        public bool Init()
        {
            IsRGBCameraSteaming = false;
            if (nxrSlamInstance != null)
            {
                Debug.Log("NXRSlam init");
                Inited = true;
                return nxrSlamInstance.Call<bool>("init");
            }
            return false;
        }

        public void UnInit()
        {
            if (nxrSlamInstance != null && Inited)
            {
                StopPlaneDetection();
                StopStreaming();
                StopBuildMap();
                Debug.Log("NXRSlam uninit");
                Inited = false;
                nxrSlamInstance.Call("uninit");
            }
        }

        /// <summary>
        /// 
        /// </summary>
        /// <param name="path">地图路径：sdcard/unity_map.bin</param>
        /// <returns></returns>
        public bool StartSlamWithMap(string path)
        {
            // 是否存在
            //if (path == null || path.Length == 0 || !File.Exists(path))
            //{
            //    Debug.Log("NXRSlam startSlamWithMap failed, file not exist !!! " + path);
            //    return false;
            //}

            if (nxrSlamInstance != null)
            {
                IsBuildingMap = true;
                Debug.Log("NXRSlam startSlamWithMap." + path);
                return nxrSlamInstance.Call<bool>("startSlamWithMap", path, new MapBuildCallback());
            }
            return false;
        }

        public void StopSlam()
        {
            if (nxrSlamInstance != null)
            {
                Debug.Log("NXRSlam stopSlam");
                nxrSlamInstance.Call("stopSlam");
            }
        }

        bool IsBuildingMap = false;
        public bool StartBuildMap()
        {
            if (nxrSlamInstance != null)
            {
                IsBuildingMap = true;
                Debug.Log("NXRSlam startRecordMap");
                return nxrSlamInstance.Call<bool>("startRecordMap", new MapBuildCallback());
            }
            return false;
        }

        /// <summary>
        /// 
        /// </summary>
        /// <param name="path">sdcard/unity_map.bin</param>
        /// <returns></returns>
        public bool StopBuildMap(string path = null)
        {
            if (nxrSlamInstance != null && IsBuildingMap)
            {
                IsBuildingMap = false;

                //if(path == null)
                //{
                //    path = "system/etc/unity_map.bin";
                //}
                Debug.Log("NXRSlam stopRecordMap." + path);
                return nxrSlamInstance.Call<bool>("stopRecordMap", path);
            }
            return false;
        }

        bool IsPlaneDetecting = false;
        public bool StartPlaneDetection()
        {
            if (nxrSlamInstance != null)
            {
                IsPlaneDetecting = true;
                Debug.Log("NXRSlam startPlaneDetection");
                return nxrSlamInstance.Call<bool>("startPlaneDetection", new PlaneDetectionCallback());
            }
            return false;
        }

        public bool StopPlaneDetection()
        {
            if (nxrSlamInstance != null && IsPlaneDetecting)
            {
                IsPlaneDetecting = false;
                Debug.Log("NXRSlam stopPlaneDetection");
                return nxrSlamInstance.Call<bool>("stopPlaneDetection");
            }
            return false;
        }

        public void Pause()
        {
            Debug.Log("NXRSlam pause");
            //StopPlaneDetection();
            //StopStreaming();
            //StopBuildMap();
        }
        
        List<RawDataType> streamingDataTypeList = new List<RawDataType>();
        AndroidJavaClass rawDataTypeCls = null;
        AndroidJavaClass rgbFormatCls = null;
        AndroidJavaClass tofStreamModeCls = null;
        AndroidJavaClass tofFpsCls = null;
        AndroidJavaClass rgbResolutionCls = null;
        public bool StartStreaming(RawDataType rawDataType)
        {
            if (nxrSlamInstance != null)
            {
                if (rawDataTypeCls == null)
                {
                    rawDataTypeCls = new AndroidJavaClass("ruiyue.nxr.slam.NXRSlam$RawDataType");
                }

                if (rgbFormatCls == null)
                {
                    rgbFormatCls = new AndroidJavaClass("ruiyue.nxr.slam.NXRSlam$RgbFormat");
                }

                if (rawDataType == RawDataType.RGB)
                {
                    AndroidJavaObject rgbFormatObj = rgbFormatCls.CallStatic<AndroidJavaObject>("valueOf", RgbFormat.FORMAT_RGB.ToString());
                    nxrSlamInstance.Call("setRgbFormat", rgbFormatObj);
                    IsRGBCameraSteaming = true;
                }

                AndroidJavaObject rawDataTypeObj = rawDataTypeCls.CallStatic<AndroidJavaObject>("valueOf", rawDataType.ToString());
                streamingDataTypeList.Add(rawDataType);
                return nxrSlamInstance.Call<bool>("startStreaming", rawDataTypeObj, new StreamDataCallback());
            }
            return false;
        }

        public void SetLed(int select, int channel, int brightness)
        {
            if (nxrSlamInstance != null)
            {
                nxrSlamInstance.Call("setLed", select, channel, brightness);
            }
        }

        public void SetAec(int aecmode, int aecgain, int aectime)
        {
            if (nxrSlamInstance != null)
            {
                nxrSlamInstance.Call("setAec", aecmode, aecgain, aectime);
            }
        }

        TOFStreamMode tofStreamMode = TOFStreamMode.ONLY_DEPTH;
        public void SetTofStreamMode(TOFStreamMode mode)
        {
            if (nxrSlamInstance != null)
            {
                if (tofStreamModeCls == null)
                {
                    tofStreamModeCls = new AndroidJavaClass("ruiyue.nxr.slam.NXRSlam$TOFStreamMode");
                }
                AndroidJavaObject tofStreamModeObj = tofStreamModeCls.CallStatic<AndroidJavaObject>("valueOf", mode.ToString());
                nxrSlamInstance.Call("setTofStreamMode", tofStreamModeObj);
                tofStreamMode = mode;
            }
        }

        public void SetTofFps(TOFFPS fps)
        {
            if (nxrSlamInstance != null)
            {
                if (tofFpsCls == null)
                {
                    tofFpsCls = new AndroidJavaClass("ruiyue.nxr.slam.NXRSlam$TOFFPS");
                }
                AndroidJavaObject tofStreamModeObj = tofFpsCls.CallStatic<AndroidJavaObject>("valueOf", fps.ToString());
                nxrSlamInstance.Call("setTOFFps", tofStreamModeObj);
            }
        }

        public void SetRgbResolution(RgbResolution rgbResolution)
        {
            if (nxrSlamInstance != null)
            {
                if (rgbResolutionCls == null)
                {
                    rgbResolutionCls = new AndroidJavaClass("ruiyue.nxr.slam.NXRSlam$RgbResolution");
                }
                AndroidJavaObject tofStreamModeObj = rgbResolutionCls.CallStatic<AndroidJavaObject>("valueOf", rgbResolution.ToString());
                nxrSlamInstance.Call("setRgbResolution", tofStreamModeObj);
            }
        }

        public bool StopStreaming(RawDataType rawDataType = RawDataType.NONE)
        {
            if (nxrSlamInstance != null)
            {
                if (rawDataType == RawDataType.NONE)
                {
                    foreach(RawDataType type in streamingDataTypeList)
                    {
                        AndroidJavaObject rawDataTypeObj = rawDataTypeCls.CallStatic<AndroidJavaObject>("valueOf", type.ToString());
                        Debug.Log("NXRSlam stopStreaming." + type.ToString());
                        nxrSlamInstance.Call<bool>("stopStreaming", rawDataTypeObj);
                    }
                    IsRGBCameraSteaming = false;
                    streamingDataTypeList.Clear();
                    return true;
                } else
                {
                    if (rawDataType == RawDataType.RGB) IsRGBCameraSteaming = false;
                    AndroidJavaObject rawDataTypeObj = rawDataTypeCls.CallStatic<AndroidJavaObject>("valueOf", rawDataType.ToString());
                    return nxrSlamInstance.Call<bool>("stopStreaming", rawDataTypeObj);
                }

            }
            return false;
        }

        private Mode CurMode = Mode.MODE_3DOF;
        public bool SwitchMode(Mode mode)
        {
            if (nxrSlamInstance != null)
            {
                CurMode = mode;
                AndroidJavaClass modeCls = new AndroidJavaClass("ruiyue.nxr.slam.NXRSlam$Mode");
                AndroidJavaObject modeObj = rawDataTypeCls.CallStatic<AndroidJavaObject>("valueOf", mode.ToString());
                return nxrSlamInstance.Call<bool>("switchMode", modeObj);
            }
            return false;
        }

#region
        public delegate void MapStatusAndQualityCallback(int status, int quality);
        public delegate void MapPercentCallback(float percent);
        public delegate void StreamingDataCallback(NXRStreamData data);
        public delegate void PlaneDetectionCallaback(NXRPlaneData data);

        public MapStatusAndQualityCallback MapStatusAndQualityEvent;
        public MapPercentCallback MapPercentEvent;
        public StreamingDataCallback StreamDataEvent;
        public PlaneDetectionCallaback PlaneDetectionEvent;

        public class MapBuildCallback : AndroidJavaProxy
        {

            public MapBuildCallback() : base("ruiyue.nxr.slam.onMapListener")
            {

            }

            public void onMapStatusAndQuality(int status, int quality)
            {
                if (Instance.MapStatusAndQualityEvent != null)
                {
                    Instance.MapStatusAndQualityEvent(status, quality);
                }
            }

            public void onMapPercent(float percent)
            {
                if (Instance.MapPercentEvent != null)
                {
                    Instance.MapPercentEvent(percent);
                }
            }
        }


        public class StreamDataCallback : AndroidJavaProxy
        {

            public StreamDataCallback() : base("ruiyue.nxr.slam.onStreamDataListener")
            {

            }

            public void onStreamData(AndroidJavaObject rawDataType, AndroidJavaObject streamData)
            {
                RawDataType type = (RawDataType)rawDataType.Call<int>("ordinal");
                if (type == RawDataType.RGB) return;

                NXRStreamData nxrStreamData = new NXRStreamData(type, streamData);
                nxrStreamData.tofStreamMode = Instance.tofStreamMode;
                if (Instance.StreamDataEvent != null)
                {
                    Instance.StreamDataEvent(nxrStreamData);
                }
            }
        }

        public class PlaneDetectionCallback : AndroidJavaProxy
        {

            public PlaneDetectionCallback() : base("ruiyue.nxr.slam.onPlaneDetectionListener")
            {

            }

            public void onPlaneDetected(AndroidJavaObject planeData)
            {
                if (planeData != null)
                {
                    NXRPlaneData nxrPlaneData = new NXRPlaneData(planeData);
                    if (Instance.PlaneDetectionEvent != null)
                    {
                        Instance.PlaneDetectionEvent(nxrPlaneData);
                    }
                }
                else
                {
                    Debug.LogError("AndroidJavaObject planeData is null !!!");
                }
            }

        }
#endregion

        public NxrSlamPlane GeneratePlaneObject(NXRPlaneData planeData)
        {
            // Debug.Log("GeneratePlaneObject:" + planeData.id);
            GameObject obj = new GameObject();
            obj.name = "NxrSlamPlane " + planeData.id;
            obj.AddComponent<MeshCollider>();
            MeshRenderer meshRenderer = obj.AddComponent<MeshRenderer>();
            meshRenderer.material = Resources.Load("PlaneMaterial", typeof(Material)) as Material;
            obj.AddComponent<MeshFilter>();
            NxrSlamPlane plnScript = obj.AddComponent<NxrSlamPlane>();
            plnScript.MakePlane(planeData);
            return plnScript;
        }

        /// <summary>
        ///  UYVY=>YUV422
        ///  [U0 Y0 V0 Y1] [U1 Y2 V1 Y3] [U2 Y4 V2 Y5]
        ///  Y = w * h, u = w/2 *h, v = w/2 *h
        ///  2bytes=16bits = 8 + 4 + 4
        ///  
        /// YUV420
        /// Y = w * h, u = w/2 *h/2, v = w/2 *h/2
        /// 1.5bytes=12bits = 8 + 2 + 2
        /// 
        /// </summary>
        /// <param name="buff"></param>
        /// <param name="videoW"></param>
        /// <param name="videoH"></param>
        /// <param name="bufY"></param>
        /// <param name="bufU"></param>
        /// <param name="bufV"></param>
        /// <param name="codec"></param>
        public void LoadYUV(byte[] buff, int videoW, int videoH, ref byte[] bufY, ref byte[] bufU, ref byte[] bufV, Codec codec = Codec.YUV420p)
        {
            float pixelBytes = codec == Codec.YUV420p ? 1.5f : 2.0f;

            int firstFrameEndIndex = (int)(videoH * videoW * pixelBytes);
        
            int yIndex = firstFrameEndIndex * 8 / 12;
            int uIndex = firstFrameEndIndex * 10 / 12;
            // int vIndex = firstFrameEndIndex;

            if(codec == Codec.YUV420p)
            {
                bufY = new byte[videoW * videoH];
                bufU = new byte[videoW * videoH >> 2];
                bufV = new byte[videoW * videoH >> 2];


                for (int i = 0; i < firstFrameEndIndex; i++)
                {
                    if (i < yIndex)
                    {
                        bufY[i] = buff[i];
                    }
                    else if (i < uIndex)
                    {
                        bufU[i - yIndex] = buff[i];
                    }
                    else
                    {
                        bufV[i - uIndex] = buff[i];
                    }
                }

            } 
            else if(codec == Codec.YUYV)
            {
                bufY = new byte[videoW * videoH];
                bufU = new byte[videoW/2 * videoH];
                bufV = new byte[videoW/2 * videoH];

                int uTmpIndex = 0;
                int vTmpIndex = 0;
                bool isUFind = false;
                for (int i = 0; i < firstFrameEndIndex; i++)
                {
                    if (i % 2 == 1)
                    {
                        // Y
                        bufY[i / 2] = buff[i];
                    }
                    else
                    {
                        // U / V
                        if (!isUFind)
                        {
                            bufU[uTmpIndex] = buff[i];
                            isUFind = true;
                            uTmpIndex++;
                        }
                        else
                        {
                            bufV[vTmpIndex] = buff[i];
                            isUFind = false;
                            vTmpIndex++;
                        }
                    }
                }
            }
            // byte[] bufUV = new byte[videoW * videoH >> 1];


            //如果是把UV分量一起写入到一张RGBA4444的纹理中时，byte[]
            //里的字节顺序应该是  UVUVUVUV....
            //这样在shader中纹理采样的结果 U 分量就存在r、g通道。
            //V 分量就存在b、a通道。

            //for(int i = 0; i < bufUV.Length; i+=2)
            //{
            //    bufUV[i] = bufU[i >> 1];
            //    bufUV[i + 1] = bufV[i >> 1];
            //}

        }

        public Color Int_To_RGB(int color)
        {
            int b = color / (256 * 256);
            int g = (color - b * 256 * 256) / 256;
            int r = color - b * 256 * 256 - g * 256;
            Color outcolor = Color.white;
            outcolor.r = r;
            outcolor.g = g;
            outcolor.b = b;
            return outcolor;
        }

        public void YUV420P_TO_RGB24(int width, int height, byte[] dataY, byte[] dataU, byte[] dataV, byte[] dataRGB24)
        {
            int index_y=0, index_u = 0, index_v = 0;
            int index_r=0, index_g = 0, index_b = 0;
            for(int i=0; i< height; i++)
            {
                for(int j=0; j<width; j++)
                {
                    index_r = (i * width + j) * 3;
                    index_g = index_r + 1;
                    index_b = index_r + 2;

                    index_y = i * width + j;
                    index_u = index_y / 4;
                    index_v = index_y / 4;

                    byte y = dataY[index_y];
                    byte u = dataU[index_u];
                    byte v = dataV[index_v];

                    float yF = y / 255.0f;
                    float uF = u / 255.0f;
                    float vF =  v / 255.0f;

                    byte r = (byte)((yF + 1.4022f * vF - 0.7011f) * 255);
                    byte g = (byte)((yF + 0.3456f * uF - 0.7145f * vF + 0.53005f) * 255);
                    byte b = (byte)((yF + 1.771f * uF - 0.8855f) * 255);

                    dataRGB24[index_r] = r;
                    dataRGB24[index_g] = g;
                    dataRGB24[index_b] = b;
                }
            }
        }
    }
}

/*
 if(RgbCamTexture2D_RGB == null && data.rgbCodecFormat == NxrSlam.Codec.YUV420p)
                {
                    RgbCamTexture2D_RGB_Data = new byte[data.width * data.height * 3];
                    RgbCamTexture2D_RGB = new Texture2D(data.width, data.height, TextureFormat.RGB24, false);
                    RgbCameraPreviewII.material.SetTexture("_MainTex", RgbCamTexture2D_RGB);
                }

                NxrSlam.Instance.YUV420P_TO_RGB24(data.width, data.height, dataY, dataU, dataV, RgbCamTexture2D_RGB_Data);
                RgbCamTexture2D_RGB.LoadRawTextureData(RgbCamTexture2D_RGB_Data);
                RgbCamTexture2D_RGB.Apply();
*/