--trainlogic.lua
--controls train entities stuff about connecting/disconnecting/colliding trains and other things

local print=function(t) minetest.log("action", t) minetest.chat_send_all(t) end

advtrains.train_accel_force=5--per second and divided by number of wagons
advtrains.train_brake_force=3--per second, not divided by number of wagons
advtrains.train_emerg_force=10--for emergency brakes(when going off track)

advtrains.audit_interval=10

advtrains.all_traintypes={}
function advtrains.register_train_type(name, drives_on, max_speed)
	advtrains.all_traintypes[name]={}
	advtrains.all_traintypes[name].drives_on=drives_on
	advtrains.all_traintypes[name].max_speed=max_speed or 10
end


advtrains.trains={}

--load initially
advtrains.fpath=minetest.get_worldpath().."/advtrains"
local file, err = io.open(advtrains.fpath, "r")
if not file then
	local er=err or "Unknown Error"
	print("[advtrains]Failed loading advtrains save file "..er)
else
	local tbl = minetest.deserialize(file:read("*a"))
	if type(tbl) == "table" then
		advtrains.trains=tbl
	end
	file:close()
end
advtrains.fpath_ws=minetest.get_worldpath().."/advtrains_wagon_save"
local file, err = io.open(advtrains.fpath_ws, "r")
if not file then
	local er=err or "Unknown Error"
	print("[advtrains]Failed loading advtrains save file "..er)
else
	local tbl = minetest.deserialize(file:read("*a"))
	if type(tbl) == "table" then
		advtrains.wagon_save=tbl
	end
	file:close()
end


advtrains.save = function()
	--print("[advtrains]saving")
	advtrains.invalidate_all_paths()
	local datastr = minetest.serialize(advtrains.trains)
	if not datastr then
		minetest.log("error", "[advtrains] Failed to serialize train data!")
		return
	end
	local file, err = io.open(advtrains.fpath, "w")
	if err then
		return err
	end
	file:write(datastr)
	file:close()
	
	-- update wagon saves
	for _,wagon in pairs(minetest.luaentities) do
		if wagon.is_wagon and wagon.initialized then
			advtrains.wagon_save[wagon.unique_id]=advtrains.merge_tables(wagon)--so, will only copy non_metatable elements
		end
	end
	
	datastr = minetest.serialize(advtrains.wagon_save)
	if not datastr then
		minetest.log("error", "[advtrains] Failed to serialize train data!")
		return
	end
	file, err = io.open(advtrains.fpath_ws, "w")
	if err then
		return err
	end
	file:write(datastr)
	file:close()
end
minetest.register_on_shutdown(advtrains.save)

advtrains.save_and_audit_timer=advtrains.audit_interval
minetest.register_globalstep(function(dtime)
	advtrains.save_and_audit_timer=advtrains.save_and_audit_timer-dtime
	if advtrains.save_and_audit_timer<=0 then
		--print("[advtrains] audit step")
		--clean up orphaned trains
		for k,v in pairs(advtrains.trains) do
			advtrains.update_trainpart_properties(k)
			if #v.trainparts==0 then
				advtrains.trains[k]=nil
			end
		end
		--save
		advtrains.save()
		advtrains.save_and_audit_timer=advtrains.audit_interval
	end
	--regular train step
	for k,v in pairs(advtrains.trains) do
		advtrains.train_step(k, v, dtime)
	end
end)

