import matplotlib.pyplot as plt
from matplotlib import animation
import numpy as np
from numba import jit
from timeit import default_timer as timer
import random

start = timer()

'''
FRACTAL

Functions and parameters to change the appearance and behavior of the fractals generated
'''

# PARAMETERS TO CHANGE THE FRACTAL GENERATED
seq = "AB"                     # sequence to alternate r values
a_lb = 2                       # b1 lower bound
a_ub = 4                       # b1 upper bound
b_lb = 2                       # b2 lower bound
b_ub = 4                       # b2 upper bound

# PARAMETERS REFINING ACCURACY OF FRACTAL PICTURE GENERATED
num_warmups = 1200             # number of "warmups" or throwaway iterations before computing lyapunov exponent
num_lyap_iterations = 120      # number of iterations used to compute the lyapunov exp
steps = 300                    # steps between b1 and b2 values on axes -- higher it is, the better the picture

# LOGISTIC MAP THAT GIVES US THE NEXT X
@jit
def F(x, curr_r):
    return (curr_r * x) * (1 - x)

# DERIVATIVE OF F -- USED TO COMPUTE THE LYAPUNOV EXPONENT
@jit
def Fprime(x, curr_r):
    ans = curr_r * (1 - (2 * x))
    ans[ans == 0] = 0.0001
    ans[ans == -np.inf] = -1000
    ans[ans == np.inf] = 1000
    return ans

# CREATING RANDOM SEQUENCE WITH A LENGTH OF THE TOTAL NUMBER OF ITERATIONS
# EACH ITERATION THE PROBABILITY WILL BE JUDGED AGAINST THIS LIST
@jit
def getrandomseq():
    problist = list()
    for i in range(num_lyap_iterations + num_warmups):
        problist.append(random.randint(0,99))
    return problist

# RETURNS THE CORRECT B-VALUE BASED ON THE CURRENT ITERATION
@jit
def getseqval(curr_iteration, a, b, probability, problist):
    randnum = problist[curr_iteration]
    index = np.mod(curr_iteration, len(seq))
    if (seq[index] == 'A'):
        if (probability <= randnum):
            return a
        else:
            return b
    else:
        if (probability <= randnum):
            return b
        else:
            return a

# RETURNS THE LYAPUNOV EXPONENT BASED ON THE SPECIFIED B1 AND B2 VALUES
@jit
def getlyapexponent(time_scale, probability, problist):
    a, b = time_scale
    lyap_prob = probability
    #print("b1", b1, "b2", b2, "prob", lyap_prob)

    x = .5          # initial value of x
    lyapsum = 0     # initializing lyapunov sum for use later

    # do warmups, to discard the early values of the iteration to allow the orbit to settle down
    for i in range(num_warmups):
        x = F(x, getseqval(i, a, b, lyap_prob, problist))

    for i in range(num_warmups, num_lyap_iterations + num_warmups):
        lyapsum += np.log( np.abs(Fprime( x, getseqval(i, a, b, lyap_prob, problist) ) ) )
        # get next x
        x = F( x, getseqval(i, a, b, lyap_prob, problist) )

    return (lyapsum / num_lyap_iterations)

'''
ANIMATING FRACTAL

Making each frame and creating animation to then save with ImageMagick
'''
plt.rcParams["animation.convert_path"] = r"C:\Program Files\ImageMagick-7.0.7-Q16\magick.exe"
plt.rcParams['animation.html'] = 'html5'

# INITIALIZING LIN-SPACE FOR FRACTAL GRID
a = np.linspace(a_lb, a_ub, steps)   #range of b1 values
b = np.linspace(b_lb, b_ub, steps)   #range of b2 values
aa, bb = np.meshgrid(a, b)

# INITIALIZING GRAPHING ELEMENTS FOR ANIMATION
fig, ax = plt.subplots()

# ADJUSTING GRAPH
lyap_cmap = plt.get_cmap('nipy_spectral')   # creating our own colormap to use "set_over" with
lyap_cmap.set_over('black')                 # any value over vmax is colored black
lyap_cmap.set_under('#5e1d77')              # any value under vmin is colored dark purple
plt.title("Probabilistic Lyapunov fractal / SEQ: " + seq)
plt.xlabel("a")
plt.ylabel("b")

# HOW TO DRAW FRAME NUMBER i
def animate(i):
    print("frame #:", i)
    probability = i

    lyap_exponents = []
    for i in range(10):
        problist = getrandomseq()
        lyap_exponents.append(getlyapexponent( (bb, aa), probability, problist ))

    grid = np.average(lyap_exponents, axis = 0)

    return grid

# CREATING FRACTAL IMAGES
ims = []
for i in range(101):
    grid = animate(i)
    subtitle = ax.text(0.5, 0.95, "Probability values will switch: {}%".format(i), bbox={'facecolor':'w', 'alpha':0.8, 'pad':5}, ha="center", transform=ax.transAxes,)
    im = ax.imshow(grid, cmap = lyap_cmap, vmin = -2, vmax = 0, origin = 'lower', extent = [a_lb, a_ub, b_lb, b_ub])
    ims.append([im, subtitle])

anim = animation.ArtistAnimation(fig, ims, interval = 200, blit = True)
anim.save(r"Lyap_Prob.gif", writer="imagemagick")

plt.show()

end = timer()
print("elapsed time: " + str(end - start))


import numpy as np

def _unnormalizedGaussian(x, mu, sigma):
    return np.exp( -((x-mu)**2) / (2*(sigma**2)) )

def _logisticSigmoid(x):
    return 1 / (1 + np.exp(-x))

def _sigmoid(x, mu, sigma):
    return _logisticSigmoid( (x-mu) / sigma )

class Basis():
    """
    Args:
        x (np.array(shape=(M, D)): input vector (D: data dimension)
            all fields should be normalized to [-1, 1]
    Return:
        basis function value vector (np.array(shape=(M, D)))
    """
    def apply(self, x):
        raise 'Not Implemented'

"""
phi_j(x) = x
"""
class SimpleBasis(Basis):
    def apply(self, x):
        return x

"""
phi_j(x) = x^j
"""
class PowerBasis(Basis):
    def apply(self, x):
        result = x.copy()
        for (m, j), value in np.ndenumerate(result):
            result[m, j] = value**(j+1)
        return result

"""
phi_j(x) with unnormalizedGaussian
    mu_j s are determined s.t. they evenly devide [-1, 1]
"""
class GaussianBasis(Basis):
    def __init__(self, sigma=1):
        self.sigma = sigma

    def apply(self, x):
        mus = np.arange(start=-1, stop=1, step=(2/x.shape[0]))
        return _unnormalizedGaussian(x, np.reshape(mus, (mus.shape[0], 1)), self.sigma)

"""
phi_j(x) with sigmoid
    mu_j s are determined s.t. they evenly devide [-1, 1]
"""
class SigmoidBasis(Basis):
    def __init__(self, sigma=1):
        self.sigma = sigma

    def apply(self, x):
        mus = np.arange(start=-1, stop=1, step=(2/x.shape[0]))
        return _sigmoid(x, np.reshape(mus, (mus.shape[0], 1)), self.sigma)


'''
Cost functions
'''

import numpy as np
import tensorflow as tf
from tensorflow import math as tfm

def kl_normal(mu_1,log_sig_sq_1,mu_2,log_sig_sq_2):
    '''
    Element-wise KL divergence between two normal distributions
    INPUTS:
        mu_1 - mean of firat distribution
        log_sig_sq_1 - log variance of first diatribution
        mu_2 - mean of second distribution
        log_sig_sq_2 - log variance of second diatribution
    OUTPUTS:
        KL - element-wise KL divergence
    '''

    v_mean = mu_2 #2
    aux_mean = mu_1 #1
    v_log_sig_sq = log_sig_sq_2 #2
    aux_log_sig_sq = log_sig_sq_1 #1
    v_log_sig = tfm.log(tfm.sqrt(tfm.exp(v_log_sig_sq))) #2
    aux_log_sig = tfm.log(tfm.sqrt(tfm.exp(aux_log_sig_sq))) #1
    KL = v_log_sig-aux_log_sig+tf.divide(tfm.exp(aux_log_sig_sq)+tfm.square(aux_mean-v_mean),2.0*tfm.exp(v_log_sig_sq))-0.5

    return KL

def np_kl_normal(mu_1,log_sig_sq_1,mu_2,log_sig_sq_2):
    '''
    Element-wise KL divergence between two normal distributions
    INPUTS:
        mu_1 - mean of firat distribution
        log_sig_sq_1 - log variance of first diatribution
        mu_2 - mean of second distribution
        log_sig_sq_2 - log variance of second diatribution
    OUTPUTS:
        KL - element-wise KL divergence
    '''

    v_mean = mu_2 #2
    aux_mean = mu_1 #1
    v_log_sig_sq = log_sig_sq_2 #2
    aux_log_sig_sq = log_sig_sq_1 #1
    v_log_sig = np.log(np.sqrt(np.exp(v_log_sig_sq))) #2
    aux_log_sig = np.log(np.sqrt(np.exp(aux_log_sig_sq))) #1
    KL = v_log_sig-aux_log_sig+np.divide(np.exp(aux_log_sig_sq)+np.square(aux_mean-v_mean),2.0*np.exp(v_log_sig_sq))-0.5

    return KL

def kl_unit(mu,log_sig_sq):
    '''
    Element-wise KL divergence between normal distributions and unit normal distribution
    INPUTS:
        mu - mean of distribution
        log_sig_sq - log variance of diatribution
    OUTPUTS:
        KL - element-wise KL divergence
    '''

    KL = -0.5*(1 + log_sig_sq - tf.square(mu) - tf.exp(log_sig_sq))

    return KL

def reparameterisation_trick(mu, log_sig_sq):
    '''
    Sample from Gaussian such that it stays differentiable
    INPUTS:
        mu - mean of distribution
        log_sig_sq - log variance of diatribution
    OUTPUTS:
        samp - sample from distribution
    '''

    eps = tf.random.normal([tf.shape(mu)[0], tf.shape(mu)[1]], 0, 1., dtype=tf.float32)
    samp = tfm.add(mu, tfm.multiply(tfm.sqrt(tfm.exp(log_sig_sq)), eps))

    return samp

def gaussian_log_likelihood(x,mu_x,log_sig_sq_x,SMALL_CONSTANT = 1e-5):
    '''
    Element-wise Gaussian log likelihood
    INPUTS:
        x = points
        mu_x - means of Gaussians
        log_sig_sq_x - log variance of Gaussian
    OPTIONAL INPUTS:
        SMALL_CONSTANT - small constant to avoid taking the log of 0 or dividing by 0
    OUTPUTS:
        log_lik - element-wise log likelihood
    '''

    # -E_q(z|x) log(p(x|z))
    normalising_factor = - 0.5 * tfm.log(SMALL_CONSTANT+tfm.exp(log_sig_sq_x)) - 0.5 * np.log(2.0 * np.pi)
    square_diff_between_mu_and_x = tfm.square(mu_x - x)
    inside_exp = -0.5 * tfm.divide(square_diff_between_mu_and_x,SMALL_CONSTANT+tfm.exp(log_sig_sq_x))
    log_lik = normalising_factor + inside_exp

    return log_lik

def np_gaussian_log_likelihood(x,mu_x, log_sig_sq_x, SMALL_CONSTANT = 1e-7):
    '''
    Element-wise Gaussian log likelihood
    INPUTS:
        x = points
        mu_x - means of Gaussians
        log_sig_sq_x - log variance of Gaussian
    OPTIONAL INPUTS:
        SMALL_CONSTANT - small constant to avoid taking the log of 0 or dividing by 0
    OUTPUTS:
        log_lik - element-wise log likelihood
    '''

    # -E_q(z|x) log(p(x|z))
    normalising_factor = - 0.5 * np.log(SMALL_CONSTANT+np.exp(log_sig_sq_x)) - 0.5 * np.log(2.0 * np.pi)
    square_diff_between_mu_and_x = np.square(mu_x - x)
    inside_exp = -0.5 * np.divide(square_diff_between_mu_and_x,SMALL_CONSTANT+np.exp(log_sig_sq_x))
    log_lik = normalising_factor + inside_exp

    return log_lik

def cvae_cost(x, y, encoder, decoder, encoder_c, wu_c=0.0):
    '''
    Cost function for conditional VAE
    INPUTS:
        x - inputs/conditions
        y - outputs to be reconstructed
        encoder - the neural network to be used for mapping x and y to mu_z and log_sig_sq_z
        decoder - the neural network to be used for mapping z and x to mu_y and log_sig_sq_y
        encoder_c - the neural network to be used for mapping x to mu_cz and log_sig_sq_cz (conditional prior distribution in the latent space)
    OUTPUTS:
        cost - the cost function
        wu_c = constant for bottleneck warmup. wu_c=0 means no bottleneck, wu_c=1 means completely imposing the bottleneck through the latent space
    '''

    x = tf.cast(x,tf.float32)
    y = tf.cast(y,tf.float32)

    # compute moments of p(z|x)
    mu_cz, log_sig_sq_cz = encoder_c.compute_moments(x)

    # compute moments of q(z|x,y)
    mu_z, log_sig_sq_z = encoder.compute_moments(x,y)

    # sample from q(z|x,y)
    z = reparameterisation_trick(mu_z, log_sig_sq_z)

    # bottleneck warmup
    x_wu = ((1.0-wu_c)*x + wu_c*tf.random.uniform(tf.shape(x)))

    # compute moments of p(y|z,x)
    mu_y, log_sig_sq_y = decoder.compute_moments(z,x_wu)

    # KL(q(z|x,y)|p(z|x))
    KLe = kl_normal(mu_z,log_sig_sq_z,mu_cz,log_sig_sq_cz)
    KLc = tfm.reduce_sum(KLe,1)
    KL = tfm.reduce_mean(tf.cast(KLc,tf.float32))

    # -E_q(z|y,x) log(p(y|z,x))
    reconstr_loss = -tfm.reduce_sum(gaussian_log_likelihood(y,mu_y,log_sig_sq_y), 1)
    cost_R = tfm.reduce_mean(reconstr_loss)

    # -ELBO
    cost = cost_R + KL

    return cost

