#if (OBI_BURST && OBI_MATHEMATICS && OBI_COLLECTIONS)
using Unity.Collections;
using Unity.Mathematics;

namespace Obi
{
    public struct BurstHeightField : BurstLocalOptimization.IDistanceFunction, IBurstCollider
    {

        public BurstColliderShape shape;
        public BurstAffineTransform colliderToSolver;
        public BurstAffineTransform solverToWorld;
        public float dt;
        public float collisionMargin;

        public BurstMath.CachedTri tri;
        public float4 triNormal;

        public HeightFieldHeader header;
        public NativeArray<float> heightFieldSamples;

        public void Evaluate(float4 point, float4 radii, quaternion orientation, ref BurstLocalOptimization.SurfacePoint projectedPoint)
        {
            point = colliderToSolver.InverseTransformPoint(point);

            float4 nearestPoint = BurstMath.NearestPointOnTri(tri, point, out float4 bary);
            float4 normal = math.normalizesafe(point - nearestPoint);

            // flip the contact normal if it points below ground: (doesn't work with holes)
            //BurstMath.OneSidedNormal(triNormal, ref normal);

            projectedPoint.point = colliderToSolver.TransformPoint(nearestPoint + normal * shape.contactOffset);
            projectedPoint.normal = colliderToSolver.TransformDirection(normal);
        }

