include "primitives.alh"
include "modelling.alh"
include "object_operations.alh"
include "library.alh"
include "conformance_scd.alh"
include "io.alh"
include "metamodels.alh"
include "compilation_manager.alh"

Element function retype_to_runtime(design_model : Element):
	Element runtime_model
	Element all_blocks
	Element all_links
	String mm_type_name
	String element_name
	String attr_name
	String attr_value
	String attribute
	String src
	String dst
	String time
	Element all_attributes

	runtime_model = instantiate_model(import_node("models/CausalBlockDiagrams_Runtime"))

	all_blocks = allInstances(design_model, "Block")
	while (list_len(all_blocks) > 0):
		element_name = set_pop(all_blocks)
		mm_type_name = reverseKeyLookup(design_model["metamodel"]["model"], dict_read_node(design_model["type_mapping"], design_model["model"][element_name]))
		element_name = instantiate_node(runtime_model, mm_type_name, element_name)
		if (is_nominal_instance(design_model, element_name, "ConstantBlock")):
			instantiate_attribute(runtime_model, element_name, "value", read_attribute(design_model, element_name, "value"))
		elif (is_nominal_instance(design_model, element_name, "ProbeBlock")):
			instantiate_attribute(runtime_model, element_name, "name", read_attribute(design_model, element_name, "name"))

	// Don't merge this together with the block conversion, as the destination block might not exist yet!
	all_links = allInstances(design_model, "Link")
	while (read_nr_out(all_links) > 0):
		element_name = set_pop(all_links)
		src = reverseKeyLookup(design_model["model"], read_edge_src(design_model["model"][element_name]))
		dst = reverseKeyLookup(design_model["model"], read_edge_dst(design_model["model"][element_name]))
		instantiate_link(runtime_model, "Link", element_name, src, dst)

	all_links = allInstances(design_model, "InitialCondition")
	while (read_nr_out(all_links) > 0):
		element_name = set_pop(all_links)
		src = reverseKeyLookup(design_model["model"], read_edge_src(design_model["model"][element_name]))
		dst = reverseKeyLookup(design_model["model"], read_edge_dst(design_model["model"][element_name]))
		instantiate_link(runtime_model, "InitialCondition", element_name, src, dst)

	return runtime_model!

Element function sanitize(new_runtime_model : Element, old_runtime_model : Element):
	Element all_blocks
	Element all_links
	String element_name
	String attr_name
	String attr_value
	String attribute
	String time
	Element all_attributes
	Float current_time

	all_blocks = allInstances(new_runtime_model, "Block")
	while (list_len(all_blocks) > 0):
		element_name = set_pop(all_blocks)
		if (dict_in(old_runtime_model["model"], element_name)):
			if (is_nominal_instance(new_runtime_model, element_name, "ICBlock")):
				instantiate_attribute(new_runtime_model, element_name, "last_in", read_attribute(old_runtime_model, element_name, "last_in"))
			if (is_nominal_instance(new_runtime_model, element_name, "IntegratorBlock")):
				instantiate_attribute(new_runtime_model, element_name, "last_out", read_attribute(old_runtime_model, element_name, "last_out"))
			instantiate_attribute(new_runtime_model, element_name, "signal", read_attribute(old_runtime_model, element_name, "signal"))
		else:
			instantiate_attribute(new_runtime_model, element_name, "signal", 0.0)

	if (dict_in(old_runtime_model["model"], "time")):
		current_time = read_attribute(old_runtime_model, "time", "current_time")
	else:
		current_time = 0

	time = instantiate_node(new_runtime_model, "Time", "time")
	instantiate_attribute(new_runtime_model, time, "start_time", current_time)
	instantiate_attribute(new_runtime_model, time, "current_time", current_time)

	return new_runtime_model!