function advtrains.train_step(id, train, dtime)
	
	--TODO check for all vars to be present
	
	--if not train.last_pos then advtrains.trains[id]=nil return end
	
	if not advtrains.pathpredict(id, train) then 
		--print("pathpredict failed(returned false)")
		train.velocity=0
		train.tarvelocity=0
		return
	end
	
	local path=advtrains.get_or_create_path(id, train)
	if not path then
		train.velocity=0
		train.tarvelocity=0
		--print("train has no path")
		return 
	end
	--apply off-track handling:
	local front_off_track=train.max_index_on_track and train.index>train.max_index_on_track
	local back_off_track=train.min_index_on_track and (train.index-train.trainlen)<train.min_index_on_track
	if front_off_track and back_off_track then--allow movement in both directions
		if train.tarvelocity>1 then train.tarvelocity=1 end
		if train.tarvelocity<-1 then train.tarvelocity=-1 end
	elseif front_off_track then--allow movement only backward
		if train.tarvelocity>0 then train.tarvelocity=0 end
		if train.tarvelocity<-1 then train.tarvelocity=-1 end
	elseif back_off_track then--allow movement only forward
		if train.tarvelocity>1 then train.tarvelocity=1 end
		if train.tarvelocity<0 then train.tarvelocity=0 end
	end
	
	--move
	if not train.velocity then
		train.velocity=0
	end
	train.index=train.index and train.index+((train.velocity/(train.path_dist[math.floor(train.index)] or 1))*dtime) or 0
	--check for collisions by finding objects
	--front
	local search_radius=4
	
	local posfront=path[math.floor(train.index+1)]
	if posfront then
		local objrefs=minetest.get_objects_inside_radius(posfront, search_radius)
		for _,v in pairs(objrefs) do
			local le=v:get_luaentity()
			if le and le.is_wagon and le.initialized and le.train_id~=id then
				advtrains.try_connect_trains(id, le.train_id)
			end
		end
	end
	local posback=path[math.floor(train.index-(train.trainlen or 0)-1)]
	if posback then
		local objrefs=minetest.get_objects_inside_radius(posback, search_radius)
		for _,v in pairs(objrefs) do
			local le=v:get_luaentity()
			if le and le.is_wagon and le.initialized and le.train_id~=id then
				advtrains.try_connect_trains(id, le.train_id)
			end
		end
	end
	
	--check for any trainpart entities if they have been unloaded. do this only if both front and end positions are loaded, to ensure train entities will be placed inside loaded area, and only every second.
	train.check_trainpartload=train.check_trainpartload-dtime
	if train.check_trainpartload<=0 and posfront and posback and minetest.get_node_or_nil(posfront) and minetest.get_node_or_nil(posback) then
		--it is better to iterate luaentites only once
		local found_uids={}
		for _,wagon in pairs(minetest.luaentities) do
			if wagon.is_wagon and wagon.initialized and wagon.train_id==id then
				if found_uids[wagon.unique_id] then
					--duplicate found, delete it
					wagon.object and wagon.object:remove()
				else
					found_uids[wagon.unique_id]=true
				end
			end
		end
		--now iterate trainparts and check. then cross them out to see if there are wagons over for any reason
		for pit, w_id in ipairs(train.trainparts) do
			if found_uids[w_id] then
				found_uids[w_id]=nil
			elseif advtrains.wagon_save[w_id] then
				--spawn a new and initialize it with the properties from wagon_save
				local le=minetest.env:add_entity(posfront, "advtrains:"..sysname):get_luaentity()
				for k,v in pairs(advtrains.wagon_save[w_id]) do
					le[k]=v
				end
			else
				--what the hell...
				local le=minetest.env:add_entity(posfront, "advtrains:"..sysname):get_luaentity()
				le.unique_id=w_id
				le.train_id=id
				le.pos_in_trainparts=pit
				advtrains.update_trainpart_properties(id, train)
			end
		end
		train.check_trainpartload=1
	end
	
	
	--handle collided_with_env
	if train.recently_collided_with_env then
		train.tarvelocity=0
		if train.velocity==0 then
			train.recently_collided_with_env=false--reset status when stopped
		end
	end
	if train.locomotives_in_train==0 then
		train.tarvelocity=0
	end
	--apply tarvel(but with physics in mind!)
	if train.velocity~=train.tarvelocity then
		local applydiff=0
		local mass=#train.trainparts
		local diff=math.abs(train.tarvelocity)-math.abs(train.velocity)
		if diff>0 then--accelerating, force will be brought on only by locomotives.
			--print("accelerating with default force")
			applydiff=(math.min((advtrains.train_accel_force*train.locomotives_in_train*dtime)/mass, math.abs(diff)))
		else--decelerating
			if front_off_track or back_off_track or train.recently_collided_with_env then --every wagon has a brake, so not divided by mass.
				--print("braking with emergency force")
				applydiff=(math.min((advtrains.train_emerg_force*dtime), math.abs(diff)))
			else
				--print("braking with default force")
				applydiff=(math.min((advtrains.train_brake_force*dtime), math.abs(diff)))
			end
		end
		train.velocity=train.velocity+(applydiff*math.sign(train.tarvelocity-train.velocity))
	end
	
