-- Konzept:
-- Das Anhalten vor Signalen hat eine unpassende Verzögerung. Auch das Beschleunigungs- / Bremsverhalten
-- einiger Rollmaterialien ist nicht angemessen.
-- Dieses Modul soll eine bessere (gleichmäßigere) Beschleunigung / Verzögerung ermöglichen.
-- Im Allgemeinen wird der Start von Beschleunigung / Verzögerung durch einen Kontaktpunkt ausgelöst.
-- Meist wird es ein Bremsen bis zum Stillstand sein; das wird durch eine konstante Verzögerung erreicht.
-- Im einfachsten Fall gibt man die Entfernung bis zum Anhalten vor.
-- Zusätzlich kann die Funktion eine halbe Zuglänge addieren, so dass immer die Zugmitte an der
-- gewünschten Stelle stehen bleibt.
-- Außerdem kann ein Wert für die Verzögerung vorgegeben werden. Dann läuft der Zug solange ohne
-- Geschwindigskeitsbeeinflussung weiter, bis der Bremsvorgang diesen Wert benötigt.
-- Die Beschleunigung / Verzögerung kann zwischenzeitlich abgebrochen werden. Dabei kann entschieden
-- werden, ob die aktuell erreichte Geschwindigkeit beibehalten werden soll oder eine zu übergebende
-- Geschwindigkeit (bspw. 0) angenommen werden soll.

require "Commodity_1";
require "Scheduler_3";
local P = require "Persistence_3";

local trains = P["Speed$trains"] or {};;

local function doCancel(train, newSpeed)
	local sign;
	if trains[train] then
		sign = trains[train].sign;
	end;
	trains[train] = nil;
	if newSpeed then
		if sign then
			newSpeed = sign * math.abs(newSpeed);
		end;
		return EEPSetTrainSpeed(train, newSpeed);
	else
		return sign ~= nil;
	end;
end;

local function getTargetSpeed(train)
	train = train or trainName;
	assertParam(1, false, "string");
	if trains[train] then
		return trains[train].targetSpeed;
	else
		return nil;
	end;
end;

local function cancel(train, newSpeed)
	if type(train) ~= "string" then
		train, newSpeed = trainName, train;
	end;
	assertParam(1, false, "string");
	assertParam(2, true, "number");
	if doCancel(train, newSpeed) then
		P["Speed$trains"] = trains;
		Debug.printf("Speed", "Speed.cancel('%s', %s) called", train, tostring(newSpeed));
		return true;
	else
		return false;
	end;
end;

local function checkTrainSpeeds()
	local curTime = Scheduler.getNow();
	for train, speedInfo in pairs(trains) do
		local ok, curSpeed = EEPGetTrainSpeed(train);
		if not ok then
			error("Speed.checkTrainSpeeds(): EEPGetTrainSpeed('"..train.."') failed.");
		end;
		curSpeed = math.abs(curSpeed);
		local dt = curTime - speedInfo.oldTime;
		local approach = (curSpeed + speedInfo.oldSpeed) / 3.6 * dt / 2;
		local distance = speedInfo.distance - approach;
		local sign = speedInfo.sign;
		local targetSpeed = speedInfo.targetSpeed;
		if distance <= 0 or math.abs(curSpeed - targetSpeed) <= 0.1 then
			if doCancel(train, sign * targetSpeed) then
				Debug.printf("Speed", "Speed.checkTrainSpeeds(): train '%s' reached his speed of %.1f km/h (distance = %.2f m, curSpeed = %.1f km/h)", train, sign * targetSpeed, distance, sign * curSpeed);
			else
				error("Speed.checkTrainSpeeds(): failed to cancel speed request for train '"..train.."' (1)");
			end;
		else
			local acceleration = (targetSpeed * targetSpeed - curSpeed * curSpeed) / (3.6 * 3.6 * 2 * distance);
			-- Debug.printf("Speed", "Speed.checkTrainSpeeds(): train '%s' (%.1f km/h) @ %.2f m requires acceleration of %.3f m/s²", train, curSpeed, distance, acceleration);
			if math.abs(acceleration) > math.abs(speedInfo.acceleration) then
				local newSpeed = curSpeed + 0.2 * acceleration * 3.6;
				local ready;
				if speedInfo.acceleration >= 0 then
					ready = newSpeed > targetSpeed;
				else
					ready = newSpeed < targetSpeed;
				end;
				if ready then
					if doCancel(train, sign * targetSpeed) then
						Debug.printf("Speed", "Speed.checkTrainSpeeds(): train '%s' reached his speed of %.1f km/h (newSpeed = %.1f km/h)", train, sign * targetSpeed, sign * newSpeed);
					else
						error("Speed.checkTrainSpeeds(): failed to cancel speed request for train '"..train.."' (2)");
					end;
				else
					if EEPSetTrainSpeed(train, sign * newSpeed) then
						-- Debug.printf("Speed", "Speed.checkTrainSpeeds(): changed speed of train '%s' to %.1f km/h", train, sign * newSpeed);
					else
						error(string.format("Speed.checkTrainSpeeds(): failed to set speed for train '%s' to .1%f", train, sign * newSpeed));
					end;
				end;
			end;
			speedInfo.distance = distance;
			speedInfo.oldTime = curTime;
			speedInfo.oldSpeed = curSpeed;
		end;
	end;
	P["Speed$trains"] = trains;
	return true;