def dual_cvae_cost(x, x2, y, encoder, decoder, encoder_c, wu_c=0.0, constrain=True):
    '''
    Cost function for conditional VAE with two conditions
    INPUTS:
        x - inputs/conditions
        x2 - second inputs/conditions
        y - outputs to be reconstructed
        encoder - the neural network to be used for mapping x, x2 and y to mu_z and log_sig_sq_z
        decoder - the neural network to be used for mapping z, x and x2 to mu_y and log_sig_sq_y
        encoder_c - the neural network to be used for mapping x and x2 to mu_cz and log_sig_sq_cz (conditional prior distribution in the latent space)
    OUTPUTS:
        cost - the cost function
        wu_c = constant for bottleneck warmup. wu_c=0 means no bottleneck, wu_c=1 means completely imposing the bottleneck through the latent space
    '''

    x = tf.cast(x,tf.float32)
    x2 = tf.cast(x2,tf.float32)
    y = tf.cast(y,tf.float32)

    # compute moments of p(z|x)
    mu_cz, log_sig_sq_cz = encoder_c.compute_moments(x,x2)

    # compute moments of q(z|x,y)
    mu_z, log_sig_sq_z = encoder.compute_moments(y,x,x2)

    # sample from q(z|x,y)
    z = reparameterisation_trick(mu_z, log_sig_sq_z)

    # bottleneck warmup
    x_wu = ((1.0-wu_c)*x + wu_c*tf.random.uniform(tf.shape(x)))
    x2_wu = ((1.0-wu_c)*x2 + wu_c*tf.random.uniform(tf.shape(x2)))

    # compute moments of p(y|z,x)
    mu_y, log_sig_sq_y = decoder.compute_moments(z,x_wu,x2_wu,constrain=constrain)

    # KL(q(z|x,y)|p(z|x))
    KLe = kl_normal(mu_z,log_sig_sq_z,mu_cz,log_sig_sq_cz)
    KLc = tfm.reduce_sum(KLe,1)
    KL = tfm.reduce_mean(tf.cast(KLc,tf.float32))

    # -E_q(z|y,x) log(p(y|z,x))
    reconstr_loss = -tfm.reduce_sum(gaussian_log_likelihood(y,mu_y,log_sig_sq_y), 1)
    cost_R = tfm.reduce_mean(reconstr_loss)

    # -ELBO
    cost = cost_R + KL

    return cost

def forward_model(targets, low_fidelity, high_fidelity, encoder, decoder, encoder_c, wu_c=0.0, difference=False):
    '''
    Cost function for multi-fidelity forward model
    INPUTS:
        targets - ground-truths to be eventually reconstructed with the inverse model
        low_fidelity - low-fidelity measurements simulated with some analytical forward model
        high_fidelity - measured high fidelity measurements to be reproduced
        encoder - the neural network to be used for mapping targets, low_fidelity and high_fidelity to mu_z and log_sig_sq_z
        decoder - the neural network to be used for mapping z, targets and low_fidelity to mu_hf and log_sig_sq_hf
        encoder_c - the neural network to be used for mapping targets and low_fidelity to mu_cz and log_sig_sq_cz (conditional prior distribution in the latent space)
    OPTIONAL INPUTS:
        difference - whether to model the high-fidelity directly (False) or the difference between high and low fidelity (True)
        wu_c = constant for bottleneck warmup. wu_c=0 means no bottleneck, wu_c=1 means completely imposing the bottleneck through the latent space
    OUTPUTS:
        cost - the cost function
    '''

    if difference==False:
        cost = dual_cvae_cost(targets, low_fidelity, high_fidelity, encoder, decoder, encoder_c, wu_c=0.0)
    else:
        cost = dual_cvae_cost(targets, low_fidelity, high_fidelity-low_fidelity, encoder, decoder, encoder_c, wu_c=0.0, constrain=False)

    return cost

def inverse_model(measurements, targets, encoder, decoder, encoder_c, wu_c=0.0):
    '''
    Cost function for inverse model
    INPUTS:
        measurements - the measurements to infer from
        targets - ground-truths to be eventually reconstructed with the inverse model
        encoder - the neural network to be used for mapping measurements and targets to mu_z and log_sig_sq_z
        decoder - the neural network to be used for mapping z and measurements to mu_targets and log_sig_sq_targets
        encoder_c - the neural network to be used for mapping measurements to mu_cz and log_sig_sq_cz (conditional prior distribution in the latent space)
    OPTIONAL INPUTS:
        wu_c = constant for bottleneck warmup. wu_c=0 means no bottleneck, wu_c=1 means completely imposing the bottleneck through the latent space
    OUTPUTS:
        cost - the cost function
    '''

    cost = cvae_cost(measurements, targets, encoder, decoder, encoder_c, wu_c=0.0)

    return cost


import numpy as np
import math

def generate_vector(x):
    """
    Taking the original independent variables matrix and
    add a row of 1 which corresponds to x_0
        Parameters:
        X:  independent variables matrix
        Return value: the matrix that contains all the
        values in the dataset, not include the outcomes variables.
    """
    vector_x = np.c_[np.ones((len(x), 1)), x]
    return vector_x

def theta_init(x):
    """
    Generate an initial value of vector θ from the original
    independent variables matrix
        Parameters:
        X:  independent variables matrix
        Return value: a vector of theta filled with initial guess
    """
    theta = np.random.randn(len(x[0]) + 1, 1)
    return theta

def sigmoid_function(x):
    """
    Calculate the sigmoid value of the inputs
        Parameters:
            X:  values
        Return value: the sigmoid value
    """
    return 1 / (1 + math.e**(-x))

def logistic_regression(x, y, learningrate, iterations):
    """
    Find the Logistics regression model for the data set
        Parameters:
            X: independent variables matrix
            y: dependent variables matrix
            learningrate: learningrate of Gradient Descent
            iterations: the number of iterations
        Return value: the final theta vector
    """
    y_new = np.reshape(y, (len(y), 1))
    cost_lst = []
    vector_x = generate_vector(x)
    theta = theta_init(x)
    m = len(x)
    for _ in range(iterations):
        gradients = 2 / m * vector_x.T.dot(
            sigmoid_function(vector_x.dot(theta)) - y_new)
        theta = theta - learningrate * gradients
        y_pred = sigmoid_function(vector_x.dot(theta))
        cost_value = -np.sum(
            np.dot(y_new.T,
                   np.log(y_pred) + np.dot(
                       (1 - y_new).T, np.log(1 - y_pred)))) / (len(y_pred))
        # Calculate the loss for each training instance
        cost_lst.append(cost_value)
    return theta

def column(matrix, i):
    """
    Returning all the values in a specific columns
        Parameters:
            X: the input matrix
            i: the column
        Return value: an array with desired column
    """
    return [row[i] for row in matrix]

def predict(x, theta):
    """
        Predict the outcome of the data set
    """
    hypo_line = theta[0]
    for i in range(1, len(theta)):
        hypo_line = hypo_line + theta[i] * column(x, i - 1)
    y_pred = sigmoid_function(hypo_line)

    for j in range(len(y_pred)):
        # print(j, y_pred[j])
        if y_pred[j] >= 0.5:
            y_pred[j] = 1
        else:
            y_pred[j] = 0

    return y_pred.reshape(len(y_pred), 1)


from mpl_toolkits.mplot3d import axes3d
from matplotlib import cm
import tensorflow as tf
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.colors as colors
import matplotlib.cm as cmx
import os

os.environ['TF_CPP_MIN_LOG_LEVEL']='2'

plt.style.use('ggplot')

# Single parameter gradient descent
# TODO: Search as to why the iteration stops at the nearest local minima
""" Local minima/optima
Gradient descent functions contain "minima" which is the lowest value
or point and maxima or the highest value. Local minima/maxima means the
lowest/highest value of the local wave/trough
"""
fig = plt.figure(figsize=(10, 6))
ax = fig.gca()
x = np.linspace(-1, 1, 200)
hz = 10
cost = np.sin(hz*x)*np.exp(-x)
# Get the difference between every value
gradient = np.diff(cost)
ax.plot(x, cost)
ax.set_ylabel('Cost')
ax.set_xlabel('Some Parameter')
# Number of iterations means the number of which the running/learning repeats
n_iterations = 500
cmap = plt.get_cmap('coolwarm')
c_norm = colors.Normalize(vmin=0, vmax=n_iterations)
scalar_map = cmx.ScalarMappable(norm=c_norm, cmap=cmap)
""" init_p means the location of the nth(init_p) number in the linear space or
where should the point start, down to the nearest local minima (if the gradient
is negative)
"""
init_p = 120 # np.random.randint(len(x)*0.2, len(x)*0.8)
# Means how the iteration should move forward
learning_rate = 1.0
# Implement a negative gradient of the function
for iter_i in range(n_iterations):
	# The dots must go down since it is decremented
	init_p -=learning_rate * gradient[int(init_p)]
	# Parameters x, y, circle color, transparency, color
	ax.plot(x[int(init_p)], cost[int(init_p)], 'ro', alpha=(iter_i + 1) / n_iterations, color=scalar_map.to_rgba(iter_i))
plt.show()

# Two-parameter gradient descent
# TODO: Understand this concept
fig = plt.figure(figsize=(10, 6))
ax = fig.gca(projection='3d')
x, y = np.mgrid[-1:1:0.02, -1:1:0.02]
X, Y, Z = x, y, np.sin(hz*x)*np.exp(-x)*np.cos(hz*y)*np.exp(-y)
ax.plot_surface(X, Y, Z, rstride=2, cstride=2, alpha=0.75, cmap='jet', shade=False)
ax.set_ylabel('Some Parameter 2')
ax.set_xlabel('Some Parameter 1')
ax.set_zlabel('Cost')
plt.show()

# Plot different learning rates
fig, axs = plt.subplots(1, 3, figsize=(20, 6))
for rate_i, learning_rate in enumerate([0.01, 1.0, 500.0]):
	ax = axs[rate_i]
	x = np.linspace(-1, 1, 200)
	hz = 10
	cost = np.sin(hz*x)*np.exp(-x)
	gradient = np.diff(cost)
	ax.plot(x, cost)
	ax.set_ylabel('Cost')
	ax.set_xlabel('Some Parameter')
	ax.set_title(str(learning_rate))
	n_iterations = 500
	cmap = plt.get_cmap('coolwarm')
	c_norm = colors.Normalize(vmin=0, vmax=n_iterations)
	init_p = 120
	for iter_i in range(n_iterations):
		init_p -= learning_rate * gradient[int(init_p)]
		ax.plot(x[int(init_p)], cost[int(init_p)], 'ro', alpha=(iter_i + 1) / n_iterations, color=scalar_map.to_rgba(iter_i))
plt.show()


import numpy as np

macierz = np.array([
    [19/12, 13/12, 5/6, 5/6, 13/12, -17/12],
    [13/12, 13/12, 5/6, 5/6, -11/12, 13/12],
    [5/6, 5/6, 5/6, -1/6, 5/6, 5/6],
    [5/6, 5/6, -1/6, 5/6, 5/6, 5/6],
    [13/12, -11/12, 5/6, 5/6, 13/12, 13/12],
    [-17/12, 13/12, 5/6, 5/6, 13/12, 19/12],
],dtype=float)

precision = 0.00000001

def wWlasna(czyPierwsza: bool, poprzedniaWWasna: float):
    y = np.array([1.0, 0.0, 0.0, 0.0, 0.0, 0.0],dtype=float)
    yPrim = 0.0
    while True:
        zk = np.matmul(macierz,y)

        if(czyPierwsza):
            zk = ortogonalizacja(zk,poprzedniaWWasna)

        zk_norm = np.linalg.norm(zk)

        yPrim = np.zeros(len(zk))
        for i in range(len(yPrim)):
            yPrim[i] += zk[i]/zk_norm

        if abs(abs(y[0]) - abs(yPrim[0])) < precision:
            print("wektor wlasny:", yPrim)
            print("wartosc wlasna:", zk_norm)
            break

        y = yPrim

    return yPrim

def ortogonalizacja(zk, e):
    a = 0
    for i in range(len(zk)):
        a += zk[i] * e[i]

    for i in range(len(zk)):
        zk[i] -= e[i] * a

    return zk

wwlasna1 = wWlasna(False,0.0)
wwlasna2 = wWlasna(True, wwlasna1)
numpyWlasna = np.linalg.eig(macierz)
print("wartosci wlasne numpy:", numpyWlasna[0])


import numpy as np

class GaussLegendreShapeFunction(object):
    num_points = None
    num_coord = None

class LinearGaussLegendre1D(GaussLegendreShapeFunction):
    num_points = 2
    num_coord = 1
    def eval(self, xi):
        '''Value of linear 1D shape function evaluated at xi

        Parameters
        ----------
        xi : real
            Point inside element in natural coordinates

        Returns
        -------
        f : ndarray of real (2,)
            The element shape function array at xi
        '''
        return np.array([1. - xi, 1. + xi]) * (xi >= -1.) * (xi <= 1.) / 2.

    def grad(self, xi):
        '''Derivative of linear 1D shape functions

        Parameters
        ----------
        xi : real
            Point inside element in natural coordinates

        Returns
        -------
        df : ndarray of real (1,2)
            The element shape function derivative array at xi
        '''
        return np.array([-1., 1.]) * (xi >= -1.) * (xi <= 1.) / 2.


# %% markdown
# # truncated_normal
# generate N samples from a truncated normal distribution with mean=mu, sigma=sigma and with bounds A and B
#
# **USAGE**:
# [out] = truncated_normal(a, b, mu, sigma, n)
#
# **INPUTS**:
# - A: lower bound
# - B: upper bound
# - mu: mean
# - sigma: sigma, standard deviation
# - N: number of samples
#
# **OUTPUTS**:
# - out: mean + truncated Gaussian noise with mean=mu, sigma=sigma, between bounds A and B
#
# **EXAMPLE USAGE**:
# - truncated_normal(-1, 1, 0.1, 0.001, 1000)

# %%
from scipy import special
from numpy.random import rand
from math import sqrt

def truncated_normal(a, b, mu, sigma, n):
    if (b - a) < 0:
        raise ValueError('Lower bound is greater then upper bound!')
    elif sigma <= 0:
        raise ValueError('Sigma is <= 0!')

    phi_l = 0.5 * special.erfc(-(((a - mu) / sigma) / sqrt(2)))
    phi_r = 0.5 * special.erfc(-(((b - mu) / sigma) / sqrt(2)))

    # Refer to http://www.maths.uq.edu.au/~chancc/10Fstat3001/ass4sol.pdf for truncated normal dist sampling below --
    # If this source does not exist then refer to code in the link below,
    # http://www.wiley.com/legacy/wileychi/koopbayesian/supp/normt_rnd.m
    return mu + sigma * (sqrt(2) * special.erfinv(2 * (phi_l + (phi_r - phi_l) * rand(int(n))) - 1))

