computeIntersections.jl

using ImplicitBVH, Makie, GLMakie
using ImplicitBVH: BBox, BSphere

function computeLevels(u::Array{T,3}) where T <: Number
	
	# compute the level sets
	cs0 = Makie.Contours.contours(1:size(u,1),1:size(u,2), u[:,:,1],[T(0.5)])
	cs1 = Makie.Contours.contours(1:size(u,1),1:size(u,2), u[:,:,2],[T(0.5)])
	
	return (cs0, cs1)
end

# pair iterator
struct PairIterator{T}
    it::Vector{T}
end

# Required method
function Base.iterate(p::PairIterator, i::Int=1)
    l = length(p.it)
    if i >= l
        return nothing
    else
        j = i+1
        return (p.it[i], p.it[j]), i+1
    end
end

# Important optional methods
Base.eltype(::Type{PairIterator{T}}) where {T} = Tuple{T,T}
Base.length(p::PairIterator) = length(p.it)-1

function segments2BSpheres(cs)
	bounding_spheres = BSphere{Float32}[]
	for c in cs.contours, l in c.lines
		for (p0,p1) in PairIterator(l.vertices)
			push!(bounding_spheres, 
				BSphere{Float32}([((p0.+p1).*0.5f0)...,0.0],sqrt(sum((p0.-p1).^2)))
			)
		end
	end
	return bounding_spheres
end

function extractSegmentPair(n::Int, cs)
	m=0
	for c in cs.contours, l in c.lines
		if n-m > length(l.vertices)-1
			m+=(length(l.vertices)-1)
		else
			for q in 1:length(l.vertices)-1
				m+=1
				if (m==n)
					return (l.vertices[q],l.vertices[q+1])
				end
			end
		end
	end
end


function computeIntersects!(intersects::Vector{NTuple{2,T}}, A, B, X, p0, p1, q0, q1) where T <: Real
	
	A[1,1] = (p1[1]-p0[1])
	A[2,2] = (q1[1]-q0[1])
	A[3,1] = (p1[2]-p0[2])
	A[4,2] = (q1[2]-q0[2])
	B[1] = -p0[1]							
	B[2] = -q0[1]
	B[3] = -p0[2]
	B[4] = -q0[2]
								
	try
		X .= A\B
		if 0 <= X[1] && X[1] < 1.0 && 0 <= X[2] && X[2] < 1
			push!(intersects, (X[3], X[4]))
		end
	catch
		@warn "Parallel segments!"
		X .= T(NaN)
	end
	
	return nothing
end

function find_cores(u)

	T = eltype(u)
		
	#
	fig = Figure()
	ax = Axis(fig[1,1], aspect=DataAspect())
	# we trust that cs0,cs1 are identical to those produced by contour!
	contour!(ax, u[:,:,1], levels=[0.5], linewidth=1, color=:black)
	contour!(ax, u[:,:,2], levels=[0.5], linewidth=1, color=:red)
	#
	
	# level sets
	cs0, cs1 = computeLevels(u)
	
	# form BSphere arrays from vertices pair-wise
	bounding_spheres0 = segments2BSpheres(cs0)
	bounding_spheres1 = segments2BSpheres(cs1)
	
	# Build BVHs using bounding boxes for nodes
	bvh0 = BVH(bounding_spheres0, BBox{T}, UInt32)
	bvh1 = BVH(bounding_spheres1, BBox{T}, UInt32)
	
	# Traverse BVH for contact detection
	traversal = traverse(
		  bvh0,
		  bvh1,
		  default_start_level(bvh0),
		  default_start_level(bvh1),
		  # previous_traversal_cache,
		  num_threads=16,
	)
	@show traversal
	
	
	intersects = Vector{NTuple{2,T}}(undef,0)
	A = zeros(T,4,4)
	A[[1,2],3] .= T(-1.0)
	A[[3,4],4] .= T(-1.0)
	B = zeros(T,4)
	X = zeros(T,4)
	for ct in traversal.contacts
		n0 = ct[1]
		n1 = ct[2]
		bs0 = bounding_spheres0[n0]
		bs1 = bounding_spheres1[n1]
		p0, p1 = extractSegmentPair(n0, cs0)
		q0, q1 = extractSegmentPair(n1, cs1)
		@assert all(bs0.x .≈ (((p0.+p1).*0.5f0)...,0.0f0))
		@assert bs0.r ≈ sqrt(sum((p0.-p1).^2))
		@assert all(bs1.x .≈ (((q0.+q1).*0.5f0)...,0.0f0))
		@assert bs1.r ≈ sqrt(sum((q0.-q1).^2))
		computeIntersects!(intersects, A, B, X, 
			p0, p1, q0, q1
		)
	end
	scatter!(ax, intersects, markersize=2, color=:green)
	
	save("./test.png", fig, px_per_unit=4)
	
	return intersects
end

function main()

	# set type
	T = Float32

	# input field (random could be stress-test)
	u = zeros(T, 512, 512, 2)
	@inbounds for i in 1:size(u,1), j in 1:size(u,2)
		u[i,j,1] = sin(-sqrt(1/prod(size(u)))*((i-size(u,1)/2)^2 + (j-size(u,2)/2)^2))
		u[i,j,2] = 0.5*(1.0 + tanh(0.01*(i-size(u,1)/2)))
	end
	
	intersects = find_cores(u)
	
	return nothing
end