-- path.lua
-- Functions for pathpredicting, put in a separate file. 

-- Naming conventions:
-- 'index' - An index of the train.path table.
-- 'offset' - A value in meters that determines how far on the path to walk relative to a certain index
-- 'n' - Referring or pointing towards the 'next' path item, the one with index+1
-- 'p' - Referring or pointing towards the 'prev' path item, the one with index-1
-- 'f' - Referring to the positive end of the path (the end with the higher index)
-- 'b' - Referring to the negative end of the path (the end with the lower index)

-- New path structure of trains:
--Tables:
-- path      - path positions. 'indices' are relative to this. At the moment, at.round_vector_floor_y(path[i])
--              is the node this item corresponds to, however, this will change in the future.
-- path_node - (reserved)
-- path_cn   - Connid of the current node that points towards path[i+1]
-- path_cp   - Connid of the current node that points towards path[i-1]
--     When the day comes on that path!=node, these will only be set if this index represents a transition between rail nodes
-- path_dist - The total distance of this path element from path element 0
-- path_dir  - The direction of this path item's transition to the next path item, which is the angle of conns[path_cn[i]].c
-- path_speed- Populated by the LZB system. The maximum speed (velocity) permitted in the moment this path item is passed.
--             (this saves brake distance calculations every step to determine LZB control). nil means no limit.
--Variables:
-- path_ext_f/b - how far path[i] is set
-- path_trk_f/b - how far the path extends along a track. beyond those values, paths are generated in a straight line.
-- path_req_f/b - how far path items were requested in the last step
--
--Distance and index:
-- There is an important difference between the path index and the actual distance on the track: The distance between two path items can be larger than 1,
-- but the corresponding index increment is still 1.
-- Indexes in advtrains can be fractional values. If they are, it means that the actual position is interpolated between the 2 adjacent path items.
-- If you need to proceed along the path by a specific actual distance, it does NOT work to simply add it to the index. You should use the path_get_index_by_offset() function.