end

--the 'leader' concept has been overthrown, we won't rely on MT's "buggy object management"
--structure of train table:
--[[
trains={
	[train_id]={
		trainparts={
			[n]=wagon_id
		}
		path={path}
		velocity
		tarvelocity
		index
		trainlen
		path_inv_level
		last_pos       |
		last_dir       | for pathpredicting.
		no_connect_for_movements (index way counter for when not to connect again) TODO implement
	}
}
--a wagon itself has the following properties:
wagon={
	unique_id
	train_id
	pos_in_train (is index difference, including train_span stuff)
	pos_in_trainparts (is index in trainparts tabel of trains)
}
inherited by metatable:
wagon_proto={
	wagon_span
}
]]

--returns new id
function advtrains.create_new_train_at(pos, pos_prev, traintype)
	local newtrain_id=os.time()..os.clock()
	while advtrains.trains[newtrain_id] do newtrain_id=os.time()..os.clock() end--ensure uniqueness(will be unneccessary)
	
	advtrains.trains[newtrain_id]={}
	advtrains.trains[newtrain_id].last_pos=pos
	advtrains.trains[newtrain_id].last_pos_prev=pos_prev
	advtrains.trains[newtrain_id].traintype=traintype
	advtrains.trains[newtrain_id].tarvelocity=0
	advtrains.trains[newtrain_id].velocity=0
	advtrains.trains[newtrain_id].trainparts={}
	return newtrain_id
end

