local vec3 = require "vec3"

local edgeutil = { }

local function calcScale(dist, angle)
	if (angle < .001) then
		return dist
	end
	
	local pi = 3.1415926535897932384626
	local pi2 = pi / 2	
	local sqrt2 = 1.41421
	
	local scale = 1.0
	if (angle >= pi2) then 
		scale = 1.0 + (sqrt2 - 1.0) * ((angle - pi2) / pi2)
	end

	return .5 * dist / math.cos(.5 * pi - .5 * angle) * angle * scale
end

function edgeutil.makeEdges(input)
	local result = { }
	
	for i = 1, #input do
		local entry = input[i]
	
		local p0 = entry[1]
		local p1 = entry[2]
		local t0 = entry[3]
		local t1 = entry[4]

		p0 = vec3.new(p0[1], p0[2], p0[3])
		p1 = vec3.new(p1[1], p1[2], p1[3])
		
		t0 = vec3.normalize(vec3.new(t0[1], t0[2], t0[3]))
		t1 = vec3.normalize(vec3.new(t1[1], t1[2], t1[3]))
		
		local length = vec3.distance(p0, p1)
		local angle = vec3.angleUnit(t0, t1)
		
		local scale = calcScale(length, angle)
		
		t0 = vec3.mul(scale, t0)
		t1 = vec3.mul(scale, t1)
		
		table.insert(result, { { p0.x, p0.y, p0.z }, { t0.x, t0.y, t0.z } } )
		table.insert(result, { { p1.x, p1.y, p1.z }, { t1.x, t1.y, t1.z } } )
	end	
	
	return result
end

function edgeutil.edgeFromPoints(points)
    local p0 = points[1]
    local p1 = points[2]
    local t = {
        p1[1] - p0[1],
        p1[2] - p0[2],
        p1[3] - p0[3],        
    }
    return edgeutil.makeEdges({ { p0, p1, t, t } })
end

function edgeutil.edgesFromPoints(pointsList)
	local result = { }
	
	for i = 1, #pointsList do
		local points = pointsList[i]
        local p0 = points[1]
        local p1 = points[2]
        local t = {
            p1[1] - p0[1],
            p1[2] - p0[2],
            p1[3] - p0[3],        
        }
		table.insert(result, { p0, p1, t, t } )
    end
    return edgeutil.makeEdges(result)
end

function edgeutil.makeCrossoverEdges(x, yStart, yEnd, z, trackDistance, leftHanded)
    local yDistance = yStart - yEnd
    if math.abs(yDistance) < 42 then
        print("ERROR edgeutil.makeCrossoverEdges: length < 42")
    end
    local yd2 = yDistance / 2
    local y2 = yStart - yd2
    
    trackDistance = leftHanded and -trackDistance or trackDistance
    local td2 = trackDistance / 2
    local x2 = x + td2
    local xEnd = x + trackDistance
  
    return {
        { {    x, yStart, z }, {    0, -yd2, 0 } },
        { {   x2,     y2, z }, {  td2, -yd2, 0 } },
        { {   x2,     y2, z }, {  td2, -yd2, 0 } },
        { { xEnd,   yEnd, z }, {    0, -yd2, 0 } },
        
        { { xEnd, yStart, z }, {    0, -yd2, 0 } },
        { {   x2,     y2, z }, { -td2, -yd2, 0 } },
        { {   x2,     y2, z }, { -td2, -yd2, 0 } },
        { {    x,   yEnd, z }, {    0, -yd2, 0 } }
    }
end 

return edgeutil