Element function create_schedule(model : Element):
	// Create nice graph first
	Element nodes
	Element successors
	String element_name
	Element incoming_links
	Element all_blocks

	nodes = allInstances(model, "Block")
	successors = create_node()
	while (read_nr_out(nodes) > 0):
		element_name = set_pop(nodes)
		if (bool_not(dict_in(successors, element_name))):
			dict_add(successors, element_name, create_node())

		if (is_nominal_instance(model, element_name, "ICBlock")):
			if (element_eq(read_attribute(model, element_name, "last_in"), read_root())):
				incoming_links = allIncomingAssociationInstances(model, element_name, "InitialCondition")
			else:
				incoming_links = create_node()
			if (is_nominal_instance(model, element_name, "DerivatorBlock")):
				Element new_incoming_links
				new_incoming_links = allIncomingAssociationInstances(model, element_name, "Link")
				while (read_nr_out(new_incoming_links) > 0):
					list_append(incoming_links, set_pop(new_incoming_links))
		else:
			incoming_links = allIncomingAssociationInstances(model, element_name, "Link")

		while (read_nr_out(incoming_links) > 0):
			String source
			source = readAssociationSource(model, set_pop(incoming_links))
			if (bool_not(dict_in(successors, source))):
				dict_add(successors, source, create_node())
			set_add(successors[source], element_name)
	
	Element values
	values = create_node()
	dict_add(values, "S", create_node())
	dict_add(values, "index", 0)
	dict_add(values, "indices", create_node())
	dict_add(values, "lowlink", create_node())
	dict_add(values, "onStack", create_node())
	dict_add(values, "successors", successors)
	dict_add(values, "SCC", create_node())

	nodes = allInstances(model, "Block")
	while (read_nr_out(nodes) > 0):
		strongconnect(set_pop(nodes), values)

	return values["SCC"]!

Void function dict_overwrite(d : Element, key : Element, value : Element):
	if (dict_in(d, key)):
		dict_delete(d, key)
	if (dict_in_node(d, key)):
		dict_delete_node(d, key)
	dict_add(d, key, value)

	return !

Integer function min(a : Integer, b : Integer):
	if (a < b):
		return a!
	else:
		return b!

Void function strongconnect(v : String, values : Element):
	if (dict_in(values["indices"], v)):
		return!

	dict_overwrite(values["indices"], v, values["index"])
	dict_overwrite(values["lowlink"], v, values["index"])
	dict_overwrite(values, "index", cast_s2i(cast_v2s(values["index"])) + 1)

	list_append(values["S"], v)
	dict_overwrite(values["onStack"], v, True)
	
	Element successors
	String w
	successors = values["successors"][v]
	while (read_nr_out(successors) > 0):
		w = set_pop(successors)
		if (bool_not(dict_in(values["indices"], w))):
			strongconnect(w, values)
			dict_overwrite(values["lowlink"], v, min(values["lowlink"][v], values["lowlink"][w]))
		elif (dict_in(values["onStack"], w)):
			if (values["onStack"][w]):
				dict_overwrite(values["lowlink"], v, min(values["lowlink"][v], values["indices"][w]))
	
	if (value_eq(values["lowlink"][v], values["indices"][v])):
		Element scc
		scc = create_node()
		// It will always differ now
		w = list_pop(values["S"])
		list_append(scc, w)
		dict_overwrite(values["onStack"], w, False)
		while (w != v):
			w = list_pop(values["S"])
			list_append(scc, w)
			dict_overwrite(values["onStack"], w, False)
		list_insert(values["SCC"], scc, 0)

	return!

Element function list_pop(list : Element):
	Integer top
	Element t
	top = list_len(list) - 1
	t = list_read(list, top)
	list_delete(list, top)
	return t!

String function readType(model : Element, name : String):
	return reverseKeyLookup(model["metamodel"]["model"], dict_read_node(model["type_mapping"], model["model"][name]))!