--returns false on failure. handle this case!
function advtrains.pathpredict(id, train)
	
	--print("pos ",x,y,z)
	--::rerun::
	if not train.index then train.index=0 end
	if not train.path or #train.path<2 then
		if not train.last_pos then
			--no chance to recover
			print("[advtrains]train hasn't saved last-pos, removing train.")
			advtrains.train[id]=nil
			return false
		end
		
		local node_ok=advtrains.get_rail_info_at(advtrains.round_vector_floor_y(train.last_pos), train.traintype)
		
		if node_ok==nil then
			--block not loaded, do nothing
			return nil
		elseif node_ok==false then
			print("[advtrains]no track here, (fail) removing train.")
			advtrains.trains[id]=nil
			return false
		end
		
		if not train.last_pos_prev then
			--no chance to recover
			print("[advtrains]train hasn't saved last-pos_prev, removing train.")
			advtrains.trains[id]=nil
			return false
		end
		
		local prevnode_ok=advtrains.get_rail_info_at(advtrains.round_vector_floor_y(train.last_pos_prev), train.traintype)
		
		if prevnode_ok==nil then
			--block not loaded, do nothing
			return nil
		elseif prevnode_ok==false then
			print("[advtrains]no track at prev, (fail) removing train.")
			advtrains.trains[id]=nil
			return false
		end
		
		train.index=(train.restore_add_index or 0)+(train.savedpos_off_track_index_offset or 0)
		--restore_add_index is set by save() to prevent trains hopping to next round index. should be between -0.5 and 0.5
		--savedpos_off_track_index_offset is set if train went off track. see below.
		train.path={}
		train.path_dist={}
		train.path[0]=train.last_pos
		train.path[-1]=train.last_pos_prev
		train.path_dist[-1]=vector.distance(train.last_pos, train.last_pos_prev)
	end
	
	local maxn=advtrains.maxN(train.path)
	while (maxn-train.index) < 2 do--pregenerate
		--print("[advtrains]maxn conway for ",maxn,minetest.pos_to_string(path[maxn]),maxn-1,minetest.pos_to_string(path[maxn-1]))
		local conway=advtrains.conway(train.path[maxn], train.path[maxn-1], train.traintype)
		if conway then
			train.path[maxn+1]=conway
			train.max_index_on_track=maxn
		else
			--do as if nothing has happened and preceed with path
			--but do not update max_index_on_track
			print("over-generating path max to index "..maxn+1)
			train.path[maxn+1]=vector.add(train.path[maxn], vector.subtract(train.path[maxn], train.path[maxn-1]))
		end
		train.path_dist[maxn]=vector.distance(train.path[maxn+1], train.path[maxn])
		maxn=advtrains.maxN(train.path)
	end
	
	local minn=advtrains.minN(train.path)
	while (train.index-minn) < (train.trainlen or 0) + 2 do --post_generate. has to be at least trainlen.
		--print("[advtrains]minn conway for ",minn,minetest.pos_to_string(path[minn]),minn+1,minetest.pos_to_string(path[minn+1]))
		local conway=advtrains.conway(train.path[minn], train.path[minn+1], train.traintype)
		if conway then
			train.path[minn-1]=conway
			train.min_index_on_track=minn
		else
			--do as if nothing has happened and preceed with path
			--but do not update min_index_on_track
			print("over-generating path min to index "..minn-1)
			train.path[minn-1]=vector.add(train.path[minn], vector.subtract(train.path[minn], train.path[minn+1]))
		end
		train.path_dist[minn-1]=vector.distance(train.path[minn], train.path[minn-1])
		minn=advtrains.minN(train.path)
	end
	if not train.min_index_on_track then train.min_index_on_track=0 end
	if not train.max_index_on_track then train.max_index_on_track=0 end
	
	--make pos/yaw available for possible recover calls
	if train.max_index_on_track<train.index then --whoops, train went too far. the saved position will be the last one that lies on a track, and savedpos_off_track_index_offset will hold how far to go from here
		train.savedpos_off_track_index_offset=train.index-train.max_index_on_track
		train.last_pos=train.path[train.max_index_on_track]
		train.last_pos_prev=train.path[train.max_index_on_track-1]
		--print("train is off-track (front), last positions kept at "..minetest.pos_to_string(train.last_pos).." / "..minetest.pos_to_string(train.last_pos_prev))
	elseif train.min_index_on_track+1>train.index then --whoops, train went even more far. same behavior
		train.savedpos_off_track_index_offset=train.index-train.min_index_on_track
		train.last_pos=train.path[train.min_index_on_track+1]
		train.last_pos_prev=train.path[train.min_index_on_track]
		--print("train is off-track (back), last positions kept at "..minetest.pos_to_string(train.last_pos).." / "..minetest.pos_to_string(train.last_pos_prev))
	else --regular case
		train.savedpos_off_track_index_offset=nil
		train.last_pos=train.path[math.floor(train.index+0.5)]
		train.last_pos_prev=train.path[math.floor(train.index-0.5)]
	end
	return train.path
end

function advtrains.get_or_create_path(id, train)
	if not train.path then return advtrains.pathpredict(id, train) end
	
	return train.path
end

function advtrains.add_wagon_to_train(wagon, train_id, index)
	local train=advtrains.trains[train_id]
	if index then
		table.insert(train.trainparts, index, wagon.unique_id)
	else
		table.insert(train.trainparts, wagon.unique_id)
	end
	--this is not the usual case!!!
	--we may set initialized because the wagon has no chance to step()
	wagon.initialized=true
	advtrains.update_trainpart_properties(train_id)
