package ua.ansymo.hintco

import org.slf4j.Logger
import org.slf4j.LoggerFactory

class PowerCorrectionPort extends StoreAdaptedPort {
	
	var String portId
	
	var double accumulatedDeficit = 0.0
	var double energyDeficit = 0.0
	
	var Kp = 1.0
	var Ki = 100.0
	
	PowerOutAdaptedPort pOut
	StoreAdaptedPort pIn
	
	Logger logger = LoggerFactory.getLogger(typeof(PowerCorrectionPort))
	
	AdaptedFMUInstance fmu
	
	new(String pId, PowerOutAdaptedPort po, StoreAdaptedPort pi, AdaptedFMUInstance f) {
		fmu = f
		portId = pId
		pOut = po
		pIn = pi
	}
	
	override startStep() {}
	
	override endStep() {}
	
	override endCosimStep(double time, double stepSize) {
		val powerIn = pIn.storedVal
		val powerOut = pOut.real
		energyDeficit = (powerOut-powerIn)*stepSize
		// Positive residual means this fmu has a deficit of energy.
		accumulatedDeficit = accumulatedDeficit + energyDeficit
	}
	
	override computeReal(double t, double dt, double h, double H) {
		val givenInput = storedVal
		val piControlOut = Kp*energyDeficit + Ki*accumulatedDeficit
		val actualInput = givenInput + piControlOut
		logger.debug("Applying power correction on port {}.{}.", 
					fmu.unit.identifier, portId)
		logger.debug("energyDeficit={}", energyDeficit)
		logger.debug("accumulatedDeficit={}", accumulatedDeficit)
		logger.debug("givenInput={}", givenInput)
		logger.debug("actualInput={}", actualInput)
		fmu.setInnerReal(portId, actualInput)
	}
}