import numpy as np

from abc import ABC, abstractmethod
from functions import Function
from typing import Iterable, Tuple


class Solver(ABC):
    def __init__(self, function: Function):
        self.function = function

    @abstractmethod
    def rank(self, samples: Iterable[Iterable[float]], elite_size: int) -> (Iterable[Iterable[float]], float):
        pass

    @abstractmethod
    def sample(self, n: int) -> Iterable[float]:
        pass

    @abstractmethod
    def update(elite: Iterable[Iterable[float]]):
        pass


class SimpleEvolutionStrategy(Solver):
    def __init__(self, function: Function, mu: Iterable[float], sigma: Iterable[float] = None):
        if sigma and len(mu) != len(sigma):
            raise Exception('Length of mu and sigma must match')
        super().__init__(function)
        self.mu = mu
        if sigma:
            self.sigma = sigma
        else:
            self.sigma = [1] * len(mu)

    def rank(self, samples: Iterable[Iterable[float]], elite_size: int) -> (Iterable[Iterable[float]], Iterable[float]):
        fitness = self.function.eval(*samples.transpose())
        samples = samples[np.argsort(fitness)]
        fitness = np.sort(fitness)
        elite = samples[0:elite_size]
        return elite, fitness[0]

    def sample(self, n: int) -> Iterable[Iterable[float]]:
        return np.array([np.random.multivariate_normal(self.mu, np.diag(self.sigma)) for _ in range(n)])

    def update(self, elite: Iterable[Iterable[float]]):
        self.mu = elite[0]


class CMAEvolutionStrategy(Solver):
    def __init__(self, function: Function, mu: Iterable[float], covariances: Iterable[float] = None):
        if covariances is not None and len(mu) != covariances.shape[0] and len(mu) != covariances.shape[1]:
            raise Exception('Length of mu and covariance matrix must match')
        super().__init__(function)
        self.mu = mu
        if covariances is not None:
            self.covariances = covariances
        else:
            self.covariances = np.array([[1.0, 0.0], [0.0, 1.0]])

    def rank(self, samples: Iterable[Iterable[float]], elite_size: int) -> (Iterable[Iterable[float]], Iterable[float]):
        fitness = self.function.eval(*samples.transpose())
        samples = samples[np.argsort(fitness)]
        fitness = np.sort(fitness)
        elite = samples[0:elite_size]
        return elite, fitness[0]

    def sample(self, n: int) -> Iterable[Iterable[float]]:
        return np.array([np.random.multivariate_normal(self.mu, self.covariances) for _ in range(n)])

    def update(self, elite: Iterable[Iterable[float]]):
        mu = self.mu
        x = elite.transpose()[0]
        y = elite.transpose()[1]
        self.mu[0] = np.average(x)
        self.mu[1] = np.average(y)
        
        # TODO fix covariance matrix calculation using the tutorial by Nikolaus Hansen
        self.covariances[0][0] = np.average((x - mu[0])**2)
        self.covariances[1][1] = np.average((y - mu[1])**2)
        self.covariances[0][1] = np.average((x - mu[0]) * (y - mu[1]))
        # self.covariances = np.cov(elite.transpose())