end
function advtrains.update_trainpart_properties(train_id, invert_flipstate)
	local train=advtrains.trains[train_id]
	local rel_pos=0
	local count_l=0
	for i, w_id in ipairs(train.trainparts) do
		for _,wagon in pairs(minetest.luaentities) do
			if wagon.is_wagon and wagon.initialized and wagon.unique_id==w_id then
				rel_pos=rel_pos+wagon.wagon_span
				wagon.train_id=train_id
				wagon.pos_in_train=rel_pos
				wagon.pos_in_trainparts=i
				wagon.old_velocity_vector=nil
				if wagon.is_locomotive then
					count_l=count_l+1
				end
				if invert_flipstate then
					wagon.wagon_flipped = not wagon.wagon_flipped
				end
				rel_pos=rel_pos+wagon.wagon_span
			end
		end
	end
	train.trainlen=rel_pos
	train.locomotives_in_train=count_l
end

function advtrains.split_train_at_wagon(wagon)
	--get train
	local train=advtrains.trains[wagon.train_id]
	local pos_for_new_train=advtrains.get_or_create_path(wagon.train_id, train)[math.floor((train.index or 0)-wagon.pos_in_train-0.5)]
	local pos_for_new_train_prev=advtrains.get_or_create_path(wagon.train_id, train)[math.floor((train.index or 0)-wagon.pos_in_train-1.5)]
	
	--before doing anything, check if both are rails. else do not allow
	if not pos_for_new_train then
		print("split_train: pos_for_new_train not set")
		return false
	end
	local node_ok=advtrains.get_rail_info_at(advtrains.round_vector_floor_y(pos_for_new_train), train.traintype)
	if not node_ok then
		print("split_train: pos_for_new_train "..minetest.pos_to_string(advtrains.round_vector_floor_y(pos_for_new_train_prev)).." not loaded or is not a rail")
		return false
	end
	
	if not train.last_pos_prev then
		print("split_train: pos_for_new_train_prev not set")
		return false
	end
	
	local prevnode_ok=advtrains.get_rail_info_at(advtrains.round_vector_floor_y(pos_for_new_train), train.traintype)
	if not prevnode_ok then
		print("split_train: pos_for_new_train_prev "..minetest.pos_to_string(advtrains.round_vector_floor_y(pos_for_new_train_prev)).." not loaded or is not a rail")
		return false
	end
	
	--create subtrain
	local newtrain_id=advtrains.create_new_train_at(pos_for_new_train, pos_for_new_train_prev, train.traintype)
	local newtrain=advtrains.trains[newtrain_id]
	--insert all wagons to new train
	for k,v in ipairs(train.trainparts) do
		if k>=wagon.pos_in_trainparts then
			table.insert(newtrain.trainparts, v)
			train.trainparts[k]=nil
		end
	end
	--update train parts
	advtrains.update_trainpart_properties(wagon.train_id)--atm it still is the desierd id.
	advtrains.update_trainpart_properties(newtrain_id)
	train.tarvelocity=0
	newtrain.velocity=train.velocity
	newtrain.tarvelocity=0
end

