From 33c839b40d48e154f5b03619a9bdce1bed1fc602 Mon Sep 17 00:00:00 2001
From: orwell96 <orwell@bleipb.de>
Date: Wed, 10 Oct 2018 21:49:52 +0200
Subject: Add signal safety control override, restructure control system

---
 advtrains_interlocking/database.lua       |   2 +-
 advtrains_interlocking/init.lua           |   5 +-
 advtrains_interlocking/lzb.lua            | 194 ++++++++++++++++++++++++++++++
 advtrains_interlocking/train_related.lua  | 183 ----------------------------
 advtrains_interlocking/train_sections.lua | 183 ++++++++++++++++++++++++++++
 5 files changed, 382 insertions(+), 185 deletions(-)
 create mode 100644 advtrains_interlocking/lzb.lua
 delete mode 100644 advtrains_interlocking/train_related.lua
 create mode 100644 advtrains_interlocking/train_sections.lua

(limited to 'advtrains_interlocking')

diff --git a/advtrains_interlocking/database.lua b/advtrains_interlocking/database.lua
index 030a5e0..af90880 100644
--- a/advtrains_interlocking/database.lua
+++ b/advtrains_interlocking/database.lua
@@ -485,7 +485,7 @@ function ildb.get_ip_signal_asp(pts, connid)
 			ildb.clear_ip_signal(pts, connid)
 			return nil
 		end
-		return asp
+		return asp, p
 	end
 	return nil
 end
diff --git a/advtrains_interlocking/init.lua b/advtrains_interlocking/init.lua
index 19501b6..d6625eb 100644
--- a/advtrains_interlocking/init.lua
+++ b/advtrains_interlocking/init.lua
@@ -8,9 +8,12 @@ local modpath = minetest.get_modpath(minetest.get_current_modname()) .. DIR_DELI
 dofile(modpath.."database.lua")
 dofile(modpath.."signal_api.lua")
 dofile(modpath.."demosignals.lua")
-dofile(modpath.."train_related.lua")
+dofile(modpath.."train_sections.lua")
 dofile(modpath.."route_prog.lua")
 dofile(modpath.."routesetting.lua")
 dofile(modpath.."tcb_ts_ui.lua")
 
+dofile(modpath.."lzb.lua")
+
+
 minetest.register_privilege("interlocking", {description = "Can set up track sections, routes and signals.", give_to_singleplayer = true})
