include "primitives.alh"
include "modelling.alh"
include "object_operations.alh"
include "conformance_scd.alh"
include "io.alh"
include "metamodels.alh"
include "mini_modify.alh"
include "utils.alh"

Boolean function main(model : Element):
	log("Start DTCBD simulation!")
	String cmd
	Boolean running
	Element schedule_init
	Element schedule_run
	Element schedule
	Float current_time

	String time
	time = set_pop(allInstances(model, "FullRuntime/Time"))
	current_time = read_attribute(model, time, "current_time")
	log("Fetching time now: " + cast_value(current_time))

	schedule_init = create_schedule(model)
	schedule_run = read_root()

	Element nodes
	Element inputs
	String node
	nodes = allInstances(model, "FullRuntime/Block")
	inputs = dict_create()
	while (set_len(nodes) > 0):
		node = set_pop(nodes)
		dict_add(inputs, node, allAssociationOrigins(model, node, "FullRuntime/Link"))

	while (bool_not(has_input())):
		if (read_attribute(model, time, "start_time") == read_attribute(model, time, "current_time")):
			schedule = schedule_init
		else:
			if (element_eq(schedule_run, read_root())):
				schedule_run = create_schedule(model)
			schedule = schedule_run
		current_time = step_simulation(model, schedule, current_time, inputs)

	instantiate_attribute(model, time, "current_time", current_time)
	log("CLOSE")
	output("CLOSE")
	return True!

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, "FullRuntime/Block")
	successors = set_create()
	while (set_len(nodes) > 0):
		element_name = set_pop(nodes)
		log("Block " + element_name + " : " + read_type(model, element_name))

		if (is_nominal_instance(model, element_name, "FullRuntime/ICBlock")):
			if (bool_not(is_physical_float(read_attribute(model, element_name, "last_in")))):
				incoming_links = allIncomingAssociationInstances(model, element_name, "FullRuntime/InitialCondition")
			else:
				incoming_links = create_node()
		else:
			incoming_links = allIncomingAssociationInstances(model, element_name, "FullRuntime/Link")

		while (set_len(incoming_links) > 0):
			String source
			source = readAssociationSource(model, set_pop(incoming_links))
			list_append(successors, create_tuple(source, element_name))
			log("Found edge: " + source + " --> " + element_name)

	Element values
	values = create_node()
	//dict_add(values, "model", model)
	//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, "predecessors", predecessors)
	//dict_add(values, "SCC", create_node())

	dict_add(values, "edges", successors)
	dict_add(values, "nodes", allInstances(model, "FullRuntime/Block"))
	dict_add(values, "dfsCounter", 0)
	dict_add(values, "orderNumber", dict_create())
	dict_add(values, "visited_topSort", set_create())
	dict_add(values, "unvisited_strongComp", set_create())

	log("Toposort")
	//nodes = get_topolist(values)
	//while (list_len(nodes) > 0):
	//	log("Strong connect")
	//	strongconnect(list_pop_final(nodes), values)

	dict_overwrite(values, "SCC", strongComp(values))

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

	log("OK")

	return values["SCC"]!

Void function topSort(values : Element):
	Element nodes_copy
	String node

	// for node in graph:
	//     node.visited = False
	dict_overwrite(values, "visited_topSort", set_create())

	// for node in graph:
	//     if not node.visited:
	//          dfsLabelling(node)
	nodes_copy = set_copy(values["nodes"])
	while (set_len(nodes_copy) > 0):
		node = set_pop(nodes_copy)
		if (bool_not(set_in(values["visited_topSort"], node))):
			dfsLabelling(values, node)
	log("Order: " + dict_to_string(values["orderNumber"]))

	return!

Element function get_successors(values : Element, node : String, key : String):
	Element edges
	Element result
	String edge

	result = set_create()
	edges = list_copy(values[key])
	while (list_len(edges) > 0):
		edge = list_pop_final(edges)
		if (cast_string(edge[0]) == node):
			set_add(result, edge[1])

	return result!

Void function dfsLabelling(values : Element, node : String):
	Element successors
	String successor

	// if not node.visited
	if (bool_not(set_in(values["visited_topSort"], node))):
		// node.visited = True
		set_add(values["visited_topSort"], node)

		// for neighbour in node.out_neighbours:
		//     dfsLabelling(neighbour, graph)
		successors = get_successors(values, node, "edges")
		while (set_len(successors) > 0):
			successor = set_pop(successors)
			dfsLabelling(values, successor)

		// node.orderNumber = dfsCounter
		dict_overwrite(values["orderNumber"], node, values["dfsCounter"])

		// dfsCounter += 1
		dict_overwrite(values, "dfsCounter", cast_integer(values["dfsCounter"]) + 1)

	return !

