# author: Christiaan Meijer (cwmeijer@gmail.com)
import random
import numpy

class FiniteDifference():
    def __init__(self):
        self.valueEstimator = None
        self.variance = 0
        self.usingfixedNumberOfEpisodes = True
        self.fixedNumberOfEpisodes = 0

    # Check and start components
    def initialize(self):
        if self.valueEstimator == None:
            raise Exception("FiniteDifference gradient estimator initialized without a valueEstimator.")
            
        if self.variance == 0:
            raise Exception("FiniteDifference gradient estimator initialized with a variance of 0.")
            
    # Generates vector of dimensions dimensions. Each element is + or - the value
    # of the self.variance.
    # @param dimensions is the number of dimensions of the parameters.
    # @return the variation (numpy array)
    def generateVariation(self, dimensions):
        variation = numpy.zeros(dimensions)
        for i in range(0, dimensions):
            r = random.randint(0,2)
            if r == 1:
                sign = 0
            else:
                if r == 0:
                    sign = -1
                else:
                    sign = +1
            variation[i] += sign * self.variance
        return variation
        
    def getGradient(self, parameters):
        dimensions = len(parameters)
        variations = numpy.array([[]])
        valueEstimations = numpy.array([[]])

        converged = False        
        gradient = numpy.zeros(dimensions)
        value = 0

        count = 0
        while(converged == False):

            currentVariation = self.generateVariation(dimensions)
            currentEvaluation = self.valueEstimator.getValue(parameters + currentVariation)
            
            augmentedCurrentVariation = numpy.append(currentVariation, [1])
            

            if variations.size == 0:
                variations = numpy.append(variations, [augmentedCurrentVariation], 1)
            else:
                variations = numpy.append(variations, [augmentedCurrentVariation], 0)

            valueEstimations = numpy.append(valueEstimations, [[currentEvaluation]], 1)
            count += 1

            # Determine whether enough data is already gathered to calculate the
            # gradient. Note that calculating the gradient can always fail with
            # a singular matrix.
            readyToCalcGradient = False
            if self.usingFixedNumberOfEpisodes:
                readyToCalcGradient = count >= self.fixedNumberOfEpisodes
            else:
                readyToCalcGradient = count > dimensions
            
            if readyToCalcGradient:
                # See Table 2 of [1]
                
                currentGradient = gradient
                try:
                    Theta = variations
                    J = valueEstimations.T
                    
                    

                    arr = numpy.dot(Theta.T, Theta)

                    mat = numpy.matrix(arr)

                    factor1 = numpy.linalg.inv(mat)

                    factor1 = numpy.array(factor1)

                    factor2 = numpy.dot(Theta.transpose(), J)
                    result = numpy.dot(factor1, factor2)
                    
                    
                    currentGradient = result[0:-1]
                    
                    Jref = result[-1:]
                    value = Jref[0][0]

                except:
                    print('Singular matrix in FD:getGradient()')
                    continue
                
                gradient = currentGradient.T[0] # reshape
                converged = True
        
        norm = numpy.linalg.norm(gradient)
        if norm == 0.0:
            randomStep = numpy.random.random(len(gradient)) - 0.5 * numpy.ones(len(gradient))
            gradient = randomStep
            norm = numpy.linalg.norm(gradient)
            print('FiniteDifference:getGradient: FD iteration resulted in zero gradient. Returning random step instead.')
        gradient = (1.0/norm) * gradient # normalize
        print('FiniteDifference:getGradient: episodes : ' + `count`)
        return gradient, value, count      

# [1] J. Peters and S. Schaal, Reinforcement learning of motor skills with policy gradients (2008)    


################################################################################
################################################################################

def runUnitTests(verbose):
    tests = [
        distanceValueOptimization,      
            ]
            
    for test in tests:
        succes, name = test(verbose)
        if succes:
            outcome = 'Succes'
        else:
            outcome = 'Failed'
        print(outcome + ' : ' + name)

def distanceValueOptimization(self):
    pass
    return not(failed), 'interpolationLineValue'
    
if __name__ == "__main__":
    import sys
    verbose = False
    if len(sys.argv) > 1:
        verbose = sys.argv[1]
    runUnitTests(verbose)


