local O={}O.__index=O local m={}m.__index=m local N={0}local P={0,0}function lerp(O,m,N)return O+(m-O)*N end function ellipsoidHeuristic(O,m,P)if N[1]~=0 then return math.sqrt((O[1]-m[1])^2+(O[1]-m[1])^2)+math.sqrt((O[2]-P[2])^2+(O[1]-P[1])^2)<=N[1]else return true end end function O.new(N,P,g,A,s,E,S)local j=setmetatable({},O)j.start=m.new(N[1],N[2],N[3])j.endpoint=m.new(P[1],P[2],P[3])j.Xrand=A[1]j.Yrand=A[2]j.Zrand=A[3]j.expandDis=s j.goalSampleRate=E j.MaxIter=S j.obstacleList=g return j end function O._Start(P,m)local g={}P.nodeList={[1]=P.start}local A=1 for m=1,20000,1 do local g if N[1]~=0 then g=P:_heuristic_random_point()else g=P:_get_random_point()end local A=P:_GetNearestListIndex(g)local s=P:_steer(g,A)if s~=false then if P:_CollisionCheck(s,P.obstacleList)then local N=P:_find_near_nodes(s,5)local g=P:_choose_parent(s,N)P.nodeList[m+99]=g P:_rewire(m+99,g,N)table.insert(P.nodeList[g.parent].children,O:_len(P.nodeList[g.parent].children)+1,m+99)if O:_len(P.nodeList)>P.MaxIter then local m={}for N,g in pairs(P.nodeList)do if O:_len(g.children)==0 and O:_len(P.nodeList[g.parent].children)>1 then m[N]=N end end if#m>1 then local O=m[math.random(1,#m-1)]table.remove(P.nodeList[P.nodeList[O].parent].children,table.find(P.nodeList,O))P.nodeList[O]=nil else local m={}for N,P in pairs(P.nodeList)do if O:_len(P.children)==0 then table.insert(m,#m+1,N)end end local N=m[math.random(1,#m-1)]table.remove(P.nodeList[P.nodeList[N].parent].children,table.find(P.nodeList,N))P.nodeList[N]=nil end end end end O:_DrawGraph(P.nodeList,P.expandDis,P.start,P.endpoint,false)warn("\073\116\101\114\097\116\105\111\110\058 "..m)m=m+1 end for O,m in pairs(P.nodeList)do local N=Instance.new("\080\097\114\116")N.Parent=workspace.Nodes N.Size=Vector3.new(3,3,3)N.Position=Vector3.new(m.x,m.y,m.z)/3 N.Anchored=true N.Shape=Enum.PartType.Ball N.Material=Enum.Material.Neon if m.parent~=nil then local O=(Vector3.new(m.x,m.y,m.z)-Vector3.new(P.nodeList[m.parent].x,P.nodeList[m.parent].y,P.nodeList[m.parent].z)).magnitude local N=Instance.new("\080\097\114\116")N.Parent=workspace.Nodes N.Size=Vector3.new(1,1,O/3)N.Position=Vector3.new(lerp(m.x,P.nodeList[m.parent].x,.5),lerp(m.y,P.nodeList[m.parent].y,.5),lerp(m.z,P.nodeList[m.parent].z,.5))/3 N.Anchored=true N.CFrame=CFrame.new(N.Position,Vector3.new(P.nodeList[m.parent].x,P.nodeList[m.parent].y,P.nodeList[m.parent].z)/3)N.Color=Color3.fromRGB(0,0,0)N.Material=Enum.Material.Neon end end O:_DrawGraph(P.nodeList,P.expandDis,P.start,P.endpoint,true)return P.nodeList end function O._DrawGraph(E,m,P,g,A,s)local S=E:_get_best_last_index(m,P,A)if S~=nil then local P=E:_gen_final_course(S,g,A,m)local j=O:_len(P)local x=0 while j>1 do local O=(Vector3.new(P[j-1][1],P[j-1][2],P[j-1][3])-Vector3.new(P[j][1],P[j][2],P[j][3])).magnitude if s==true then local O=Instance.new("\080\097\114\116")O.Anchored=true O.Parent=workspace.Nodes O.Size=Vector3.new(8,8,8)O.Position=Vector3.new(P[j-1][1],P[j-1][2],P[j-1][3])/3 O.Material=Enum.Material.Neon O.Shape=Enum.PartType.Ball O.Color=Color3.fromRGB(42,255,19)end x=x+O j-=1 end if x<N[1]then N[1]=x elseif N[1]==0 then N[1]=x end else N[1]=0 end end function O._gen_final_course(A,m,N,P,g)local s={{P.x;P.y;P.z}}while g[m].parent~=nil do local N=g[m]table.insert(s,O:_len(s)+1,{N.x;N.y,N.z})m=N.parent end table.insert(s,O:_len(s)+1,{N.x;N.y;N.z})return s end function O._path_validation(O)local m=O:_get_best_last_index()if m~=nil then while O.nodeList[m].parent~=nil do local N=m m=O.nodeList[m].parent local P=O.nodeList[N].x-O.nodeList[m].x local g=O.nodeList[N].y-O.nodeList[m].y local A=math.sqrt(P^2+g^2)if not O:_check_collision_extend(O.nodeList[m].x,O.nodeList[m].y,O.nodeList[N].x,O.nodeList[N].y,A)then table.remove(O.nodeList.children,N)O:_remove_branch(N)end end end end function O._remove_branch(m,O)for O in m.nodelist[O].children do m:_remove_branch(O)end table.remove(m.nodeList,O)end function O._choose_parent(N,O,m)if#m==0 then return O end local P={}for m,g in pairs(m)do local A=O.x-N.nodeList[g].x local s=O.y-N.nodeList[g].y local E=O.z-N.nodeList[g].z local S=math.sqrt((A^2+s^2)+E^2)if N:_check_collision_extend(N.nodeList[g].x,N.nodeList[g].y,N.nodeList[g].z,O.x,O.y,O.z,S)then table.insert(P,#P+1,N.nodeList[g].cost+S)else table.insert(P,#P+1,"\105\110\102")end end local g=math.min(table.unpack(P))local A=m[table.find(P,g)]if g==math.huge then print("\109\105\110\099\111\115\116 \105\115 \105\110\102")return O end O.cost=g O.parent=A return O end function O._heuristic_random_point(O)local m if math.random(0,100)>O.goalSampleRate then local P=Vector3.new(O.start.x,O.start.y,O.start.z)local g=Vector3.new(O.endpoint.x,O.endpoint.y,O.endpoint.z)local A=N[1]local s=math.sqrt(((P.X-g.X)^2+(P.Y-g.Y)^2)+(P.Z-g.Z)^2)/2 local E=math.sqrt((A/2)^2-s^2)local S=P+(g-P)*.5 local j=math.random(-(A/2),A/2)local x=(math.abs(E*2)/math.abs(A))*math.sqrt((A/2)^2-j^2)local H=math.random(-x/2,x/2)local e=math.abs(E/2)*math.sqrt((1-j^2/(A/2)^2)-H^2/(E/2)^2)local Y=math.random(-e,e)local B=(CFrame.new(S,P)*CFrame.new(Y,H,j)).Position m={B.X;B.Y,B.Z}else m={O.endpoint.x,O.endpoint.y,O.endpoint.z}end return m end function O._len(m,O)local N=0 for O,m in pairs(O)do N=N+1 end return N end function O._steer(P,O,N)local g=P.nodeList[N]local A=O[1]-g.x local s=O[2]-g.y local E=O[3]-g.z local S=math.sqrt((A^2+s^2)+s^2)local j=m.new(g.x,g.y,g.z)j.x=g.x+(O[1]-g.x)*(P.expandDis/S)j.y=g.y+(O[2]-g.y)*(P.expandDis/S)j.z=g.z+(O[3]-g.z)*(P.expandDis/S)j.cost=g.cost+P.expandDis j.parent=N return j end function O._get_random_point(O)local m if math.random(0,100)>O.goalSampleRate then m={math.random(0,O.Xrand);math.random(0,O.Yrand),math.random(0,O.Zrand)}else m={O.endpoint.x,O.endpoint.y,O.endpoint.z}end return m end function O._get_best_last_index(g,m,N,P)local A={}for O,m in pairs(m)do table.insert(A,O,math.sqrt(((m.x-P.x)^2+(m.y-P.y)^2)+(m.z-P.z)^2))end local s={}for O,m in pairs(A)do if m<=N then table.insert(s,O,O)end end if O:_len(s)==0 then return nil end for O,N in pairs(s)do if m[N].cost==math.min(m[N].cost)then return N end end return nil end function O._calc_dist_to_goal(N,O,m)return math.sqrt(O^2+m^2)end function O._find_near_nodes(N,O,m)local P=N.expandDis*m local g={}local A={}local s={}local E={}for m,N in pairs(N.nodeList)do table.insert(g,m,{(N.x-O.x)^2;(N.y-O.y)^2;(N.z-O.z)^2})end for O,m in pairs(g)do table.insert(A,O,(m[1]+m[2])+m[3])end for O,m in pairs(A)do if m<=P^2 then table.insert(s,O,m)end end for O,m in pairs(N.nodeList)do if s[O]~=nil then table.insert(E,#E+1,O)end end return E end function O._rewire(g,m,N,P)local A=O:_len(g.nodeList)for O,P in pairs(P)do local A=g.nodeList[P]local s=N.x-A.x local E=N.y-A.y local S=N.z-A.z local j=math.sqrt((s^2+E^2)+S^2)local x=N.cost+j if A.cost>x then if g:_check_collision_extend(A.x,A.y,A.z,N.x,N.y,N.z,j)then table.remove(g.nodeList[A.parent].children,table.find(g.nodeList[A.parent].children,P))A.parent=m A.cost=x table.insert(N.children,#N.children+1,P)end end end end function O._check_collision_extend(S,O,N,P,g,A,s,E)local j=m.new(O,N)for m=1,math.floor(E/20),1 do j.x=O+(g-O)*((20*m)/E)j.y=N+(A-N)*((20*m)/E)j.z=P+(s-P)*((20*m)/E)if not S:_CollisionCheck(j,S.obstacleList)then return false end end return true end function O._GetNearestListIndex(m,O)local N={}local P={}local g={}for m,P in pairs(m.nodeList)do table.insert(N,#N+1,{(P.x-O[1])^2,(P.y-O[2])^2,(P.z-O[3])^2})end for O,m in pairs(N)do table.insert(P,O,(m[1]+m[2])+m[3])end for O,m in pairs(m.nodeList)do table.insert(g,#g+1,O)end return g[table.find(P,math.min(table.unpack(P)))]end function O._CollisionCheck(N,O,m)for m,N in pairs(m)do local P,g,A,s,E,S=N[1]+4,N[2]+4,N[3]+4,N[4]+4,N[5]+4,N[6]+4 if O.x>P and O.x<P+s then if O.y>g and O.y<g+E then if O.z>A and O.z<A+S then return false end end end end return true end function m.new(O,N,P)local g=setmetatable({},m)g.x=O g.y=N g.z=P g.cost=.0 g.parent=nil g.children={}return g end return O
---@diagnostic disable: undefined-field, lowercase-global
-- NicolaiXeno 14. January 2022

