/*
 * Copyright (C) 2012-2018 CypherCore <http://github.com/CypherCore>
 * 
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

using Framework.GameMath;
using System;
using System.Collections.Generic;
using System.IO;
using System.Linq;
using System.Runtime.InteropServices;

namespace Game.Collision
{
    public class BIH
    {
        public BIH()
        {
            init_empty();
        }

        void init_empty()
        {
            tree= new uint[3];
            objects = new uint[0];
            // create space for the first node
            tree[0] = (3u << 30); // dummy leaf
        }

        void buildHierarchy(List<uint> tempTree, buildData dat, BuildStats stats)
        {
            // create space for the first node
            tempTree.Add(3u << 30); // dummy leaf
            tempTree.Add(0);
            tempTree.Add(0);

            // seed bbox
            AABound gridBox = new AABound();
            gridBox.lo = bounds.Lo;
            gridBox.hi = bounds.Hi;
            AABound nodeBox = gridBox;
            // seed subdivide function
            subdivide(0, (int)(dat.numPrims - 1), tempTree, dat, gridBox, nodeBox, 0, 1, stats);
        }

        void subdivide(int left, int right, List<uint> tempTree, buildData dat, AABound gridBox, AABound nodeBox, int nodeIndex, int depth, BuildStats stats)
        {
            if ((right - left + 1) <= dat.maxPrims || depth >= 64)
            {
                // write leaf node
                stats.updateLeaf(depth, right - left + 1);
                createNode(tempTree, nodeIndex, left, right);
                return;
            }
            // calculate extents
            int axis = -1, prevAxis, rightOrig;
            float clipL = float.NaN, clipR = float.NaN, prevClip = float.NaN;
            float split = float.NaN, prevSplit;
            bool wasLeft = true;
            while (true)
            {
                prevAxis = axis;
                prevSplit = split;
                // perform quick consistency checks
                Vector3 d = gridBox.hi - gridBox.lo;
                for (int i = 0; i < 3; i++)
                {
                    if (nodeBox.hi[i] < gridBox.lo[i] || nodeBox.lo[i] > gridBox.hi[i])
                        Log.outError(LogFilter.Server, "Reached tree area in error - discarding node with: {0} objects", right - left + 1);
                }
                // find longest axis
                axis = (int)d.primaryAxis();
                split = 0.5f * (gridBox.lo[axis] + gridBox.hi[axis]);
                // partition L/R subsets
                clipL = float.NegativeInfinity;
                clipR = float.PositiveInfinity;
                rightOrig = right; // save this for later
                float nodeL = float.PositiveInfinity;
                float nodeR = float.NegativeInfinity;
                for (int i = left; i <= right; )
                {
                    int obj = (int)dat.indices[i];
                    float minb = dat.primBound[obj].Lo[axis];
                    float maxb = dat.primBound[obj].Hi[axis];
                    float center = (minb + maxb) * 0.5f;
                    if (center <= split)
                    {
                        // stay left
                        i++;
                        if (clipL < maxb)
                            clipL = maxb;
                    }
                    else
                    {
                        // move to the right most
                        int t = (int)dat.indices[i];
                        dat.indices[i] = dat.indices[right];
                        dat.indices[right] = (uint)t;
                        right--;
                        if (clipR > minb)
                            clipR = minb;
                    }
                    nodeL = Math.Min(nodeL, minb);
                    nodeR = Math.Max(nodeR, maxb);
                }
                // check for empty space
                if (nodeL > nodeBox.lo[axis] && nodeR < nodeBox.hi[axis])
                {
                    float nodeBoxW = nodeBox.hi[axis] - nodeBox.lo[axis];
                    float nodeNewW = nodeR - nodeL;
                    // node box is too big compare to space occupied by primitives?
                    if (1.3f * nodeNewW < nodeBoxW)
                    {
                        stats.updateBVH2();
                        int nextIndex1 = tempTree.Count();
                        // allocate child
                        tempTree.Add(0);
                        tempTree.Add(0);
                        tempTree.Add(0);
                        // write bvh2 clip node
                        stats.updateInner();
                        tempTree[nodeIndex + 0] = (uint)((axis << 30) | (1 << 29) | nextIndex1);
                        tempTree[nodeIndex + 1] = floatToRawIntBits(nodeL);
                        tempTree[nodeIndex + 2] = floatToRawIntBits(nodeR);
                        // update nodebox and recurse
                        nodeBox.lo[axis] = nodeL;
                        nodeBox.hi[axis] = nodeR;
                        subdivide(left, rightOrig, tempTree, dat, gridBox, nodeBox, nextIndex1, depth + 1, stats);
                        return;
                    }
                }
                // ensure we are making progress in the subdivision
                if (right == rightOrig)
                {
                    // all left
                    if (prevAxis == axis && MathFunctions.fuzzyEq(prevSplit, split))
                    {
                        // we are stuck here - create a leaf
                        stats.updateLeaf(depth, right - left + 1);
                        createNode(tempTree, nodeIndex, left, right);
                        return;
                    }
                    if (clipL <= split)
                    {
                        // keep looping on left half
                        gridBox.hi[axis] = split;
                        prevClip = clipL;
                        wasLeft = true;
                        continue;
                    }
                    gridBox.hi[axis] = split;
                    prevClip = float.NaN;
                }
                else if (left > right)
                {
                    // all right
                    right = rightOrig;
                    if (prevAxis == axis && MathFunctions.fuzzyEq(prevSplit, split))
                    {
                        // we are stuck here - create a leaf
                        stats.updateLeaf(depth, right - left + 1);
                        createNode(tempTree, nodeIndex, left, right);
                        return;
                    }

                    if (clipR >= split)
                    {
                        // keep looping on right half
                        gridBox.lo[axis] = split;
                        prevClip = clipR;
                        wasLeft = false;
                        continue;
                    }
                    gridBox.lo[axis] = split;
                    prevClip = float.NaN;
                }
                else
                {
                    // we are actually splitting stuff
                    if (prevAxis != -1 && !float.IsNaN(prevClip))
                    {
                        // second time through - lets create the previous split
                        // since it produced empty space
                        int nextIndex0 = tempTree.Count;
                        // allocate child node
                        tempTree.Add(0);
                        tempTree.Add(0);
                        tempTree.Add(0);
                        if (wasLeft)
                        {
                            // create a node with a left child
                            // write leaf node
                            stats.updateInner();
                            tempTree[nodeIndex + 0] = (uint)((prevAxis << 30) | nextIndex0);
                            tempTree[nodeIndex + 1] = floatToRawIntBits(prevClip);
                            tempTree[nodeIndex + 2] = floatToRawIntBits(float.PositiveInfinity);
                        }
                        else
                        {
                            // create a node with a right child
                            // write leaf node
                            stats.updateInner();
                            tempTree[nodeIndex + 0] = (uint)((prevAxis << 30) | (nextIndex0 - 3));
                            tempTree[nodeIndex + 1] = floatToRawIntBits(float.NegativeInfinity);
                            tempTree[nodeIndex + 2] = floatToRawIntBits(prevClip);
                        }
                        // count stats for the unused leaf
                        depth++;
                        stats.updateLeaf(depth, 0);
                        // now we keep going as we are, with a new nodeIndex:
                        nodeIndex = nextIndex0;
                    }
                    break;
                }
            }
            // compute index of child nodes
            int nextIndex = tempTree.Count;
            // allocate left node
            int nl = right - left + 1;
            int nr = rightOrig - (right + 1) + 1;
            if (nl > 0)
            {
                tempTree.Add(0);
                tempTree.Add(0);
                tempTree.Add(0);
            }
            else
                nextIndex -= 3;
            // allocate right node
            if (nr > 0)
            {
                tempTree.Add(0);
                tempTree.Add(0);
                tempTree.Add(0);
            }
            // write leaf node
            stats.updateInner();
            tempTree[nodeIndex + 0] = (uint)((axis << 30) | nextIndex);
            tempTree[nodeIndex + 1] = floatToRawIntBits(clipL);
            tempTree[nodeIndex + 2] = floatToRawIntBits(clipR);
            // prepare L/R child boxes
            AABound gridBoxL = gridBox;
            AABound gridBoxR = gridBox;
            AABound nodeBoxL = nodeBox;
            AABound nodeBoxR = nodeBox;
            gridBoxL.hi[axis] = gridBoxR.lo[axis] = split;
            nodeBoxL.hi[axis] = clipL;
            nodeBoxR.lo[axis] = clipR;
            // recurse
            if (nl > 0)
                subdivide(left, right, tempTree, dat, gridBoxL, nodeBoxL, nextIndex, depth + 1, stats);
            else
                stats.updateLeaf(depth + 1, 0);
            if (nr > 0)
                subdivide(right + 1, rightOrig, tempTree, dat, gridBoxR, nodeBoxR, nextIndex + 3, depth + 1, stats);
            else
                stats.updateLeaf(depth + 1, 0);
        }

        public bool readFromFile(BinaryReader reader)
        {
            var lo = reader.Read<Vector3>();
            var hi = reader.Read<Vector3>();
            bounds = new AxisAlignedBox(lo, hi);

            uint treeSize = reader.ReadUInt32();
            tree = new uint[treeSize];
            for (var i = 0; i < treeSize; i++)
                tree[i] = reader.ReadUInt32();

            var count = reader.ReadUInt32();
            objects = new uint[count];
            for (var i = 0; i < count; i++)
                objects[i] = reader.ReadUInt32();

            return true;
        }

        public void build<T>(List<T> primitives, uint leafSize = 3, bool printStats = false) where T : IModel
        {
            if (primitives.Count == 0)
            {
                init_empty();
                return;
            }

            buildData dat;
            dat.maxPrims = (int)leafSize;
            dat.numPrims = (uint)primitives.Count;
            dat.indices = new uint[dat.numPrims];
            dat.primBound = new AxisAlignedBox[dat.numPrims];
            bounds = primitives[0].getBounds();
            for (int i = 0; i < dat.numPrims; ++i)
            {
                dat.indices[i] = (uint)i;
                dat.primBound[i] = primitives[i].getBounds();
                bounds.merge(dat.primBound[i]);
            }
            List<uint> tempTree = new List<uint>();
            BuildStats stats = new BuildStats();
            buildHierarchy(tempTree, dat, stats);

            objects = new uint[dat.numPrims];
            for (int i = 0; i < dat.numPrims; ++i)
                objects[i] = dat.indices[i];

            tree = tempTree.ToArray();
        }

        public uint primCount() { return (uint)objects.Length; }

        public void intersectRay(Ray r, WorkerCallback intersectCallback, ref float maxDist, bool stopAtFirst = false)
        {
            float intervalMin = -1.0f;
            float intervalMax = -1.0f;
            Vector3 org = r.Origin;
            Vector3 dir = r.Direction;
            Vector3 invDir = new Vector3();
            for (int i = 0; i < 3; ++i)
            {
                invDir[i] = 1.0f / dir[i];
                if (MathFunctions.fuzzyNe(dir[i], 0.0f))
                {
                    float t1 = (bounds.Lo[i] - org[i]) * invDir[i];
                    float t2 = (bounds.Hi[i] - org[i]) * invDir[i];
                    if (t1 > t2)
                        MathFunctions.Swap<float>(ref t1, ref t2);
                    if (t1 > intervalMin)
                        intervalMin = t1;
                    if (t2 < intervalMax || intervalMax < 0.0f)
                        intervalMax = t2;
                    // intervalMax can only become smaller for other axis,
                    //  and intervalMin only larger respectively, so stop early
                    if (intervalMax <= 0 || intervalMin >= maxDist)
                        return;
                }
            }

            if (intervalMin > intervalMax)
                return;
            intervalMin = Math.Max(intervalMin, 0.0f);
            intervalMax = Math.Min(intervalMax, maxDist);

            uint[] offsetFront = new uint[3];
            uint[] offsetBack = new uint[3];
            uint[] offsetFront3 = new uint[3];
            uint[] offsetBack3 = new uint[3];
            // compute custom offsets from direction sign bit

            for (int i = 0; i < 3; ++i)
            {
                offsetFront[i] = floatToRawIntBits(dir[i]) >> 31;
                offsetBack[i] = offsetFront[i] ^ 1;
                offsetFront3[i] = offsetFront[i] * 3;
                offsetBack3[i] = offsetBack[i] * 3;

                // avoid always adding 1 during the inner loop
                ++offsetFront[i];
                ++offsetBack[i];
            }

            StackNode[] stack = new StackNode[64];
            int stackPos = 0;
            int node = 0;

            while (true)
            {
                while (true)
                {
                    uint tn = tree[node];
                    uint axis = (uint)(tn & (3 << 30)) >> 30;
                    bool BVH2 = Convert.ToBoolean(tn & (1 << 29));
                    int offset = (int)(tn & ~(7 << 29));
                    if (!BVH2)
                    {
                        if (axis < 3)
                        {
                            // "normal" interior node
                            float tf = (intBitsToFloat(tree[(int)(node + offsetFront[axis])]) - org[axis]) * invDir[axis];
                            float tb = (intBitsToFloat(tree[(int)(node + offsetBack[axis])]) - org[axis]) * invDir[axis];
                            // ray passes between clip zones
                            if (tf < intervalMin && tb > intervalMax)
                                break;
                            int back = (int)(offset + offsetBack3[axis]);
                            node = back;
                            // ray passes through far node only
                            if (tf < intervalMin)
                            {
                                intervalMin = (tb >= intervalMin) ? tb : intervalMin;
                                continue;
                            }
                            node = offset + (int)offsetFront3[axis]; // front
                            // ray passes through near node only
                            if (tb > intervalMax)
                            {
                                intervalMax = (tf <= intervalMax) ? tf : intervalMax;
                                continue;
                            }
                            // ray passes through both nodes
                            // push back node
                            stack[stackPos].node = (uint)back;
                            stack[stackPos].tnear = (tb >= intervalMin) ? tb : intervalMin;
                            stack[stackPos].tfar = intervalMax;
                            stackPos++;
                            // update ray interval for front node
                            intervalMax = (tf <= intervalMax) ? tf : intervalMax;
                            continue;
                        }
                        else
                        {
                            // leaf - test some objects
                            int n = (int)tree[node + 1];
                            while (n > 0)
                            {
                                bool hit = intersectCallback.Invoke(r, objects[offset], ref maxDist, stopAtFirst);
                                if (stopAtFirst && hit) 
                                    return;
                                --n;
                                ++offset;
                            }
                            break;
                        }
                    }
                    else
                    {
                        if (axis > 2)
                            return; // should not happen
                        float tf = (intBitsToFloat(tree[(int)(node + offsetFront[axis])]) - org[axis]) * invDir[axis];
                        float tb = (intBitsToFloat(tree[(int)(node + offsetBack[axis])]) - org[axis]) * invDir[axis];
                        node = offset;
                        intervalMin = (tf >= intervalMin) ? tf : intervalMin;
                        intervalMax = (tb <= intervalMax) ? tb : intervalMax;
                        if (intervalMin > intervalMax)
                            break;
                        continue;
                    }
                } // traversal loop
                do
                {
                    // stack is empty?
                    if (stackPos == 0)
                        return;
                    // move back up the stack
                    stackPos--;
                    intervalMin = stack[stackPos].tnear;
                    if (maxDist < intervalMin)
                        continue;
                    node = (int)stack[stackPos].node;
                    intervalMax = stack[stackPos].tfar;
                    break;
                } while (true);
            }
        }

        public void intersectPoint(Vector3 p, WorkerCallback intersectCallback)
        {
            if (!bounds.contains(p))
                return;

            StackNode[] stack = new StackNode[64];
            int stackPos = 0;
            int node = 0;

            while (true)
            {
                while (true)
                {
                    uint tn = tree[node];
                    uint axis = (uint)(tn & (3 << 30)) >> 30;
                    bool BVH2 = Convert.ToBoolean(tn & (1 << 29));
                    int offset = (int)(tn & ~(7 << 29));
                    if (!BVH2)
                    {
                        if (axis < 3)
                        {
                            // "normal" interior node
                            float tl = intBitsToFloat(tree[node + 1]);
                            float tr = intBitsToFloat(tree[node + 2]);
                            // point is between clip zones
                            if (tl < p[(int)axis] && tr > p[axis])
                                break;
                            int right = offset + 3;
                            node = right;
                            // point is in right node only
                            if (tl < p[(int)axis])
                            {
                                continue;
                            }
                            node = offset; // left
                            // point is in left node only
                            if (tr > p[axis])
                            {
                                continue;
                            }
                            // point is in both nodes
                            // push back right node
                            stack[stackPos].node = (uint)right;
                            stackPos++;
                            continue;
                        }
                        else
                        {
                            // leaf - test some objects
                            uint n = tree[node + 1];
                            while (n > 0)
                            {
                                intersectCallback.Invoke(p, objects[offset]); // !!!
                                --n;
                                ++offset;
                            }
                            break;
                        }
                    }
                    else // BVH2 node (empty space cut off left and right)
                    {
                        if (axis > 2)
                            return; // should not happen
                        float tl = intBitsToFloat(tree[node + 1]);
                        float tr = intBitsToFloat(tree[node + 2]);
                        node = offset;
                        if (tl > p[axis] || tr < p[axis])
                            break;
                        continue;
                    }
                } // traversal loop

                // stack is empty?
                if (stackPos == 0)
                    return;
                // move back up the stack
                stackPos--;
                node = (int)stack[stackPos].node;
            }
        }

        void createNode(List<uint> tempTree, int nodeIndex, int left, int right)
        {
            // write leaf node
            tempTree[nodeIndex + 0] = (uint)((3 << 30) | left);
            tempTree[nodeIndex + 1] = (uint)(right - left + 1);
        }

        struct buildData
        {
            public uint[] indices;
            public AxisAlignedBox[] primBound;
            public uint numPrims;
            public int maxPrims;
        }
        struct StackNode
        {
            public uint node;
            public float tnear;
            public float tfar;
        }
        public class BuildStats
        {
            public int numNodes;
            public int numLeaves;
            public int sumObjects;
            public int minObjects;
            public int maxObjects;
            public int sumDepth;
            public int minDepth;
            public int maxDepth;
            int[] numLeavesN = new int[6];
            int numBVH2;

            public BuildStats()
            {
                numNodes = 0;
                numLeaves = 0;
                sumObjects = 0;
                minObjects = 0x0FFFFFFF;
                maxObjects = -1;
                sumDepth = 0;
                minDepth = 0x0FFFFFFF;
                maxDepth = -1;
                numBVH2 = 0;

                for (int i = 0; i < 6; ++i)
                    numLeavesN[i] = 0;
            }

            public void updateInner() { numNodes++; }
            public void updateBVH2() { numBVH2++; }
            public void updateLeaf(int depth, int n)
            {
                numLeaves++;
                minDepth = Math.Min(depth, minDepth);
                maxDepth = Math.Max(depth, maxDepth);
                sumDepth += depth;
                minObjects = Math.Min(n, minObjects);
                maxObjects = Math.Max(n, maxObjects);
                sumObjects += n;
                int nl = Math.Min(n, 5);
                ++numLeavesN[nl];
            }
        }


        AxisAlignedBox bounds;
        uint[] tree;
        uint[] objects;

        [StructLayout(LayoutKind.Explicit)]
        public struct FloatToIntConverter
        {
            [FieldOffset(0)]
            public uint IntValue;
            [FieldOffset(0)]
            public float FloatValue;
        }

        uint floatToRawIntBits(float f)
        {
            FloatToIntConverter converter = new FloatToIntConverter();
            converter.FloatValue = f;
            return converter.IntValue;
        }
        float intBitsToFloat(uint i)
        {
            FloatToIntConverter converter = new FloatToIntConverter();
            converter.IntValue = i;
            return converter.FloatValue;
        }

    }
    public struct AABound
    {
        public Vector3 lo, hi;
    }
}