module Controller_SA

semantic adaptation reactive moore controller_sa

for fmu Controller controller
at "./path/to/Controller.fmu"
input ports obj_detected, passenger_up, passenger_down, passenger_stop, driver_up, driver_down, driver_stop
output ports up, down, stop

/*
Added support for simple wildcars in the input ports.
According to Casper and Kenneth, some FMUs have thousands of ports...
*/
input ports armature_current -> obj_detected, 
			passenger_*, // These are not supported for now.
			driver_*; // These are not supported for now.

output ports	u,
				d;

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;

in var next_time_step := -1;
in var stored_arm_current := init_armature_current;
control var step_size;
control var aux_obj_detected := 0;
control var get_next_step := true;
control var previous_arm_current := init_armature_current;

in rules {
	get_next_step -> {
		next_time_step := get_next_time_step(controller) + last_execution_time(controller); 
		/*
		The get_next_time_step(controller) function is equivalent to the following snippet:
		save_state(controller);
		internal_transition := do_step(controller, last_execution_time(controller), MAX);
		next_time_step := last_execution_time(controller) + internal_transition;
		rollback(controller);
		*/
	} --> { };
	
	true -> {
		stored_arm_current := armature_current
	} --> {
		obj_detected := aux_obj_detected; // Sets this input to the FMU 
	};
}

control rules {
	aux_obj_detected := false;
	step_size := H;
	if ((not is_close(previous_arm_current, CROSSING, REL_TOL, ABS_TOL) and previous_arm_current < CROSSING)
				and (not is_close(stored_arm_current, CROSSING, REL_TOL, ABS_TOL) and stored_arm_current > CROSSING)) { // crossing, but not within tolerance
		var negative_value := previous_arm_current - CROSSING;
		var positive_value := stored_arm_current - CROSSING;
		step_size := (H * (- negative_value)) / (positive_value - negative_value);
	} else {
		if ((not is_close(previous_arm_current, CROSSING, REL_TOL, ABS_TOL) and previous_arm_current < CROSSING)
					and is_close(stored_arm_current, CROSSING, REL_TOL, ABS_TOL )) { // crossing within tolerance found
			aux_obj_detected := true;
		}
	}
	
	if (aux_obj_detected == true or t >= next_time_step) {
		var aux_h = do_step(controller, t-e, e); // do a step, then decide next internal transition
		assert aux_h == e; // this must always be the case, otherwise it is better not to use the timed transition adaptation.
		get_next_step := true; // next time the setValues is called, the internal transition will be set again.
	} else {
		get_next_step := false;
	}
	if is_close(step_size, H, REL_TOL, ABS_TOL) {
		// Step accepted, so store the known input.
		// We cannot store the armature current at the in rules because we may not accept the step and because they may be called multiple times. If that happens, we want to still have the old value of armature current, to compare it with the new one.
		previous_arm_current := stored_arm_current;
	}
	return step_size;
}

out var stored_up := init_up;
out var stored_down := init_down;

out rules {
	true -> {
		/*
		Previously, there was this intruction here:
		internal_transition := get_next_time_step(controller) + t
		
		However, it cannot be here, since there is no guarantee in the control rules block, that the doStep of the controller will be called.
		*/
	} --> {};
	
	/*
	What does "otherwise var" mean?
	Suppose I have the following rules:
		var1 == true and var2 == false -> ...
		otherwise var1 -> ...
	
	controller.up == true -> { stored_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; } --> { }; 
	*/
	controller.up -> {stored_up := 1} --> {u := stored_up};
	not controller.up -> {stored_up := 0} --> {u := stored_up};
	
	controller.down -> {stored_down := 1} --> {d := stored_down};
	not controller.down -> {stored_down := 0} --> {d := stored_down};
	
	controller.stop -> {stored_down := 0; stored_up :=0 } --> {u := stored_up ; d := stored_down};
	
}