import PowerWindowModel

module Controller_SA

semantic adaptation controller_sa
for fmu controller
input ports armature_current -> obj_detected, passenger_up, passenger_down, driver_up, driver_down
output ports up, down
param REL_TOL = 0.0001;
param ABS_TOL = 1e-8;
param CROSSING = 5;
param init_armature_current = CROSSING;
param init_up = 0;
param init_down = 0;
control var internal_transition;
control rules { // overwriting master! What to do in case of roll back? How to reuse generic algorithm
	if (not is_set(internal_transition)) { // only at start of simulation
		var state := get_state(controller);
		internal_transition := do_step(controller, MAX) + t; // may return internal transition time or MAX
		set_state(controller, state);
	}
	var step_size; // may be used to determine roll-back
	if (signal == true or t >= internal_transition) {
		step_size := do_step(controller, H); // do a step, then decide next internal transition
		var state := get_state(controller);
		internal_transition := do_step(controller, MAX) + t;
		set_state(controller, state);
	}
	return step_size;
}
in var stored_armature_current := init_armature_current;
in rules {
	true -> {
		obj_detected := false;
		if (not (not is_close(stored_armature_current, CROSSING, REL_TOL, ABS_TOL) and stored_armature_current > CROSSING)
					and (not is_close(armature_current, CROSSING, REL_TOL, ABS_TOL) and armature_current > CROSSING)) { // crossing, but not within tolerance
			var negative_value := stored_armature_current - CROSSING;
			var positive_value := armature_current - CROSSING;
			var new_step_size := (H * (- negative_value)) / (positive_value - negative_value);
			discard(new_step_size);
		} else {
			if (not (not is_close(stored_armature_current, CROSSING, REL_TOL, ABS_TOL) and stored_armature_current > CROSSING)
						and is_close(armature_current, CROSSING, REL_TOL, ABS_TOL)) { // crossing within tolerance found
				obj_detected := true;
			}
		}
		stored_armature_current := armature_current;
	} --> { };
}
out var stored_up := init_up;
out var stored_down := init_down;
out rules {
	up == true -> { up := 1; } --> { };
	otherwise up -> { } --> { };
	down == true -> { down := 1; } --> { };
	otherwise down -> { } --> { };
	stop == true -> { up := 0; down := 0; } --> { };
	otherwise stop -> { } --> { };
	true -> { up := stored_up; stored_up := up; } --> { };
	true -> { down := stored_down; stored_down := down; } --> { }; 
}