# %% markdown
# ## Testing
# Tests are passed if histograms exhibit similar (normal) distributions with the specified
# mean and sigma and truncations.

# %%
def test_truncated_normal(a=-1.0, b=1.0, mu=0.1, sigma=0.001, n=10000.0):
    # numpy
    import numpy as np

    # package enabling access/control of matlab from python
    import matlab.engine

    # matlab instance with relevant paths
    eng = matlab.engine.start_matlab()

    # paths to matlab helper and model functions
    eng.addpath('../original')

    # generate output
    python_output = truncated_normal(a, b, mu, sigma, n)
    matlab_output = np.asarray(eng.truncated_normal(a, b, mu, sigma, n))

    # result is stochastic, so we only test for the same shape
    assert np.shape(python_output) == np.shape(matlab_output)

    # and visualize histograms of output
    import matplotlib.pyplot as plt
    fig, axs = plt.subplots(1, 2, sharey=True, tight_layout=True)
    n_bins = 100

    # We can set the number of bins with the `bins` kwarg
    axs[0].hist(python_output, bins=n_bins)
    axs[1].hist(matlab_output, bins=n_bins)
    plt.show()

    print('Tests are passed if histograms exhibit similar (truncated normal) distributions!')

# %%
# run tests only when this is main file!
if __name__ == '__main__':
    test_truncated_normal()

# %%


#!/usr/bin/env python

import numpy as np

def make_float(values):
    ret_values = []
    for v in values:
        ret_values.append(float(v))
    return ret_values

def raw_moving_average(x,n, type='simple'):
    """
    compute an n period moving average.
    type is 'simple' | 'exponential'
    """

    x = np.asarray(x)
    if type=='simple':
        weights = np.ones(n)
    else:
        weights = np.exp(np.linspace(-1., 0., n))

    weights /= weights.sum()

    a =  np.convolve(x, weights, mode='full')[:len(x)]
    a[:n] = a[n]
    return a

def moving_average(x, n, type='simple'):
    try:
        return make_float(raw_moving_average(x, n, type))
    except:
        return None

def relative_strength(prices, n=14):
    """
    compute the n period relative strength indicator
    http://stockcharts.com/school/doku.php?id=chart_school:glossary_r#relativestrengthindex
    http://www.investopedia.com/terms/r/rsi.asp
    """

    deltas = np.diff(prices)
    seed = deltas[:n+1]
    up = seed[seed>=0].sum()/n
    down = -seed[seed<0].sum()/n
    rs = up/down
    rsi = np.zeros_like(prices)
    rsi[:n] = 100. - 100./(1.+rs)

    for i in range(n, len(prices)):
        delta = deltas[i-1] # cause the diff is 1 shorter

        if delta>0:
            upval = delta
            downval = 0.
        else:
            upval = 0.
            downval = -delta

        up = (up*(n-1) + upval)/n
        down = (down*(n-1) + downval)/n

        rs = up/down
        rsi[i] = 100. - 100./(1.+rs)

    return make_float(rsi)

def moving_average_convergence(x, nslow=26, nfast=12):
    """
    compute the MACD (Moving Average Convergence/Divergence) using a fast and slow exponential moving avg'
    return value is emaslow, emafast, macd which are len(x) arrays
    """
    try:
        emaslow = raw_moving_average(x, nslow, type='exponential')
        emafast = raw_moving_average(x, nfast, type='exponential')
        macd = emafast - emaslow
        signal = raw_moving_average(macd, 9, type='exponential')
        return {
            '26EMA' : make_float(emaslow),
            '12EMA' : make_float(emafast),
            'MACD' : make_float(macd),
            'Signal' : make_float(signal),
            'Histogram' : make_float(macd - signal)
        }
    except:
        return None

def averages(x):
    return {
        '5d' : moving_average(x, 5),
        '10d' : moving_average(x, 10),
        '50d' : moving_average(x, 50),
        '200d' : moving_average(x, 200),
        'convergence' : moving_average_convergence(x)
    }


# -*- coding: utf-8 -*-
import numpy as np
import csv
a = []
B = []

with open('LUINPUT.csv', 'r') as csvfile:
    # o nome 'spamreader' abaixo é só exemplo, poderia ser qq. coisa
    spamreader = csv.reader(csvfile, delimiter=',') # separe por vírgula

    # o módulo csv detectará novas linhas automaticamente
    for linha in spamreader:
        if len(linha) == 0:
            break
        a.append(linha)

    for linha in spamreader:
        B.append(linha)

u = []
l = []
linesQty = len(a)
columnsQty = len(a[0])

matrix = np.array(a, float)
u = matrix
#B = np.array(b,float)
#B = np.transpose(b)

if np.linalg.det(matrix) == 0:
  print("Matriz singular. Não pode fazer LU")
  exit
elif linesQty != columnsQty:
  print("Matriz nao e quadrada")
else:
  for j in range(0,columnsQty - 1):
    pivot = u[j,j]
    if pivot == 0:
      break
    print("Pivo da coluna ",j+1," é ", pivot)
    mTemp = np.identity(linesQty,float)
    lTemp = np.identity(linesQty,float)
    for k in range(j+1, linesQty):
      if u[k,j] != 0:
        mTemp[k,j] = (-1 * u[k,j])/pivot
        lTemp[k,j] = u[k,j]/pivot

    print("M",j+1,":\n")
    print(mTemp)
    print("L",j+1,":\n")
    print(lTemp)

    u = np.matmul(mTemp, u)
    print("U",":\n")
    print(u)

    if len(l) == 0:
      l = lTemp
    else:
      l = np.matmul(l,lTemp)

    print("L",":\n")
    print(l)

  print("\n\nA = LU\n")
  print("A = \n")
  print(matrix)
  print("\n\nL",":\n")
  print(l)
  print("\n\nU",":\n")
  print(u)
  print ("\n\nLU:")
  print(np.matmul(l,u))

  B = np.array(B[0],float)
  B = np.transpose(B)
  print 'B: ', B
  U = u
  print 'X: ', np.matmul(np.linalg.inv(np.matmul(l,u)), B)
  import pdb; pdb.set_trace()
  print 'Det: ', np.power(np.prod(U.diagonal()), 2)

  # # Calculating Y

  # y = np.zeros([linesQty,1])

  # for i in range(0,linesQty):
    # if(i == 0):
        # y[i,0] = b[i,0]/l[i,i]
    # else:
        # sum = 0
        # for k in range(0,i):
            # sum += y[k,0]*l[i,k]
        # y[i,0] = (b[i,0] - sum)/l[i,i]

  # print("\n\nY:\n")
  # print(y)

  # # Calculating X
  # x = np.zeros([linesQty,1])

  # for i in range(linesQty - 1, -1, -1):
    # if(i == linesQty - 1):
        # x[i,0] = y[i,0]/u[i,i]
    # else:
        # sum = 0
        # for k in range(i+1, linesQty):
            # sum += x[k,0]*u[i,k]
        # x[i,0] = (y[i,0] - sum)/u[i,i]

  # print("\n\nX:\n")
  # print(x)


# Program 08b: The Lorenz attractor. See Figure 8.11.
# In this case, the odeint numerical solver was used to solve the ODE.

import numpy as np
from scipy.integrate import odeint
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

# Lorenz paramters and initial conditions
sigma, beta, rho = 10, 2.667, 28
x0, y0, z0 = 0, 1, 1.05

# Maximum time point and total number of time points
tmax, n = 100, 10000

def Lorenz(X, t, sigma, beta, rho):
    """The Lorenz equations"""
    x, y, z = X
    dx = -sigma * (x - y)
    dy = rho*x - y - x*z
    dz = -beta*z + x*y
    return (dx, dy, dz)

# Integrate the Lorenz equations on the time grid t.
t = np.linspace(0, tmax, n)
f = odeint(Lorenz, (x0, y0, z0), t, args=(sigma, beta, rho))
x, y, z = f.T

# Plot the Lorenz attractor using a Matplotlib 3D projection.
fig = plt.figure()
ax = Axes3D(fig)

ax.plot(x, y, z, 'b-', lw=0.5)

ax.set_xlabel('x', fontsize=15)
ax.set_ylabel('y', fontsize=15)
ax.set_zlabel('z', fontsize=15)
plt.tick_params(labelsize=15)
ax.set_title('Lorenz Attractor', fontsize=15)

plt.show()


##############################################
# The MIT License (MIT)
# Copyright (c) 2016 Kevin Walchko
# see LICENSE for full details
##############################################
from ins_nav.wgs84 import RE, FLATTENING, E2
from ins_nav.utils import RAD2DEG, DEG2RAD
import numpy as np
from math import sqrt, atan2, sin, cos
# from collections import namedtuple
# from enum import IntFlag
# import attr

def ecef2llh(x, y, z):
    """
    ecef: Earth Centered Earth Fixed in [m, m, m]
    llh: latitude, longitude, height (or altitude) in [deg, deg, m]
    """
    p = sqrt(x**2 + y**2)
    b = RE*(1-FLATTENING)
    ep = (RE**2 - b**2)/(b**2)
    theta = atan2(z*RE, p*b)

    l = atan2(y, x)
    L = atan2(z+ep*b*sin(theta)**3, p-E2*RE*cos(theta)**3)
    Re = RE/sqrt(1-E2*sin(L)**2)
    h = p/cos(L) - Re

    L = RAD2DEG * L
    l = RAD2DEG * l
    return np.array([L, l, h])

def llh2ecef(lat, lon, H):
    """
    ecef: Earth Centered Earth Fixed in [m, m, m]
    llh: latitude, longitude, height (or altitude) in [deg, deg, m]

    phi = lat
    lambda = lon
    H = height or altitude
    """
    lat = DEG2RAD * lat
    lon = DEG2RAD * lon
    e2 = E2 # 0.00669437999014
    re = RE # 6378137.0  # m
    rm = re * (1.0 - e2) / pow(1.0 - e2 * sin(lat)**2, 3.0 / 2.0)
    rn = re / sqrt(1.0 - e2 * sin(lat)**2)
    x = (rn + H) * cos(lat) * cos(lon)
    y = (rn + H) * cos(lat) * sin(lon)
    z = (rm + H) * sin(lat)
    return np.array([x, y, z])

def llh2DCM(lat, lon, h, w):
    # lat/lon [rads]
    # h = height [m]
    # w = wander azmuth
    sL = sin(lat)
    cL = cos(lat)
    sl = sin(lon)
    cl = cos(lon)
    Cgn = np.array([[w[1], w[0], 0], [-w[0], w[1], 0], [0, 0, 1]])
    Ceg = np.array([[-sL*cl, -sL*sl, cL], [-sl, cl, 0], [-cL*cl, -cL*sl, -sL]])
    return Cgn.dot(Ceg)

# FrameTypes = IntFlag("FrameTypes", "ned enu")

# @attr.s(slots=True, frozen=True)
# class NavigationFrame:
#     """
#     Base local navigation frame class. A user should not call this directly,
#     rather derive new frames from it like a Wandering Azimuth frame class.
#
#     The assumption is, we always go between ECEF <-> Nav frame. The Nav frame
#     can be NED or ENU or something else derrived from NavigationFrame.
#     """
#     origin = attr.ib()
#     R = attr.ib()
#     type = attr.ib(init=False, default=None)
#
#     # def ecef2nav(self, p_ecef):
#     def to_nav(self, p_ecef):
#         """
#         pos_ecef: vector in ECEF to convert to a local navigation frame
#         """
#         r = self.R.T
#         return r.dot(p_ecef - self.origin)
#
#     # def nav2ecef(self, p_nav):
#     def to_ecef(self, p_nav):
#         """
#         pos_nav: vector in local navigation frame to convert to ECEF
#         """
#         return self.R.dot(p_nav) + self.origin
#
# @attr.s(slots=True, frozen=True)
# class ENU(NavigationFrame):
#     """
#     East-North-Up
#     reference: https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#From_ECEF_to_ENU
#     """
#     type = FrameTypes.enu
#
#
# @attr.s(slots=True, frozen=True)
# class NED(NavigationFrame):
#     """
#     North-East-Down
#
#     pos_ecef: vector in ECEF to convert
#     origin_ecef: origin of local frame in ECEF
#
#     reference: https://en.wikipedia.org/wiki/Local_tangent_plane_coordinates
#     """
#     type = FrameTypes.ned
#
#     @staticmethod
#     def from_ll(lat, lon, alt):
#         """
#         Creates a local navigation from lat, lon, alt [deg,deg,m]
#         """
#         o_ecef = llh2ecef(lat, lon, alt)
#
#         lat = DEG2RAD*lat
#         lon = DEG2RAD*lon
#
#         R = np.array(
#             [
#                 [-np.sin(lat)*np.cos(lon), -np.sin(lat)*np.sin(lon), np.cos(lat)],
#                 [-np.sin(lon), np.cos(lon), 0],
#                 [-np.cos(lat)*np.cos(lon), -np.cos(lat)*np.sin(lon), -np.sin(lat)]
#             ]
#         )
#
#         return NED(o_ecef, R)
#
#     @staticmethod
#     def from_ecef(x, y, z):
#         """
#         Creates a local navigation frame from (x,y,z) [m,m,m] in ECEF frame
#         """
#         o_ecef = np.array([x,y,z])
#
#         lat, lon, _ = ecef2llh(x,y,z)
#
#         lat = DEG2RAD*lat
#         lon = DEG2RAD*lon
#
#         R = np.array(
#             [
#                 [-np.sin(lat)*np.cos(lon), -np.sin(lat)*np.sin(lon), np.cos(lat)],
#                 [-np.sin(lon), np.cos(lon), 0],
#                 [-np.cos(lat)*np.cos(lon), -np.cos(lat)*np.sin(lon), -np.sin(lat)]
#             ]
#         )
#
#         return NED(o_ecef, R)