Boolean function solve_scc(model : Element, scc : Element):
	Element m
	Integer i
	Integer j
	String block
	String blocktype
	Element incoming
	String selected
	Float constant
	Element t

	// Construct the matrix first, with as many rows as there are variables
	// Number of columns is 1 higher
	i = 0
	m = create_node()
	while (i < read_nr_out(scc)):
		j = 0
		t = create_node()
		while (j < (read_nr_out(scc) + 1)):
			list_append(t, 0.0)
			j = j + 1
		list_append(m, t)
		i = i + 1

	log("Matrix ready!")
	// Matrix initialized to 0.0
	i = 0
	while (i < read_nr_out(scc)):
		log("Creating matrix row")
		// First element of scc
		block = scc[i]
		blocktype = readType(model, block)

		// First write 1 in the current block
		dict_overwrite(m[i], i, 1.0)

		// Now check all blocks that are incoming
		if (blocktype == "AdditionBlock"):
			constant = 0.0
		elif (blocktype == "MultiplyBlock"):
			constant = 1.0

		log("Generating matrix for " + blocktype)
		log("Block: " + block)
		incoming = allIncomingAssociationInstances(model, block, "Link")

		Integer index_to_write_constant
		index_to_write_constant = -1
		log("Iterating over incoming")
		while (read_nr_out(incoming) > 0):
			log("Iteration")
			selected = readAssociationSource(model, set_pop(incoming))

			if (set_in(scc, selected)):
				// Part of the loop, so in the index of selected in scc
				// Five options:
				if (blocktype == "AdditionBlock"):
					// 1) AdditionBlock
					// Add the negative of this signal, which is as of yet unknown
					// x = y + z --> x - y - z = 0
					dict_overwrite(m[i], list_index_of(scc, selected), -1.0)
				elif (blocktype == "MultiplyBlock"):
					// 2) MultiplyBlock
					if (index_to_write_constant != -1):
						return False!
					index_to_write_constant = list_index_of(scc, selected)
				elif (blocktype == "NegatorBlock"):
					// 3) NegatorBlock
					// Add the positive of the signal, which is as of yet unknown
					dict_overwrite(m[i], list_index_of(scc, selected), 1.0)
				elif (blocktype == "DelayBlock"):
					// 5) DelayBlock
					// Just copies a single value
					dict_overwrite(m[i], list_index_of(scc, selected), -1.0)
				else:
					// Block that cannot be handled
					return False!
			else:
				// A constant, which we can assume is already computed and thus usable
				if (blocktype == "AdditionBlock"):
					constant = constant + v2f(read_attribute(model, selected, "signal"))
					dict_overwrite(m[i], read_nr_out(scc), constant)
				elif (blocktype == "MultiplyBlock"):
					constant = constant * v2f(read_attribute(model, selected, "signal"))
					// Not written to constant part, as multiplies a variable

				// Any other block is impossible:
				// * Constant would never be part of a SCC
				// * Delay would never get an incoming constant
				// * Negation and Inverse only get 1 input, which is a variable in a loop
				// * Integrator and Derivator never get an incoming constant

		if (index_to_write_constant != -1):
			dict_overwrite(m[i], index_to_write_constant, -constant)

		i = i + 1

	// Constructed a complete matrix, so we can start!
	log("Constructed matrix to solve:")
	log(matrix2string(m))

	// Solve matrix now
	eliminateGaussJordan(m)

	// Now go over m and set signals for each element
	// Assume that everything worked out...
	i = 0
	while (i < read_nr_out(m)):
		block = scc[i]
		unset_attribute(model, block, "signal")
		instantiate_attribute(model, block, "signal", m[i][read_nr_out(scc)])
		log((("Solved " + block) + " to ") + cast_v2s(m[i][read_nr_out(scc)]))
		i = i + 1

	return True!

Integer function list_index_of(lst : Element, elem : Element):
	Integer i
	i = 0
	while (i < read_nr_out(lst)):
		if (value_eq(list_read(lst, i), elem)):
			return i!
		else:
			i = i + 1
	return -1!