end;

local function change(train, targetSpeed, distance, addHalfTrainLength, acceleration)
	if type(train) ~= "string" then
		train, targetSpeed, distance, addHalfTrainLength, acceleration =
			trainName, train, targetSpeed, distance, addHalfTrainLength;
	end;
	if type(addHalfTrainLength) == "number" then
		acceleration = addHalfTrainLength;
		addHalfTrainLength = false;
	end;
	assertParam(1, false, "string");
	assertParam(2, false, "number");
	assertParam(3, false, "number");
	assertParam(4, true, "boolean");
	assertParam(5, true, "number");
	if addHalfTrainLength then
		local ok, trainLength = EEPGetTrainLength(train);
		if ok then
			distance = distance + 0.5 * trainLength;
		else
			error("Speed.change(): EEPGetTrainLength('"..train.."') failed.");
		end;
	end;
	local ok, curSpeed = EEPGetTrainSpeed(train);
	if not ok then
		error("Speed.change(): EEPGetTrainSpeed('"..train.."') failed.");
	end;
	local sign;
	if curSpeed < 0 then
		sign = -1;
		curSpeed = - curSpeed;
	elseif curSpeed == 0 then
		if targetSpeed < 0 then
			sign = -1;
		else
			sign = 1;
		end;
	else
		sign = 1;
	end;
	targetSpeed = math.abs(targetSpeed);
	if acceleration then
		if targetSpeed < curSpeed then
			acceleration = -math.abs(acceleration);
		else
			acceleration = math.abs(acceleration);
		end;
	else
		acceleration = (targetSpeed * targetSpeed - curSpeed * curSpeed) / (3.6 * 3.6 * 2 * distance);
	end;
	Debug.printf("Speed", "Speed, new request: train = '%s', curSpeed = %.1f km/h, targetSpeed = %.1f km/h, distance = %.2f m, acceleration = %.3f m/s²", train, sign * curSpeed, sign * targetSpeed, distance, acceleration);
	trains[train] = {
		sign = sign,
		oldTime = Scheduler.getNow(),
		oldSpeed = curSpeed,
		targetSpeed = targetSpeed,
		distance = distance,
		acceleration = acceleration
	};
	P["Speed$trains"] = trains;
	return true;
end;

local function reset()
	Debug.printf("Speed", "Speed: dropping %d train(s)", trains and #trains or 0);
	trains = {};
	P["Speed$trains"] = trains;
end;

Speed = {
	_VERSION = "1.0";
	
	-- Funktion change(train, targetSpeed, distance, addHalfTrainLength, acceleration)
	-- Bedeutung: ändert die Geschwindigkeit des Zuges auf die angegebene Zielgeschwindigkeit auf der
	--   angegebenen Entfernung, optional verlängert um die halbe Zuglänge, optional mit der vorgegebenen
	--   Beschleunigung / Verzögerung. Bei Vorgabe einer Beschleunigung / Verzögerung beginnt die Änderung
	--   der Geschwindigkeit erst, wenn es gemäß der Vorgabe erforderlich wird. Bis dahin wird die
	--   Geschwindigkeit nicht beeinflusst.
	-- Parameter:
	--   train: Name des Zugs (optional)
	--   targetSpeed: Zielgeschwindigkeit [km/h]
	--   distance: Entfernung [m] bis zum Erreichen der Zielgeschwindigkeit
	--   addHalfTrainLength: Wenn true, wird die halbe Zuglänge zur Entfernung addiert (optional)
	--   acceleration: Beschleunigung / Verzögerung [m/s²] (optional)
	-- Rückgabewert:
	--   true, wenn erfolgreich ausgeführt
	change = change;
	
	stopAfter = function(distance, addHalfTrainLength, acceleration)
		return change(0.0, distance, addHalfTrainLength, acceleration);
	end;
	
	-- Funktion getTargetSpeed(train)
	-- Bedeutung: Wenn der angegebene oder auslösende Zugs aktuell von diesem Modul gesteuert wird,
	--   wird die Zielgeschwindigkeit zurückgegeben, andernfalls nil.
	-- Parameter:
	--   train: Name des Zuges (optional)
	-- Rückgabewert:
	--   Zielgeschwindigkeitsanpassung oder nil (siehe oben).
	getTargetSpeed = getTargetSpeed;
	
	-- Funktion cancel(train, newSpeed)
	-- Bedeutung: bricht die Geschwindigkeitsanpassung ab und gibt dem Zug (optional) die gegebene neue
	--   Geschwindigkeit.
	-- Parameter:
	--   train: Name des Zuges
	--   newSpeed: neue Zielgeschwindigkeit (optional)
	-- Rückgabewert:
	--   true, wenn der Zug aktuell einer Geschwindigkeitsanpassung unterliegt, sonst false.
	cancel = cancel;
	
	-- Funktion reset()
	-- Bedeutung: löscht alle angeforderten Geschwindigkeitsänderungen
	-- Parameter: keine
	-- Rückgabewert: keiner
	reset = reset;
	
}

Scheduler.addRequest(checkTrainSpeeds, -1);

print("Speed Version " .. Speed._VERSION);

return Speed;