# def ecef2enu():
#     """
#     reference: https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#From_ECEF_to_ENU
#     """
#     pass
#
# def ecef2ned(pos_ecef, llh_origin):
#     """
#     pos_ecef: vector in ECEF to convert
#     origin_ecef: origin of local frame in ECEF
#
#     reference: https://en.wikipedia.org/wiki/Local_tangent_plane_coordinates
#     """
#
#     lat = DEG2RAD*ned_origin[0]
#     lon = DEG2RAD*ned_origin[1]
#
#     R = np.array(
#         [
#             [-np.sin(lat)*np.cos(lon), -np.sin(lat)*np.sin(lon), np.cos(lat)],
#             [-np.sin(lon), np.cos(lon), 0],
#             [-np.cos(lat)*np.cos(lon), -np.cos(lat)*np.sin(lon), -np.sin(lat)]
#         ]
#     )
#
#     ned_origin_ecef = llh2ecef(ned_origin)
#
#     ned = R.dot(pos_ecef - ned_origin_ecef)
#
#     return ned

# New ------------------------------------------------------------------------------
#------------------------------------------------------------------------------------


# exampleOptimization.py
# JS 10/3/2021
import numpy as np
import scipy.optimize, scipy.linalg
import matplotlib.pyplot as plt

def biexp(p,x):
    a, b, c = p
    return c*np.exp(-a*x)*(1-np.exp(-b*x))

def objectiveFunction(p,xdata,ydata):
    ysim = biexp(p,xdata)
    error = ydata - ysim
    return error

# generate simulated data for fitting
p0 = [1,2,3]
xdata = np.linspace(0,5)
ydata = biexp(p0,xdata) + 0.1*np.random.normal(size=len(xdata)) # add noise std dev 0.1

# run optimization
result = scipy.optimize.least_squares(objectiveFunction, p0, args=(xdata,ydata))
paramEst = result.x
covariance = result.cost*scipy.linalg.inv((np.matmul(result.jac.T,result.jac)))/len(ydata)

# plot the results
plt.plot(xdata,ydata,'o',xdata,biexp(paramEst,xdata),'-')
plt.show()

# generate parameter ensemble
n = 10;
paramEnsemble = np.random.multivariate_normal(paramEst,covariance,n)

paramStd = np.std(paramEnsemble,axis=0)

# plot parameter estimates
plt.bar(['a','b','c'],paramEst)
plt.errorbar(['a','b','c'],paramEst,paramStd)
plt.show()

# plot simulatons with the parameter ensemble
y = biexp(paramEst,xdata)
plt.plot(xdata,y)
for i in range(n):
    y = biexp(paramEnsemble[i],xdata)
    plt.plot(xdata,y)
plt.show()


import numpy as np
from scipy import stats, integrate

# Compute $\omega_\sigma(m)$ for a range of $m$s
def doubly_spatially_integrated_Gaussian_one_dim( sigma, support ):
    # Use expression (Owen, 1980) to resolve double spatial integration over Gaussian
    normal_zero_sigma = stats.norm( loc = 0, scale = sigma )
    return (
      (support + 1) * normal_zero_sigma.cdf( support + 1 )+
      (support - 1) * normal_zero_sigma.cdf( support - 1 )
    - (2 * support) * normal_zero_sigma.cdf( support     )
    + sigma**2 * (
                      normal_zero_sigma.pdf( support + 1 )+
                      normal_zero_sigma.pdf( support - 1 )
    - 2 *             normal_zero_sigma.pdf( support     )
     ) )

# Compute $\omega_\sigma(m)*\omega_\sigma(n)$ for a range of $m$s, $n$s and $\sigma$s
def stacked_doubly_spatially_integrated_Gaussian( sigmas, support ):
    # Initialize output, number of $m$s by number of $n$s by number of $\sigma$s
    stacked_output = np.empty( (support.size, support.size, sigmas.size) )
    for index in range( sigmas.size ):
        # Get one-dimensional for range
        auxiliar = doubly_spatially_integrated_Gaussian_one_dim( sigmas[index], support )
        # Outer product of two-dimensional in range x range
        stacked_output[...,index] = np.transpose( auxiliar )*auxiliar
    return stacked_output

# Once spatially integrated Gaussian, for simulating optical blur
def spatially_integrated_Gaussian( sigma = 2.279 ):
    support = np.array( [np.arange( -np.ceil( 3*sigma ), np.ceil( 3*sigma )+1, 1 )]  )
    normal_zero_sigma = norm( loc = 0, scale = sigma )
    auxiliar = ( normal_zero_sigma.cdf( support + .5 )
                    - normal_zero_sigma.cdf( support  -  .5 ) )
    return auxiliar * np.transpose( auxiliar )

def obtain_discrete_kernels( sigma_limits ):
    # (!!) Extreme inefficiency, all filters are the same size, the maximum one (!!) See figure for smallest kernel below
    # Compute support for maximum sigma
    support = np.array( [np.arange( -np.ceil( 3*sigma_limits[-1] ), np.ceil( 3*sigma_limits[-1] )+1, 1 )], dtype = np.float32)
    # Initialize array
    kernels = np.empty( (support.size, support.size, sigma_limits.size-1), dtype = np.float32 )
    for index in range( sigma_limits.size-1 ):
        # Compute integral over \sigma
        kernels[...,index] = integrate.fixed_quad( stacked_doubly_spatially_integrated_Gaussian,
                                                   sigma_limits[index], sigma_limits[index+1],
                                                   args = (support,) )[0]
        kernels[...,index] = kernels[...,index] / np.sqrt( sigma_limits[index + 1] - sigma_limits[index] )
    return kernels


# -*- coding: utf-8 -*-
import numpy as np

numeric_tuple = (int, float, np.float32, np.float64, np.float128)

def coupled_logarithm(value: [int, float, np.ndarray],
                      kappa: [int, float] = 0.0,
                      dim: int = 1
                      ) -> [float, np.ndarray]:
    """
    Generalization of the logarithm function, which defines smooth
    transition to power functions.

    Parameters
    ----------
    value : Input variable in which the coupled logarithm is applied to.
            Accepts int, float, and np.ndarray data types.
    kappa : Coupling parameter which modifies the coupled logarithm function.
            Accepts int and float data types.
    dim : The dimension (or rank) of value. If value is scalar, then dim = 1.
          Accepts only int data type.
    """
    # convert value into np.ndarray (if scalar) to keep consistency
    value = np.array(value) if isinstance(value, numeric_tuple) else value
    assert isinstance(value, np.ndarray), "value must be an int, float, or np.ndarray."
    assert 0. not in value, "value must not be or contain np.ndarray zero(s)."
    if kappa == 0.:
        coupled_log_value = np.log(value)  # divide by 0 if x == 0
    else:
        coupled_log_value = (1. / kappa) * (value**(kappa / (1. + dim*kappa)) - 1.)
    return coupled_log_value

def coupled_exponential(value: [int, float, np.ndarray],
                        kappa: float = 0.0,
                        dim: int = 1
                        ) -> [float, np.ndarray]:
    """
    Generalization of the exponential function.

    Parameters
    ----------
    value : [float, np.ndarray]
        Input values in which the coupled exponential is applied to.
    kappa : float,
        Coupling parameter which modifies the coupled exponential function.
        The default is 0.0.
    dim : int, optional
        The dimension of x, or rank if x is a tensor. The default is 1.

    Returns
    -------
    float
        The coupled exponential values.

    """
    # convert number into np.ndarray to keep consistency
    isinstance(value, (int, float, ))
    value = np.array(value) if isinstance(value, numeric_tuple) else value
    assert isinstance(value, np.ndarray), "value must be an int, float, or np.ndarray."
    # assert 0 not in value, "value must not be or contain np.ndarray zero(s)."
    assert isinstance(dim, int) and dim >= 0, "dim must be an integer greater than or equal to 0."
    # check that -1/d <= kappa
    # assert -1/dim <= kappa, "kappa must be greater than or equal to -1/dim."

    if kappa == 0:
        # Does not have to be vectorized
        coupled_exp_value = np.exp(value)
    else:
        #coupled_exp_value = np.vectorize(_coupled_exponential_scalar)(value, kappa, dim)

        #Positive base, function operates as normal
        condition_1 = (1 + kappa*value) > 0

        #Negative base and positive exponent should return 0
        condition_2 = ((1 + kappa*value) <= 0) & (((1 + dim*kappa)/kappa) > 0)

        coupled_exp_value = np.where(condition_1, (1 + kappa*value)**((1+dim*kappa)/kappa), float('inf'))
        coupled_exp_value = np.where(condition_2, 0, coupled_exp_value)

    return coupled_exp_value

# inner function that takes in the value on a scalar-by-sclar basis
'''
def _coupled_exponential_scalar(value, kappa, dim):
    if (1 + kappa*value) > 0:
        return (1 + kappa*value)**((1 + dim*kappa)/kappa)
    elif ((1 + dim*kappa)/kappa) > 0:
        return 0.
    else:
        return float('inf')
'''


import numpy as np
import matplotlib.pyplot as plt
import casadi as cas

# define functions
def R_ntc(R_N, B, T, T_N):
    return R_N * np.exp(B*(1/T-1/T_N))

def U_meas(U_ges, R_1, R_2):

    return U_ges/(R_1+R_2) * R_2

def Temp(U_meas, R_1, U_ges, B, R_N, T_N):
    R_NTC = U_meas * R_1 / (U_ges - U_meas)
    T_kelvin = 1 / (np.log(R_NTC / R_N) / B + 1 / T_N)
    return T_kelvin - 273.15

def Temp_adc(adc_value, R_1, U_ges, B, R_N, T_N):
    return Temp(U_ges/4096 * adc_value, R_1, U_ges, B, R_N, T_N)

# Check if Sensor is a NTC and find out the Resistance

R_measured = [103e3, 120e3, 70e3, 15.2e3]
T_measured_C = [25, 20, 33, 74]

R_N = 103e3
T_N = 298.15
U_ges = 3.3
T_range_C = [0, 200]

# Kelvin from Temp
T_range = [i + 273.15 for i in T_range_C]
T_measured = [i + 273.15 for i in T_measured_C]

# Fit B
B_sym = cas.SX.sym("B")
f = 0
for R, T in zip(R_measured, T_measured):
    f += (R-R_ntc(R_N, B_sym, T, T_N))**2

nlp = {'x': B_sym, 'f': f}
S = cas.nlpsol('S', 'ipopt', nlp)
res = S()

B = res['x'].full()[0]

# Search for the Value of R_1 which minimizes the deviation of Measured Voltage at a specific Temperature
T_Nenn_C = 65
T_Nenn = T_Nenn_C + 273.15

R_1 = cas.SX.sym("R_1")
T_sym = cas.SX.sym("T")

U = U_meas(U_ges, R_1, R_ntc(R_N, B, T_sym, T_N))
jac = cas.Function("dudT", [R_1, T_sym], [cas.jacobian(U, T_sym)])

lbx = [0]
ubx = [cas.inf]
nlp = {'x': R_1, 'f': jac(R_1, T_Nenn)}

S = cas.nlpsol('S', 'ipopt', nlp)
res = S(lbx=lbx, ubx=ubx)

R_1 = res['x'].full()[0]

# plot
T = np.linspace(T_range[0], T_range[1], num=200)
T_C = np.linspace(T_range_C[0], T_range_C[1], num=200)
print("R_1: %e" % R_1)
print("B: %e" % B)
plt.figure()
plt.title("Fitted and Measured Resistance of the NTC")
plt.plot(T_C, R_ntc(R_N, B, T, T_N), label="R_fitted")
plt.plot(T_measured_C, R_measured, "x", label="R_measured")
plt.xlabel("Temp [Grad]")
plt.ylabel("R [Ohm]")
plt.legend()

U_range = U_meas(U_ges, R_1, R_ntc(R_N, B, T, T_N))
plt.figure()
plt.title("Voltage vs. Temperature with optimal Resistance. Vertical line shows the optimal Temperature.")
plt.plot(T_C, U_range, label="R/NTC")
plt.vlines(T_Nenn_C, U_range.min(), U_range.max())
plt.xlabel("Temp [Grad]")
plt.ylabel("U [V]")
plt.legend()

# Calibrate Sensor from ADC measurements

# measurements

adc_voltage_measured = [2.21634, 2.17343, 1.59904, 1.49781, 0.84637, 0.71773, 0.59042, 0.67618, 0.18674, 0.22936,0.18058,0.16265, 0.31080, 0.35350, 0.51420, 0.54871, 0.69035, 0.74026, 0.93252, 0.97848, 1.14037, 1.22280, 1.62269, 1.66503, 2.68944, 2.70243]
T_measured = [42, 43, 60, 65, 92, 99, 106, 101, 156, 146, 158, 161, 133, 128, 112, 109, 100, 97, 87, 85, 78, 75, 61, 59, 24, 23]

# Fit Model
R1 = 24.9e3
B_sym = cas.SX.sym("B")
Rn_sym = cas.SX.sym("Rn")
Tn_sym = cas.SX.sym("Tn")
syms = cas.vertcat(B_sym, Rn_sym, Tn_sym)
x0 = [B, 103e3, T_N]
f = 0
for T, adc_voltage in zip(T_measured, adc_voltage_measured):
    f += (T-Temp(adc_voltage, R1, U_ges, B_sym, Rn_sym, Tn_sym))**2

f += (Rn_sym-103e3)**2+(Tn_sym-T_N)**2
nlp = {'x': syms, 'f': f}
S = cas.nlpsol('S', 'ipopt', nlp)
res = S(x0=x0)

B = res['x'].full()[0]
Rn = res['x'].full()[1]
Tn = res['x'].full()[2]

print("B:", B, "R1:", R1, "Rn:", Rn, "Tn:", Tn)

plt.figure()
plt.title("Fitted Model for measured Voltage vs. Temperature")
adc_range = np.linspace(min(adc_voltage_measured), max(adc_voltage_measured))
plt.plot(adc_range, Temp(adc_range, R1, U_ges, B, Rn, Tn), label="Fitted Model")
plt.plot(adc_voltage_measured, T_measured, 'x', label="Measured")
plt.xlabel("ADC voltage [V]")
plt.ylabel("Temp [Grad]")
plt.legend()

plt.show()


"""
Module which implements feature PCA compression and PCA analysis of feature importance.
"""

import pandas as pd
import numpy as np
from scipy.stats import weightedtau, kendalltau, spearmanr, pearsonr

