package ua.ansymo.hintco

import java.io.File
import java.util.HashMap
import java.util.List
import java.util.Map
import java.util.Set
import org.eclipse.core.runtime.Assert
import org.intocps.fmi.Fmi2Status
import org.intocps.fmi.FmuInvocationException
import org.slf4j.Logger
import org.slf4j.LoggerFactory

class CosimRunUtils {
	
	static Logger logger = LoggerFactory.getLogger(typeof(CosimRunUtils))
	
	def static instantiateUnits(List<UnitInstance> units, Map<UnitInstance, IFmuInstance> fmuInstanceMap, IFmuLoader fmuLoader, Map<Scenario, List<PrecendenceNode>> scenarioNodesMap) {
		for (unit : units) {
			val fmuInstance = fmuLoader.instantiate(unit, scenarioNodesMap)
			logger.debug("Unit {} instantiated", unit)
			
			fmuInstanceMap.put(unit, fmuInstance)
		}
	}
	
	def static exitInitMode(List<UnitInstance> units, Map<UnitInstance, IFmuInstance> fmuInstanceMap) {
		logger.debug("Exiting initialization mode.")
		// Exit init mode
		for (unit : units) {
			val result = fmuInstanceMap.get(unit).exitInitializationMode()
			if (result !== Fmi2Status.OK){
				throw new FmuInvocationException('''Unit «unit» exit initialization mode failed.''')
			}
			logger.debug("Unit {} exit initialization mode.", unit)
		}
	}
	
	def static intoInitMode(List<UnitInstance> units, Map<UnitInstance, IFmuInstance> fmuInstanceMap) {
		for (unit: units){
			val fmuInstance = fmuInstanceMap.get(unit)
			val result = fmuInstance.enterInitializationMode()
			if (result !== Fmi2Status.OK){
				throw new FmuInvocationException('''Unit «unit» enter initialization mode failed.''')
			}
			
			logger.debug("Unit {} enter initialization mode", unit)
		}
	}
	
	def static setupExperiment(List<UnitInstance> units, Map<UnitInstance, IFmuInstance> fmuInstanceMap) {
		for (unit: units){
			val fmuInstance = fmuInstanceMap.get(unit)
			fmuInstance.setupExperiment()
			logger.debug("Unit {} experiment setup", unit)
			
			// TODO Set the configured parameter values.
		}
	}
	
	def static getOutputSnapshot(List<UnitInstance> units, Map<UnitInstance, IFmuInstance> fmuInstanceMap, 
											Map<OutputPortInstance, Double> outVals,
											Set<OutputPortInstance> filter
	){
		logger.debug("Getting filtered output snapshot: {}", filter)
		for (u : units) {
			val fmu = fmuInstanceMap.get(u)
			fmu.outputSnapshot(outVals, filter)
		}
		return outVals
	}
		
	def static freeUnits(List<UnitInstance> units, Map<UnitInstance, IFmuInstance> fmuInstanceMap) {
		for (unit : units) {
			fmuInstanceMap.get(unit).free()
			logger.debug("Unit {} free.", unit)
		}
	}
	
	def static resetUnits(List<UnitInstance> units, Map<UnitInstance, IFmuInstance> fmuInstanceMap, IFmuLoader fmuLoader, Map<Scenario, List<PrecendenceNode>> scenarioNodesMap, boolean hardReset) {
		for (unit : units) {
			if (hardReset){
				// Terminate Unit
				fmuInstanceMap.get(unit).free()
				
				// Create new instance
				val fmuInstance = fmuLoader.instantiate(unit, scenarioNodesMap)
				logger.debug("Unit {} instantiated", unit)
				
				// Replace the old unit in the map
				fmuInstanceMap.put(unit, fmuInstance)
			} else {
				fmuInstanceMap.get(unit).reset()				
			}
			logger.debug("Unit {} free.", unit)
		}
	}
	