Element function dfsCollect(values : Element, start_node : String):
	Element result
	String successor
	Element successors

	result = set_create()

	// if not node.visited
	if (set_in(values["unvisited_strongComp"], start_node)):
		list_append(result, start_node)
		// node.visited = True
		set_remove(values["unvisited_strongComp"], start_node)

		// for neighbour in node.out_neighbours:
		//     dfsLabelling(neighbour, graph)
		successors = get_successors(values, start_node, "rev_edges")
		while (set_len(successors) > 0):
			successor = set_pop(successors)
			list_extend(result, dfsCollect(values, successor))

	return result!

String function highest_orderNumber(values : Element):
	Integer max
	String max_element
	Element search
	String elem

	max = -1
	search = set_copy(values["unvisited_strongComp"])
	while (set_len(search) > 0):
		elem = set_pop(search)
		if (cast_integer(values["orderNumber"][elem]) > max):
			max = values["orderNumber"][elem]
			max_element = elem
		
	return max_element!

Element function reverse_edges(edges : Element):
	Element result
	Element elem

	result = list_create()
	edges = list_copy(edges)
	while (list_len(edges) > 0):
		elem = list_pop_final(edges)
		list_append(result, create_tuple(elem[1], elem[0]))
	return result!

Element function strongComp(values : Element):
	Element graph
	Element sccs
	String start_node
	Element strong_components
	Element component

	sccs = list_create()

	topSort(values)

	dict_overwrite(values, "unvisited_strongComp", set_copy(values["nodes"]))

	dict_overwrite(values, "rev_edges", reverse_edges(values["edges"]))
	strong_components = list_create()

	while (set_len(values["unvisited_strongComp"]) > 0):
		start_node = highest_orderNumber(values)

		component = dfsCollect(values, start_node)
		list_append(sccs, component)
		log("Got strong component: " + list_to_string(component))

	return sccs!

Element function get_topolist(values : Element):
	Element result
	Element predecessors
	Element remaining
	String current_element
	Element cur_predecessors

	result = list_create()
	predecessors = dict_copy(values["predecessors"])

	while (dict_len(predecessors) > 0):
		remaining = dict_keys(predecessors)
		while (set_len(remaining) > 0):
			current_element = set_pop(remaining)
			cur_predecessors = predecessors[current_element]
			if (set_len(set_overlap(list_to_set(result), cur_predecessors)) == set_len(cur_predecessors)):
				// All predecessors of this node have already been visited
				dict_delete(predecessors, current_element)
				remaining = dict_keys(predecessors)
				list_append(result, current_element)

	return result!

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