def _get_eigen_vector(dot_matrix, variance_thresh):
    """
    Snippet 8.5, page 119. Computation of Orthogonal Features

    Get eigen values and eigen vector from matrix which explain % variance_thresh of total variance.

    :param dot_matrix: (np.array): Matrix for which eigen values/vectors should be computed.
    :param variance_thresh: (float): Percentage % of overall variance which compressed vectors should explain.
    :return: (pd.Series, pd.DataFrame): Eigen values, Eigen vectors.
    """
    # Compute eigen_vec from dot prod matrix, reduce dimension
    eigen_val, eigen_vec = np.linalg.eigh(dot_matrix)
    idx = eigen_val.argsort()[::-1]  # Arguments for sorting eigen_val desc
    eigen_val, eigen_vec = eigen_val[idx], eigen_vec[:, idx]

    # 2) Only positive eigen_vals
    eigen_val = pd.Series(eigen_val, index=['PC_' + str(i + 1) for i in range(eigen_val.shape[0])])
    eigen_vec = pd.DataFrame(eigen_vec, index=dot_matrix.index, columns=eigen_val.index)
    eigen_vec = eigen_vec.loc[:, eigen_val.index]

    # 3) Reduce dimension, form PCs
    cum_var = eigen_val.cumsum() / eigen_val.sum()
    dim = cum_var.values.searchsorted(variance_thresh)
    eigen_val, eigen_vec = eigen_val.iloc[:dim + 1], eigen_vec.iloc[:, :dim + 1]
    return eigen_val, eigen_vec

def _standardize_df(data_frame):
    """
    Helper function which divides df by std and extracts mean.

    :param data_frame: (pd.DataFrame): to standardize
    :return: (pd.DataFrame): standardized data frame
    """
    return data_frame.sub(data_frame.mean(), axis=1).div(data_frame.std(), axis=1)

def get_orthogonal_features(feature_df, variance_thresh=.95):
    """
    Snippet 8.5, page 119. Computation of Orthogonal Features.

    Get PCA orthogonal features.

    :param feature_df: (pd.DataFrame): Dataframe of features.
    :param variance_thresh: (float): Percentage % of overall variance which compressed vectors should explain.
    :return: (pd.DataFrame): Compressed PCA features which explain %variance_thresh of variance.
    """
    # Given a dataframe of features, compute orthogonal features
    feature_df_standard = _standardize_df(feature_df)  # Standardize
    dot_matrix = pd.DataFrame(np.dot(feature_df_standard.T, feature_df_standard), index=feature_df.columns,
                              columns=feature_df.columns)
    _, eigen_vec = _get_eigen_vector(dot_matrix, variance_thresh)
    pca_features = np.dot(feature_df_standard, eigen_vec)
    return pca_features

def get_pca_rank_weighted_kendall_tau(feature_imp, pca_rank):
    """
    Snippet 8.6, page 121. Computation of Weighted Kendall's Tau Between Feature Importance and Inverse PCA Ranking.

    :param feature_imp: (np.array): Feature mean importance.
    :param pca_rank: (np.array): PCA based feature importance rank.
    :return: (float): Weighted Kendall Tau of feature importance and inverse PCA rank with p_value.
    """
    return weightedtau(feature_imp, pca_rank ** -1.0)

def feature_pca_analysis(feature_df, feature_importance, variance_thresh=0.95):
    """
    Perform correlation analysis between feature importance (MDI for example, supervised) and PCA eigen values
    (unsupervised). High correlation means that probably the pattern identified by the ML algorithm is not entirely overfit.

    :param feature_df: (pd.DataFrame): Features dataframe.
    :param feature_importance: (pd.DataFrame): Individual MDI feature importance.
    :param variance_thresh: (float): Percentage % of overall variance which compressed vectors should explain in PCA compression.
    :return: (dict): Dictionary with kendall, spearman, pearson and weighted_kendall correlations and p_values.
    """
    feature_df_standard = _standardize_df(feature_df)  # Standardize
    dot = pd.DataFrame(np.dot(feature_df_standard.T, feature_df_standard), index=feature_df.columns,
                       columns=feature_df.columns)
    eigen_val, eigen_vec = _get_eigen_vector(dot, variance_thresh)

    # Compute correlations between eigen values for each eigen vector vs mdi importance
    all_eigen_values = []  # All eigen values in eigen vectors
    corr_dict = {'Pearson': [], 'Spearman': [], 'Kendall': []}  # Dictionary containing correlation metrics
    for vec in eigen_vec.columns:
        all_eigen_values.extend(abs(eigen_vec[vec].values * eigen_val[vec]))

    # We need to repeat importance array # of eigen vector times to generate correlation for all_eigen_values
    repeated_importance_array = np.tile(feature_importance['mean'].values, len(eigen_vec.columns))

    for corr_type, function in zip(corr_dict.keys(), [pearsonr, spearmanr, kendalltau]):
        corr_coef = function(repeated_importance_array, all_eigen_values)
        corr_dict[corr_type] = corr_coef

    # Get Rank based weighted Tau correlation
    feature_pca_rank = (eigen_val * eigen_vec).abs().sum(axis=1).rank(
        ascending=False)  # Sum of absolute values across all eigen vectors
    corr_dict['Weighted_Kendall_Rank'] = get_pca_rank_weighted_kendall_tau(feature_importance['mean'].values,
                                                                           feature_pca_rank.values)
    return corr_dict


from scipy.integrate import solve_dde
import matplotlib.pyplot as plt
import numpy as np

"""
The Kermack–McKendrick model (Infectious Disease Modelling
in Hairer 1987 Solving Ordinary Differential Equations I.)

Tested features:
    - none terminal event location

"""

def fun(t,y,Z):
    y_tau1 = Z[:,0]
    y_tau10 = Z[:,1]
    return [-y[0] * y_tau1[1]  + y_tau10[1],
            y[0] * y_tau1[1] -  y[1],
            y[1] - y_tau10[1]]

def zero_y0(t,y,Z):
    y_tau1 = Z[:,0]
    y_tau10 = Z[:,1]
    return -y[0] * y_tau1[1]  + y_tau10[1]
zero_y0.direction = -1
zero_y0.terminal = False

def zero_y1(t,y,Z):
    y_tau1 = Z[:,0]
    return y[0] * y_tau1[1] -  y[1]
zero_y1.direction = -1
zero_y1.terminal = False

def zero_y2(t,y,Z):
    y_tau10 = Z[:,1]
    return y[1] - y_tau10[1]
zero_y2.direction = -1
zero_y2.terminal = False

t0 = 0.0
tf = 40
tau1 = 1.
tau10 = 10.

y0 = [5.,.1,1.0]
delays = [tau1,tau10]
tspan = [t0, tf]

zeros = [zero_y0,zero_y1,zero_y2]
atol = 1e-10
rtol = 1e-5
sol23 = solve_dde(fun, tspan, delays, y0, y0, method='RK23', events=zeros, atol=atol, rtol=rtol)

t = sol23.t
y0 = sol23.y[0,:]
y1 = sol23.y[1,:]
y2 = sol23.y[2,:]

y0_e = sol23.y_events[0][:,0]
y1_e = sol23.y_events[1][:,1]
y2_e = sol23.y_events[2][:,2]

import scipy.io as spio
path_mat = 'data_dde23/virusEvents.mat'
mat = spio.loadmat(path_mat, squeeze_me=True)
t0_e_mat = mat['x1']
t1_e_mat = mat['x2']
t2_e_mat = mat['x3']

te_mat = [t0_e_mat, t1_e_mat, t2_e_mat]

for i in range(len(sol23.t_events)):
    err = np.abs((sol23.t_events[i] - te_mat[i]) / te_mat[i])
    print('     t_event y%s :' % (i))
    print("         solver_dde method='RK23' = %s" % (sol23.t_events[i]))
    print('         Matlab dde23 =             %s' % (sol23.t_events[i]))
    print('         relative error =           %s ' % (err))

y0_e_mat = mat['y1']

y1_e_mat = mat['y2']
y2_e_mat = mat['y3']

plt.figure(figsize=(14,10))
plt.plot(t,y0,'r',label='y0')
plt.plot(t,y1,'b',label='y1')
plt.plot(t,y2,'g',label='y2')
plt.plot(sol23.t_events[0],y0_e,'s r', markersize=10)
plt.plot(sol23.t_events[1],y1_e,'s b', markersize=10)
plt.plot(sol23.t_events[2],y2_e,'s g', markersize=10)
plt.plot(t0_e_mat,y0_e_mat,'C1 o',label='dde23 t_events')
plt.plot(t1_e_mat,y1_e_mat,'C1 o')
plt.plot(t2_e_mat,y2_e_mat,'C1 o')
plt.xlabel(r'$t$')
plt.ylabel(r'$y$')
plt.legend()
plt.savefig('figures/KermackMcKendrick/y')
plt.show()


"""This file contains code used in "Think DSP",
by Allen B. Downey, available from greenteapress.com

Copyright 2014 Allen B. Downey
License: GNU GPLv3 http://www.gnu.org/licenses/gpl.html
"""

from __future__ import print_function, division

import thinkdsp
import thinkplot
import thinkstats2
import numpy as np

PI2 = 2 * np.pi

def make_sine(offset):
    """Makes a 440 Hz sine wave with the given phase offset.

    offset: phase offset in radians

    returns: Wave objects
    """
    signal = thinkdsp.SinSignal(freq=440, offset=offset)
    wave = signal.make_wave(duration=0.5, framerate=10000)
    return wave

def corrcoef(xs, ys):
    """Coefficient of correlation.

    ddof=0 indicates that we should normalize by N, not N-1.

    xs: sequence
    ys: sequence

    returns: float
    """
    return np.corrcoef(xs, ys, ddof=0)[0, 1]

def plot_sines():
    """Makes figures showing correlation of sine waves with offsets.
    """
    wave1 = make_sine(0)
    wave2 = make_sine(offset=1)

    thinkplot.preplot(2)
    wave1.segment(duration=0.01).plot(label='wave1')
    wave2.segment(duration=0.01).plot(label='wave2')

    corr_matrix = np.corrcoef(wave1.ys, wave2.ys, ddof=0)
    print(corr_matrix)

    thinkplot.save(root='autocorr1',
                   xlabel='Time (s)',
                   ylim=[-1.05, 1.05])

    offsets = np.linspace(0, PI2, 101)

    corrs = []
    for offset in offsets:
        wave2 = make_sine(offset)
        corr = corrcoef(wave1.ys, wave2.ys)
        corrs.append(corr)

    thinkplot.plot(offsets, corrs)
    thinkplot.save(root='autocorr2',
                   xlabel='Offset (radians)',
                   ylabel='Correlation',
                   xlim=[0, PI2],
                   ylim=[-1.05, 1.05])

def plot_shifted(wave, offset=0.002, start=0.2):
    """Plots two segments of a wave with different start times.

    wave: Wave
    offset: difference in start time (seconds)
    start: start time in seconds
    """
    thinkplot.preplot(num=2)
    segment1 = wave.segment(start=start, duration=0.01)
    segment1.plot(linewidth=2, alpha=0.8)

    segment2 = wave.segment(start=start-offset, duration=0.01)
    segment2.shift(offset)
    segment2.plot(linewidth=2, alpha=0.4)

    corr = segment1.corr(segment2)
    text = r'$\rho =$ %.2g' % corr
    thinkplot.text(start+0.0005, -0.8, text)
    thinkplot.config(xlabel='Time (s)', ylim=[-1.05, 1.05])

def serial_corr(wave, lag=1):
    """Computes serial correlation with given lag.

    wave: Wave
    lag: integer, how much to shift the wave

    returns: float correlation coefficient
    """
    n = len(wave)
    y1 = wave.ys[lag:]
    y2 = wave.ys[:n-lag]
    corr = corrcoef(y1, y2)
    return corr

def plot_serial_corr():
    """Makes a plot showing serial correlation for pink noise.
    """
    np.random.seed(19)

    betas = np.linspace(0, 2, 21)
    corrs = []

    for beta in betas:
        signal = thinkdsp.PinkNoise(beta=beta)
        wave = signal.make_wave(duration=1.0, framerate=11025)
        corr = serial_corr(wave)
        corrs.append(corr)

    thinkplot.plot(betas, corrs)
    thinkplot.config(xlabel=r'Pink noise parameter, $\beta$',
                     ylabel='Serial correlation',
                     ylim=[0, 1.05])
    thinkplot.save(root='autocorr3')

