module Controller_SA

/*
 * This file is not used in the case study.
 * The adaptations it does have been split.
 */

semantic adaptation reactive moore ControllerSA controller_sa
at "./path/to/ControllerSA.fmu"

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

/*
At some point in the future, we support for simple wildcars in the input ports.
According to Casper and Kenneth, some FMUs have thousands of ports...
*/
input ports armature_current -> controller.obj_detected, 
			passenger_up -> controller.passenger_up, // You could write passenger_* here.
			passenger_down -> controller.passenger_down,
			passenger_stop -> controller.passenger_stop,
			driver_up -> controller.driver_up, // You could write driver_* here.
			driver_down -> controller.driver_down,
			driver_stop -> controller.driver_stop

output ports	u,
				d

param	REL_TOL := 0.0001,
		ABS_TOL := 1e-8,
		CROSSING := 5.0,
		INIT_ARMATURE_CURRENT := 0.0,
		INIT_U := 0.0,
		INIT_D := 0.0;

control var	aux_obj_detected := 0,
			get_next_step := true,
			previous_arm_current := INIT_ARMATURE_CURRENT;
control rules {
	var step_size := H;
	aux_obj_detected := false;
	if ((not is_close(previous_arm_current, CROSSING, REL_TOL, ABS_TOL) and previous_arm_current < CROSSING)
				and (not is_close(future_arm_current, CROSSING, REL_TOL, ABS_TOL) and future_arm_current > CROSSING)) { // crossing, but not within tolerance
		var negative_value := previous_arm_current - CROSSING;
		var positive_value := future_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(future_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 := future_arm_current;
	}
	return step_size;
}

in var	next_time_step := -1.0,
		future_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 -> {
		future_arm_current := armature_current;
	} --> {
		obj_detected := aux_obj_detected; // Sets this input to the FMU 
	};
}

out var stored_up := INIT_U,
		stored_down := INIT_D;
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; };
	
}