93b0a905d11689233529e92f774a263c9626bf49
# author: Christiaan Meijer (cwmeijer@gmail.com)
import random
import numpy

class FiniteDifference():
    def __init__(self):
        self.valueEstimator = None
        self.variance = 0
        self.usingfixedNumberOfEpisodes = True
        self.fixedNumberOfEpisodes = 0

    # Check and start components
    def initialize(self):
        if self.valueEstimator == None:
            raise Exception("FiniteDifference gradient estimator initialized without a valueEstimator.")
            
        if self.variance == 0:
            raise Exception("FiniteDifference gradient estimator initialized with a variance of 0.")
            
    # Generates vector of dimensions dimensions. Each element is + or - the value
    # of the self.variance.
    # @param dimensions is the number of dimensions of the parameters.
    # @return the variation (numpy array)
    def generateVariation(self, dimensions):
        variation = numpy.zeros(dimensions)
        for i in range(0, dimensions):
            r = random.randint(0,2)
            if r == 1:
                sign = 0
            else:
                if r == 0:
                    sign = -1
                else:
                    sign = +1
            variation[i] += sign * self.variance
        return variation
        
    def getGradient(self, parameters):
        dimensions = len(parameters)
        variations = numpy.array([[]])
        valueEstimations = numpy.array([[]])

        converged = False        
        gradient = numpy.zeros(dimensions)
        value = 0

        count = 0
        while(converged == False):

            currentVariation = self.generateVariation(dimensions)
            currentEvaluation = self.valueEstimator.getValue(parameters + currentVariation)
            
            augmentedCurrentVariation = numpy.append(currentVariation, [1])
            

            if variations.size == 0:
                variations = numpy.append(variations, [augmentedCurrentVariation], 1)
            else:
                variations = numpy.append(variations, [augmentedCurrentVariation], 0)

            valueEstimations = numpy.append(valueEstimations, [[currentEvaluation]], 1)
            count += 1

            # Determine whether enough data is already gathered to calculate the
            # gradient. Note that calculating the gradient can always fail with
            # a singular matrix.
            readyToCalcGradient = False
            if self.usingFixedNumberOfEpisodes:
                readyToCalcGradient = count >= self.fixedNumberOfEpisodes
            else:
                readyToCalcGradient = count > dimensions
            
            if readyToCalcGradient:
                # See Table 2 of [1]
                
                currentGradient = gradient
                try:
                    Theta = variations
                    J = valueEstimations.T
                    
                    

                    arr = numpy.dot(Theta.T, Theta)

                    mat = numpy.matrix(arr)

                    factor1 = numpy.linalg.inv(mat)

                    factor1 = numpy.array(factor1)

                    factor2 = numpy.dot(Theta.transpose(), J)
                    result = numpy.dot(factor1, factor2)
                    
                    
                    currentGradient = result[0:-1]
                    
                    Jref = result[-1:]
                    value = Jref[0][0]

                except:
                    print('Singular matrix in FD:getGradient()')
                    continue
                
                gradient = currentGradient.T[0] # reshape
                converged = True
        
        norm = numpy.linalg.norm(gradient)
        if norm == 0.0:
            randomStep = numpy.random.random(len(gradient)) - 0.5 * numpy.ones(len(gradient))
            gradient = randomStep
            norm = numpy.linalg.norm(gradient)
            print('FiniteDifference:getGradient: FD iteration resulted in zero gradient. Returning random step instead.')
        gradient = (1.0/norm) * gradient # normalize
        print('FiniteDifference:getGradient: episodes : ' + `count`)
        return gradient, value, count      

# [1] J. Peters and S. Schaal, Reinforcement learning of motor skills with policy gradients (2008)    


################################################################################
################################################################################

def runUnitTests(verbose):
    tests = [
        distanceValueOptimization,      
            ]
            
    for test in tests:
        succes, name = test(verbose)
        if succes:
            outcome = 'Succes'
        else:
            outcome = 'Failed'
        print(outcome + ' : ' + name)

def distanceValueOptimization(self):
    pass
    return not(failed), 'interpolationLineValue'
    
if __name__ == "__main__":
    import sys
    verbose = False
    if len(sys.argv) > 1:
        verbose = sys.argv[1]
    runUnitTests(verbose)