	def static fixPointOutputs(List<OutputPortInstance> childOutputPorts, Map<UnitInstance, IFmuInstance> fmuInstanceMap, int maxIt) {
		// Fixpoint initial values, by following the same order of get.values as specified in nodes.
		// Uses a map to store the previous value of each output. Then, whenever a new value of that output is obtained, checks if is the same as the previous value.
		var outVals = newHashMap()
		var prevOutVals = newHashMap()
		
		// Initialize
		for (out : childOutputPorts){
			prevOutVals.put(out, Double.POSITIVE_INFINITY)
			logger.debug("Output port {}.{} initialized to infinity", out.unit.identifier, out.identifier)
		}
		
		var converged=false
		var i = 0
		
		logger.debug("Starting fixed point iteration for time 0.0.")
		while (!converged && i<maxIt){
			logger.debug("Iteration {} start.", i)
			
			for (out : childOutputPorts){
				// TODO The getReal method of the hierarchical units needs to pull the latest value from the internal unit.
				val outV = fmuInstanceMap.get(out.unit).getReal(out.identifier)
				logger.debug("GetReal {}.{} at iteration {} yields {}.", out.unit.identifier, out.identifier, i, outV)
				
				// Store the new value, for next iteration
				outVals.put(out, outV)
				
				// Propagate outV to inputs.
				// TODO: Take types into account. For now we assume everything is real.
				for (trgPort : out.valueTo){
					fmuInstanceMap.get(trgPort.unit).setReal(trgPort.identifier, outV)
					logger.debug("SetReal {}.{} to {} at iteration {}.", trgPort.unit.identifier, trgPort.identifier, outV, i)
				}
			}
			
			converged = MathUtils.converged(outVals, prevOutVals, 1e-8)
			
			// Swap buffers
			val aux = prevOutVals
			prevOutVals = outVals
			outVals = aux
			
			if (!converged){
				logger.debug("Iteration {} end. Not Converged.", i)
			} else {
				logger.debug("Iteration {} end. Converged.", i)
			}
			i++
		}
		
		return outVals
	}
	
	def static executeStep(Scenario scenario,  double time, double stepSize, List<PrecendenceNode> nodes, 
							List<UnitInstance> units, Set<InputPortInstance> inPorts, 
							Map<UnitInstance, IFmuInstance> fmuInstanceMap,
							HashMap<OutputPortInstance, Double> replayValues
	) {
		logger.debug("Co-simulation step of {} start at time {} -> {}.", scenario.identifier, time, time + stepSize)
		
		// Let units know that a cosim step is starting.
		for (u : units){
			logger.debug("Replay values {} at time {}.", replayValues, time)
			fmuInstanceMap.get(u).startCosimStep(time, stepSize)
		}
		
		// If there are values to replay, set them to the appropriate input ports.
		if (replayValues !== null){
			for (outP : replayValues.keySet){
				val outV = replayValues.get(outP)
				propagateValueToInputs(outV, outP, inPorts, fmuInstanceMap, time)
			}
		}
		
		for (n : nodes) {
			switch (n) {
				UnitInstance: {
					// doStep
					fmuInstanceMap.get(n).doStep(time,stepSize)
					logger.debug("DoStep {} at time {}.", n.identifier, time)
				}
				OutputPortInstance case !inPorts.contains(n):{ // Ignore input ports, as these are set after the get output ports
					// Propagate values
					val outV = fmuInstanceMap.get(n.unit).getReal(n.identifier)
					logger.debug("GetReal {}.{} at time {} yields {}.", n.unit.identifier, n.identifier, time, outV)
					
					// Propagate outV to the selected inputs (not all inputs need to be set because of Hierarchical units).
					propagateValueToInputs(outV, n, inPorts, fmuInstanceMap, time)
				}
				default: {
					// Do Nothing for InputPort instances.
				}
			}
		}
		
		// Let units know that a cosim step has finished.
		for (u : units){
			fmuInstanceMap.get(u).endCosimStep(time, stepSize)
		}
		
		logger.debug("Co-simulation step of {} end at time {} -> {}.", scenario.identifier, time, time + stepSize)
	}
	
	def static propagateValueToInputs(double outV, OutputPortInstance n, Set<InputPortInstance> inPorts, Map<UnitInstance, IFmuInstance> fmuInstanceMap, double time) {
		for (trgPort : n.valueTo.filter[inPorts.contains(it)]){
			fmuInstanceMap.get(trgPort.unit).setReal(trgPort.identifier, outV)
			logger.debug("SetReal {}.{} to {} at time {}.", trgPort.unit.identifier, trgPort.identifier, outV, time)
		}
	}
	
}