-- creates the path data structure, reconstructing the train from a position and a connid
-- Important! train.drives_on must exist while calling this method
-- returns: true - successful
--           nil - node not yet available/unloaded, please wait
--         false - node definitely gone, remove train
function advtrains.path_create(train, pos, connid, rel_index)
	local posr = advtrains.round_vector_floor_y(pos)
	local node_ok, conns, rhe = advtrains.get_rail_info_at(pos, train.drives_on)
	if not node_ok then
		return node_ok
	end
	local mconnid = advtrains.get_matching_conn(connid, #conns)
	train.index = rel_index
	train.path = { [0] = { x=posr.x, y=posr.y+rhe, z=posr.z } }
	train.path_cn = { [0] = connid }
	train.path_cp = { [0] = mconnid }
	train.path_dist = { [0] = 0 }
	
	train.path_dir = {
		[0] = advtrains.conn_angle_median(conns[mconnid].c, conns[connid].c)
	}
	
	train.path_speed = { }
	
	train.path_ext_f=0
	train.path_ext_b=0
	train.path_trk_f=0
	train.path_trk_b=0
	train.path_req_f=0
	train.path_req_b=0
	
	advtrains.occ.set_item(train.id, posr, 0)
	return true
end

-- Sets position and connid to properly restore after a crash, e.g. in order
-- to save the train or to invalidate its path
-- Assumes that the train is in clean state
-- if invert ist true, setrestore will use the end index
function advtrains.path_setrestore(train, invert)
	local idx = train.index
	if invert then
		idx = train.end_index
	end
	
	local pos, connid, frac = advtrains.path_getrestore(train, idx, invert, true)
	
	train.last_pos = pos
	train.last_connid = connid
	train.last_frac = frac
end
-- Get restore position, connid and frac (in this order) for a train that will originate at the passed index
-- If invert is set, it will return path_cp and multiply frac by -1, in order to reverse the train there.
function advtrains.path_getrestore(train, index, invert)
	local idx = index
	local cns = train.path_cn
	
	if invert then
		cns = train.path_cp
	end
	
	local fli = atfloor(index)
	advtrains.path_get(train, fli)
	if fli > train.path_trk_f then
		fli = train.path_trk_f
	end
	if fli < train.path_trk_b then
		fli = train.path_trk_b
	end
	return advtrains.path_get(train, fli),
			cns[fli],
			(idx - fli) * (invert and -1 or 1)
end

-- Invalidates a path
-- this is supposed to clear stuff from the occupation tables
-- This function throws a warning whenever any code calls it while the train steps are run, since that must not happen.
-- The ignore_lock parameter can be used to ignore this, however, it should then be accompanied by a call to train_ensure_init
-- before returning from the calling function.
function advtrains.path_invalidate(train, ignore_lock)
	if advtrains.lock_path_inval and not ignore_lock then
		atwarn("Train ",train.train_id,": Illegal path invalidation has occured during train step:")
		atwarn(debug.traceback())
	end

	if train.path then
		for i,p in pairs(train.path) do
			advtrains.occ.clear_all_items(train.id, advtrains.round_vector_floor_y(p))
		end
	end
	train.path = nil
	train.path_dist = nil
	train.path_cp = nil
	train.path_cn = nil
	train.path_dir = nil
	train.path_speed = nil
	train.path_ext_f=0
	train.path_ext_b=0
	train.path_trk_f=0
	train.path_trk_b=0
	train.path_req_f=0
	train.path_req_b=0
	
	train.dirty = true
	--atdebug(train.id, "Path invalidated")
end

-- Keeps the path intact, but invalidates all path nodes from the specified index (inclusive)
-- onwards. This has the advantage that we don't need to recalculate the whole path, and we can do it synchronously.
function advtrains.path_invalidate_ahead(train, start_idx, ignore_when_passed)
	if not train.path then
		-- the path wasn't even initialized. Nothing to do
		return
	end

	local idx = atfloor(start_idx)
	--atdebug("Invalidate_ahead:",train.id,"start_index",start_idx,"cur_idx",train.index)
	
	if(idx <= train.index - 0.5) then
		if ignore_when_passed then
			--atdebug("ignored passed")
			return
		end
		advtrains.path_print(train, atwarn)
		error("Train "+train.id+": Cannot path_invalidate_ahead start_idx="+idx+" as train has already passed!")
	end
	
	-- leave current node in path, it won't change. What might change is the path onward from here (e.g. switch)
	local i = idx + 1
	while train.path[i] do
		advtrains.occ.clear_specific_item(train.id, advtrains.round_vector_floor_y(train.path[i]), i)
		i = i+1
	end
	train.path_ext_f=idx
	train.path_trk_f=math.min(idx, train.path_trk_f)
	
	-- callbacks called anyway for current node, because of LZB
	advtrains.run_callbacks_invahead(train.id, train, idx)
end

-- Prints a path using the passed print function
-- This function should be 'atprint', 'atlog', 'atwarn' or 'atdebug', because it needs to use print_concat_table
function advtrains.path_print(train, printf)
	printf("path_print: tid =",train.id," index =",train.index," end_index =",train.end_index," vel =",train.velocity)
	if not train.path then
		printf("path_print: Path is invalidated/inexistant.")
		return
	end
	printf("i:	CP	Position	Dir				CN		Dist		Speed")
	for i = train.path_ext_b, train.path_ext_f do
		if i==train.path_trk_b then
			printf("--Back on-track border here--")
		end
		printf(i,":	",train.path_cp[i],"	",train.path[i],"	",train.path_dir[i],"	",train.path_cn[i],"		",train.path_dist[i],"		",train.path_speed[i])
		if i==train.path_trk_f then
			printf("--Front on-track border here--")		
		end
	end
end

-- Function to get path entry at a position. This function will automatically calculate more of the path when required.
-- returns: pos, on_track
function advtrains.path_get(train, index)
	if not train.path then
		error("For train "..train.id..": path_get called but there's no path set yet!")
	end
	if index ~= atfloor(index) then
		error("For train "..train.id..": Called path_get() but index="..index.." is not a round number")
	end
	
	local pef = train.path_ext_f
	-- generate forward (front of train, positive)
	while index > pef do
		local pos = train.path[pef]
		local connid = train.path_cn[pef]
		local node_ok, this_conns, adj_pos, adj_connid, conn_idx, nextrail_y, next_conns
		if pef == train.path_trk_f then
			node_ok, this_conns = advtrains.get_rail_info_at(pos)
			if not node_ok then error("For train "..train.id..": Path item "..pef.." on-track but not a valid node!") end
			adj_pos, adj_connid, conn_idx, nextrail_y, next_conns = advtrains.get_adjacent_rail(pos, this_conns, connid, train.drives_on)
		end
		pef = pef + 1
		if adj_pos then
			advtrains.occ.set_item(train.id, adj_pos, pef)
		
			-- If we have split points, notify accordingly
			local mconnid = advtrains.get_matching_conn(adj_connid, #next_conns)
			if #next_conns==3 and adj_connid==1 and train.points_split and train.points_split[advtrains.encode_pos(adj_pos)] then
				--atdebug(id,"has split points restored at",adj_pos)
				mconnid = 3
			end
		
			adj_pos.y = adj_pos.y + nextrail_y
			train.path_cp[pef] = adj_connid
			train.path_cn[pef] = mconnid
			train.path_dir[pef] = advtrains.conn_angle_median(next_conns[adj_connid].c, next_conns[mconnid].c)
			train.path_trk_f = pef
		else
			-- off-track fallback behavior
			adj_pos = advtrains.pos_add_angle(pos, train.path_dir[pef-1])
			--atdebug("Offtrack overgenerating(front) at",adj_pos,"index",peb,"trkf",train.path_trk_f)
			train.path_dir[pef] = train.path_dir[pef-1]
		end
		train.path[pef] = adj_pos
		train.path_dist[pef] = train.path_dist[pef-1] + vector.distance(pos, adj_pos)