from throughputTest import *
from measureThroughput import *
from virtualManagement import PhysicalHost
from numpy import *
import time
import math

class ControlThread(threading.Thread):
    def run(self):
        """When called, given an object, executes its runServer method.
        Could be modified to run other methods as well"""
        self.object.runFeedackControl()
	self.object.done = "True"
    def __init__(self, threadId,object):
        threading.Thread.__init__(self)
        self.object = object
        self.threadId = threadId

class ThroughputControl():
	#Receive the parameters described Above:
	#desiredThroughput: The control system's Reference Throughput
	#domainID: The ID of the domain being controlled
	#physicalHost: An instance of the object that is used to connect the controller to the hypervisor
	#throughputTest: An instance of the test with Iper (only for test purposes, it will be removed)

	#Optional parameters
	#initalKp and initalKi: Initial Controller Parameters
	#adaptiveControlStatus: a boolean variable that indicates wheter the adaptive control will be used
	#initalCap: the initial cap given to the domain
	#errorPercentageAcceptable: The maximum error percentage (percentag related to the desiredThroughput) acceptable
	#samplePeriod: The period of the controller actions
	#systemParametrizationInitialA and systemParametrizationInitialB: The initial plant parametrization (used in system identification)
	#polesZerosR and polesZerosTheta: The desired poles of the feedback system. Indicated by the absolute valur(r) and its angle(theta)

	def __init__(self,desiredThroughput,domainID,physicalHost,throughputTest,adaptiveControlStatus=True,initialKp=-0.000003422,initialKi=0.000010158,initialCap=1,errorPercentageAcceptable=0.1,samplePeriod=1,systemParametrizationInitialA=0.0915,systemParametrizationInitialB=32259,polesZerosR=0.44932,polesZerosTheta=0.99794):
		self.initialCap = initialCap
		self.setkp(initialKp)
		self.setki(initialKi)
		self.setkpPLUSki()	
		self.setReferenceInput(desiredThroughput)
		self.setSamplePeriod(samplePeriod)
		self.setDomainID(domainID)
		measure = 0
		self.setLastMeasure(measure)
		self.setLastControllerOutput(math.log(self.initialCap,10))	
		self.setLastError(0)
		#Create the physicalHost instance
		self.physicalHost = physicalHost
		#Set the inital cap value
		self.physicalHost.setSchedParametersCap(self.initialCap,self.getDomainID())
		self.setErrorPercentageAcceptable(errorPercentageAcceptable)
		self.done = "False"
		#Set the adaptive control variables
		self.setAdaptiveControlStatus(adaptiveControlStatus)
		self.setFi(0,0)
		self.setZ(0)
		self.setEpsilon(0)
		theta = array([[systemParametrizationInitialA,systemParametrizationInitialB]]).T
		self.setCurrentTheta(theta)
		self.setLastTheta(theta)
		self.setPolesZerosR(polesZerosR)
		self.setPolesZerosTheta(polesZerosTheta)
		#The measure object - NEW
		#self.measureObject = measureObject
		#For Test purposes (indicate how the deadzone can lower control messages"
		self.samplesCount = 0
		self.actionsCount = 0	
		#For Test purposes ( list that has the parameters a and b for the model)
		self.parameterA = [systemParametrizationInitialA]
		self.parameterB = [systemParametrizationInitialB]		

		#Throughput Test Instance - OLD!! For Test purposes (duration,samplePeriod,packetSize,kbitRate,destination,testando)	 	
		self.test = throughputTest

	#Set Error Percentage Acceptable (The value must be in range from 0 to 1)
	def setErrorPercentageAcceptable(self,errorPercentageAcceptable):
		self.errorPercentageAcceptable = errorPercentageAcceptable
	#Get Error Percentage Acceptable
	def getErrorPercentageAcceptable(self):
		return float(self.errorPercentageAcceptable)
	#Set Domain ID
	def setDomainID(self,domainID):
		self.domainID = domainID
	#Get Domain ID
	def getDomainID(self):
		return self.domainID
	#Set Sample Period
	def setSamplePeriod(self,samplePeriod):
		self.samplePeriod = float(samplePeriod)
	#Get Sample Period	
	def getSamplePeriod(self):
		return float(self.samplePeriod)
	#Set Reference Input
	def setReferenceInput(self,reference):
		self.referenceInput = float(reference)
	#Get Reference Input
	def getReferenceInput(self):
		return float(self.referenceInput)
	#Set kp
	def setkp(self,kp):
		self.kp = float(kp)
	#Get kp
	def getkp(self):
		return float(self.kp)
	#Set ki
	def setki(self,ki):
		self.ki = float(ki)
	#Get ki
	def getki(self):
		return float(self.ki)
	#Set kp + ki
	def setkpPLUSki(self):
		self.kpPLUSki = self.getkp() + self.getki()	
	#Get kp + ki
	def getkpPLUSki(self):
		return float(self.kpPLUSki)
	#Set Current controller output
	def setCurrentControllerOutput(self,output):
		self.currentControllerOutput = float(output)
	#Get Current controller output
	def getCurrentControllerOutput(self):
		return float(self.currentControllerOutput)
	#Set Last controller output
	def setLastControllerOutput(self,output):
		self.lastControllerOutput = float(output)
	#Get Last controller output
	def getLastControllerOutput(self):
		return float(self.lastControllerOutput)
	#Set current control error
	def setCurrentError(self,error):
		self.currentError = float(error)
	#Return current control error	
	def getCurrentError(self):
		return float(self.currentError)
	#Set the last control error	
	def setLastError(self,error):
		self.lastError = float(error)
	#Return last control error	
	def getLastError(self):
		return float(self.lastError)
	#Set the last measure 
	def setLastMeasure(self,lastMeasure):
		self.lastMeasure = float(lastMeasure)
	#Get the last measure
	def getLastMeasure(self):
		return float(self.lastMeasure)
	#Set fi
	def setFi(self,y,u):
		self.fi = array([[y,u]]).T

	#Set and Get for Adaptive Control Variables
	#Set Adaptive Control Status	
	def setAdaptiveControlStatus(self,status):
		self.adaptiveControlStatus = status
	#Get Adaptive Control Status	
	def getAdaptiveControlStatus(self):
		return self.adaptiveControlStatus
	#Get fi
	def getFi(self):
		return self.fi
	#Set z
	def setZ(self,z):
		self.z = z
	#Get z
	def getZ(self):
		return self.z 
	#Set Epsilon
	def setEpsilon(self,epsilon):
		self.epsilon = epsilon
	#Get Epsilon
	def getEpsilon(self):
		return self.epsilon
	#Set Current Theta:
	def setCurrentTheta(self,theta):
		self.currentTheta = theta
	#Get Current Theta:
	def getCurrentTheta(self):
		return self.currentTheta
	#Set Last Theta:
	def setLastTheta(self,theta):
		self.lastTheta = theta
	#Get Last Theta:
	def getLastTheta(self):
		return self.lastTheta
	#Set Poles and Zeros R
	def setPolesZerosR(self,r):
		self.polesZerosR = float(r)
	#Get Poles and Zeros R
	def getPolesZerosR(self):
		return self.polesZerosR
	#Set Poles and Zeros Theta
	def setPolesZerosTheta(self,theta):
		self.polesZerosTheta = float(theta)
	#Get Poles and Zeros Theta
	def getPolesZerosTheta(self):
		return self.polesZerosTheta

		
	#Wait first sample based on an acquiring method.
	#If the mechanism waits more than 1.3*samplePeriod, we will stop waiting and leave the sensorAction to treat this abscence of measure.
	def waitFirstSample(self):
		waitTime = self.getSamplePeriod()/10
		#Start the measure gatherer - NEW
		#MeasureClientThread("3",self.measureObject).start()
		#Throughput Test Instance - OLD!!
		IperfThread("1",self.test).start()
		count = 0
		stopWaiting = "False"
		#NEW while ( self.measureObject.getMeasureCollectedStatus() == True ) and (stopWaiting == "False"):
		while ( self.test.getMeasureCollectedStatus() == True ) and (stopWaiting == "False"):
			time.sleep(waitTime)
			count = count + 1
			if (count >= 12):
				print "Wait Time Expired " + str(self.getDomainID())
				stopWaiting = "True"
	#Sensor system output based on an acquiring method(ex: using throughputTest for test purposes)
	def sensorAction(self):
		#Get the measure from test. The measure is in bits/s , so this method convert to kbit/s
		#NEW measure = float(self.measureObject.getLastMeasure())/1000
		#OLD
		measure = float(self.test.getLastMeasure())/1000
		measure = measure
		#Test if the measure is valid (consider throughput=0 as noise)
		if (measure >= 0):
			self.setLastMeasure(measure)
			return True
		else:
			#If the measure is a noise, we must consider the last LastMeasure as the current LastMeasure
			self.setLastMeasure(self.getLastMeasure())
			return False
	
	#Perform error evaluation
	def errorEvaluation(self):
		error = self.getReferenceInput() - self.getLastMeasure()
		self.setCurrentError(error)

	#Perfom the controller action
	def controllerAction(self):
		output = self.getLastControllerOutput() + ( self.getkpPLUSki()*self.getCurrentError() ) - ( self.getkp()*self.getLastError() )
		self.setCurrentControllerOutput(output)	

	#Actuate
	def actuatorAction(self):
		#The controller outpur is represented as log10(cap). To discover cap, this method does 10^controllerOutput. This method also rounds the cap because xen does not allow cap with decimal digits.
		cap = 10**(self.getCurrentControllerOutput())
		cap = int(round(cap))		
		if cap == 0:
			cap = 1
		elif cap > 100:
			cap = 100
		#Store the last cotroller output needed by the next control period
		lastControllerOutput = math.log(cap,10)
		self.setLastControllerOutput(lastControllerOutput)
		#Sum the cap with the constant cap value
		cap = cap
		if cap > 100:
			cap = 100
		#print "CAP=" + str(cap)
		#print "Actuating"
		self.physicalHost.setSchedParametersCap(cap,self.getDomainID())
		self.actionsCount = self.actionsCount + 1	
		#print "OK"


	#Collect parametrization signals z, fi and theta. Must Run after Sensor Action!!
	def parametrizationSignalsEvaluation(self):
		#Collect y(k)
		self.setZ(self.getLastMeasure())
		#Collect y(k-1)
		yK_1 = self.getReferenceInput() - self.getLastError()
		#Evaluate the new fi
		self.setFi(yK_1,self.getLastControllerOutput())

		#Gradient Algorithm - Projection

		#Evaluate Epsilon
		c = 0.0001
		alfa = 0.001
		lastThetaTransdotfi = dot(self.getLastTheta().T,self.getFi())
		fiTransdotfi = dot(self.getFi().T,self.getFi()) + c
		epsilon = (self.getZ() - lastThetaTransdotfi)/(fiTransdotfi)
		self.setEpsilon(epsilon)
		epsilondotFi = dot(float(self.getEpsilon()),self.getFi())
		epsilondotFi = dot(alfa,epsilondotFi)
		theta = add(self.getLastTheta(),epsilondotFi)
		self.setLastTheta(theta)
		self.setCurrentTheta(theta)

	#Evaluate new control parameters based on system identification
	def evaluateControlParameters(self):
		kp = (float(self.getCurrentTheta()[0]) - math.pow(self.getPolesZerosR(),2))/float(self.getCurrentTheta()[1])
		kp_plus_ki = ((float(self.getCurrentTheta()[0]) + 1) - (2*self.getPolesZerosR()*math.cos(self.getPolesZerosTheta())))/float(self.getCurrentTheta()[1])
		ki = kp_plus_ki - kp
		self.setkp(kp)
		self.setki(ki)
		self.setkpPLUSki()

	
	#Run the feedback control
	def runFeedackControl(self):
		self.waitFirstSample()
		#NEW while self.measureObject.getExperimentStatus() == True :
		#OLD
		while self.test.done == False :
			#print "Kp= " + str(self.getkp()) + " Ki= " + str(self.getki())
			noise = self.sensorAction()
			if(noise == False):
				print "Last Measure Not Collected " + str(self.getDomainID())
			self.errorEvaluation()
			#print self.getCurrentTheta()
			a = float(self.getCurrentTheta()[0])
			b = float(self.getCurrentTheta()[1])
			self.parameterA.append(a)
			self.parameterB.append(b)
			self.parametrizationSignalsEvaluation()
			#Only actuate if the error is greater than desired
			outputDesired = self.getReferenceInput()
			if ( abs(self.getCurrentError()) > abs(self.getErrorPercentageAcceptable()*outputDesired) ):
				self.controllerAction()
				self.actuatorAction()
				if ( self.getAdaptiveControlStatus() == True):
					self.evaluateControlParameters()
				waitPeriod = self.getSamplePeriod()		
			#Store the last error needed by the next control period
			self.setLastError(self.getCurrentError())
			#print "Measure " + str(self.getDomainID()) + " " + str(self.getLastMeasure())	
			self.samplesCount = self.samplesCount + 1
			time.sleep(waitPeriod)

		#Write the result for a and b to file
		fileName = self.test.resultName
		resultFileA = open(fileName + "_parameterA",'a')
		i = 0
		for parameter in self.parameterA:
			resultLine = str(i) + " " + str(float(parameter)) + "\n"
			resultFileA.write(resultLine)
			i += 1
		resultFileA.close()
		resultFileB = open(fileName + "_parameterB",'a')
		i = 0
		for parameter in self.parameterB:
			resultLine = str(i) + " " + str(float(parameter)) + "\n"
			resultFileB.write(resultLine)
			i += 1
		resultFileB.close()