diff --git a/advtrains_interlocking/lzb.lua b/advtrains_interlocking/lzb.lua
new file mode 100644
index 0000000..c9e40ab
--- /dev/null
+++ b/advtrains_interlocking/lzb.lua
@@ -0,0 +1,194 @@
+-- lzb.lua
+-- Enforced and/or automatic train override control, obeying signals
+
+--[[
+Documentation of train.lzb table
+train.lzb = {
+	trav = Current index that the traverser has advanced so far
+	travsht = boolean indicating whether the train will be a shunt move at "trav"
+	travspd = speed restriction at end of traverser
+	travwspd = warning speed res.
+	oncoming = table containing oncoming signals, in order of appearance on the path
+		{
+			pos = position of the signal (not the IP!)
+			idx = where this is on the path
+			spd = speed allowed to pass (determined dynamically)
+		}
+}
+each step, for every item in "oncoming", we need to determine the location to start braking (+ some safety margin)
+and, if we passed this point for at least one of the items, initiate brake.
+When speed has dropped below, say 3, decrease the margin to zero, so that trains actually stop at the signal IP.
+The spd variable and travsht need to be updated on every aspect change. it's probably best to reset everything when any aspect changes
+
+The traverser stops at signals that result in spd==0, because changes beyond there are likely.
+]]
+
+local il = advtrains.interlocking
+
+local BRAKE_SPACE = 10
+local AWARE_ZONE  = 50
+
+local ADD_STAND  =  2
+local ADD_SLOW   =  1
+local ADD_FAST   =  10
+
+local SHUNT_SPEED_MAX = 4
+
+local function look_ahead(id, train)
+	
+	local acc = advtrains.get_acceleration(train, 1)
+	local vel = train.velocity
+	local brakedst = -(vel*vel) / (2*acc)
+	
+	local brake_i = advtrains.path_get_index_by_offset(train, train.index, brakedst + BRAKE_SPACE)
+	--local aware_i = advtrains.path_get_index_by_offset(train, brake_i, AWARE_ZONE)
+	
+	local lzb = train.lzb
+	local trav = lzb.trav
+	local travspd = lzb.travspd
+	local travwspd = lzb.travwspd
+	local lspd
+	
+	train.debug = lspd
+	
+	while trav <= brake_i and (not lspd or lspd>0) do
+		trav = trav + 1
+		local pos = advtrains.path_get(train, trav)
+		local pts = advtrains.roundfloorpts(pos)
+		local cn  = train.path_cn[trav]
+		-- check offtrack
+		if trav > train.path_trk_f then
+			lspd = 0
+			table.insert(lzb.oncoming, {
+				idx = trav-1,
+				spd = 0,
+			})
+		else
+			-- check for signal
+			local asp, spos = il.db.get_ip_signal_asp(pts, cn)
+			--atdebug("trav: ",pos, cn, asp, spos, "travsht=", lzb.travsht)
+			if asp then
+				local nspd = 0
+				--interpreting aspect and determining speed to proceed
+				if lzb.travsht then
+					--shunt move
+					if asp.shunt.free then
+						nspd = SHUNT_SPEED_MAX
+					elseif asp.shunt.proceed_as_main and asp.main.free then
+						nspd = asp.main.speed
+						lzb.travsht = false
+					end
+				else
+					--train move
+					if asp.main.free then
+						nspd = asp.main.speed
+					elseif asp.shunt.free then
+						nspd = SHUNT_SPEED_MAX
+						lzb.travsht = true
+					end
+				end
+				-- nspd can now be: 1. !=0: new speed restriction, 2. =0: stop here or 3. nil: keep travspd
+				if nspd then
+					if nspd == -1 then
+						travspd = nil
+					else
+						travspd = nspd
+					end
+				end
+				
+				local nwspd = asp.info.w_speed
+				if nwspd then
+					if nwspd == -1 then
+						travwspd = nil
+					else
+						travwspd = nwspd
+					end
+				end
+				--atdebug("ns,wns,ts,wts", nspd, nwspd, travspd, travwspd)
+				lspd = travspd
+				if travwspd and (not lspd or lspd>travwspd) then
+					lspd = travwspd
+				end
+				
+				table.insert(lzb.oncoming, {
+					pos = spos,
+					idx = trav,
+					spd = lspd,
+				})
+				-- TODO register aspect change callback!
+			end
+		end
+	end
+	
+	lzb.trav = trav
+	lzb.travspd = travspd
+	lzb.travwspd = travwspd
+	
+end
+
+--[[
+Distance needed to accelerate from v0 to v1 with constant acceleration a:
+
+         v1 - v0     a   / v1 - v0 \ 2
+s = v0 * -------  +  - * | ------- |
+            a        2   \    a    /
+]]
+
+local function apply_control(id, train)
+	local lzb = train.lzb
+	
+	local i = 1
+	while i<#lzb.oncoming do
+		if lzb.oncoming[i].idx < train.index then
+			train.speed_restriction = lzb.oncoming[i].spd
+			table.remove(lzb.oncoming, i)
+		else
+			i = i + 1
+		end
+	end
+	
+	for i, it in ipairs(lzb.oncoming) do
+		local a = advtrains.get_acceleration(train, 1) --should be negative
+		local v0 = train.velocity
+		local v1 = it.spd
+		if v1 and v1 <= v0 then
+			local f = (v1-v0) / a
+			local s = v0*f + a*f*f/2
+			
+			local st = s + ADD_SLOW
+			if v0 > 3 then
+				st = s + ADD_FAST
+			end
+			if v0<=0 then
+				st = s + ADD_STAND
+			end
+			
+			local i = advtrains.path_get_index_by_offset(train, it.idx, -st)
+			
+			--train.debug = dump({v0f=v0*f, aff=a*f*f,v0=v0, v1=v1, f=f, a=a, s=s, st=st, i=i, idx=train.index})
+			if i <= train.index then
+				-- Gotcha! Braking...
+				train.ctrl.lzb = 1
+				--train.debug = train.debug .. "BRAKE!!!"
+				return
+			end
+		end
+	end
+	train.ctrl.lzb = nil
+end
+
+
+advtrains.te_register_on_new_path(function(id, train)
+	train.lzb = {
+		trav = atfloor(train.index),
+		travsht = train.is_shunt,
+		oncoming = {}
+	}
+	train.ctrl.lzb = nil
+	look_ahead(id, train)
+end)
+
+advtrains.te_register_on_update(function(id, train)
+	look_ahead(id, train)
+	apply_control(id, train)
+end)
diff --git a/advtrains_interlocking/train_related.lua b/advtrains_interlocking/train_related.lua
deleted file mode 100644
index 8bc9716..0000000
--- a/advtrains_interlocking/train_related.lua
+++ /dev/null
@@ -1,183 +0,0 @@
--- train_related.lua
--- Occupation of track sections - mainly implementation of train callbacks
-
---[[
-Track section occupation is saved as follows
-
-In train:
-train.il_sections = {
-	[n] = {ts_id = <...> (origin = <sigd>)}
-}
--- "origin" is the TCB (signal describer) the train initially entered this section
-
-In track section
-ts.trains = {
-	[n] = <train_id>
-}
-
-When any inconsistency is detected, we will assume the most restrictive setup.
-It will be possible to indicate a section "free" via the GUI.
-]]
-
-local ildb = advtrains.interlocking.db
-
-
-local function itexist(tbl, com)
-	for _,item in ipairs(tbl) do
-		if (item==com) then
-			return true
-		end
-	end
-	return false
-end
-local function itkexist(tbl, ikey, com)
-	for _,item in ipairs(tbl) do
-		if item[ikey] == com then
-			return true
-		end
-	end
-	return false
-end
-
-local function itremove(tbl, com)
-	local i=1
-	while i <= #tbl do
-		if tbl[i] == com then
-			table.remove(tbl, i)
-		else
-			i = i + 1
-		end
-	end
-end
-local function itkremove(tbl, ikey, com)
-	local i=1
-	while i <= #tbl do
-		if tbl[i][ikey] == com then
-			table.remove(tbl, i)
-		else
-			i = i + 1
-		end
-	end
-end
-
-local function setsection(tid, train, ts_id, ts, origin)
-	-- train
-	if not train.il_sections then train.il_sections = {} end
-	if not itkexist(train.il_sections, "ts_id", ts_id) then
-		table.insert(train.il_sections, {ts_id = ts_id, origin = origin})
-	end
-	
-	-- ts
-	if not ts.trains then ts.trains = {} end
-	if not itexist(ts.trains, tid) then
-		table.insert(ts.trains, tid)
-	end
-	
-	-- route setting - clear route state
-	if ts.route then
-		if ts.route.first then
-			-- this is the first route section. clear route status from origin sigd
-			local tcbs = advtrains.interlocking.db.get_tcbs(ts.route.origin)
-			if tcbs then
-				tcbs.route_committed = nil
-				tcbs.aspect = nil
-				advtrains.interlocking.update_signal_aspect(tcbs)
-				if tcbs.route_auto then
-					advtrains.interlocking.route.update_route(ts.route.origin, tcbs)
-				else
-					tcbs.routeset = nil
-				end
-			end
-		end
-		ts.route = nil
-	end
-	
-end
-
-local function freesection(tid, train, ts_id, ts)
-	-- train
-	if not train.il_sections then train.il_sections = {} end
-	itkremove(train.il_sections, "ts_id", ts_id)
-	
-	-- ts
-	if not ts.trains then ts.trains = {} end
-	itremove(ts.trains, tid)
-	
-	if ts.route_post then
-		advtrains.interlocking.route.free_route_locks(ts_id, ts.route_post.locks)
-		if ts.route_post.next then
-			--this does nothing when the train went the right way, because
-			-- "route" info is already cleared.
-			advtrains.interlocking.route.cancel_route_from(ts.route_post.next)
-		end
-		ts.route_post = nil
-	end
-	-- This must be delayed, because this code is executed in-between a train step
-	-- TODO use luaautomation timers?
-	minetest.after(0, advtrains.interlocking.route.update_waiting, "ts", ts_id)
-end
-
-
--- This is regular operation
--- The train is on a track and drives back and forth
-
--- This sets the section for both directions, to be failsafe
-advtrains.tnc_register_on_enter(function(pos, id, train, index)
-	local tcb = ildb.get_tcb(pos)
-	if tcb then
-		for connid=1,2 do
-			local ts = tcb[connid].ts_id and ildb.get_ts(tcb[connid].ts_id)
-			if ts then
-				setsection(id, train, tcb[connid].ts_id, ts, {p=pos, s=connid})
-			end
-		end
-	end
-end)
-
-
--- this time, of course, only clear the backside (cp connid)
-advtrains.tnc_register_on_leave(function(pos, id, train, index)
-	local tcb = ildb.get_tcb(pos)
-	if tcb and train.path_cp[index] then
-		local connid = train.path_cp[index]
-		local ts = tcb[connid].ts_id and ildb.get_ts(tcb[connid].ts_id)
-		if ts then
-			freesection(id, train, tcb[connid].ts_id, ts)
-		end
-	end
-end)
-
--- those callbacks are needed to account for created and removed trains (also regarding coupling)
-
-advtrains.te_register_on_create(function(id, train)
-	-- let's see what track sections we find here
-	local index = atround(train.index)
-	local pos = advtrains.path_get(train, index)
-	local ts_id, origin = ildb.get_ts_at_pos(pos)
-	if ts_id then
-		local ts = ildb.get_ts(ts_id)
-		if ts then
-			setsection(id, train, ts_id, ts, origin)
-		else
-			atwarn("ILDB corruption: TCB",origin," has invalid TS reference")
-		end
-	elseif ts_id==nil then
-		atwarn("Train",id,": Unable to determine whether to block a track section!")
-	else
-		--atdebug("Train",id,": Outside of interlocked area!")
-	end
-end)
-
-advtrains.te_register_on_remove(function(id, train)
-	if train.il_sections then
-		for idx, item in ipairs(train.il_sections) do
-			
-			local ts = item.ts_id and ildb.get_ts(item.ts_id)
-			
-			if ts and ts.trains then
-				itremove(ts.trains, id)
-			end
-		end
-		train.il_sections = nil
-	end
-end)
diff --git a/advtrains_interlocking/train_sections.lua b/advtrains_interlocking/train_sections.lua
new file mode 100644
index 0000000..8bc9716
--- /dev/null
+++ b/advtrains_interlocking/train_sections.lua
@@ -0,0 +1,183 @@
+-- train_related.lua
+-- Occupation of track sections - mainly implementation of train callbacks
+
+--[[
+Track section occupation is saved as follows
+
+In train:
+train.il_sections = {
+	[n] = {ts_id = <...> (origin = <sigd>)}
+}
+-- "origin" is the TCB (signal describer) the train initially entered this section
+
+In track section
+ts.trains = {
+	[n] = <train_id>
+}
+
+When any inconsistency is detected, we will assume the most restrictive setup.
+It will be possible to indicate a section "free" via the GUI.
+]]
+
+local ildb = advtrains.interlocking.db
+
+
+local function itexist(tbl, com)
+	for _,item in ipairs(tbl) do
+		if (item==com) then
+			return true
+		end
+	end
+	return false
+end
+local function itkexist(tbl, ikey, com)
+	for _,item in ipairs(tbl) do
+		if item[ikey] == com then
+			return true
+		end
+	end
+	return false
+end
+
+local function itremove(tbl, com)
+	local i=1
+	while i <= #tbl do
+		if tbl[i] == com then
+			table.remove(tbl, i)
+		else
+			i = i + 1
+		end
+	end
+end
+local function itkremove(tbl, ikey, com)
+	local i=1
+	while i <= #tbl do
+		if tbl[i][ikey] == com then
+			table.remove(tbl, i)
+		else
+			i = i + 1
+		end
+	end
+end
+
+local function setsection(tid, train, ts_id, ts, origin)
+	-- train
+	if not train.il_sections then train.il_sections = {} end
+	if not itkexist(train.il_sections, "ts_id", ts_id) then
+		table.insert(train.il_sections, {ts_id = ts_id, origin = origin})
+	end
+	
+	-- ts
+	if not ts.trains then ts.trains = {} end
+	if not itexist(ts.trains, tid) then
+		table.insert(ts.trains, tid)
+	end
+	
+	-- route setting - clear route state
+	if ts.route then
+		if ts.route.first then
+			-- this is the first route section. clear route status from origin sigd
+			local tcbs = advtrains.interlocking.db.get_tcbs(ts.route.origin)
+			if tcbs then
+				tcbs.route_committed = nil
+				tcbs.aspect = nil
+				advtrains.interlocking.update_signal_aspect(tcbs)
+				if tcbs.route_auto then
+					advtrains.interlocking.route.update_route(ts.route.origin, tcbs)
+				else
+					tcbs.routeset = nil
+				end
+			end
+		end
+		ts.route = nil
+	end
+	
+end
+
+local function freesection(tid, train, ts_id, ts)
+	-- train
+	if not train.il_sections then train.il_sections = {} end
+	itkremove(train.il_sections, "ts_id", ts_id)
+	
+	-- ts
+	if not ts.trains then ts.trains = {} end
+	itremove(ts.trains, tid)
+	
+	if ts.route_post then
+		advtrains.interlocking.route.free_route_locks(ts_id, ts.route_post.locks)
+		if ts.route_post.next then
+			--this does nothing when the train went the right way, because
+			-- "route" info is already cleared.
+			advtrains.interlocking.route.cancel_route_from(ts.route_post.next)
+		end
+		ts.route_post = nil
+	end
+	-- This must be delayed, because this code is executed in-between a train step
+	-- TODO use luaautomation timers?
+	minetest.after(0, advtrains.interlocking.route.update_waiting, "ts", ts_id)
+end
+
+
+-- This is regular operation
+-- The train is on a track and drives back and forth
+
+-- This sets the section for both directions, to be failsafe
+advtrains.tnc_register_on_enter(function(pos, id, train, index)
+	local tcb = ildb.get_tcb(pos)
+	if tcb then
+		for connid=1,2 do
+			local ts = tcb[connid].ts_id and ildb.get_ts(tcb[connid].ts_id)
+			if ts then
+				setsection(id, train, tcb[connid].ts_id, ts, {p=pos, s=connid})
+			end
+		end
+	end
+end)
+
+
+-- this time, of course, only clear the backside (cp connid)
+advtrains.tnc_register_on_leave(function(pos, id, train, index)
+	local tcb = ildb.get_tcb(pos)
+	if tcb and train.path_cp[index] then
+		local connid = train.path_cp[index]
+		local ts = tcb[connid].ts_id and ildb.get_ts(tcb[connid].ts_id)
+		if ts then
+			freesection(id, train, tcb[connid].ts_id, ts)
+		end
+	end
+end)
+
+-- those callbacks are needed to account for created and removed trains (also regarding coupling)
+
+advtrains.te_register_on_create(function(id, train)
+	-- let's see what track sections we find here
+	local index = atround(train.index)
+	local pos = advtrains.path_get(train, index)
+	local ts_id, origin = ildb.get_ts_at_pos(pos)
+	if ts_id then
+		local ts = ildb.get_ts(ts_id)
+		if ts then
+			setsection(id, train, ts_id, ts, origin)
+		else
+			atwarn("ILDB corruption: TCB",origin," has invalid TS reference")
+		end
+	elseif ts_id==nil then
+		atwarn("Train",id,": Unable to determine whether to block a track section!")
+	else
+		--atdebug("Train",id,": Outside of interlocked area!")
+	end
+end)
+
+advtrains.te_register_on_remove(function(id, train)
+	if train.il_sections then
+		for idx, item in ipairs(train.il_sections) do
+			
+			local ts = item.ts_id and ildb.get_ts(item.ts_id)
+			
+			if ts and ts.trains then
+				itremove(ts.trains, id)
+			end
+		end
+		train.il_sections = nil
+	end
+end)
-- 
cgit v1.2.3