local rrt = {}
rrt.__index = rrt

local Node = {}
Node.__index = Node

local globalvariable = {0}
local oldposition = {0,0}

function lerp(a,b,t)
	return a+(b-a)*t
end

function ellipsoidHeuristic(t,p1,p2)
	if globalvariable[1] ~= 0 then
		return math.sqrt((t[1]-p1[1])^2 + (t[1]-p1[1])^2) + math.sqrt((t[2]-p2[2])^2 + (t[1]-p2[1])^2) <= globalvariable[1]
	else
		return true
	end
end

-- PARAMETERS

function rrt.new(start, goal, obstacleList, randArea, expandDis, goalSampleRate, MaxIter)
	local self = setmetatable({}, rrt)
	self.start = Node.new(start[1], start[2], start[3])
	self.endpoint = Node.new(goal[1], goal[2], goal[3])
	self.Xrand = randArea[1]
	self.Yrand = randArea[2]
	self.Zrand = randArea[3]
	self.expandDis = expandDis
	self.goalSampleRate = goalSampleRate
	self.MaxIter = MaxIter
	self.obstacleList = obstacleList
	return self
end

function rrt:_Start(info)
	local savedIteration = {}
	self.nodeList = {[1]= self.start}

	local i = 1

	for i = 1,20000 do
		local rnd
		if globalvariable[1] ~= 0 then
			rnd = self:_heuristic_random_point()
		else
			rnd = self:_get_random_point()
		end

		--if ellipsoidHeuristic(rnd,{self.start.x,self.start.y},{self.endpoint.x,self.endpoint.y}) then
		local nind = self:_GetNearestListIndex(rnd)
		local newNode = self:_steer(rnd, nind)

		if newNode ~= false then
			if self:_CollisionCheck(newNode, self.obstacleList) then

				local nearinds = self:_find_near_nodes(newNode,5) -- Identifies neighbouring node to the new Node.
				local newNode = self:_choose_parent(newNode, nearinds) -- Identifies nearest node and assigns it as a parent to the new Node.
				self.nodeList[i+99] = newNode


				--print("nearinds")
				--print(nearinds)
				self:_rewire(i+99, newNode, nearinds) -- Evaluate branches and optimise.
				table.insert(self.nodeList[newNode.parent].children,(rrt:_len(self.nodeList[newNode.parent].children))+1,i+99)


				if rrt:_len(self.nodeList) > self.MaxIter then
					local leaves = {}

					for key, node in pairs(self.nodeList) do
						if rrt:_len(node.children) == 0 and rrt:_len(self.nodeList[node.parent].children) > 1 then 
							leaves[key] = key
						end
					end

					if #leaves > 1 then
						local ind = leaves[math.random(1,(#leaves)-1)]
						table.remove(self.nodeList[self.nodeList[ind].parent].children, table.find(self.nodeList,ind))	
						self.nodeList[ind] = nil
					else
						local leaves2 = {}
						for key, node in pairs(self.nodeList) do
							if rrt:_len(node.children) == 0 then
								table.insert(leaves2,(#leaves2)+1,key)
							end
						end

						local ind = leaves2[math.random(1,(#leaves2)-1)]
						table.remove(self.nodeList[self.nodeList[ind].parent].children, table.find(self.nodeList,ind))	
						self.nodeList[ind] = nil

					end
				end
			end
			--end

		end

		--[[if i%100 == 0 then

			for _,v in pairs(workspace.Nodes:GetChildren()) do
				v:Destroy()
			end
			
			for _,node in pairs(self.nodeList) do
				local prt = Instance.new("Part")
				prt.Parent = workspace.Nodes
				prt.Size = Vector3.new(3,3,3)
				prt.Position = Vector3.new(node.x,node.y,node.z)/3
				prt.Anchored = true
				prt.Shape = Enum.PartType.Ball
				prt.Material = Enum.Material.Neon
				if node.parent ~= nil then
					local Magnitude = (Vector3.new(node.x,node.y,node.z)-Vector3.new(self.nodeList[node.parent].x,self.nodeList[node.parent].y,self.nodeList[node.parent].z)).magnitude
					local prt = Instance.new("Part")
					prt.Parent = workspace.Nodes
					prt.Size = Vector3.new(2,2,Magnitude/3)
					prt.Position = Vector3.new(lerp(node.x,self.nodeList[node.parent].x,.5),lerp(node.y,self.nodeList[node.parent].y,.5),lerp(node.z,self.nodeList[node.parent].z,.5))/3
					prt.Anchored = true
					prt.CFrame = CFrame.new(prt.Position,Vector3.new(self.nodeList[node.parent].x,self.nodeList[node.parent].y,self.nodeList[node.parent].z)/3)
					prt.Color = Color3.fromRGB(0,0,0)
					prt.Material = Enum.Material.Neon
				end
			end
			wait()

		end]]
		rrt:_DrawGraph(self.nodeList,self.expandDis,self.start,self.endpoint,false)
		
		warn("Iteration: "..i)
		i = i + 1
	end
	
	for _,node in pairs(self.nodeList) do
		local prt = Instance.new("Part")
		prt.Parent = workspace.Nodes
		prt.Size = Vector3.new(3,3,3)
		prt.Position = Vector3.new(node.x,node.y,node.z)/3
		prt.Anchored = true
		prt.Shape = Enum.PartType.Ball
		prt.Material = Enum.Material.Neon
		if node.parent ~= nil then
			local Magnitude = (Vector3.new(node.x,node.y,node.z)-Vector3.new(self.nodeList[node.parent].x,self.nodeList[node.parent].y,self.nodeList[node.parent].z)).magnitude
			local prt = Instance.new("Part")
			prt.Parent = workspace.Nodes
			prt.Size = Vector3.new(1,1,Magnitude/3)
			prt.Position = Vector3.new(lerp(node.x,self.nodeList[node.parent].x,.5),lerp(node.y,self.nodeList[node.parent].y,.5),lerp(node.z,self.nodeList[node.parent].z,.5))/3
			prt.Anchored = true
			prt.CFrame = CFrame.new(prt.Position,Vector3.new(self.nodeList[node.parent].x,self.nodeList[node.parent].y,self.nodeList[node.parent].z)/3)
			prt.Color = Color3.fromRGB(0,0,0)
			prt.Material = Enum.Material.Neon
		end
	end
	rrt:_DrawGraph(self.nodeList,self.expandDis,self.start,self.endpoint,true)

	return self.nodeList
end

-- Path Validation

function rrt:_DrawGraph(nodelist,expand,start,endpoint,debounce)
	local lastIndex = self:_get_best_last_index(nodelist,expand,endpoint)
	if lastIndex ~= nil then
		local path = self:_gen_final_course(lastIndex,start,endpoint,nodelist)

		local ind = rrt:_len(path)
		local pathlength = 0

		while ind > 1 do
			local distance = ((Vector3.new(path[ind-1][1], path[ind-1][2], path[ind-1][3]))-(Vector3.new(path[ind][1], path[ind][2], path[ind][3]))).magnitude
			if debounce == true then
				local prt = Instance.new("Part")
				prt.Anchored = true
				prt.Parent = workspace.Nodes
				prt.Size = Vector3.new(8,8,8)
				prt.Position = Vector3.new(path[ind-1][1], path[ind-1][2], path[ind-1][3])/3
				prt.Material = Enum.Material.Neon
				prt.Shape = Enum.PartType.Ball
				prt.Color = Color3.fromRGB(42, 255, 19)
			end
			pathlength = pathlength + distance
			ind-=1
		end

		if pathlength < globalvariable[1] then
			globalvariable[1] = pathlength
		elseif globalvariable[1] == 0 then
			globalvariable[1] = pathlength
		end
	else
		globalvariable[1] = 0
	end
end

function rrt:_gen_final_course(goalind,start,endpoint,nodelist)
	local path = {{endpoint.x, endpoint.y, endpoint.z}}

	while nodelist[goalind].parent ~= nil do
		local node = nodelist[goalind]
		table.insert(path,rrt:_len(path) + 1,{node.x, node.y, node.z})
		goalind = node.parent
	end
	table.insert(path,rrt:_len(path) + 1,{start.x, start.y, start.z})
	return path
end


function rrt:_path_validation()
	local lastIndex = self:_get_best_last_index()
	if lastIndex ~= nil then
		while self.nodeList[lastIndex].parent ~= nil do
			local nodeInd = lastIndex
			lastIndex = self.nodeList[lastIndex].parent

			local dx = self.nodeList[nodeInd].x - self.nodeList[lastIndex].x
			local dy = self.nodeList[nodeInd].y - self.nodeList[lastIndex].y
			local d = math.sqrt(dx^2 + dy^2)

			if not self:_check_collision_extend(self.nodeList[lastIndex].x, self.nodeList[lastIndex].y, self.nodeList[nodeInd].x,self.nodeList[nodeInd].y, d) then
				table.remove(self.nodeList.children, nodeInd)					
				self:_remove_branch(nodeInd) 
			end
		end
	end
end

function rrt:_remove_branch(nodeInd)
	for ix in self.nodelist[nodeInd].children do
		self:_remove_branch(ix)
	end

	table.remove(self.nodeList, nodeInd)
end

--TODO Issue found
function rrt:_choose_parent(newNode, nearinds)
	if #nearinds == 0 then
		return newNode
	end

	local dlist = {}

	for _,i in pairs(nearinds) do
		local dx = newNode.x - self.nodeList[i].x
		local dy = newNode.y - self.nodeList[i].y
		local dz = newNode.z - self.nodeList[i].z
		local d = math.sqrt(dx^2 + dy^2 + dz^2)

		if self:_check_collision_extend(self.nodeList[i].x, self.nodeList[i].y, self.nodeList[i].z, newNode.x, newNode.y, newNode.z, d) then
			table.insert(dlist,(#dlist) + 1,self.nodeList[i].cost + d)
		else
			table.insert(dlist,(#dlist) + 1,"inf")
		end
	end

	local mincost = math.min(table.unpack(dlist))
	local minind = nearinds[table.find(dlist,mincost)]

	if mincost == math.huge then
		print("mincost is inf")

		return newNode
	end

	newNode.cost = mincost
	newNode.parent = minind

	return newNode
end

function rrt:_heuristic_random_point()
	local rnd
	if math.random(0, 100) > self.goalSampleRate then
		local start = Vector3.new(self.start.x,self.start.y,self.start.z)
		local endpoint = Vector3.new(self.endpoint.x,self.endpoint.y,self.endpoint.z)

		local MajorAxis = globalvariable[1]
		local FocalLength = (math.sqrt(((start.X-endpoint.X)^2)+((start.Y-endpoint.Y)^2)+((start.Z-endpoint.Z)^2))/2)
		local MinorAxis = math.sqrt(((MajorAxis/2)^2)-FocalLength^2)

		local Center = start+(endpoint-start)*.5

		local xa = math.random(-(MajorAxis/2),MajorAxis/2)

		local intersept = (math.abs(MinorAxis*2)/math.abs(MajorAxis))*math.sqrt(((MajorAxis/2)^2)-xa^2)
		local randomintersepty = math.random(-intersept/2,intersept/2)

		
		local Z1 = math.abs(MinorAxis/2)*math.sqrt(1-xa^2/(MajorAxis/2)^2-randomintersepty^2/(MinorAxis/2)^2)

		local randominterseptz = math.random(-Z1,Z1)

		local randomposition = (CFrame.new(Center,start) * CFrame.new(randominterseptz,randomintersepty,xa)).Position
		
		--[[local prt = Instance.new("Part",workspace.Nodes)
		prt.Anchored = true
		prt.Position = (CFrame.new(workspace.Start.Position+(workspace.End.Position-workspace.Start.Position)*.5,workspace.Start.Position) * CFrame.new(randominterseptz/3,randomintersepty/3,xa/3)).Position
		prt.Size = Vector3.new(4,4,4)
		prt.Shape = Enum.PartType.Ball]]
		
		rnd = {randomposition.X,randomposition.Y,randomposition.Z}
	else
		rnd = {self.endpoint.x, self.endpoint.y, self.endpoint.z}
	end
	return rnd
end


function rrt:_len(t)
	local n = 0

	for _,v in pairs(t) do
		n = n + 1
	end
	return n
end

function rrt:_steer(rnd, nind)

	-- expand tree

	local nearestNode = self.nodeList[nind]

	local dx = rnd[1] - nearestNode.x
	local dy = rnd[2] - nearestNode.y
	local dz = rnd[3] - nearestNode.z
	local d = math.sqrt(dx^2 + dy^2 + dy^2)

	local newNode = Node.new(nearestNode.x, nearestNode.y, nearestNode.z)
	newNode.x = nearestNode.x + (rnd[1]-nearestNode.x) * (self.expandDis/d)
	newNode.y = nearestNode.y + (rnd[2]-nearestNode.y) * (self.expandDis/d)
	newNode.z = nearestNode.z + (rnd[3]-nearestNode.z) * (self.expandDis/d)

	newNode.cost = nearestNode.cost + self.expandDis
	newNode.parent = nind
	--warn(newNode)
	return newNode
end


function rrt:_get_random_point()
	local rnd

	if math.random(0,100) > self.goalSampleRate then
		rnd = {math.random(0, self.Xrand), math.random(0, self.Yrand), math.random(0, self.Zrand)}
	else
		rnd = {self.endpoint.x, self.endpoint.y, self.endpoint.z}
	end

	--warn(rnd[1],rnd[2])
	return rnd
end

function rrt:_get_best_last_index(nodelist,expand,endpoint)
	local disglist = {}

	for key, node in pairs(nodelist) do
		table.insert(disglist,key,math.sqrt(((node.x-endpoint.x)^2) + ((node.y-endpoint.y) ^2) + ((node.z-endpoint.z)^2)))
	end

	local goalinds = {}

	for index, element in pairs(disglist) do
		if element <= expand then
			table.insert(goalinds,index,index)
		end
	end

	if rrt:_len(goalinds) == 0 then
		return nil
	end

	for _,i in pairs(goalinds) do
		if nodelist[i].cost == math.min(nodelist[i].cost) then
			return i
		end
	end

	return nil
end

function rrt:_calc_dist_to_goal(a,b)
	return math.sqrt(a^2 + b^2)
end


function rrt:_find_near_nodes(newNode, value)
	local r = self.expandDis * value

	local dlist = {}
	local dlist2 = {}
	local dlist3 = {}
	local nearinds = {}

	for Index,Child in pairs(self.nodeList) do
		--warn("NodeList: "..Index)
		table.insert(dlist,Index,{(Child.x - newNode.x)^2,(Child.y - newNode.y)^2,(Child.z - newNode.z)^2})
	end

	for Index,Child in pairs(dlist) do
		table.insert(dlist2,Index,Child[1]+Child[2]+Child[3])
	end

	for Index,Child in pairs(dlist2) do
		if Child <= r^2 then
			--warn("Found Node At: "..Index)
			table.insert(dlist3,Index,Child)
		end
	end

	for Index,Child in pairs(self.nodeList) do
		if dlist3[Index] ~= nil then
			--warn("Node Assigned: "..Index)
			table.insert(nearinds,(#nearinds)+1,Index)
		end
	end

	--warn(nearinds)
	return nearinds
end

function rrt:_rewire(newNodeInd, newNode, nearinds)
	local nnode = rrt:_len(self.nodeList)
	for set,element in pairs(nearinds) do
		local nearNode = self.nodeList[element]

		local dx = newNode.x - nearNode.x
		local dy = newNode.y - nearNode.y
		local dz = newNode.z - nearNode.z
		local d = math.sqrt(dx^2 + dy^2 + dz^2)

		local scost = newNode.cost + d

		if nearNode.cost > scost then
			if self:_check_collision_extend(nearNode.x, nearNode.y, nearNode.z, newNode.x, newNode.y, newNode.z, d) then
				table.remove(self.nodeList[nearNode.parent].children,table.find(self.nodeList[nearNode.parent].children,element))

				nearNode.parent = newNodeInd
				nearNode.cost = scost

				table.insert(newNode.children,(#newNode.children) +1,element)

			end
		end
	end
end

function rrt:_check_collision_extend(nix, niy, niz, ix, iy, iz, d)
	local tmpNode = Node.new(nix,niy)

	for i = 1,math.floor(d/20) do
		tmpNode.x = nix + (ix - nix) * ((20*i)/d)
		tmpNode.y = niy + (iy - niy) * ((20*i)/d)
		tmpNode.z = niz + (iz - niz) * ((20*i)/d)
		if not self:_CollisionCheck(tmpNode, self.obstacleList) then
			return false
		end
	end

	return true
end

function rrt:_GetNearestListIndex(rnd) 
	local dlist = {}
	local dlist2 = {}
	local nearinds = {}

	for Index,Child in pairs(self.nodeList) do
		table.insert(dlist,(#dlist)+1,{(Child.x - rnd[1])^2,(Child.y - rnd[2])^2,(Child.z - rnd[3])^2})
	end

	for Index,Child in pairs(dlist) do
		table.insert(dlist2,Index,Child[1]+Child[2]+Child[3])
	end

	for Index,Child in pairs(self.nodeList) do
		table.insert(nearinds,(#nearinds)+1,Index)
	end

	return nearinds[table.find(dlist2,math.min(table.unpack(dlist2)))]
end

function rrt:_CollisionCheck(nodepart, obstacleList)
	for set,element in pairs(obstacleList) do
		local sx,sy,sz,ex,ey,ez = element[1]+4,element[2]+4,element[3]+4,element[4]+4,element[5]+4,element[6]+4
		if nodepart.x > sx and nodepart.x < sx+ex then
			if nodepart.y > sy and nodepart.y < sy+ey then
				if nodepart.z > sz and nodepart.z < sz+ez then
					return false
				end
			end
		end
	end
	return true
end


function Node.new(x,y,z)
	local self = setmetatable({}, Node)

	self.x = x
	self.y = y 
	self.z = z
	self.cost = 0.0
	self.parent = nil
	self.children = {}

	return self 
end

return rrt