using System;
using System.Collections.Generic;
using System.Linq;
using Unity.VisualScripting;
using UnityEngine;
using UnityEngine.Profiling;

namespace RayTracer
{
    public class BVH
    {
        public enum SplitMode
        {
            EQUAL_COUNTS = 0,
            MIDDLE,
            SAH
        };

        public class PrimitiveInfo
        {
            public Vector3 centroid;
            public AABB bounds;
            public int primitiveID;

            public PrimitiveInfo(int id, AABB bbox)
            {
                primitiveID = id;
                bounds = bbox;
                centroid = bounds.min * 0.5f + bounds.max * 0.5f;
            }
        };

        public class BuildNode
        {
            public AABB bounds;
            public BuildNode[] children;
            public int splitAxis;
            public int primitiveOffset;
            public int primitiveCount;

            public BuildNode()
            {
                bounds = new AABB();
                children = new BuildNode[2];
            }

            public void InitLeaf(int first, int n, AABB bbox)
            {
                primitiveOffset = first;
                primitiveCount = n;
                bounds = bbox;
                children = new BuildNode[2];
                children[0] = null;
                children[1] = null;
            }

            public void InitInterior(int axis, BuildNode child0, BuildNode child1)
            {
                children = new BuildNode[2];
                children[0] = child0;
                children[1] = child1;
                bounds.Union(children[0].bounds);
                bounds.Union(children[1].bounds);
                splitAxis = axis;
                primitiveCount = 0;
            }
        };

        public struct LinearBVHNode
        {
            public Vector4 l_xy;
            public Vector4 r_xy;
            public Vector4 lr_z;
            public int offset;
            public int primitiveCount;
            public int axis;
            public int pad;
        };

        public class BucketInfo
        {
            public AABB bounds;
            public int count;

            public BucketInfo()
            {
                bounds = new AABB();
                count = 0;
            }
        };

        Triangle[] triangles;

        SplitMode splitMode = SplitMode.SAH;

        int maxDepth = 32;

        int maxPrimitivesInNode = 32;
        List<int> primitiveIndices;

        int bucketsCount = 16;
        List<BucketInfo> buckets;
        List<float> sahCost;

        AABB bvhBounds;

        BuildNode root;

        List<LinearBVHNode> nodes;

        public List<LinearBVHNode> GetNodes()
        {
            return nodes;
        }

        public List<int> GetPrimitiveIndexes()
        {
            return primitiveIndices;
        }

        int FindIfNot<T>(List<T> data, int first, int last, Func<T, bool> predicate)
        {
            int i = first;
            for (; i != last; i++)
            {
                if (!predicate(data[i]))
                {
                    return i;
                }
            }

            return last;
        }

        int Partition<T>(List<T> data, int first, int last, Func<T, bool> predicate)
        {
            int curr = FindIfNot<T>(data, first, last, predicate);

            if (curr == last)
            {
                return curr;
            }

            for (int i = curr++; i != last; i++)
            {
                if (predicate(data[i]))
                {
                    T tmp = data[i];
                    data[i] = data[curr];
                    data[curr] = tmp;

                    curr++;
                }
            }

            return curr;
        }