Void function strongconnect(v : String, values : Element):
	// if (v.index is undefined) then
	//    strongconnect(V)
	if (dict_in(values["indices"], v)):
		return!

	// v.index := index
	dict_overwrite(values["indices"], v, values["index"])
	// v.lowlink := indwx
	dict_overwrite(values["lowlink"], v, values["index"])
	// index := index + 1
	dict_overwrite(values, "index", cast_integer(values["index"]) + 1)

	// S.push(v)
	list_append(values["S"], v)
	// v.onStack := true
	dict_overwrite(values["onStack"], v, True)
	
	Element successors
	String w
	successors = values["successors"][v]
	while (set_len(successors) > 0):
		// for each (v, w) in E do
		w = set_pop(successors)
		// if (w.index is undefined) then
		if (bool_not(dict_in(values["indices"], w))):
			// strongconnect(w)
			strongconnect(w, values)
			// v.lowlink := min(v.lowlink, w.lowlink)
			dict_overwrite(values["lowlink"], v, min(values["lowlink"][v], values["lowlink"][w]))
		elif (dict_in(values["onStack"], w)):
			// else if (w.onStack)
			if (values["onStack"][w]):
				// v.lowlink := min(v.lowlink, w.index)
				dict_overwrite(values["lowlink"], v, min(values["lowlink"][v], values["indices"][w]))
	
	// if (v.lowlink == v.index) then
	if (value_eq(values["lowlink"][v], values["indices"][v])):
		Element scc
		// Start a new strongly connected component
		scc = create_node()
		// It will always differ now
		// w := S.pop()
		w = list_pop_final(values["S"])
		// w.onStack = false
		dict_overwrite(values["onStack"], w, False)
		// add w to current strongly connected component
		list_append(scc, w)
		// while w != v
		while (w != v):
			// w := S.pop()
			w = list_pop_final(values["S"])
			// w.onStack = false
			dict_overwrite(values["onStack"], w, False)
			// add w to current strongly connected component
			list_append(scc, w)
		// output the current strongly connected component
		list_insert(values["SCC"], scc, 0)
		log("Found new SCC: " + list_to_string(scc))
	return!

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

	// Matrix initialized to 0.0
	i = 0
	while (i < read_nr_out(scc)):
		// First element of scc
		block = scc[i]
		blocktype = read_type(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 == "FullRuntime/AdditionBlock"):
			constant = 0.0
		elif (blocktype == "FullRuntime/MultiplyBlock"):
			constant = 1.0

		incoming = allIncomingAssociationInstances(model, block, "FullRuntime/Link")

		Integer index_to_write_constant
		index_to_write_constant = -1
		while (read_nr_out(incoming) > 0):
			selected = readAssociationSource(model, set_pop(incoming))

			if (list_in(scc, selected)):
				// Part of the loop, so in the index of selected in scc
				// Five options:
				if (blocktype == "FullRuntime/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 == "FullRuntime/MultiplyBlock"):
					// 2) MultiplyBlock
					if (index_to_write_constant != -1):
						return False!
					index_to_write_constant = list_index_of(scc, selected)
				elif (blocktype == "FullRuntime/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 == "FullRuntime/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 == "FullRuntime/AdditionBlock"):
					constant = constant + cast_float(read_attribute(model, selected, "signal"))
					dict_overwrite(m[i], read_nr_out(scc), constant)
				elif (blocktype == "FullRuntime/MultiplyBlock"):
					constant = constant * cast_float(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(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]
		instantiate_attribute(model, block, "signal", m[i][read_nr_out(scc)])
		log((("Solved " + block) + " to ") + cast_string(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!

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

	delta_t = 0.1

	memory_blocks = set_create()
	i = 0
	while (i < list_len(schedule)):
		scc = list_read(schedule, i)
		i = i + 1

		if (list_len(scc) > 1):
			if (bool_not(solve_scc(model, scc))):
				log("ALGEBRAIC_LOOP")
				output("ALGEBRAIC_LOOP")
				return time!
		else:
			block = list_read(scc, 0)

			// Execute "block"
			blocktype = read_type(model, block)
			//log("Execute block " + block + " : " + blocktype)
			incoming = set_copy(inputs[block])
			if (blocktype == "FullRuntime/ConstantBlock"):
				signal = cast_float(read_attribute(model, block, "value"))
			elif (blocktype == "FullRuntime/AdditionBlock"):
				signal = 0.0
				while (set_len(incoming) > 0):
					selected = set_pop(incoming)
					signal = signal + cast_float(read_attribute(model, selected, "signal"))
			elif (blocktype == "FullRuntime/MultiplyBlock"):
				signal = 1.0
				while (set_len(incoming) > 0):
					selected = set_pop(incoming)
					signal = signal * cast_float(read_attribute(model, selected, "signal"))
			elif (blocktype == "FullRuntime/NegatorBlock"):
				signal = 0.0
				while (set_len(incoming) > 0):
					selected = set_pop(incoming)
					signal = float_neg(cast_float(read_attribute(model, selected, "signal")))
			elif (blocktype == "FullRuntime/InverseBlock"):
				signal = 0.0
				while (set_len(incoming) > 0):
					selected = set_pop(incoming)
					signal = float_division(1.0, cast_float(read_attribute(model, selected, "signal")))
			elif (blocktype == "FullRuntime/DelayBlock"):
				signal = 0.0
				if (bool_not(is_physical_float(read_attribute(model, block, "last_in")))):
					// No memory yet, so use initial condition
					incoming = allAssociationOrigins(model, block, "FullRuntime/InitialCondition")
					while (set_len(incoming) > 0):
						selected = set_pop(incoming)
						signal = cast_float(read_attribute(model, selected, "signal"))
				else:
					signal = read_attribute(model, block, "last_in")
				set_add(memory_blocks, block)
			elif (blocktype == "FullRuntime/ProbeBlock"):
				while (set_len(incoming) > 0):
					signal = cast_float(read_attribute(model, set_pop(incoming), "signal"))
					output(cast_string(time) + " " + cast_string(read_attribute(model, block, "name")) + " " + cast_string(signal))
					log(cast_string(time) + " " + cast_string(read_attribute(model, block, "name")) + " " + cast_string(signal))

			instantiate_attribute(model, block, "signal", signal)
	
	while (set_len(memory_blocks) > 0):
		block = set_pop(memory_blocks)
		// Update memory
		incoming = set_copy(inputs[block])
		while (set_len(incoming) > 0):
			selected = set_pop(incoming)
			instantiate_attribute(model, block, "last_in", cast_float(read_attribute(model, selected, "signal")))

	// Increase simulation time
	return time + delta_t!

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 (cast_float(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 (cast_float(m[f][j]) == 0.0):
						// Also zero, so continue
						f = f + 1
					else:
						// Found non-zero, so swap row
						t = cast_float(m[f])
						dict_overwrite(m, f, cast_float(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 = cast_float(m[i][j])
		while (f < read_nr_out(m[i])):
			dict_overwrite(m[i], f, float_division(cast_float(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 = cast_float(m[f][j])
				while (g < read_nr_out(m[f])):
					dict_overwrite(m[f], g, cast_float(m[f][g]) - (divisor * cast_float(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_string(m[i][j]) + ", "
			j = j + 1
		i = i + 1
		result = result + "\n"
	return result!