def autocorr(wave):
    """Computes and plots the autocorrelation function.

    wave: Wave
    """
    lags = range(len(wave.ys)//2)
    corrs = [serial_corr(wave, lag) for lag in lags]
    return lags, corrs

def plot_pink_autocorr(beta, label):
    """Makes a plot showing autocorrelation for pink noise.

    beta: parameter of pink noise
    label: string label for the plot
    """
    signal = thinkdsp.PinkNoise(beta=beta)
    wave = signal.make_wave(duration=1.0, framerate=11025)
    lags, corrs = autocorr(wave)
    thinkplot.plot(lags, corrs, label=label)

def plot_autocorr():
    """Plots autocorrelation for pink noise with different parameters
    """
    np.random.seed(19)
    thinkplot.preplot(3)

    for beta in [1.7, 1.0, 0.3]:
        label = r'$\beta$ = %.1f' % beta
        plot_pink_autocorr(beta, label)

    thinkplot.config(xlabel='Lag',
                     ylabel='Correlation',
                     xlim=[-5, 1000],
                     ylim=[-0.05, 1.05])
    thinkplot.save(root='autocorr4')

def plot_singing_chirp():
    """Makes a spectrogram of the vocal chirp recording.
    """
    wave = thinkdsp.read_wave('28042__bcjordan__voicedownbew.wav')
    wave.normalize()

    duration = 0.01
    segment = wave.segment(start=0.2, duration=duration)

    # plot two copies of the wave with a shift
    plot_shifted(wave, start=0.2, offset=0.0023)
    thinkplot.save(root='autocorr7')

    # plot the autocorrelation function
    lags, corrs = autocorr(segment)
    thinkplot.plot(lags, corrs)
    thinkplot.config(xlabel='Lag (index)',
                     ylabel='Correlation',
                     ylim=[-1.05, 1.05],
                     xlim=[0, 225])
    thinkplot.save(root='autocorr8')

    # plot the spectrogram
    gram = wave.make_spectrogram(seg_length=1024)
    gram.plot(high=4200)

    thinkplot.config(xlabel='Time (s)',
                     ylabel='Frequency (Hz)',
                     xlim=[0, 1.4],
                     ylim=[0, 4200])
    thinkplot.save(root='autocorr5')

    # plot the spectrum of one segment
    spectrum = segment.make_spectrum()
    spectrum.plot(high=1000)
    thinkplot.config(xlabel='Frequency (Hz)', ylabel='Amplitude')
    thinkplot.save(root='autocorr6')

def plot_correlate():
    """Plots the autocorrelation function computed by np.
    """
    wave = thinkdsp.read_wave('28042__bcjordan__voicedownbew.wav')
    wave.normalize()
    segment = wave.segment(start=0.2, duration=0.01)

    lags, corrs = autocorr(segment)

    N = len(segment)
    corrs2 = np.correlate(segment.ys, segment.ys, mode='same')
    lags = np.arange(-N//2, N//2)
    thinkplot.plot(lags, corrs2)
    thinkplot.config(xlabel='Lag',
                     ylabel='Correlation',
                     xlim=[-N//2, N//2])
    thinkplot.save(root='autocorr9')

def main():
    plot_sines()
    plot_serial_corr()
    plot_autocorr()
    plot_singing_chirp()
    plot_correlate()

if __name__ == '__main__':
    main()


import random
import numpy as np

def cx_arithmetic(ind1, ind2):
    """
    Arithmetic crossover with random alphas
    """
    alphas = np.random.rand(len(ind1))
    c1 = (1 - alphas) * ind1 + alphas * ind2
    c2 = alphas * ind1 + (1 - alphas) * ind2
    ind1[:] = c1
    ind2[:] = c2
    return ind1, ind2

def cx_blx(ind1, ind2, alpha):
    """
    Blend-alpha crossover with specific alpha
    """
    c_max = np.maximum(ind1, ind2)
    c_min = np.minimum(ind1, ind2)
    inteval = c_max - c_min
    c1 = np.random.uniform(c_min - inteval * alpha, c_max + inteval * alpha)
    c2 = np.random.uniform(c_min - inteval * alpha, c_max + inteval * alpha)
    ind1[:] = np.clip(c1, 0, 1)
    ind2[:] = np.clip(c2, 0, 1)
    return ind1, ind2

def mut_gaussian(individual, mu, sigma, indpb):
    """
    Gaussian mutation.

    :param individual: individual to mutate.
    :param mu: mean of the gaussian distribution.
    :param sigma: standard deviation of the gaussian distribution.
    :param indpb: probability of an gene to mutate.

    Returns: the individual and a True if it was mutated. False otherwise.
    """
    size = len(individual)
    mutated = False
    for i in range(size):
        if random.random() < indpb:
            mutated = True
            individual[i] += random.gauss(mu, sigma)
            if individual[i] > 1:
                individual[i] = 1
            elif individual[i] < 0:
                individual[i] = 0
    return individual, mutated


#autor: Rodolfo Quispe
import tensorflow as tf
import numpy as np
import matplotlib.pyplot as plt

#define the original function
def f(x, w):
  return w[0]*(x**3) + w[1]*(x**2) + w[2]*(x) + w[3]

def plot_results(x, y, w):
  fig = plt.figure(figsize=(12, 6))
  fig.add_subplot(111)

  # Plot the target t versus the input x
  plt.plot(x, y, 'o', label='data')
  # Plot the initial line
  x = list(range(-10, 10))
  plt.plot(x, [f(i,w) for i in x],'g-',label='f(x)')

  #set extra plot parameters
  plt.title('Question 6: Stochastic Gradient Descent on Tensorflow')
  plt.xlim(-6,6)
  plt.ylim(-60,60)
  plt.xlabel('x')
  plt.ylabel('f(x)', fontsize=15)
  plt.legend(loc=2)
  plt.grid()

  plt.show()
  fig.savefig("t01_6.png")

if __name__ == "__main__":

  #Model parameters
  a = tf.Variable([np.random.rand()], tf.float32)
  b = tf.Variable([np.random.rand()], tf.float32)
  c = tf.Variable([np.random.rand()], tf.float32)
  d = tf.Variable([np.random.rand()], tf.float32)

  # Our model of y = a*x^3 + b*x^2 + c*x + d
  x = tf.placeholder(tf.float32)
  cubic_model = a*x*x*x + b*x*x + c*x + d
  y = tf.placeholder(tf.float32)

  # Our error is defined as the sum of the squares
  loss = tf.reduce_sum(tf.square(cubic_model - y))

  # Defining AdamOptimizer to calulate gradient
  optimizer = tf.train.AdamOptimizer(learning_rate=0.01)
  train = optimizer.minimize(loss)

  #training data
  x_train = np.array([-5, -4, -3, -2, -1, 0, 1, 2, 3, 4, 5])
  y_train = np.array([-53.9, -28.5, -20.7, -3.6, -9.8, 5.0,
                        4.2, 5.1, 11.4, 27.4, 44.0])

  # number of gradient descent iterations
  nb_of_iterations = 200

  # training loop
  init = tf.global_variables_initializer()
  sess = tf.Session()
  sess.run(init) # reset values to wrong

  errors = []

  #SGD optimization with random order
  order = list(range(len(x_train)))
  for i in range(nb_of_iterations):
    np.random.shuffle(order)
    for j in order:
      x_value = x_train[j]
      y_value = y_train[j]
      sess.run(train, {x:x_value, y:y_value})
    errors.append(sess.run(loss, {x:x_train, y:y_train}))

  #recover solution
  curr_a, curr_b, curr_c, curr_d, curr_loss  =  sess.run(
   [a, b, c, d, loss], {x:x_train, y:y_train})

  # show solution
  print ('iterations', nb_of_iterations)
  print ('cost', curr_loss)
  print ('best parameters', curr_a, curr_b, curr_c, curr_d)
  plot_results(x_train, y_train, np.array([curr_a, curr_b,
                                          curr_c, curr_d]))

  '''
  Extra implementaions

  #batch optimizacion
  for i in range(nb_of_iterations):
    sess.run(train, {x:x_train, y:y_train})
    errors.append(sess.run(loss, {x:x_train, y:y_train}))

  #SGD optimization
  for i in range(nb_of_iterations):
    for x_value, y_value in zip(x_train, y_train):
      sess.run(train, {x:x_value, y:y_value})
    errors.append(sess.run(loss, {x:x_train, y:y_train}))
  '''


# -*- coding: utf-8 -*-
'''
Copyright (c) 2015 by Tobias Houska

This file is part of Statistical Parameter Estimation Tool (SPOTPY).

:author: Tobias Houska

This tool holds functions for statistic analysis. It takes Python-lists and
returns the objective function value of interest.
'''

import numpy as np

def bias(evaluation,simulation):
    """
    Bias

        .. math::

         Bias=\\frac{1}{N}\\sum_{i=1}^{N}(e_{i}-s_{i})

    :evaluation: Observed data to compared with simulation data.
    :type: list

    :simulation: simulation data to compared with evaluation data
    :type: list

    :return: Bias
    :rtype: float
    """
    if len(evaluation)==len(simulation):
        bias_values=[]
        for i in range(len(evaluation)):
            if evaluation[i] == -99999:
                '''
                Cleans out No Data values
                '''
                print('Wrong Results! Clean out No Data Values')
                pass

            else:
                bias_values.append(float(simulation[i]) - float(evaluation[i]))
        bias_sum = np.sum(bias_values[0:len(bias_values)])
        bias = bias_sum/len(bias_values)
        return float(bias)

    else:
        print("Error: evaluation and simulation lists does not have the same length.")
        return np.nan

def nashsutcliff(evaluation,simulation):
    """
    Nash-Sutcliff model efficinecy

        .. math::

         NSE = 1-\\frac{\\sum_{i=1}^{N}(e_{i}-s_{i})^2}{\\sum_{i=1}^{N}(e_{i}-\\bar{e})^2}

    :evaluation: Observed data to compared with simulation data.
    :type: list

    :simulation: simulation data to compared with evaluation data
    :type: list

    :return: Nash-Sutcliff model efficiency
    :rtype: float

    """
    if len(evaluation)==len(simulation):
        s,e=np.array(simulation),np.array(evaluation)
        #s,e=simulation,evaluation
        mean_observed = np.mean(e)
        # compute numerator and denominator
        numerator = sum((e - s) ** 2)
        denominator = sum((e - mean_observed)**2)
        # compute coefficient
        return 1 - (numerator/denominator)

    else:
        print("Error: evaluation and simulation lists does not have the same length.")
        return np.nan

def lognashsutcliff(evaluation,simulation):
    """
    log Nash-Sutcliff model efficiency

        .. math::

         NSE = 1-\\frac{\\sum_{i=1}^{N}(log(e_{i})-log(s_{i}))^2}{\\sum_{i=1}^{N}(log(e_{i})-log(\\bar{e})^2}-1)*-1

    :evaluation: Observed data to compared with simulation data.
    :type: list

    :simulation: simulation data to compared with evaluation data
    :type: list

    :return: log Nash-Sutcliff model efficiency
    :rtype: float

    """
    if len(evaluation)==len(simulation):
        return float(1 - sum((np.log(simulation)-np.log(evaluation))**2)/sum((np.log(evaluation)-np.mean(np.log(evaluation)))**2))
    else:
        print("Error: evaluation and simulation lists does not have the same length.")
        return np.nan

def log_p(evaluation,simulation,scale=0.1):
    """
    Logarithmic probability distribution

    :evaluation: Observed data to compared with simulation data.
    :type: list

    :simulation: simulation data to compared with evaluation data
    :type: list

    :return: Logarithmic probability distribution
    :rtype: float
    """
    #from scipy import stats
    #logLik = np.mean( stats.norm.logpdf(evaluation, loc=simulation, scale=.1) )
    if len(evaluation)==len(simulation):
        y        = (np.array(evaluation)-np.array(simulation))/scale
        normpdf = -y**2 / 2 - np.log(np.sqrt(2*np.pi))
        return np.mean(normpdf)
    else:
        print("Error: evaluation and simulation lists does not have the same length.")
        return np.nan

def correlationcoefficient(evaluation,simulation):
    """
    Correlation Coefficient

        .. math::

         r = \\frac{\\sum ^n _{i=1}(e_i - \\bar{e})(s_i - \\bar{s})}{\\sqrt{\\sum ^n _{i=1}(e_i - \\bar{e})^2} \\sqrt{\\sum ^n _{i=1}(s_i - \\bar{s})^2}}

    :evaluation: Observed data to compared with simulation data.
    :type: list

    :simulation: simulation data to compared with evaluation data
    :type: list

    :return: Corelation Coefficient
    :rtype: float
    """
    if len(evaluation)==len(simulation):
        Corelation_Coefficient = np.corrcoef(evaluation,simulation)[0,1]
        return Corelation_Coefficient
    else:
        print("Error: evaluation and simulation lists does not have the same length.")
        return np.nan

def rsquared(evaluation,simulation):
    """
    Coefficient of Determination

        .. math::

         r^2=(\\frac{\\sum ^n _{i=1}(e_i - \\bar{e})(s_i - \\bar{s})}{\\sqrt{\\sum ^n _{i=1}(e_i - \\bar{e})^2} \\sqrt{\\sum ^n _{i=1}(s_i - \\bar{s})^2}})^2

    :evaluation: Observed data to compared with simulation data.
    :type: list

    :simulation: simulation data to compared with evaluation data
    :type: list

    :return: Coefficient of Determination
    :rtype: float
    """
    if len(evaluation)==len(simulation):
        return correlationcoefficient(evaluation,simulation)**2
    else:
        print("Error: evaluation and simulation lists does not have the same length.")
        return np.nan

def mse(evaluation,simulation):
    """
    Mean Squared Error

        .. math::

         MSE=\\frac{1}{N}\\sum_{i=1}^{N}(e_{i}-s_{i})^2

    :evaluation: Observed data to compared with simulation data.
    :type: list

    :simulation: simulation data to compared with evaluation data
    :type: list

    :return: Mean Squared Error
    :rtype: float
    """

    if len(evaluation)==len(simulation):

        MSE_values=[]

        for i in range(len(evaluation)):
            MSE_values.append((simulation[i] - evaluation[i])**2)

        MSE_sum = np.sum(MSE_values[0:len(evaluation)])

        MSE=MSE_sum/(len(evaluation))
        return MSE
    else:
        print("Error: evaluation and simulation lists does not have the same length.")
        return np.nan

def rmse(evaluation,simulation):
    """
    Root Mean Squared Error

        .. math::

         RMSE=\\sqrt{\\frac{1}{N}\\sum_{i=1}^{N}(e_{i}-s_{i})^2}

    :evaluation: Observed data to compared with simulation data.
    :type: list

    :simulation: simulation data to compared with evaluation data
    :type: list

    :return: Root Mean Squared Error
    :rtype: float
    """
    if len(evaluation)==len(simulation):
        return np.sqrt(mse(evaluation,simulation))
    else:
        print("Error: evaluation and simulation lists does not have the same length.")
        return np.nan

def mae(evaluation,simulation):
    """
    Mean Absolute Error

        .. math::

         MAE=\\frac{1}{N}\\sum_{i=1}^{N}(\\left |  e_{i}-s_{i} \\right |)

    :evaluation: Observed data to compared with simulation data.
    :type: list

    :simulation: simulation data to compared with evaluation data
    :type: list

    :return: Mean Absolute Error
    :rtype: float
    """
    if len(evaluation)==len(simulation):

        MAE_values=[]

        for i in range(len(evaluation)):
            MAE_values.append(np.abs(simulation[i] - evaluation[i]))

        MAE_sum = np.sum(MAE_values[0:len(evaluation)])

        MAE = MAE_sum/(len(evaluation))

        return MAE
    else:
        print("Error: evaluation and simulation lists does not have the same length.")
        return np.nan

def rrmse(evaluation,simulation):
    """
    Relative Root Mean Squared Error

        .. math::

         RRMSE=\\frac{\\sqrt{\\frac{1}{N}\\sum_{i=1}^{N}(e_{i}-s_{i})^2}}{\\bar{e}}

    :evaluation: Observed data to compared with simulation data.
    :type: list

    :simulation: simulation data to compared with evaluation data
    :type: list

    :return: Relative Root Mean Squared Error
    :rtype: float
    """

    if len(evaluation)==len(simulation):

        RRMSE = rmse(evaluation,simulation)/np.mean(simulation)
        return RRMSE

    else:
        print("Error: evaluation and simulation lists does not have the same length.")
        return np.nan

def agreementindex(evaluation,simulation):
    """
    Agreement Index (d) developed by Willmott (1981)

        .. math::

         d = 1 - \\frac{\\sum_{i=1}^{N}(e_{i} - s_{i})^2}{\\sum_{i=1}^{N}(\\left | s_{i} - \\bar{e} \\right | + \\left | e_{i} - \\bar{e} \\right |)^2}

    :evaluation: Observed data to compared with simulation data.
    :type: list

    :simulation: simulation data to compared with evaluation data
    :type: list

    :return: Agreement Index
    :rtype: float
    """
    if len(evaluation)==len(simulation):
        simulation,evaluation=np.array(simulation),np.array(evaluation)
        Agreement_index=1 -(np.sum((evaluation-simulation)**2))/(np.sum(
                (np.abs(simulation-np.mean(evaluation))+np.abs(evaluation-np.mean(evaluation)))**2))
        return Agreement_index
    else:
        print("Error: evaluation and simulation lists does not have the same length.")
        return np.nan

def covariance(evaluation,simulation):
    """
    Covariance

        .. math::
         Covariance = \\frac{1}{N} \\sum_{i=1}^{N}((e_{i} - \\bar{e}) * (s_{i} - \\bar{s}))

    :evaluation: Observed data to compared with simulation data.
    :type: list

    :simulation: simulation data to compared with evaluation data
    :type: list

    :return: Covariance
    :rtype: float
    """
    if len(evaluation)==len(simulation):
        Covariance_values = []

        for i in range(len(evaluation)):
            Covariance_values.append((evaluation[i]-np.mean(evaluation))*(simulation[i]-np.mean(simulation)))

        Covariance = np.sum(Covariance_values)/(len(evaluation))
        return Covariance
    else:
        print("Error: evaluation and simulation lists does not have the same length.")
        return np.nan

def decomposed_mse(evaluation,simulation):
    """
    Decomposed MSE developed by Kobayashi and Salam (2000)

        .. math ::
         dMSE = (\\frac{1}{N}\\sum_{i=1}^{N}(e_{i}-s_{i}))^2 + SDSD + LCS

         SDSD = (\\sigma(e) - \\sigma(s))^2

         LCS = 2 \\sigma(e) \\sigma(s) * (1 - \\frac{\\sum ^n _{i=1}(e_i - \\bar{e})(s_i - \\bar{s})}{\\sqrt{\\sum ^n _{i=1}(e_i - \\bar{e})^2} \\sqrt{\\sum ^n _{i=1}(s_i - \\bar{s})^2}})

    :evaluation: Observed data to compared with simulation data.
    :type: list

    :simulation: simulation data to compared with evaluation data
    :type: list

    :return: Decomposed MSE
    :rtype: float
    """

    if len(evaluation)==len(simulation):

        Decomposed_MSE = str(round((bias(evaluation,simulation))**2,2))+'(bias**2) + '+str(round((_standarddeviation(evaluation)-_standarddeviation(simulation))**2,2))+'(SDSD) + '+str(round(2*_standarddeviation(evaluation)*_standarddeviation(simulation)*(1-correlationcoefficient(evaluation,simulation)),2))+'(LCS)'

        return Decomposed_MSE
    else:
        print("Error: evaluation and simulation lists does not have the same length.")
        return np.nan

def kge(evaluation,simulation, return_all=False):
    """
    Kling-Gupta Efficiency

    Corresponding paper:
    Gupta, Kling, Yilmaz, Martinez, 2009, Decomposition of the mean squared error and NSE performance criteria: Implications for improving hydrological modelling

    output:
        kge: Kling-Gupta Efficiency
    optional_output:
        cc: correlation
        alpha: ratio of the standard deviation
        beta: ratio of the mean
    """
    if len(evaluation)==len(simulation):
        cc    = np.corrcoef(evaluation,simulation)[0,1]
        alpha = np.std(simulation)/np.std(evaluation)
        beta  = np.sum(simulation)/np.sum(evaluation)
        kge   = 1- np.sqrt( (cc-1)**2 + (alpha-1)**2 + (beta-1)**2 )
        if return_all:
            return kge, cc, alpha, beta
        else:
            return kge
    else:
        print("Error: evaluation and simulation lists does not have the same length.")
        return np.nan

def _variance(evaluation):
    """
    Variance

        .. math::

         Variance = \\frac{1}{N}\\sum_{i=1}^{N}(e_{i}-\\bar{e})^2

    :evaluation: Observed data to compared with simulation data.
    :type: list

    :return: Variance
    :rtype: float
    """
    Variance_values = []
    for i in range(len(evaluation)):
        Variance_values.append((evaluation[i]-np.mean(evaluation))**2)
    Variance = np.sum(Variance_values)/len(evaluation)
    return Variance

def _standarddeviation(evaluation):
    """
    Standard Derivation (sigma)

        .. math::
         sigma = \\sqrt{\\frac{1}{N}\\sum_{i=1}^{N}(e_{i}-\\bar{e})^2}

    :evaluation: Observed data to compared with simulation data.
    :type: list

    :simulation: simulation data to compared with evaluation data
    :type: list

    :return: Standard Derivation
    :rtype: float
    """

    return np.sqrt(_variance(evaluation))


import math
import numpy as np
from scipy.integrate import quad
import sympy as sp
from sympy import simplify, solve

def test():
    print('test')

def bisection(f, a, b, tol):
    i, x0, error = 0, 0, tol*10
    P, ERROR, I = [], [], []
    while error >= tol:
        x = (b - a)/2
        p = a + x
        P.append(p)
        if f(a)*f(p) > 0: a = p
        else: b = p
        error = x - x0
        ERROR.append(error); I.append(i)
        x0 = x
        i += 1
    return P, ERROR, I

def clamped_cubic_spline(X, f, a, b):
    def algorithm(f, m):
        n = m - 1
        Y, YP = np.zeros(m), np.zeros(m)
        YP_sym = sp.diff(f, sym_x)
        i = 0
        while i <= n:
            xi = X[i]
            Y[i] = f.evalf(subs={sym_x: xi})
            YP[i] = YP_sym.evalf(subs={sym_x: xi})
            i += 1
        # STEP 1:   build list, h_i
        H, i = np.zeros(n), 0
        while i < n:
            H[i] = X[i+1] - X[i]
            i += 1
        # STEP 2:   define alpha list endpoints
        A, AP, ALPHA = Y, YP, np.zeros(m)
        ALPHA[0] = 3*(A[1] - A[0])/H[0] - 3*AP[0]
        ALPHA[n] = 3*AP[n] - 3*(A[n] - A[n-1])/H[n-1]
        # STEP 3:   build list, alpha_i
        i = 1
        while i <= n-1:
            ALPHA[i] = 3/H[i]*(A[i+1] - A[i]) - 3/H[i-1]*(A[i] - A[i-1])
            i += 1
        # Algorithm 6.7 to solve tridiagonal
        # STEP 4:   define l, mu, and z first points
        L, MU, Z, C = np.zeros(m), np.zeros(m), np.zeros(m), np.zeros(m)
        L[0], MU[0] = 2*H[0], 0.5
        Z[0] = ALPHA[0]/L[0]
        # STEP 5:   build lists l, mu, and z
        i = 1
        while i <= n-1:
            L[i] = 2*(X[i+1] - X[i-1]) - H[i-1]*MU[i-1]
            MU[i] = H[i]/L[i]
            Z[i] = (ALPHA[i] - H[i-1]*Z[i-1])/L[i]
            i += 1
        # STEP 6:   define l, z, and c endpoints
        L[n] = H[n-1]*(2-MU[i-1])
        Z[n] = (ALPHA[n] - H[n-1]*Z[n-1])/L[n]
        C[n] = Z[n]
        # STEP 7:   build lists b, c, and d
        B, D, i, j = np.zeros(n), np.zeros(n), 1, 0
        while i <= n:
            j = n-i
            C[j] = Z[j] - MU[j]*C[j+1]
            B[j] = (A[j+1] - A[j])/H[j] - H[j]*(C[j+1] + 2*C[j])/3
            D[j] = (C[j+1] - C[j])/(3*H[j])
            i += 1
        return Y, A, B, C, D
    sym_x, m = sp.Symbol('x'), len(X)
    Y, A, B, C, D = algorithm(f(sym_x), m)
    splines_j, j = [], 0
    while j < n-1:
        xj, aj, bj, cj, dj = X[j], A[j], B[j], C[j], D[j]
        sj = aj + bj*(sym_x - xj) + cj*(sym_x - xj)**2 + dj*(sym_x - xj)**3
        splines_j.append(sj)
        j += 1
    spline = sum(Sj)
    return Y, splines_j, spline

def composite_simpson(f, a, b, h):
    n = math.ceil((b-a)/h)
    XJ1, XJ2, XJ, = [], [], []
    YJ1, YJ2, YJ, = [], [], []
    XJ.append(a); YJ.append(f(a))
    j, y1, z1 = 1, 0, 0
    while j <= (n/2)-1:
        xj = a + 2*j*h
        yj = f(xj)
        XJ1.append(xj); YJ1.append(yj)
        z1 += yj
        j += 1
    k, y2, z2 = 1, 0, 0
    while k <= n/2:
        xj = a + (2*k - 1)*h
        yj = f(xj)
        XJ2.append(xj); YJ2.append(yj)
        z2 += yj
        k += 1
    l = 0
    while l < len(XJ1):
        XJ.append(XJ2[l]); YJ.append(YJ2[l])
        XJ.append(XJ1[l]); YJ.append(YJ1[l])
        l += 1
    XJ.append(XJ2[l]); YJ.append(YJ2[l])
    XJ.append(b); YJ.append(f(b))
    F = h/3*(f(a) + 2*z1 + 4*z2 + f(b))
    return XJ, YJ, F

def composite_trapz(f, a, b, h):
    XJ, YJ = [], []
    XJ.append(a); YJ.append(f(a))
    j, n, y, z = 1, math.ceil((b-a)/h), 0, 0
    while j <= n-1:
        x_j = a + j*h
        XJ.append(x_j)
        y_j = f(x_j)
        YJ.append(y_j)
        z += y_j
        j += 1
    XJ.append(b); YJ.append(f(b))
    return XJ, YJ, h/2*(f(a) + 2*z + f(b))

def gaussian_quadrature(function, a, b):
    return integrate(function, a, b)

def integrate(function, a, b):
    F, error = quad(function, a, b)
    return F, error

def l_infinity_norm(x, x0):
    norm_i, i = np.zeros_like(x0), 0
    while i < n:
        norm_i[i] = np.abs(x[i] - x0[i])
        i += 1
    return np.amax(norm_i)

def lagrange_polynomial(X, Y):
    def term(xk, yk):
        num, den, L_k = [], [], []
        for xl in X:
            if xl != xk:
                num.append(sym_x-xl)
                den.append(xk-xl)
        L_k = (np.divide(np.prod(num), np.prod(den)))
        return L_k * yk
    def error(n, xi):
        roots, g, xi_error = [], [], []
        i = 0
        while i <= n:
            root = X[i]
            roots.append(sym_x - root)
            g = np.prod(roots)
            k = 0
            while k <= n:
                xi = sp.diff(xi, sym_x)
                k += 1
            dxi = np.abs(xi.evalf(subs={sym_x: root})/(math.factorial(k)))
            xi_error.append(np.abs(dxi))
            xi_err = np.max(xi_error)
            g_prime = sp.diff(g, sym_x)
            r = solve(g_prime)
            if i == 0:
                x = g_prime
                gx = g.evalf(subs={sym_x: x})
            if i == 1:
                x = r[0]
                gx = g.evalf(subs={sym_x: x})
            else:
                R = []
                for s in r:
                    if not isinstance(s, complex):
                        R.append(g.evalf(subs={sym_x: s}))
                gx = np.amax(np.abs(R))
            i += 1
        return np.abs(xi_err*gx)
    sym_x, yn, bound = sp.Symbol('x'), [], []
    for xk in X:
        k = X.index(xk)
        yn.append(term(xk, Y[k]))
        bound.append(error(k, sum(yn)))
    return yn, sum(yn), bound

def linear_least_squares(X_i, Y_i, n):
    def poly(X):
        terms, k = [], 0
        for x in X:
            terms.append(x*(sym_x**k))
            k += 1
        p = simplify(sum(terms))
        err, i = 0, 0
        for x_i in X_i:
            px = p.subs(sym_x, x_i)
            err += (Y_i[i] - px)**2
            i += 1
        return p, err
    m, sym_x = len(X_i), sp.Symbol('x')
    A, x = np.zeros((n+1, n+1)), np.zeros((n+1,1))
    b, i = np.zeros_like(x), 0
    while i <= n:
        j = 0
        while j <= n:
            a_ij, k = 0, 0
            while k < m:
                a_ij += (X_i[k])**(i + j)
                k += 1
            A[i][j] = a_ij
            j += 1
        b_i, k = 0, 0
        while k < m:
            b_i += Y_i[k]*(X_i[k]**(i))
            k += 1
        b[i] = b_i
        i += 1
    x = np.transpose(np.linalg.solve(A, b))
    X, terms, k = x[0], [], 0
    for x in X:
        terms.append(x*(sym_x**k))
        k += 1
    polynomial = simplify(sum(terms))
    E, i = 0, 0
    for x_i in X_i:
        E += (Y_i[i] - polynomial.subs(sym_x, x_i))**2
        i += 1
    return polynomial, E

def newton_difference(X, FX, m, n, x, state):
    def fterm(i, j):
        fij = (fxn[i][j] - fxn[i-1][j])/(fxn[i][0] - fxn[i-j][0])
        return fij
    fxn, coeff, term, poly = np.zeros((m,n)), [], [], []
    m, n = m - 1, n - 1
    fxn[:m,0], fxn[:m,1], j = X, FX, 1
    while j <= n:
        i = 1
        while i <= m:
            fk = fterm(i, j)
            fxn[i][j+1] = fk
            if state == 'forward' and i == j:
                coeff.append(fk)
            if state == 'backward' and i == m - 1:
                coeff.append(fk)
            i += 1
        j += 1
    for c in coeff:
        k = coeff.index(c)
        term.append(sym_x - X[k])
        poly.append(c*np.prod(term))
    if state == 'forward': polynomial = simplify(sum(poly) + FX[0])
    if state == 'backward': polynomial = simplify(sum(poly) + FX[m])
    px = polynomial.subs(sym_x, x)
    return polynomial, px

def richard_extrapolation(function, x0, h, order, state):
    def f(h):
        x = x0 + h
        return x, function(x)
    i, X, FX = 0, [], []
    while i < order:
        dx = h / (2**order) * (2**i)
        x_i, fx_i = f(dx)
        X.append(x_i); FX.append(fx_i)
        i += 1
    m = len(X)
    n = m + 1
    return newton_difference(X, FX, m, n, x0, state)

def single_variable_newton_raphson(f, x, p0, tol):
    i, error = 0, tol*10
    P, ERROR, I = [], [], []
    while error >= tol:
        fp0 = f(p0)
        df = sp.lambdify(x, sp.diff(f))
        dfp0 = df(p0)
        p = p0 - (fp0/dfp0)
        P.append(p)
        error = abs(p - p0)
        ERROR.append(error); I.append(i)
        p0 = p
        i += 1
    return P, ERROR, I

def vector_converge(A, x0, b, tol, type, method, w):
    def jacobi(x0):
        while i < n:
            j, y = 0, 0.
            while j < n:
                if j != i:
                    y += A[i][j]*x0[j]
                    j += 1
            xi[i] = (-y + b[i])/A[i][i]
            i += 1
        return xi
    def gauss_seidel(x0):
        while i < n:
            j, y1, y2 = 0, 0., 0.
            while j < i-1:
                y1 += A[i][j]*xi[j]
                j += 1
            j = i + 1
            while j < n:
                y2 += A[i][j]*x0[j]
                j += 1
            xi[i] = (-y1 - y2 + b[i])/A[i][i]
            i += 1
        return xi
    def successive_over_relaxation(x0):
        while i < n:
            gauss_seidel(x0)
            xi[i] = (1 - w)*x0[i] + w*gauss_seidel(x0)
            i += 1
        return xi
    n = len(x0)
    x0, b, norm, k = np.reshape(x0,(n,1)), np.reshape(b,(n,1)), tol*10, 0
    X, NORM, K = [], [], []
    X.append(x0); K.append(k)
    while norm > tol:
        xi, i = np.zeros_like(x0), 0
        if method == 'jacobi': xi = jacobi(x0)
        if method == 'gauss_seidel': xi = gauss_seidel(x0)
        if method == 'successive_over_relaxation': xi = successive_over_relaxation(x0)
        if type == 'l_infinity': norm = l_infinity_norm(xi, x0)
        if type == 'l_2': norm = l_2_norm(xi, x0)
        X.append(xi); NORM.append(norm); K.append(k)
        x0 = xi
        k += 1
    m, n = len(X[0]), len(X)
    X_mat, j = np.zeros((m,n)), 0
    while j < n:
        i = 0
        while i < m:
            X_mat[i][j] = float(X[j][i])
            i += 1
        j += 1
    return X_mat, NORM, K

import numpy,from scipy,import sympy,from sympy


import numpy as np
from matplotlib import pyplot as plt

from sspy.filters import KalmanFilter
from sspy.smoothers import KalmanSmoother

from sspy.util import model_noisy, model_noiseless, plot_estimate, rmse

## Reseed random generator
_reseed = True

# Process model
F = np.array([[1, 1, 0], [0, 1, 1], [0, 0, 1]])
# H
H = np.array([[1, 0, 0]])

# Process noise covariance
Q = np.diag(np.array([0., 0.01, 0.001]))

# Observation noise covariance
R = np.array([[400]])

# Initial state and estimation error covariance (assume Q)
x0 = np.array([[0., 0.1, 0.05]]).T
P0 = np.diag(np.array([10, 1, 0.1])) #Q.copy()

# Data dimensions
n_x = 3
n_y = 1

# Number of observations
n_t = 100
if _reseed:
    seed = np.random.randint(0,np.iinfo(np.int32).max)
    print(seed)
else:
    seed = 1892303842
np.random.seed(seed)

## Kalman filter subroutine
def run_filter(y_noisy):
    kf = KalmanFilter(x0, P0, F, Q, H, R, _verbose=False)

    x_estimate = np.zeros((n_x, n_t))
    P_estimate = np.zeros((n_x, n_x, n_t))
    y_estimate = np.zeros((n_y, n_t))
    Py_estimate = np.zeros((n_y, n_y, n_t))

    x_estimate[:,0] = x0.ravel()
    y_estimate[:,0] = (H @ x0).ravel()
    P_estimate[:,:,0] = P0
    Py_estimate[:,:,0] = H @ P0 @ H.T + R

    for i_ in range(1, n_t):
        kf.predict()
        kf.update(y_noisy[:,i_])
        x_estimate[:,i_] = kf.state['expected'].ravel()
        P_estimate[:,:,i_] = kf.state['err_cov']
        y_estimate[:,i_] = (H @ x_estimate[:,i_].reshape(n_x,1)).ravel()
        Py_estimate[:,:,i_] = H @ P_estimate[:,:,i_] @ H.T + R

    return x_estimate, P_estimate, y_estimate, Py_estimate, kf

## Kalman (Rauch-Tung-Streibel) smoother subroutine
def run_smoother(kf):
    rts = KalmanSmoother.from_filter(kf)
    states = rts.smooth()

    x_smoothed = np.zeros_like(x_true)
    P_smoothed = np.zeros((n_x, n_x, n_t))
    y_smoothed = np.zeros_like(y_true)
    Py_smoothed = np.zeros((n_y, n_y, n_t))

    x_smoothed[:,0] = x0.ravel()
    y_smoothed[:,0] = (H @ x0).ravel()
    P_smoothed[:,:,0] = P0

    for i_ in range(1, n_t):
        x_smoothed[:,i_] = states[i_]['expected'].ravel()
        P_smoothed[:,:,i_] = states[i_]['err_cov']
        y_smoothed[:,i_] = (H @ x_smoothed[:,i_].reshape(n_x,1)).ravel()
        Py_smoothed[:,:,i_] = H @ P_smoothed[:,:,i_] @ H.T + R

    return x_smoothed, P_smoothed, y_smoothed, Py_smoothed, rts

## Run Kalman prediction (n_s * n_t filter samples)
def run_predictor(y_noisy, ratio=0.8):
    pr = KalmanFilter(x0, P0, F, Q, H, R, _verbose=False)

    n_s = int(np.fix(ratio * n_t))

    x_predict = np.zeros((n_x, n_t-n_s))
    P_predict = np.zeros((n_x, n_x, n_t-n_s))
    y_predict = np.zeros((n_y, n_t-n_s))
    Py_predict = np.zeros((n_y, n_y, n_t-n_s))

    for i_ in range(1, n_s):#%n_t):
        pr.predict()
        pr.update(y_noisy[:,i_])

    for i_ in range(0,n_t-n_s):
        pr.predict()
        x_predict[:,i_]   = pr.state['expected'].ravel()
        P_predict[:,:,i_] = pr.state['err_cov']
        y_predict[:,i_] = (H @ x_predict[:,i_].reshape(n_x,1)).ravel()
        Py_predict[:,:,i_] = H @ P_predict[:,:,i_] @ H.T + R

    return x_predict, P_predict, y_predict, Py_predict

### PLOTTING
def plot_filter():
    f, ax = plt.subplots(3, 1, sharex='all')
    ax[0].set_title('Filter estimates')

    ax[0].plot(y_true.T, 'k-')
    ax[0].plot(y_noisy.T, 'b.')
    plot_estimate(np.arange(n_t), y_estimate, P_estimate[0,0,:], ax=ax[0])

    ax[0].set_xlabel('$t$')
    ax[0].set_ylabel('$x$')
    ax[0].legend(labels=['true state','noisy measurements','filtered estimate','estimate confidence (0.95)'])

    lbl = ['$\dot{x}$','$\ddot{x}$']
    for i in range(2):
        #ax[i+1].figure(figsize=(14,4))
        ax[i+1].plot(x_true[i+1,:].ravel(),'k-')

        plot_estimate(np.arange(n_t),
                      x_estimate[i+1,:], P_estimate[i+1,i+1,:],
                      ax=ax[i+1])

        ax[i+1].set_xlabel('$t$')
        ax[i+1].set_ylabel(lbl[i])
        ax[i+1].legend(labels=['true state','filtered estimate','estimate confidence (0.95)'])

def plot_smoother():
    f, ax = plt.subplots(3, 1, sharex='all')
    ax[0].set_title('Smoother estimate')
    ax[0].plot(y_true.T, 'k-')
    ax[0].plot(y_estimate.T, 'r--', lw=3)

    plot_estimate(np.arange(n_t), y_smoothed, P_smoothed[0,0,:], c='b',ax=ax[0])

    ax[0].set_xlabel('$t$')
    ax[0].set_ylabel('$x$')
    ax[0].legend(labels=['true state','filtered estimate','smoothed estimate','smoother confidence (0.95)'])

    lbl = ['$\dot{x}$','$\ddot{x}$']
    for i in range(2):
        #plt.figure(figsize=(14,4))
        ax[i+1].plot(x_true[i+1,:].ravel(),'k-')
        ax[i+1].plot(x_estimate[i+1,:].ravel(), 'r--', lw=3)

        plot_estimate(np.arange(n_t), x_smoothed[i+1,:], P_smoothed[i+1,i+1,:], c='b',ax=ax[i+1])

        ax[i+1].set_xlabel('$t$')
        ax[i+1].set_ylabel(lbl[i])
        ax[i+1].legend(labels=['true state','filtered estimate','smoothed estimate','smoother confidence (0.95)'])

def plot_prediction(r):
    plt.figure()
    plt.title('Predictor (%3.2f)' % r)

    plt.plot(y_true.T, 'k-')
    plt.plot(y_noisy[:,0:n_s+1].T, 'b.')

    plot_estimate(np.arange(n_s+1), y_estimate[:,0:n_s+1], P_estimate[0,0,0:n_s+1])

    plot_estimate(np.arange(n_s,n_t), y_predict, P_predict[0,0,:], c='g')

    plt.xlabel('$t$')
    plt.ylabel('$x$')
    plt.legend(labels=['true state', 'noisy measurements', 'filtered estimate','prediction','estimation confidence (0.95)','prediction confidence (0.95)'])

### RUN TESTS
num_test = 1000
filter_ratios = [0.9, 0.75, 0.5, 0.2, 0.]

f_rmse = np.zeros((n_x + n_y, num_test))
s_rmse = np.zeros((n_x + n_y, num_test))
p_rmse = np.zeros((n_x + n_y, num_test, len(filter_ratios)))

for k in range(num_test):
    if not k % 10:
        print(k)
    # Model systems with noise
    x_true, y_true, y_noisy = model_noisy(x0, F, Q, None, H, R, n=n_t)

    # Run filter
    x_estimate, P_estimate, y_estimate, Py_estimate, kf = run_filter(y_noisy)
    # Run smoother
    x_smoothed, P_smoothed, y_smoothed, Py_smoothed, _ = run_smoother(kf)
    # Calculate RMSE
    f_rmse[:,k] = rmse(np.vstack([x_true,y_true]),
                       np.vstack([x_estimate, y_estimate])).ravel()
    s_rmse[:,k] = rmse(np.vstack([x_true,y_true]),
                       np.vstack([x_smoothed, y_smoothed])).ravel()

    if k is num_test-1:
        plot_filter()
        plot_smoother()

    for j, r_ in enumerate(filter_ratios):
        x_predict, P_predict, y_predict, Py_predict = run_predictor(y_noisy, r_)

        n_s = int(np.fix(r_ * n_t))
        p_rmse[:,k,j] = rmse(np.vstack([x_true[:,n_s:n_t],y_true[:,n_s:n_t]]),
                             np.vstack([x_predict, y_predict])).ravel()
        if k is num_test-1 and (j is 1 or j is 2):
            plot_prediction(r_)

## Show results
f_m, f_s = np.mean(f_rmse, 1), np.std(f_rmse, 1)
s_m, s_s = np.mean(s_rmse, 1), np.std(s_rmse, 1)
p_m, p_s = np.mean(p_rmse, 1).squeeze(), np.std(p_rmse, 1).squeeze()

lbl = ['x[0]','x[1]','x[2]','y[0]']
print('        RMSE             ||   prediction ')
print('_____|_filter__|_smoother||___0.90___|___0.75___|___0.50___|___0.20___|___0.00___')
for i in range(4):
    print('%s | %6.5f | %6.5f || %8.4f | %8.4f | %8.4f | %8.4f | %8.4f' %\
            (lbl[i],f_m[i],s_m[i],p_m[i,0],p_m[i,1],p_m[i,2],p_m[i,3],p_m[i,4]))
    print(' +/- | %6.5f | %6.5f || %8.4f | %8.4f | %8.4f | %8.4f | %8.4f'  %\
                (f_s[i], s_s[i],p_s[i,0],p_s[i,1],p_s[i,2],p_s[i,3],p_s[i,4]))
    print('=================================================================================')
# Plot all
plt.show()


A = [[2.,  1.,  4.,  1.],
     [3.,  4., -1., -1.],
     [1., -4.,  1.,  5.],
     [2., -2.,  1.,  3.]]

v = [-4., 3., 9., 7. ]

N = len(v)

# Gaussian elimination
for m in range(N):
    div = A[m][m]
    for n in range(N):
        A[m][n] /= div
    v[m] /= div
    for l in range(m+1,N):
        mult = A[l][m]
        for o in range(N):
            A[l][o] -= mult*A[m][o]
        v[l] -= mult*v[m]

# Backsubstitution
x = [0 for i in range(N)]
for m in range(N-1,-1,-1):
    x[m] = v[m]
    for l in range(m+1,N):
        x[m] -= A[m][l]*x[l]

print(x)

# Using numpy:
A = [[2.,  1.,  4.,  1.],
     [3.,  4., -1., -1.],
     [1., -4.,  1.,  5.],
     [2., -2.,  1.,  3.]]

v = [-4., 3., 9., 7. ]
import numpy
print(numpy.linalg.solve(A,v))