--there are 4 cases:
--1/2. F<->R F<->R regular, put second train behind first
--->frontpos of first train will match backpos of second
--3.   F<->R R<->F flip one of these trains, take the other as new train
--->backpos's will match
--4.   R<->F F<->R flip one of these trains and take it as new parent
--->frontpos's will match
function advtrains.try_connect_trains(id1, id2)
	local train1=advtrains.trains[id1]
	local train2=advtrains.trains[id2]
	if not train1 or not train2 then return end
	if not train1.path or not train2.path then return end
	if train1.traintype~=train2.traintype then
		--TODO implement collision without connection
		return
	end
	if #train1.trainparts==0 or #train2.trainparts==0 then return end
	
	local frontpos1=train1.path[math.floor(train1.index+0.5)]
	local backpos1=train1.path[math.floor(train1.index-(train1.trainlen or 2)+0.5)]
	local frontpos2=train2.path[math.floor(train2.index+0.5)]
	local backpos2=train2.path[math.floor(train2.index-(train1.trainlen or 2)+0.5)]
	
	if not frontpos1 or not frontpos2 or not backpos1 or not backpos2 then return end
	
	--case 1 (first train is front)
	if vector.equals(frontpos2, backpos1) then
		advtrains.do_connect_trains(id1, id2)
	--case 2 (second train is front)
	elseif vector.equals(frontpos1, backpos2) then
		advtrains.do_connect_trains(id2, id1)
	--case 3 
	elseif vector.equals(backpos2, backpos1) then
		advtrains.invert_train(id2)
		advtrains.do_connect_trains(id1, id2)
	--case 4 
	elseif vector.equals(frontpos2, frontpos1) then
		advtrains.invert_train(id1)
		advtrains.do_connect_trains(id1, id2)
	end
end
function advtrains.do_connect_trains(first_id, second_id)
	local first_wagoncnt=#advtrains.trains[first_id].trainparts
	local second_wagoncnt=#advtrains.trains[second_id].trainparts
	
	for _,v in ipairs(advtrains.trains[second_id].trainparts) do
		table.insert(advtrains.trains[first_id].trainparts, v)
	end
	--kick it like physics (with mass being #wagons)
	local new_velocity=((advtrains.trains[first_id].velocity*first_wagoncnt)+(advtrains.trains[second_id].velocity*second_wagoncnt))/(first_wagoncnt+second_wagoncnt)
	advtrains.trains[second_id]=nil
	advtrains.update_trainpart_properties(first_id)
	advtrains.trains[first_id].velocity=new_velocity
	advtrains.trains[first_id].tarvelocity=0
end

function advtrains.invert_train(train_id)
	local train=advtrains.trains[train_id]
	
	local old_path=advtrains.get_or_create_path(train_id, train)
	train.path={}
	train.index=-train.index+train.trainlen
	train.velocity=-train.velocity
	train.tarvelocity=-train.tarvelocity
	for k,v in pairs(old_path) do
		train.path[-k]=v
	end
	local old_trainparts=train.trainparts
	train.trainparts={}
	for k,v in ipairs(old_trainparts) do
		table.insert(train.trainparts, 1, v)--notice insertion at first place
	end
	advtrains.update_trainpart_properties(train_id, true)
end

function advtrains.is_train_at_pos(pos)
	--print("istrainat: pos "..minetest.pos_to_string(pos))
	local checked_trains={}
	local objrefs=minetest.get_objects_inside_radius(pos, 2)
	for _,v in pairs(objrefs) do
		local le=v:get_luaentity()
		if le and le.is_wagon and le.initialized and le.train_id and not checked_trains[le.train_id] then
			--print("istrainat: checking "..le.train_id)
			checked_trains[le.train_id]=true
			local path=advtrains.get_or_create_path(le.train_id, le:train())
			if path then
				--print("has path")
				for i=math.floor(le:train().index-le:train().trainlen+0.5),math.floor(le:train().index+0.5) do
					if path[i] then
						--print("has pathitem "..i.." "..minetest.pos_to_string(path[i]))
						if vector.equals(advtrains.round_vector_floor_y(path[i]), pos) then
							return true
						end
					end
				end
			end
		end
	end
	return false
end
function advtrains.invalidate_all_paths()
	--print("invalidating all paths")
	for k,v in pairs(advtrains.trains) do
		if v.index then
			v.restore_add_index=v.index-math.floor(v.index+0.5)
		end
		v.path=nil
		v.index=nil
		v.min_index_on_track=nil
		v.max_index_on_track=nil
	end
end