Void function step_simulation(model : Element, schedule : Element):
	String time
	Float signal
	Element incoming
	String selected
	String block
	String elem
	String blocktype
	Element memory_blocks
	Integer i
	Float delta_t
	Element scc

	time = "time"
	delta_t = 0.1

	memory_blocks = create_node()
	output("SIM_TIME " + cast_v2s(read_attribute(model, time, "current_time")))
	i = 0
	while (i < read_nr_out(schedule)):
		scc = list_read(schedule, i)
		i = i + 1

		if (list_len(scc) > 1):
			log("Solving algebraic loop!")
			if (bool_not(solve_scc(model, scc))):
				output("ALGEBRAIC_LOOP")
				return !
		else:
			block = set_pop(scc)

			// Execute "block"
			blocktype = readType(model, block)
			if (blocktype == "ConstantBlock"):
				signal = read_attribute(model, block, "value")
			elif (blocktype == "AdditionBlock"):
				signal = 0.0
				incoming = allIncomingAssociationInstances(model, block, "Link")
				while (read_nr_out(incoming) > 0):
					selected = readAssociationSource(model, set_pop(incoming))
					signal = signal + cast_s2f(cast_v2s(read_attribute(model, selected, "signal")))
			elif (blocktype == "MultiplyBlock"):
				signal = 1.0
				incoming = allIncomingAssociationInstances(model, block, "Link")
				while (read_nr_out(incoming) > 0):
					selected = readAssociationSource(model, set_pop(incoming))
					signal = signal * cast_s2f(cast_v2s(read_attribute(model, selected, "signal")))
			elif (blocktype == "NegatorBlock"):
				incoming = allIncomingAssociationInstances(model, block, "Link")
				signal = 0.0
				while (read_nr_out(incoming) > 0):
					selected = readAssociationSource(model, set_pop(incoming))
					signal = float_neg(cast_s2f(cast_v2s(read_attribute(model, selected, "signal"))))
			elif (blocktype == "InverseBlock"):
				signal = 0.0
				incoming = allIncomingAssociationInstances(model, block, "Link")
				while (read_nr_out(incoming) > 0):
					selected = readAssociationSource(model, set_pop(incoming))
					signal = float_division(1.0, cast_s2f(cast_v2s(read_attribute(model, selected, "signal"))))
			elif (blocktype == "DelayBlock"):
				signal = 0.0
				if (element_eq(read_attribute(model, block, "last_in"), read_root())):
					// No memory yet, so use initial condition
					incoming = allIncomingAssociationInstances(model, block, "InitialCondition")
					while (read_nr_out(incoming) > 0):
						selected = readAssociationSource(model, set_pop(incoming))
						signal = cast_s2f(cast_v2s(read_attribute(model, selected, "signal")))
				else:
					signal = read_attribute(model, block, "last_in")
					unset_attribute(model, block, "last_in")
				set_add(memory_blocks, block)
			elif (blocktype == "IntegratorBlock"):
				if (element_eq(read_attribute(model, block, "last_in"), read_root())):
					// No history yet, so use initial values
					incoming = allIncomingAssociationInstances(model, block, "InitialCondition")
					while (read_nr_out(incoming) > 0):
						selected = readAssociationSource(model, set_pop(incoming))
						signal = cast_s2f(cast_v2s(read_attribute(model, selected, "signal")))
				else:
					signal = cast_s2f(cast_v2s(read_attribute(model, block, "last_in"))) + (delta_t * cast_s2f(cast_v2s(read_attribute(model, block, "last_out"))))
					unset_attribute(model, block, "last_in")
					unset_attribute(model, block, "last_out")
				instantiate_attribute(model, block, "last_out", signal)
				set_add(memory_blocks, block)
			elif (blocktype == "DerivatorBlock"):
				if (element_eq(read_attribute(model, block, "last_in"), read_root())):
					// No history yet, so use initial values
					incoming = allIncomingAssociationInstances(model, block, "InitialCondition")
					while (read_nr_out(incoming) > 0):
						selected = readAssociationSource(model, set_pop(incoming))
						signal = cast_s2f(cast_v2s(read_attribute(model, selected, "signal")))
				else:
					incoming = allIncomingAssociationInstances(model, block, "Link")
					while (read_nr_out(incoming) > 0):
						selected = readAssociationSource(model, set_pop(incoming))
						signal = (cast_s2f(cast_v2s(read_attribute(model, selected, "signal"))) - cast_s2f(cast_v2s(read_attribute(model, block, "last_in")))) / delta_t
					unset_attribute(model, block, "last_in")
				set_add(memory_blocks, block)
			elif (blocktype == "ProbeBlock"):
				incoming = allIncomingAssociationInstances(model, block, "Link")
				while (read_nr_out(incoming) > 0):
					selected = readAssociationSource(model, set_pop(incoming))
					signal = cast_s2f(cast_v2s(read_attribute(model, selected, "signal")))
					output((("SIM_PROBE " + cast_v2s(read_attribute(model, block, "name"))) + " ") + cast_v2s(signal))

			unset_attribute(model, block, "signal")
			instantiate_attribute(model, block, "signal", signal)
	output("SIM_END")
	
	while (read_nr_out(memory_blocks) > 0):
		block = set_pop(memory_blocks)
		// Update memory
		incoming = allIncomingAssociationInstances(model, block, "Link")
		while (read_nr_out(incoming) > 0):
			selected = readAssociationSource(model, set_pop(incoming))
			instantiate_attribute(model, block, "last_in", cast_s2f(cast_v2s(read_attribute(model, selected, "signal"))))

	// Increase simulation time
	Float new_time
	new_time = cast_s2f(cast_v2s(read_attribute(model, time, "current_time"))) + delta_t
	unset_attribute(model, time, "current_time")
	instantiate_attribute(model, time, "current_time", new_time)

	return !