        BuildNode RecursiveBuild(List<PrimitiveInfo> info, int begin, int end, ref int totalNodes, int depth)
        {
            Profiler.BeginSample("BVH - Setup Node Bounds");

            // Allocate node
            BuildNode node = new BuildNode();
            node.children[0] = null;
            node.children[1] = null;

            // Increment node counter
            totalNodes++;

            // Calculate node bounds
            AABB bounds = new AABB();
            for (int i = begin; i < end; i++)
            {
                bounds.max.x = Mathf.Max(bounds.max.x, info[i].bounds.max.x);
                bounds.max.y = Mathf.Max(bounds.max.y, info[i].bounds.max.y);
                bounds.max.z = Mathf.Max(bounds.max.z, info[i].bounds.max.z);
                bounds.min.x = Mathf.Min(bounds.min.x, info[i].bounds.min.x);
                bounds.min.y = Mathf.Min(bounds.min.y, info[i].bounds.min.y);
                bounds.min.z = Mathf.Min(bounds.min.z, info[i].bounds.min.z);
            }

            Profiler.EndSample();

            // If there is less primitives than max prims in node count, create leaf node
            int primitives = end - begin;
            if ((primitives <= maxPrimitivesInNode) || (depth >= maxDepth))
            {
                Profiler.BeginSample("BVH - Build Leaf Node - Prims");

                int idx = primitiveIndices.Count;

                for (int i = 0; i < primitives; i++)
                {
                    primitiveIndices.Add(info[begin + i].primitiveID);
                }

                node.InitLeaf(idx, primitives, bounds);

                Profiler.EndSample();

                return node;
            }
            else
            {
                Profiler.BeginSample("BVH - Centroid Computation");

                // Compute bounds of primitive centroids
                AABB centroidBounds = new AABB();
                for (int i = begin; i < end; i++)
                {
                    centroidBounds.max.x = Mathf.Max(centroidBounds.max.x, info[i].centroid.x);
                    centroidBounds.max.y = Mathf.Max(centroidBounds.max.y, info[i].centroid.y);
                    centroidBounds.max.z = Mathf.Max(centroidBounds.max.z, info[i].centroid.z);
                    centroidBounds.min.x = Mathf.Min(centroidBounds.min.x, info[i].centroid.x);
                    centroidBounds.min.y = Mathf.Min(centroidBounds.min.y, info[i].centroid.y);
                    centroidBounds.min.z = Mathf.Min(centroidBounds.min.z, info[i].centroid.z);
                }

                // Get splitting dimension
                int dim = centroidBounds.GetLongestAxis();

                Profiler.EndSample();

                // If we can't split by axis (degenerate triangles case - longest axis being 0), create leaf
                if (centroidBounds.min[dim] == centroidBounds.max[dim] || end - begin <= maxPrimitivesInNode)
                {
                    Profiler.BeginSample("BVH - Build Leaf Node - Conditions");

                    int idx = primitiveIndices.Count;

                    for (int i = 0; i < primitives; i++)
                    {
                        primitiveIndices.Add(info[begin + i].primitiveID);
                    }

                    node.InitLeaf(idx, primitives, bounds);

                    Profiler.EndSample();

                    return node;
                }
                // Otherwise we need to split
                else
                {
                    Profiler.BeginSample("BVH - Build Interior Node");

                    int mid = 0;

                    switch (splitMode)
                    {
                        case SplitMode.MIDDLE:
                            Profiler.BeginSample("BVH - Split Middle");
                            float pmid = (centroidBounds.min[dim] + centroidBounds.max[dim]) * 0.5f;
                            mid = Partition<PrimitiveInfo>(info, begin, end, (PrimitiveInfo pi) =>
                            {
                                return pi.centroid[dim] < pmid;
                            });
                            if (mid != begin && mid != end)
                            {
                                break;
                            }

                            mid = (begin + end) / 2;
                            info.Sort((PrimitiveInfo a, PrimitiveInfo b) =>
                            {
                                return (int)(a.centroid[dim] - b.centroid[dim]);
                            });
                            Profiler.EndSample();
                            break;

                        case SplitMode.EQUAL_COUNTS:
                            Profiler.BeginSample("BVH - Split Equal Counts");
                            mid = (begin + end) / 2;
                            info.Sort((PrimitiveInfo a, PrimitiveInfo b) =>
                            {
                                return (int)(a.centroid[dim] - b.centroid[dim]);
                            });
                            Profiler.EndSample();
                            break;

                        case SplitMode.SAH:
                            Profiler.BeginSample("BVH - Split SAH");
                            int bestAxis = 0;
                            int minBucket = 0;
                            float minCost = 999999.0f;

                            Profiler.BeginSample("BVH - Calculate SAH");
                            for (int axis = 0; axis < 3; axis++)
                            {
                                // Init buckets for SAH computation
                                for (int i = 0; i < bucketsCount; i++)
                                {
                                    buckets[i].bounds = new AABB();
                                    buckets[i].count = 0;
                                    sahCost[i] = 0.0f;
                                }

                                // Prepare buckets data by calculating their bounds
                                for (int i = begin; i < end; i++)
                                {
                                    int b = (int)((float)bucketsCount * ((info[i].centroid[axis] - centroidBounds.min[axis]) / (centroidBounds.max[axis] - centroidBounds.min[axis])));
                                    if (b >= bucketsCount)
                                    {
                                        b = bucketsCount - 1;
                                    }

                                    if (b < 0)
                                    {
                                        b = 0;
                                    }

                                    buckets[b].count++;

                                    buckets[b].bounds.max.x = Mathf.Max(buckets[b].bounds.max.x, info[i].bounds.max.x);
                                    buckets[b].bounds.max.y = Mathf.Max(buckets[b].bounds.max.y, info[i].bounds.max.y);
                                    buckets[b].bounds.max.z = Mathf.Max(buckets[b].bounds.max.z, info[i].bounds.max.z);
                                    buckets[b].bounds.min.x = Mathf.Min(buckets[b].bounds.min.x, info[i].bounds.min.x);
                                    buckets[b].bounds.min.y = Mathf.Min(buckets[b].bounds.min.y, info[i].bounds.min.y);
                                    buckets[b].bounds.min.z = Mathf.Min(buckets[b].bounds.min.z, info[i].bounds.min.z);
                                }

                                // Calculate costs for splitting after each bucket
                                for (int i = 0; i < bucketsCount - 1; i++)
                                {
                                    AABB b0 = new AABB();
                                    AABB b1 = new AABB();

                                    int count0 = 0;
                                    int count1 = 0;

                                    for (int j = 0; j <= i; j++)
                                    {
                                        b0.max.x = Mathf.Max(b0.max.x, buckets[j].bounds.max.x);
                                        b0.max.y = Mathf.Max(b0.max.y, buckets[j].bounds.max.y);
                                        b0.max.z = Mathf.Max(b0.max.z, buckets[j].bounds.max.z);
                                        b0.min.x = Mathf.Min(b0.min.x, buckets[j].bounds.min.x);
                                        b0.min.y = Mathf.Min(b0.min.y, buckets[j].bounds.min.y);
                                        b0.min.z = Mathf.Min(b0.min.z, buckets[j].bounds.min.z);

                                        count0 += buckets[j].count;
                                    }

                                    for (int j = i + 1; j < bucketsCount; j++)
                                    {
                                        b1.max.x = Mathf.Max(b1.max.x, buckets[j].bounds.max.x);
                                        b1.max.y = Mathf.Max(b1.max.y, buckets[j].bounds.max.y);
                                        b1.max.z = Mathf.Max(b1.max.z, buckets[j].bounds.max.z);
                                        b1.min.x = Mathf.Min(b1.min.x, buckets[j].bounds.min.x);
                                        b1.min.y = Mathf.Min(b1.min.y, buckets[j].bounds.min.y);
                                        b1.min.z = Mathf.Min(b1.min.z, buckets[j].bounds.min.z);

                                        count1 += buckets[j].count;
                                    }

                                    sahCost[i] = 1.0f + (count0 * b0.GetSurfaceArea() + count1 * b1.GetSurfaceArea()) / bounds.GetSurfaceArea();
                                }

                                // Select best cost bucket
                                for (int i = 0; i < bucketsCount; i++)
                                {
                                    if (sahCost[i] < minCost)
                                    {
                                        minCost = sahCost[i];
                                        minBucket = i;
                                        bestAxis = axis;
                                    }
                                }
                            }

                            dim = bestAxis;
                            Profiler.EndSample();

                            // Create interior node
                            Profiler.BeginSample("BVH - Partition Interior Node");
                            mid = Partition<PrimitiveInfo>(info, begin, end, (PrimitiveInfo pi) =>
                            {
                                int b = (int)((float)bucketsCount * ((pi.centroid[dim] - centroidBounds.min[dim]) / (centroidBounds.max[dim] - centroidBounds.min[dim])));
                                if (b >= bucketsCount)
                                {
                                    b = bucketsCount - 1;
                                }

                                return b <= minBucket;
                            });
                            Profiler.EndSample();

                            Profiler.BeginSample("BVH - Sort Build Info");
                            if (begin == mid || mid == end)
                            {
                                mid = (begin + end) / 2;

                                if (dim == 0)
                                {
                                    info.OrderBy(pi => pi.centroid.x);
                                    //info.Sort((PrimitiveInfo a, PrimitiveInfo b) => { return (int)(a.centroid.x - b.centroid.x); });
                                }
                                else if (dim == 1)
                                {
                                    info.OrderBy(pi => pi.centroid.y);
                                    //info.Sort((PrimitiveInfo a, PrimitiveInfo b) => { return (int)(a.centroid.y - b.centroid.y); });
                                }
                                else
                                {
                                    info.OrderBy(pi => pi.centroid.z);
                                    //info.Sort((PrimitiveInfo a, PrimitiveInfo b) => { return (int)(a.centroid.z - b.centroid.z); });
                                }

                                /*info.Sort((PrimitiveInfo a, PrimitiveInfo b) =>
                                {
                                    return (int)(a.centroid[dim] - b.centroid[dim]);
                                });*/
                            }
                            Profiler.EndSample();

                            Profiler.EndSample();
                            break;
                    }

                    Profiler.EndSample();

                    // Split the node
                    node.InitInterior(dim,
                        RecursiveBuild(info, begin, mid, ref totalNodes, depth + 1),
                        RecursiveBuild(info, mid, end, ref totalNodes, depth + 1));

                    return node;

                    // Perform SAH based splitting, loop through axes and calculate SAH
                    /*int bestAxis = 0;
                    int minBucket = 0;
                    float minCost = 0.0f;

                    for (int axis = 0; axis < 3; axis++)
                    {
                        // Init buckets for SAH computation
                        for (int i = 0; i < bucketsCount; i++)
                        {
                            buckets[i].bounds = AABB.Identity();
                            buckets[i].count = 0;
                            sahCost[i] = 0.0f;
                        }

                        // Prepare buckets data by calculating their bounds
                        for (int i = begin; i < end; i++)
                        {
                            int b = (int)((float)bucketsCount * ((info[i].centroid[axis] - centroidBounds.min[axis]) / (centroidBounds.max[axis] - centroidBounds.min[axis])));
                            if (b >= bucketsCount)
                            {
                                b = bucketsCount - 1;
                            }

                            buckets[b].count++;
                            buckets[b].bounds.Union(info[i].bounds);
                        }

                        // Calculate costs for splitting after each bucket
                        for (int i = 0; i < bucketsCount - 1; i++)
                        {
                            AABB b0 = AABB.Identity();
                            AABB b1 = AABB.Identity();

                            int count0 = 0;
                            int count1 = 0;

                            for (int j = 0; j <= i; j++)
                            {
                                b0.Union(buckets[j].bounds);
                                count0 += buckets[j].count;
                            }

                            for (int j = i + 1; j < bucketsCount; j++)
                            {
                                b1.Union(buckets[j].bounds);
                                count1 += buckets[j].count;
                            }

                            sahCost[i] = 1.0f + (count0 * b0.GetSurfaceArea() + count1 * b1.GetSurfaceArea()) / bounds.GetSurfaceArea();
                        }

                        // Select best cost bucket
                        for (int i = 0; i < bucketsCount; i++)
                        {
                            if (sahCost[i] < minCost)
                            {
                                minCost = sahCost[i];
                                minBucket = i;
                                bestAxis = axis;
                            }
                        }
                    }

                    dim = bestAxis;

                    // Create interior node
                    List<PrimitiveInfo> partitioned = new List<PrimitiveInfo>(info);
                    int mid = Partition<PrimitiveInfo>(partitioned, 0, partitioned.Count, (PrimitiveInfo pi) =>
                    {
                        int b = (int)((float)bucketsCount * ((pi.centroid[dim] - centroidBounds.min[dim]) / (centroidBounds.max[dim] - centroidBounds.min[dim])));
                        if (b >= bucketsCount)
                        {
                            b = bucketsCount - 1;
                        }

                        return b <= minBucket;
                    });

                    if (begin == mid || mid == end)
                    {
                        mid = (begin + end) / 2;
                        partitioned.Sort((PrimitiveInfo a, PrimitiveInfo b) =>
                        {
                            return (a.centroid[dim] < b.centroid[dim] ? -1 : 1);
                        });
                    }

                    node.InitInterior(dim,
                        RecursiveBuild(partitioned, begin, mid, ref totalNodes),
                        RecursiveBuild(partitioned, mid, end, ref totalNodes));

                    return node;*/
                }
            }
        }
        private int FlattenBVH(BuildNode node, ref int offset)
        {
            int tempOffset = offset;
            LinearBVHNode n = nodes[tempOffset];
            offset++;

            if(node.primitiveCount > 0)
            {
                n.offset = node.primitiveOffset;
                n.primitiveCount = node.primitiveCount;

                nodes[tempOffset] = n;
            }
            else
            {
                n.l_xy = new Vector4(node.children[0].bounds.min.x,
                    node.children[0].bounds.max.x,
                    node.children[0].bounds.min.y,
                    node.children[0].bounds.max.y);

                n.r_xy = new Vector4(node.children[1].bounds.min.x,
                    node.children[1].bounds.max.x,
                    node.children[1].bounds.min.y,
                    node.children[1].bounds.max.y);

                n.lr_z = new Vector4(node.children[0].bounds.min.z,
                    node.children[0].bounds.max.z,
                    node.children[1].bounds.min.z,
                    node.children[1].bounds.max.z);

                n.axis = node.splitAxis;
                n.primitiveCount = 0;

                FlattenBVH(node.children[0], ref offset);
                n.offset = FlattenBVH(node.children[1], ref offset);

                nodes[tempOffset] = n;
            }

            return tempOffset;
        }