        public void Contacts(int colliderIndex,
                             int rigidbodyIndex,
                              NativeArray<BurstRigidbody> rigidbodies,

                              NativeArray<float4> positions,
                              NativeArray<quaternion> orientations,
                              NativeArray<float4> velocities,
                              NativeArray<float4> radii,

                              NativeArray<int> simplices,
                              in BurstAabb simplexBounds,
                              int simplexIndex,
                              int simplexStart,
                              int simplexSize,

                              NativeQueue<BurstContact>.ParallelWriter contacts,
                              int optimizationIterations,
                              float optimizationTolerance)
        {
            if (shape.dataIndex < 0) return;

            triNormal = float4.zero;

            var co = new BurstContact() { bodyA = simplexIndex, bodyB = colliderIndex };

            int resolutionU = (int)shape.center.x;
            int resolutionV = (int)shape.center.y;

            // calculate terrain cell size:
            float cellWidth = shape.size.x / (resolutionU - 1);
            float cellHeight = shape.size.z / (resolutionV - 1);

            // calculate particle bounds min/max cells:
            int2 min = new int2((int)math.floor(simplexBounds.min[0] / cellWidth), (int)math.floor(simplexBounds.min[2] / cellHeight));
            int2 max = new int2((int)math.floor(simplexBounds.max[0] / cellWidth), (int)math.floor(simplexBounds.max[2] / cellHeight));

            for (int su = min[0]; su <= max[0]; ++su)
            {
                if (su >= 0 && su < resolutionU - 1)
                {
                    for (int sv = min[1]; sv <= max[1]; ++sv)
                    {
                        if (sv >= 0 && sv < resolutionV - 1)
                        {
                            // calculate neighbor sample indices:
                            int csu1 = math.clamp(su + 1, 0, resolutionU - 1);
                            int csv1 = math.clamp(sv + 1, 0, resolutionV - 1);

                            // sample heights:
                            float h1 = heightFieldSamples[header.firstSample + sv * resolutionU + su] * shape.size.y;
                            float h2 = heightFieldSamples[header.firstSample + sv * resolutionU + csu1] * shape.size.y;
                            float h3 = heightFieldSamples[header.firstSample + csv1 * resolutionU + su] * shape.size.y;
                            float h4 = heightFieldSamples[header.firstSample + csv1 * resolutionU + csu1] * shape.size.y;

                            if (h1 < 0) continue;
                            h1 = math.abs(h1);
                            h2 = math.abs(h2);
                            h3 = math.abs(h3);
                            h4 = math.abs(h4);

                            float min_x = su * shape.size.x / (resolutionU - 1);
                            float max_x = csu1 * shape.size.x / (resolutionU - 1);
                            float min_z = sv * shape.size.z / (resolutionV - 1);
                            float max_z = csv1 * shape.size.z / (resolutionV - 1);

                            float4 convexPoint;
                            float4 simplexBary = BurstMath.BarycenterForSimplexOfSize(simplexSize);

                            // ------contact against the first triangle------:
                            float4 v1 = new float4(min_x, h3, max_z, 0);
                            float4 v2 = new float4(max_x, h4, max_z, 0);
                            float4 v3 = new float4(min_x, h1, min_z, 0);

                            tri.Cache(v1, v2, v3);
                            triNormal.xyz = math.normalizesafe(math.cross((v2 - v1).xyz, (v3 - v1).xyz));

                            var colliderPoint = BurstLocalOptimization.Optimize<BurstHeightField>(ref this, positions, orientations, radii, simplices, simplexStart, simplexSize,
                                                                                ref simplexBary, out convexPoint, optimizationIterations, optimizationTolerance);

                            float4 velocity = float4.zero;
                            float simplexRadius = 0;
                            for (int j = 0; j < simplexSize; ++j)
                            {
                                int particleIndex = simplices[simplexStart + j];
                                simplexRadius += radii[particleIndex].x * simplexBary[j];
                                velocity += velocities[particleIndex] * simplexBary[j];
                            }

                            float4 rbVelocity = float4.zero;
                            if (rigidbodyIndex >= 0)
                                rbVelocity = BurstMath.GetRigidbodyVelocityAtPoint(rigidbodyIndex, colliderPoint.point, rigidbodies, solverToWorld);

                            float dAB = math.dot(convexPoint - colliderPoint.point, colliderPoint.normal);
                            float vel = math.dot(velocity - rbVelocity, colliderPoint.normal);

                            if (vel * dt + dAB <= simplexRadius + shape.contactOffset + collisionMargin)
                            {
                                co.pointB = colliderPoint.point;
                                co.normal = colliderPoint.normal;
                                co.pointA = simplexBary;
                                contacts.Enqueue(co);
                            }

                            // ------contact against the second triangle------:
                            v1 = new float4(min_x, h1, min_z, 0);
                            v2 = new float4(max_x, h4, max_z, 0);
                            v3 = new float4(max_x, h2, min_z, 0);

                            tri.Cache(v1, v2, v3);
                            triNormal.xyz = math.normalizesafe(math.cross((v2 - v1).xyz, (v3 - v1).xyz));

                            colliderPoint = BurstLocalOptimization.Optimize<BurstHeightField>(ref this, positions, orientations, radii, simplices, simplexStart, simplexSize,
                                                                                ref simplexBary, out convexPoint, optimizationIterations, optimizationTolerance);

                            velocity = float4.zero;
                            simplexRadius = 0;
                            for (int j = 0; j < simplexSize; ++j)
                            {
                                int particleIndex = simplices[simplexStart + j];
                                simplexRadius += radii[particleIndex].x * simplexBary[j];
                                velocity += velocities[particleIndex] * simplexBary[j];
                            }

                            rbVelocity = float4.zero;
                            if (rigidbodyIndex >= 0)
                                rbVelocity = BurstMath.GetRigidbodyVelocityAtPoint(rigidbodyIndex, colliderPoint.point, rigidbodies, solverToWorld);

                            dAB = math.dot(convexPoint - colliderPoint.point, colliderPoint.normal);
                            vel = math.dot(velocity - rbVelocity, colliderPoint.normal);

                            if (vel * dt + dAB <= simplexRadius + shape.contactOffset + collisionMargin)
                            {
                                co.pointB = colliderPoint.point;
                                co.normal = colliderPoint.normal;
                                co.pointA = simplexBary;

                                contacts.Enqueue(co);
                            }
                        }
                    }
                }
            }
            
        }

    }

}
#endif