Void function execute_cbd(design_model : Element):
	String verify_result
	Element runtime_model
	Element old_runtime_model
	String cmd
	Boolean running
	Element schedule_init
	Element schedule_run
	Element schedule
	String conforming

	old_runtime_model = instantiate_model(import_node("models/CausalBlockDiagrams_Runtime"))
	runtime_model = retype_to_runtime(design_model)
	runtime_model = sanitize(runtime_model, old_runtime_model)
	running = False
	conforming = conformance_scd(design_model)
	if (conforming == "OK"):
		output("CONFORMANCE_OK")
	else:
		output("CONFORMANCE_FAIL")

	schedule_init = create_schedule(runtime_model)
	schedule_run = read_root()

	while (True):
		// If we are running, we just don't block for input and automatically do a step if there is no input
		if (running):
			if (has_input()):
				cmd = input()
			else:
				cmd = "step"
		else:
			cmd = input()

		// Process input
		if (cmd == "simulate"):
			// Simulation should toggle running to True, but only if the model is conforming
			if (conforming == "OK"):
				running = True
			else:
				output("CONFORMANCE_FAIL " + conforming)

		elif (cmd == "step"):
			// Stepping should make a single step, but first need to pick the correct schedule to use
			if (conforming == "OK"):
				if (read_attribute(runtime_model, "time", "start_time") == read_attribute(runtime_model, "time", "current_time")):
					schedule = schedule_init
				else:
					if (element_eq(schedule_run, read_root())):
						schedule_run = create_schedule(runtime_model)
					schedule = schedule_run
				// TODO remove
				schedule = create_schedule(runtime_model)
				step_simulation(runtime_model, schedule)
			else:
				output("CONFORMANCE_FAIL " + conforming)

		elif (cmd == "pause"):
			// Pausing merely stops a running simulation
			running = False

		elif (cmd == "read_available_attributes"):
			// Returns a list of all available attributes
			Element attr_list
			Element attrs
			Element attr
			attr_list = getAttributeList(design_model, input())
			attrs = dict_keys(attr_list)
			while (0 < read_nr_out(attrs)):
				attr = set_pop(attrs)
				output("AVAILABLE_ATTR_VALUE " + cast_v2s(attr))
				output("AVAILABLE_ATTR_TYPE " + cast_v2s(dict_read(attr_list, attr)))
			output("AVAILABLE_ATTR_END")

		elif (cmd == "read_attribute"):
			// Returns the value of an attribute
			output("ATTR_VALUE " + cast_v2s(read_attribute(design_model, input(), input())))

		elif (bool_or(bool_or(cmd == "set_attribute", cmd == "instantiate_node"), bool_or(cmd == "delete_element", cmd == "instantiate_association"))):
			// Modify the structure
			if (cmd == "set_attribute"):
				// Setting an attribute
				String element_name
				String attribute_name
				element_name = input()
				attribute_name = input()

				// Delete it if it exists already
				if (bool_not(element_eq(read_attribute(design_model, element_name, attribute_name), read_root()))):
					unset_attribute(design_model, element_name, attribute_name)

				// And finally set it
				instantiate_attribute(design_model, element_name, attribute_name, input())

			elif (cmd == "instantiate_node"):
				// Instantiate a node
				instantiate_node(design_model, input(), input())

			elif (cmd == "instantiate_association"):
				// Instantiate an association
				instantiate_link(design_model, input(), input(), input(), input())

			elif (cmd == "delete_element"):
				// Delete the provided element
				model_delete_element(design_model, input())

			// After changes, we check whether or not the design model conforms
			conforming = conformance_scd(design_model)
			if (conforming == "OK"):
				// Conforming, so do the retyping and sanitization step
				runtime_model = retype_to_runtime(design_model)
				runtime_model = sanitize(runtime_model, old_runtime_model)
				schedule_init = create_schedule(runtime_model)
				schedule_run = read_root()
				old_runtime_model = runtime_model
				output("CONFORMANCE_OK")
			else:
				// Not conforming, so stop simulation and block for input (preferably a modify to make everything consistent again)
				running = False
				output("CONFORMANCE_FAIL " + conforming)
		else:
			log("Did not understand command: " + cmd)