        public BVH(Triangle[] tris)
        {
            float begin = Time.realtimeSinceStartup;

            Profiler.BeginSample("BVH - Prepare Arrays");

            triangles = tris;

            primitiveIndices = new List<int>();

            buckets = new List<BucketInfo>();
            sahCost = new List<float>();
            for (int i = 0; i < bucketsCount; i++)
            {
                buckets.Add(new BucketInfo());
                sahCost.Add(0.0f);
            }

            List<PrimitiveInfo> info = new List<PrimitiveInfo>(triangles.Length);

            Profiler.EndSample();

            Profiler.BeginSample("BVH - Bounds");

            bvhBounds = new AABB();
            for (int i = 0; i < triangles.Length; i++)
            {
                AABB triBounds = new AABB();
                triBounds.max.x = Mathf.Max(triangles[i].a.x, Mathf.Max(triangles[i].b.x, triangles[i].c.x));
                triBounds.max.y = Mathf.Max(triangles[i].a.y, Mathf.Max(triangles[i].b.y, triangles[i].c.y));
                triBounds.max.z = Mathf.Max(triangles[i].a.z, Mathf.Max(triangles[i].b.z, triangles[i].c.z));
                triBounds.min.x = Mathf.Min(triangles[i].a.x, Mathf.Min(triangles[i].b.x, triangles[i].c.x));
                triBounds.min.y = Mathf.Min(triangles[i].a.y, Mathf.Min(triangles[i].b.y, triangles[i].c.y));
                triBounds.min.z = Mathf.Min(triangles[i].a.z, Mathf.Min(triangles[i].b.z, triangles[i].c.z));

                PrimitiveInfo pi = new PrimitiveInfo(i, triBounds);

                info.Add(pi);

                bvhBounds.max.x = Mathf.Max(bvhBounds.max.x, triBounds.max.x);
                bvhBounds.max.y = Mathf.Max(bvhBounds.max.y, triBounds.max.y);
                bvhBounds.max.z = Mathf.Max(bvhBounds.max.z, triBounds.max.z);
                bvhBounds.min.x = Mathf.Min(bvhBounds.min.x, triBounds.min.x);
                bvhBounds.min.y = Mathf.Min(bvhBounds.min.y, triBounds.min.y);
                bvhBounds.min.z = Mathf.Min(bvhBounds.min.z, triBounds.min.z);
            }

            Profiler.EndSample();

            int totalNodes = 0;
            root = RecursiveBuild(info, 0, info.Count, ref totalNodes, 0);

            nodes = new List<LinearBVHNode>();
            for (int i = 0; i < totalNodes; i++)
            {
                nodes.Add(new LinearBVHNode());
            }

            int offset = 0;
            FlattenBVH(root, ref offset);

            Debug.Log("Time elapsed: " + (Time.realtimeSinceStartup - begin) * 1000 + " ms\n" +
                "Statistics:\n" +
                "\ttTotal number of primitives: " + primitiveIndices.Count + "\n" +
                "\ttTotal number of nodes:" + totalNodes);
        }

        /*private Stack<BuildNode> traversalStack = new Stack<BuildNode>();*/
        private IntersectResult c0res = new IntersectResult();
        private IntersectResult c1res = new IntersectResult();
        private IntersectResult triRes = new IntersectResult();


        private int[] traversalStack = new int[32];

        public int IntersectRay(Ray r, ref IntersectResult result)
        {
            Profiler.BeginSample("Traversal - Begin");

            int hitIndex = -1;
            float t = result.distance;
            float baryU = 0.0f;
            float baryV = 0.0f;

            int[] stack = traversalStack;
            int stackPtr = 0;
            stack[stackPtr] = 0x7FFFFFFF;

            int nodeID = 0;

            float tmin = 0.0f;
            float tmax = 10000.0f;

            r.direction.Normalize();

            Vector4 inv = new Vector4(1.0f / r.direction.x, 1.0f / r.direction.y, 1.0f / r.direction.z, 1.0f);
            Vector4 oinv = new Vector4(r.origin.x * inv.x, r.origin.y * inv.y, r.origin.z * inv.z, 1.0f);

            Profiler.EndSample();

            Profiler.BeginSample("Traversal - Loop");

            do
            {
                int primCount = nodes[nodeID].primitiveCount;

                if (primCount == 0)
                {
                    Profiler.BeginSample("Traversal - Interior Node");

                    Vector4 n0xy = nodes[nodeID].l_xy;
                    Vector4 n1xy = nodes[nodeID].r_xy;
                    Vector4 nz = nodes[nodeID].lr_z;

                    // Test against child AABBs
                    AABB bb0 = new AABB(new Vector3(n0xy.x, n0xy.z, nz.x), new Vector3(n0xy.y, n0xy.w, nz.y));
                    AABB bb1 = new AABB(new Vector3(n1xy.x, n1xy.z, nz.z), new Vector3(n1xy.y, n1xy.w, nz.w));

                    c0res.distance = t;
                    bool traverseChild0 = Intersect.RayAABB(r, bb0, ref c0res);

                    c1res.distance = t;
                    bool traverseChild1 = Intersect.RayAABB(r, bb1, ref c1res);

                    /*float c0lox = n0xy.x * inv.x - oinv.x;
                    float c0hix = n0xy.y * inv.x - oinv.x;
                    float c0loy = n0xy.z * inv.y - oinv.y;
                    float c0hiy = n0xy.w * inv.y - oinv.y;
                    float c0loz = nz.x * inv.z - oinv.z;
                    float c0hiz = nz.y * inv.z - oinv.z;
                    float c1loz = nz.z * inv.z - oinv.z;
                    float c1hiz = nz.w * inv.z - oinv.z;
                    float c0min = Mathf.Max(Mathf.Max(Mathf.Min(c0lox, c0hix), Mathf.Min(c0loy, c0hiy)), Mathf.Max(Mathf.Min(c0loz, c0hiz), tmin));
                    float c0max = Mathf.Min(Mathf.Min(Mathf.Max(c0lox, c0hix), Mathf.Max(c0loy, c0hiy)), Mathf.Min(Mathf.Max(c0loz, c0hiz), tmax));
                    float c1lox = n1xy.x * inv.x - oinv.x;
                    float c1hix = n1xy.y * inv.x - oinv.x;
                    float c1loy = n1xy.z * inv.y - oinv.y;
                    float c1hiy = n1xy.w * inv.y - oinv.y;
                    float c1min = Mathf.Max(Mathf.Max(Mathf.Min(c1lox, c1hix), Mathf.Min(c1loy, c1hiy)), Mathf.Max(Mathf.Min(c1loz, c1hiz), tmin));
                    float c1max = Mathf.Min(Mathf.Min(Mathf.Max(c1lox, c1hix), Mathf.Max(c1loy, c1hiy)), Mathf.Min(Mathf.Max(c1loz, c1hiz), tmax));

                    bool traverseChild0 = (c0max >= c0min);
                    bool traverseChild1 = (c1max >= c1min);*/

                    float c0min = c0res.distance;
                    float c1min = c1res.distance;

                    // If no children was hit, get node from stack
                    if (!traverseChild0 && !traverseChild1)
                    {
                        nodeID = stack[stackPtr];
                        stackPtr--;
                    }
                    else
                    {
                        int first_child = nodeID + 1;
                        int second_child = nodes[nodeID].offset;
                        nodeID = (traverseChild0) ? first_child : second_child;

                        if (traverseChild0 && traverseChild1)
                        {
                            if (c1min < c0min)
                            {
                                nodeID = second_child;
                                stackPtr++;
                                stack[stackPtr] = first_child;
                            }
                            else
                            {
                                stackPtr++;
                                stack[stackPtr] = second_child;
                            }
                        }
                    }

                    Profiler.EndSample();
                }
                else
                {
                    Profiler.BeginSample("Traversal - Leaf Node");

                    int primOffset = nodes[nodeID].offset;
                    for (int i = 0; i < nodes[nodeID].primitiveCount; i++)
                    {
                        int triIndex = primitiveIndices[primOffset + i];

                        //IntersectResult triRes = new IntersectResult();
                        triRes.distance = t;

                        Profiler.BeginSample("Traversal - Ray-Triangle");

                        if (Intersect.RayTriangle(r, triangles[triIndex], ref triRes))
                        {
                            if (triRes.distance < t && triRes.distance > 0.0f)
                            {
                                hitIndex = primOffset + i;
                                t = triRes.distance;
                                baryU = triRes.baryU;
                                baryV = triRes.baryV;
                            }
                        }

                        Profiler.EndSample();
                    }

                    nodeID = stack[stackPtr];
                    stackPtr--;

                    Profiler.EndSample();
                }
            }
            while (nodeID != 0x7FFFFFFF);

            Profiler.EndSample();

            if (hitIndex >= 0)
            {
                result.distance = t;
                result.baryU = baryU;
                result.baryV = baryV;
                return hitIndex;
            }
            else
            {
                return -1;
            }

            /*Profiler.BeginSample("Traversal - Begin");

            //Stack<BuildNode> stack;
            //stack = new Stack<BuildNode>();
            Stack<BuildNode> stack = traversalStack;
            stack.Clear();
            stack.Push(root);

            int hitIndex = -1;
            float t = result.distance;
            float baryU = 0.0f;
            float baryV = 0.0f;

            Profiler.EndSample();

            Profiler.BeginSample("Traversal - Loop");

            while (stack.Count > 0)
            {
                BuildNode n = stack.Pop();

                if (n.primitiveCount == 0)
                {
                    Profiler.BeginSample("Traversal - Interior Node");

                    Profiler.BeginSample("Traversal - Ray-AABB");

                    //IntersectResult c0res = new IntersectResult();
                    c0res.distance = t;
                    bool c0hit = Intersect.RayAABB(r, n.children[0].bounds, ref c0res);

                    //IntersectResult c1res = new IntersectResult();
                    c1res.distance = t;
                    bool c1hit = Intersect.RayAABB(r, n.children[1].bounds, ref c1res);

                    Profiler.EndSample();

                    if (!c0hit && !c1hit)
                    {
                        Profiler.EndSample();

                        continue;
                    }
                    else
                    {
                        if (c0hit && c1hit)
                        {
                            if (c0res.distance < c1res.distance)
                            {
                                stack.Push(n.children[1]);
                                stack.Push(n.children[0]);
                            }
                            else
                            {
                                stack.Push(n.children[0]);
                                stack.Push(n.children[1]);
                            }
                        }
                        else
                        {
                            if (c0hit)
                            {
                                stack.Push(n.children[0]);
                            }
                            else
                            {
                                stack.Push(n.children[1]);
                            }
                        }
                    }

                    Profiler.EndSample();
                }
                else
                {
                    Profiler.BeginSample("Traversal - Leaf Node");

                    int primOffset = n.primitiveOffset;
                    for (int i = 0; i < n.primitiveCount; i++)
                    {
                        int triIndex = primitiveIndices[primOffset + i];

                        //IntersectResult triRes = new IntersectResult();
                        triRes.distance = t;

                        Profiler.BeginSample("Traversal - Ray-Triangle");

                        if (Intersect.RayTriangle(r, triangles[triIndex], ref triRes))
                        {
                            if (triRes.distance < t && triRes.distance > 0.0f)
                            {
                                hitIndex = primOffset + i;
                                t = triRes.distance;
                                baryU = triRes.baryU;
                                baryV = triRes.baryV;
                            }
                        }

                        Profiler.EndSample();
                    }

                    Profiler.EndSample();
                }
            }

            Profiler.EndSample();

            if (hitIndex >= 0)
            {
                result.distance = t;
                result.baryU = baryU;
                result.baryV = baryV;
                return hitIndex;
            }
            else
            {
                return -1;
            }*/
        }

        public List<int> GetIndices()
        {
            return primitiveIndices;
        }

        public BuildNode GetHelperBuildNode(int index)
        {
            Stack<BuildNode> stack = new Stack<BuildNode>();
            stack.Push(root);

            int steps = -1;

            while (stack.Count > 0)
            {
                steps++;
                BuildNode node = stack.Pop();

                if (index == steps)
                {
                    return node;
                }

                if (node.primitiveCount == 0)
                {
                    stack.Push(node.children[1]);
                    stack.Push(node.children[0]);
                }
            }

            return null;
        }
    }
}