Float function v2f(i : Element):
	return cast_s2f(cast_v2s(i))!

Void function eliminateGaussJordan(m : Element):
	Integer i
	Integer j
	Integer f
	Integer g
	Boolean searching
	Element t
	Float divisor

	i = 0
	j = 0

	while (i < read_nr_out(m)):
		// Make sure pivot m[i][j] != 0, swapping if necessary
		while (v2f(m[i][j]) == 0.0):
			// Is zero, so find row which is not zero
			f = i + 1
			searching = True
			while (searching):
				if (f >= read_nr_out(m)):
					// No longer any rows left, so just increase column counter
					searching = False
					j = j + 1
				else:
					if (v2f(m[f][j]) == 0.0):
						// Also zero, so continue
						f = f + 1
					else:
						// Found non-zero, so swap row
						t = v2f(m[f])
						dict_overwrite(m, f, v2f(m[i]))
						dict_overwrite(m, i, t)
						searching = False
			// If we have increased j, we will just start the loop again (possibly), as m[i][j] might be zero again

		// Pivot in m[i][j] guaranteed to not be 0
		// Now divide complete row by value of m[i][j] to make it equal 1
		f = j
		divisor = v2f(m[i][j])
		while (f < read_nr_out(m[i])):
			dict_overwrite(m[i], f, float_division(v2f(m[i][f]), divisor))
			f = f + 1

		// Eliminate all rows in the j-th column, except the i-th row
		f = 0
		while (f < read_nr_out(m)):
			if (bool_not(f == i)):
				g = j
				divisor = v2f(m[f][j])
				while (g < read_nr_out(m[f])):
					dict_overwrite(m[f], g, v2f(m[f][g]) - (divisor * v2f(m[i][g])))
					g = g + 1
			f = f + 1

		// Increase row and column
		i = i + 1
		j = j + 1

	return !

String function matrix2string(m : Element):
	Integer i
	Integer j
	String result

	result = ""
	i = 0
	while (i < read_nr_out(m)):
		j = 0
		while (j < read_nr_out(m[i])):
			result = result + cast_v2s(m[i][j])
			result = result + ", "
			j = j + 1
		i = i + 1
		result = result + "\n"
	return result!

Void function main():
	Element model
	String verify_result

	while (True):
		execute_cbd(instantiate_model(import_node("models/CausalBlockDiagrams_Design")))