#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
@author: alejandro
"""

import numpy as np
from math import exp, sin, cos, pi
import matplotlib.pyplot as plt
import plotly.express as px
import plotly.graph_objects as go
from plotly.offline import init_notebook_mode, plot
import os
np.random.seed(1)

#init_notebook_mode()

#Establece un punto de parada
def esperar():
  input("Pulsa una tecla para continuar...")
  plt.close('all')

###Funciones para el ejercicio 1 (GD)
def E(u,v):
    return (u**3*exp(v-2)-2*v**2*exp(-u))**2

#Derivada parcial de E con respecto a u
def dEu(u,v):
    return 2*(u**3*exp(v-2)-2*v**2*exp(-u))*(3*u**2*exp(v-2)+2*v**2*exp(-u))

#Derivada parcial de E con respecto a v
def dEv(u,v):
    return 2*(u**3*exp(v-2)-2*v**2*exp(-u))*(u**3*exp(v-2)-4*v*exp(-u))

#Gradiente de E
def gradE(u,v):
    return np.array([dEu(u,v), dEv(u,v)])

#GD
def gradient_descent(loss, gradLoss, lr=0.01, initial_point=np.array([0.0, 0.0], dtype=np.float64), max_iter=1000000,
                     error2get=float('-inf'), decay=1.0):
    hist = [initial_point]
    prevL = loss(*initial_point)
    hist_loss = [prevL]
    n_iters = 0
    while n_iters < max_iter:
      n_iters += 1
      hist.append( hist[-1]-lr*gradLoss(*hist[-1]) )
      currLoss = loss(*hist[-1])
      hist_loss.append(currLoss)
      if currLoss < error2get:
        break;
      else:
        prevL = currLoss
        lr *= decay
    return hist, n_iters, hist_loss

def f(x,y):
    return (x + 2)**2 + 2*(y - 2)**2 + 2*sin(2*pi*x)*sin(2*pi*y)

def f_arr(x,y):
    return (x + 2)**2 + 2*(y - 2)**2 + 2*np.sin(2*pi*x)*np.sin(2*pi*y)

def dfx(x,y):
    return 2*(x+2)+4*pi*cos(2*pi*x)*sin(2*pi*y)

def dfy(x,y):
    return 4*(y-2) + 4*pi*sin(2*pi*x)*cos(2*pi*y)

def gradf(x,y):
    return np.array([dfx(x,y), dfy(x,y)])

#Función para mostrar la gráfica de una función 2D junto con los puntos seguidos
# por el proceso de optimización con GD. Las llamadas a esta función se encuentran
# comentadas
def display_gd(loss, hist, title, x_min=-3, x_max=3, y_min=-3, y_max=3):
    import plotly.graph_objects as go
    x = np.linspace(x_min, x_max, 50)
    y = np.linspace(y_min, y_max, 50)
    X, Y = np.meshgrid(x, y)
    Z = loss(X,Y)

    x_traj = [ point[0] for point in hist ]
    y_traj = [ point[1] for point in hist ]
    z_traj = [ loss(point[0], point[1]) for point in hist ]

    fig = go.Figure(
                    data=[go.Surface(x=x, y=y, z=Z)],
                    layout=go.Layout(
                              title=go.layout.Title(text=title),
                              autosize=False,
                              scene=go.layout.Scene(
                                  xaxis_title="x",
                                  yaxis_title="y",
                                  zaxis_title="f(x,y)",
                              ),
                              width=600, height=600,
                              margin=dict(l=20, r=20, b=30, t=30)
                          )
    )
    fig.add_traces(data=[go.Scatter3d(x=x_traj, y=y_traj, z=z_traj)])

    #fig.show()
    plot(fig)

###Funciones para el ejercicio 2 (sobre regresión lineal)
def pseudoinverse(A):
      return np.linalg.inv(A.T@A)@A.T

def sgd(X, y, minibatch_size, lr=0.1, epochs=50, decay=1.0):
    data = np.copy(X)
    data = np.insert(data, 0, np.ones((len(data))), axis=1)
    w = np.random.uniform(low=0.0, high=1.0, size=data.shape[1])
    n_iters = 0
    while n_iters < epochs:
        n_iters += 1
        ind_chks = chunks(np.random.permutation(len(data)), minibatch_size)
        for indices in ind_chks:
            data_minibatch = data[indices]
            y_minibatch = y[indices]
            stoch_gradient = 2/minibatch_size*np.array([
                                  np.sum( data_minibatch[:,j]*(data_minibatch@w-y_minibatch) )
                                  for j in range(len(w))
                              ])
            w -= lr * stoch_gradient
        lr *= decay
    return w

def chunks(lst, n):
    """Yield successive n-sized chunks from lst."""
    for i in range(0, len(lst), n):
        yield lst[i:i + n]

def sign(x):
    if x >= 0:
      return 1
    return -1

def line_from_w(w):
    return [0,0.8], [-w[0]/w[2], (-w[0]-0.8*w[1])/w[2]]

def error(pred, truth):
    return 1-np.sum(np.equal(pred,truth))/len(truth)

def erm(X,y,w):
    return np.mean((X@w-y)**2)

def simula_unif(N, d, size):
	  return np.random.uniform(-size,size,(N,d))

def scatter_data(data, ax=None, label=None, title=None):
    if ax==None:
        _, ax = plt.subplots()
    if title!=None:
        ax.set_title(title)
    ax.scatter(data[:,0], data[:,1], label=label)
    return ax

def muestra_dataset(X, y, label1, label2, title=None):
    ax = scatter_data(X[y==label1], label=str(label1), title=title)
    _ = scatter_data(X[y==label2], ax=ax, label=str(label2))
    ax.legend(loc='upper right')
    plt.show()
    return ax

def muestra_preds(X, y, label1, label2, title=None):
    ax = scatter_data(X[y>0], label=str(label1), title=title)
    _ = scatter_data(X[y<0], ax=ax, label=str(label2))
    ax.legend(loc='upper right')
    return ax

#Cambia aleatoriamente el signo de un 10% de las etiquetas
def random_sign_change(foX, perc):
    new_foX = np.copy(foX)
    to_change = np.random.choice( np.arange(len(foX)), int(len(foX)*perc) )
    new_foX[to_change] = -new_foX[to_change]
    return new_foX

#Encapsula el experimento de repetir el muestreo uniforme de train y test, la etiquetación de
# los elementos y la clasificación mediante SGD, tantas veces como se indique
# por el parámetro reps (para esta práctica, 1000 repeticiones)
def test_experimento(reps, f):
    f_vec = np.vectorize(f)
    sum_error_clasif_tr = 0.0
    sum_error_clasif_te = 0.0
    sum_Ein = 0.0
    sum_Eout = 0.0
    for i in range(reps):
        X = simula_unif(1000, 2, 1)
        X_test = simula_unif(1000, 2, 1)

        foX = f_vec(X[:,0], X[:,1])
        foX = random_sign_change(foX, 0.1)
        foX_te = f_vec(X_test[:,0], X_test[:,1])
        foX_te = random_sign_change(foX_te, 0.1)

        w_sgd = sgd(X, foX, 32, lr=0.01, epochs=10)

        X_tr_mat = np.insert( np.copy(X), 0, np.ones((len(X))), axis=1 )
        X_te_mat = np.insert( np.copy(X_test), 0, np.ones((len(X_test))), axis=1 )

        preds_sgd_trainset = X_tr_mat @ w_sgd
        preds_sgd_trainset = np.array(list(map(sign, preds_sgd_trainset)))
        preds_sgd_testset = X_te_mat @ w_sgd
        preds_sgd_testset = np.array(list(map(sign, preds_sgd_testset)))

        sum_error_clasif_tr += error(preds_sgd_trainset, foX)
        sum_error_clasif_te += error(preds_sgd_testset, foX_te)

        sum_Ein += erm(X_tr_mat, foX, w_sgd)
        sum_Eout += erm(X_te_mat, foX_te, w_sgd)

    avg_error_clasif_tr = sum_error_clasif_tr/reps
    avg_error_clasif_te = sum_error_clasif_te/reps
    avg_Ein = sum_Ein/reps
    avg_Eout = sum_Eout/reps

    return avg_error_clasif_tr, avg_error_clasif_te, avg_Ein, avg_Eout

#Devuelve la lista de transformaciones no lineales que aplicar a los elementos
# del dataset
def transformations():
    return [
              (lambda x1, x2: x1),
              (lambda x1, x2: x2),
              (lambda x1, x2: x1*x2),
              (lambda x1, x2: x1*x1),
              (lambda x1, x2: x2*x2)
           ]

#Genera un nuevo conjunto de datos a partir de X, formado por una columna de 1s
# y otra columna por cada transformación en la lista transformations
def transform_characteristics(X, transformations):
    # Aplicamos las transformaciones deseadas sobre las características originales
    trans_data = np.array([
                np.array([ f(*x_row) for f in transformations ])
                for x_row in X
    ])
    return trans_data

#Encapsula el experimento de repetir el muestreo uniforme de train y test, la etiquetación de
# los elementos y la clasificación mediante SGD, tantas veces como se indique
# por el parámetro reps (para esta práctica, 1000 repeticiones). Añade el cálculo
# de las características no lineales
def test_NLChars(reps, f, batch=32):
    f_vec = np.vectorize(f)
    sum_error_clasif_tr = 0.0
    sum_error_clasif_te = 0.0
    sum_Ein = 0.0
    sum_Eout = 0.0
    for i in range(reps):
        X = simula_unif(1000, 2, 1)
        X_test = simula_unif(1000, 2, 1)

        foX = f_vec(X[:,0], X[:,1])
        foX = random_sign_change(foX, 0.1)
        foX_te = f_vec(X_test[:,0], X_test[:,1])
        foX_te = random_sign_change(foX_te, 0.1)

        NL_X = transform_characteristics(X, transformations())
        w_sgd = sgd(NL_X, foX, batch, lr=0.01, epochs=10)

        X_tr_mat = np.insert( np.copy(NL_X), 0, np.ones((len(X))), axis=1 )
        NL_X_test = transform_characteristics(X_test, transformations())
        X_te_mat = np.insert( np.copy(NL_X_test), 0, np.ones((len(NL_X_test))), axis=1 )

        preds_sgd_trainset = X_tr_mat @ w_sgd
        preds_sgd_trainset = np.array(list(map(sign, preds_sgd_trainset)))
        preds_sgd_testset = X_te_mat @ w_sgd
        preds_sgd_testset = np.array(list(map(sign, preds_sgd_testset)))

        sum_error_clasif_tr += error(preds_sgd_trainset, foX)
        sum_error_clasif_te += error(preds_sgd_testset, foX_te)

        sum_Ein += erm(X_tr_mat, foX, w_sgd)
        sum_Eout += erm(X_te_mat, foX_te, w_sgd)

    avg_error_clasif_tr = sum_error_clasif_tr/reps
    avg_error_clasif_te = sum_error_clasif_te/reps
    avg_Ein = sum_Ein/reps
    avg_Eout = sum_Eout/reps

    return avg_error_clasif_tr, avg_error_clasif_te, avg_Ein, avg_Eout

###Funciones relacionadas con el BONUS sobre el método de Newton
#Segundas derivadas parciales
def d2fxx(x,y):
    return 2 - 8*(pi**2)*sin(2*pi*x)*sin(2*pi*y)

def d2fxy(x,y):                                       # Igual a d2fyx
    return 8*(pi**2)*cos(2*pi*x)*cos(2*pi*y)

def d2fyy(x,y):
    return 4 - 8*(pi**2)*sin(2*pi*x)*sin(2*pi*y)

def hessianf(x,y):
    return np.array(
                    [[d2fxx(x,y), d2fxy(x,y)],
                     [d2fxy(x,y), d2fyy(x,y)]]
                   )
#Newton
def NR_method1(loss, gradLoss, hessianLoss, lr=0.1, initial_point=np.array([0.0, 0.0], dtype=np.float64),
              max_iter=1000000, decay=1.0):
    hist = [initial_point]
    hist_loss = [loss(*initial_point)]
    n_iters = 0
    while n_iters < max_iter:
        n_iters += 1
        invH = np.linalg.inv(hessianLoss(*hist[-1]))
        nextP = hist[-1] - lr*invH@gradLoss(*hist[-1])
        nextLoss = loss(*nextP)
        hist.append( nextP )
        hist_loss.append( nextLoss )
        lr *= decay
    return hist, n_iters, hist_loss

#Añade el cambio de signo para corregir la convergencia a puntos críticos
# no mínimos
def NR_method2(loss, gradLoss, hessianLoss, lr=0.1, initial_point=np.array([0.0, 0.0], dtype=np.float64),
              max_iter=1000000, decay=1.0):
    hist = [initial_point]
    hist_loss = [loss(*initial_point)]
    n_iters = 0
    while n_iters < max_iter:
      n_iters += 1
      invH = np.linalg.inv(hessianLoss(*hist[-1]))
      s = -1 if np.linalg.det(invH) < 0 or np.linalg.det(invH) > 0 and np.trace(invH) < 0 else 1
      nextP = hist[-1] - lr*invH@gradLoss(*hist[-1])*s
      nextLoss = loss(*nextP)
      hist.append( nextP )
      hist_loss.append( nextLoss )
      lr *= decay
    return hist, n_iters, hist_loss

class P1:

    def __init__(self):
        self.hist_loss1 = None
        self.hist_loss2 = None

    def apartado11y12(self):
        hist, it, hist_loss = gradient_descent(loss=E, gradLoss=gradE, lr=0.1, initial_point = np.array([1.0,1.0]), error2get=1e-14)

        print ('Número de iteraciones: ', it)
        print ('Coordenadas obtenidas: (', hist[-1][0], ', ', hist[-1][1],')')
        print ('Valor de la función objetivo: (', hist_loss[-1],')')

    def apartado13a(self):
        hist, it, self.hist_loss1 = gradient_descent(loss=f, gradLoss=gradf, lr=0.01, initial_point = np.array([-1.0,1.0]), max_iter=50)

        print('eta=0.01')
        print ('Número de iteraciones: ', it)
        print ('Coordenadas obtenidas: (', hist[-1][0], ', ', hist[-1][1],')')
        print ('Valor de la función objetivo: (', self.hist_loss1[-1],')')

        #display_gd(f_arr, hist, title="Ejercicio 1.3.a: eta = 0.01", x_min=-2, x_max=0, y_min=0, y_max=2)

        esperar()

        hist, it, self.hist_loss2 = gradient_descent(loss=f, gradLoss=gradf, lr=0.1, initial_point = np.array([-1.0,1.0]), max_iter=50)

        print('eta=0.1')
        print ('Número de iteraciones: ', it)
        print ('Coordenadas obtenidas: (', hist[-1][0], ', ', hist[-1][1],')')
        print ('Valor de la función objetivo: (', self.hist_loss2[-1],')')

        #display_gd(f_arr, hist, title="Ejercicio 1.3.a: eta = 0.1", x_min=-5, x_max=0, y_min=0, y_max=4)

        esperar()

        ###Mostramos en una gráfica la evolución de la función de pérdida a lo largo de
        ### cada experimento. Observamos que la gráfica correspondiente al GD con un lr
        ### más bajo presenta una evolución mucho más estable.
        plt.plot(np.arange( max(len(self.hist_loss1), len(self.hist_loss2)) ),
                 self.hist_loss1, label="lr = 0.01")
        plt.plot(np.arange( max(len(self.hist_loss1), len(self.hist_loss2)) ),
                 self.hist_loss2, label="lr = 0.1")
        plt.legend()
        plt.show()

    def apartado13b(self):
        h1, _, hl1 = gradient_descent(loss=f, gradLoss=gradf, lr=0.01, initial_point = np.array([-0.5,-0.5]), max_iter=50)
        h2, _, hl2 = gradient_descent(loss=f, gradLoss=gradf, lr=0.01, initial_point = np.array([1.0,1.0]), max_iter=50)
        h3, _, hl3 = gradient_descent(loss=f, gradLoss=gradf, lr=0.01, initial_point = np.array([2.1,-2.1]), max_iter=50)
        h4, _, hl4 = gradient_descent(loss=f, gradLoss=gradf, lr=0.01, initial_point = np.array([-3.0,3.0]), max_iter=50)
        h5, _, hl5 = gradient_descent(loss=f, gradLoss=gradf, lr=0.01, initial_point = np.array([-2.0,2.0]), max_iter=50)

        print ('Coordenadas (-0.5, -0.5): (%.4f, %.4f)' % (h1[-1][0], h1[-1][1]))
        print ('Función objetivo (-0.5, -0.5): %.4f' % hl1[-1])

        print ('Coordenadas (1.0,1.0): (%.4f, %.4f)' % (h2[-1][0], h2[-1][1]))
        print ('Función objetivo (1.0,1.0): %.4f' % hl2[-1])

        print ('Coordenadas (2.1,-2.1): (%.4f, %.4f)' % (h3[-1][0], h3[-1][1]))
        print ('Función objetivo (2.1,-2.1): %.4f' % hl3[-1])

        print ('Coordenadas (-3.0,3.0):(%.4f, %.4f)' % (h4[-1][0], h4[-1][1]))
        print ('Función objetivo (-3.0,3.0): %.4f' % hl4[-1])

        print ('Coordenadas (-2.0,2.0): (%.4f, %.4f)' % (h5[-1][0], h5[-1][1]))
        print ('Función objetivo (-2.0,2.0): %.4f' % hl5[-1])

        esperar()

        #Calculamos la desviación típica de las soluciones obtenidas con cada punto inicial
        solutions = np.array([h1[-1], h2[-1], h3[-1], h4[-1], h5[-1]])
        std_solutions = np.std(solutions, axis=0)
        print('Desviación en las soluciones:', std_solutions)

        f_solutions = np.array([hl1[-1], hl2[-1], hl3[-1], hl4[-1], hl5[-1]])
        std_f_solutions = np.std(f_solutions, axis=0)
        print('Desviación en el valor objetivo:', std_f_solutions)

    def apartado13(self):
        self.apartado13a()
        esperar()

        self.apartado13b()

    def apartado1(self):
        self.apartado11y12()
        esperar()

        self.apartado13()

    def apartado21(self):
        #Cargamos los datos de entrenamiento y test
        X_train = np.load('datos/X_train.npy')
        y_train = np.load('datos/y_train.npy')
        X_test = np.load('datos/X_test.npy')
        y_test = np.load('datos/y_test.npy')

        #Eliminamos aquellas instancias de las muestras de entrenamiento y de test
        # que no estén etiquetadas como 1 ó 5
        X_train = X_train[np.logical_or(y_train==1, y_train==5)]
        y_train = y_train[np.logical_or(y_train==1, y_train==5)]
        X_test = X_test[np.logical_or(y_test==1, y_test==5)]
        y_test = y_test[np.logical_or(y_test==1, y_test==5)]

        print('Tamaño del conjunto de entrenamiento:', len(y_train))
        print('Tamaño del conjunto de test:', len(y_test))

        y_train_labels = np.array(list(map(lambda x: 1 if x==1 else -1, y_train)), dtype=np.float64)
        y_test_labels = np.array(list(map(lambda x: 1 if x==1 else -1, y_test)), dtype=np.float64)

        #Mostramos un scatter plot con las muestras de entrenamiento y test
        muestra_dataset(X_train, y_train, 1, 5, title="Conjunto de entrenamiento")
        muestra_dataset(X_test, y_test, 1, 5, title="Conjunto de test")
        esperar()

        #Cálculo de los pesos óptimos con la pseudoinversa
        X = np.insert( X_train, 0, np.ones((len(X_train))), axis=1 )
        w = pseudoinverse(X)@y_train_labels

        #X_tr_mat y X_te_mat son las matrices con los datos de entrenamiento
        # y test a las que se le añade una columna de 1s al principio
        X_tr_mat = np.insert( X_train, 0, np.ones((len(X_train))), axis=1 )
        preds_trainset = X_tr_mat @ w
        preds_trainset = np.array(list(map(sign, preds_trainset)))

        X_te_mat = np.insert( X_test, 0, np.ones((len(X_test))), axis=1 )
        preds = X_te_mat @ w
        preds = np.array(list(map(sign, preds)))

        print("Errores obtenidos con la pseudoinversa:")

        err_clasf_tr=error(preds_trainset, y_train_labels)
        err_clasf_te=error(preds, y_test_labels)

        print("Error de clasificación en train:", err_clasf_tr)
        print("Error de clasificación en test:", err_clasf_te)

        Ein = erm(X_tr_mat, y_train_labels, w)
        Eout = erm(X_te_mat, y_test_labels, w)

        print("Ein:", Ein)
        print("Eout:", Eout)
        esperar()

        #Plot de las predicciones
        line = line_from_w(w)
        ax_tr = muestra_preds(X_train, preds_trainset, 1, 5, "Predicciones de entrenamiento")
        ax_tr.plot(line[0], line[1], color='g')
        ax_te = muestra_preds(X_test, preds, 1, 5, "Predicciones de test")
        ax_te.plot(line[0], line[1], color='g')
        esperar()

        #Cálculo de los pesos óptimos con SGD
        w_sgd = sgd(X_train, y_train_labels, 32, lr=0.01, epochs=10)

        preds_sgd_trainset = np.insert(X_train, 0, np.ones((len(X_train))), axis=1 ) @ w_sgd
        preds_sgd_trainset = np.array(list(map(sign, preds_sgd_trainset)))

        preds_sgd = np.insert( X_test, 0, np.ones((len(X_test))), axis=1 ) @ w_sgd
        preds_sgd = np.array(list(map(sign, preds_sgd)))

        print("Errores obtenidos con SGD:")
        err_clasf_tr=error(preds_sgd_trainset, y_train_labels)
        err_clasf_te=error(preds_sgd, y_test_labels)

        print("Error de clasificación en train:", err_clasf_tr)
        print("Error de clasificación en test:", err_clasf_te)

        X_tr_mat = np.insert( np.copy(X_train), 0, np.ones((len(X_train))), axis=1 )
        X_te_mat = np.insert( np.copy(X_test), 0, np.ones((len(X_test))), axis=1 )
        Ein = erm(X_tr_mat, y_train_labels, w_sgd)
        Eout = erm(X_te_mat, y_test_labels, w_sgd)

        print("Ein:", Ein)
        print("Eout:", Eout)
        esperar()

        #Plot de las predicciones
        line_sgd = line_from_w(w_sgd)
        ax_tr = muestra_preds(X_train, preds_sgd_trainset, 1, 5, "Predicciones de entrenamiento")
        ax_tr.plot(line_sgd[0], line_sgd[1], color='g')
        ax_te = muestra_preds(X_test, preds_sgd, 1, 5, "Predicciones de test")
        ax_te.plot(line_sgd[0], line_sgd[1], color='g')

    def apartado22(self):
        X = simula_unif(1000, 2, 1)
        _ = scatter_data(X)

        def f(x1, x2):
            return sign((x1-0.2)**2 + x2**2-0.6)

        f_vec = np.vectorize(f)
        foX = f_vec(X[:,0], X[:,1])
        foX = random_sign_change(foX, 0.1)

        ax = scatter_data(X[foX==-1], label="f = -1")
        _ = scatter_data(X[foX==1], ax, label="f = 1")
        ax.legend()
        plt.show()
        esperar()

        ##USO DE CARACTERÍSTICAS LINEALES
        w_sgd = sgd(X, foX, 32, lr=0.01, epochs=10)

        preds_sgd_trainset = np.insert( np.copy(X), 0, np.ones((len(X))), axis=1 ) @ w_sgd
        preds_sgd_trainset = np.array(list(map(sign, preds_sgd_trainset)))

        print("Características lineales:")
        error_clasif = error(preds_sgd_trainset, foX)
        print("Error de clasificación:", error_clasif)

        X_tr_mat = np.insert( np.copy(X), 0, np.ones((len(X))), axis=1 )
        Ein = erm(X_tr_mat, foX, w_sgd)
        print("Ein:", Ein)
        esperar()

        #Plot de las predicciones
        line_sgd = line_from_w(w_sgd)
        ax_tr = muestra_preds(X, preds_sgd_trainset, -1, 1, "Modelo ajustado")
        ax_te = muestra_preds(X, foX, -1, 1, "Modelo ajustado (etiquetas originales)")
        esperar()

        avg_error_clasif_tr, avg_error_clasif_te, avg_Ein, avg_Eout = test_experimento(reps=1000, f=f)
        print("Errores medios (ECin, ECout, Ein, Eout):", avg_error_clasif_tr, avg_error_clasif_te, avg_Ein, avg_Eout)
        esperar()

        X = simula_unif(1000, 2, 1)
        foX = f_vec(X[:,0], X[:,1])
        foX = random_sign_change(foX, 0.1)

        ##USO DE CARACTERÍSTICAS NO LINEALES
        NL_X = transform_characteristics(X, transformations())
        w_sgd = sgd(NL_X, foX, 16, lr=0.01, epochs=10)

        X_tr_mat = np.insert( np.copy(NL_X), 0, np.ones((len(X))), axis=1 )
        preds_sgd_trainset = X_tr_mat @ w_sgd
        preds_sgd_trainset = np.array(list(map(sign, preds_sgd_trainset)))

        print("Características no lineales:")
        error_clasif = error(preds_sgd_trainset, foX)
        print("Error de clasificación:", error_clasif)

        Ein = erm(X_tr_mat, foX, w_sgd)
        print("Ein:", Ein)
        esperar()

        line_sgd = line_from_w(w_sgd)
        ax_tr = muestra_preds(X, preds_sgd_trainset, -1, 1, "Modelo ajustado")
        ax_te = muestra_preds(X, foX, -1, 1, "Modelo ajustado (etiquetas originales)")
        esperar()

        print("Errores medios (ECin, ECout, Ein, Eout) según el tamaño de batch:")
        avg_error_clasif_tr, avg_error_clasif_te, avg_Ein, avg_Eout = test_NLChars(reps=1000, f=f, batch=256)
        print("256->", avg_error_clasif_tr, avg_error_clasif_te, avg_Ein, avg_Eout)

        avg_error_clasif_tr, avg_error_clasif_te, avg_Ein, avg_Eout = test_NLChars(reps=1000, f=f, batch=128)
        print("128->", avg_error_clasif_tr, avg_error_clasif_te, avg_Ein, avg_Eout)

        avg_error_clasif_tr, avg_error_clasif_te, avg_Ein, avg_Eout = test_NLChars(reps=1000, f=f, batch=64)
        print("64->", avg_error_clasif_tr, avg_error_clasif_te, avg_Ein, avg_Eout)

        avg_error_clasif_tr, avg_error_clasif_te, avg_Ein, avg_Eout = test_NLChars(reps=1000, f=f, batch=32)
        print("32->", avg_error_clasif_tr, avg_error_clasif_te, avg_Ein, avg_Eout)

        avg_error_clasif_tr, avg_error_clasif_te, avg_Ein, avg_Eout = test_NLChars(reps=1000, f=f, batch=16)
        print("16->", avg_error_clasif_tr, avg_error_clasif_te, avg_Ein, avg_Eout)

        avg_error_clasif_tr, avg_error_clasif_te, avg_Ein, avg_Eout = test_NLChars(reps=1000, f=f, batch=8)
        print("8->", avg_error_clasif_tr, avg_error_clasif_te, avg_Ein, avg_Eout)

    def apartado2(self):
        self.apartado21()
        esperar()

        self.apartado22()

    def BONUS(self):
        print("Optimización de f con el método de Newton:")
        hist, it, hist_loss_NR = NR_method1(loss=f, gradLoss=gradf, hessianLoss=hessianf,
                                           lr=0.1, initial_point = np.array([-1.0,1.0]), max_iter=50,
                                           decay=1.0)

        print ('Número de iteraciones: ', it)
        print ('Coordenadas obtenidas: (', hist[-1][0], ', ', hist[-1][1],')')
        print ('Valor de la función objetivo: (', hist_loss_NR[-1],')')

        #display_gd(f_arr, hist, title="BONUS: método de Newton-Raphson 1", x_min=-2, x_max=0, y_min=0, y_max=2)
        esperar()

        #Mostramos en una gráfica la evolución de la función de pérdida a lo largo de
        # cada experimento.
        plt.plot(np.arange( max(len(hist_loss_NR), len(hist_loss_NR)) ),
                 hist_loss_NR, label="NR")
        plt.legend()
        plt.show()
        esperar()

        print("Optimización usando versión modificada de Newton:")
        hist, it, hist_loss_NR = NR_method2(loss=f, gradLoss=gradf, hessianLoss=hessianf,
                                           lr=0.1, initial_point = np.array([-1.0,1.0]), max_iter=50,
                                           decay=1.0)

        print ('Número de iteraciones: ', it)
        print ('Coordenadas obtenidas: (', hist[-1][0], ', ', hist[-1][1],')')
        print ('Valor de la función objetivo: (', hist_loss_NR[-1],')')

        #display_gd(f_arr, hist, title="BONUS: método de Newton-Raphson 2", x_min=-2, x_max=0, y_min=0, y_max=2)
        esperar()

        plt.plot(np.arange( max(len(hist_loss_NR), len(hist_loss_NR)) ),
                 hist_loss_NR, label="NR; lr = 0.1")
        plt.plot(np.arange( max(len(self.hist_loss1), len(self.hist_loss2)) ),
                 self.hist_loss1, label="lr = 0.01")
        plt.plot(np.arange( max(len(self.hist_loss1), len(self.hist_loss2)) ),
                 self.hist_loss2, label="lr = 0.1")
        plt.legend()
        plt.show()
        esperar()

        print("Prueba con distintos puntos iniciales:")
        h1, _, hl1 = NR_method2(loss=f, gradLoss=gradf, hessianLoss=hessianf, lr=0.1, initial_point = np.array([-0.5,-0.5]), max_iter=50)
        h2, _, hl2 = NR_method2(loss=f, gradLoss=gradf, hessianLoss=hessianf, lr=0.1, initial_point = np.array([1.0,1.0]), max_iter=50)
        h3, _, hl3 = NR_method2(loss=f, gradLoss=gradf, hessianLoss=hessianf, lr=0.1, initial_point = np.array([2.1,-2.1]), max_iter=50)
        h4, _, hl4 = NR_method2(loss=f, gradLoss=gradf, hessianLoss=hessianf, lr=0.1, initial_point = np.array([-3.0,3.0]), max_iter=50)
        h5, _, hl5 = NR_method2(loss=f, gradLoss=gradf, hessianLoss=hessianf, lr=0.1, initial_point = np.array([-2.0,2.0]), max_iter=50)

        print ('Coordenadas (-0.5, -0.5): (%.4f, %.4f)' % (h1[-1][0], h1[-1][1]))
        print ('Función objetivo (-0.5, -0.5): %.4f' % hl1[-1])

        print ('Coordenadas (1.0,1.0): (%.4f, %.4f)' % (h2[-1][0], h2[-1][1]))
        print ('Función objetivo (1.0,1.0): %.4f' % hl2[-1])

        print ('Coordenadas (2.1,-2.1): (%.4f, %.4f)' % (h3[-1][0], h3[-1][1]))
        print ('Función objetivo (2.1,-2.1): %.4f' % hl3[-1])

        print ('Coordenadas (-3.0,3.0):(%.4f, %.4f)' % (h4[-1][0], h4[-1][1]))
        print ('Función objetivo (-3.0,3.0): %.4f' % hl4[-1])

        print ('Coordenadas (-2.0,2.0): (%.4f, %.4f)' % (h5[-1][0], h5[-1][1]))
        print ('Función objetivo (-2.0,2.0): %.4f' % hl5[-1])

def main():
    p1 = P1()
    p1.apartado1()
    esperar()

    p1.apartado2()
    esperar()

    p1.BONUS()
    esperar()

    if os.path.exists("temp-plot.html"):
        os.remove("temp-plot.html") # one file at a time

if __name__ == "__main__":
    main()


'''
SpecialFuncUtil.py

Special mathematical functions, like multivariate gammaln.
'''

from collections import defaultdict
import numpy as np
from scipy.special import gammaln, digamma

LOGPI = np.log(np.pi)
LOGTWO = np.log(2.)
LOGTWOPI = np.log( 2.*np.pi )
EPS = 10*np.finfo(float).eps

MVgCache = defaultdict( lambda: dict())
def MVgammaln(x, D):
  ''' Compute log of the D-dimensional multivariate Gamma func. for input x

      Notes: Caching gives big speedup!
      -------
       caching : 208 sec for 5 iters of CGS on K=50, D=2 problem with N=10000
      no cache : 300 sec
  '''
  try:
    return MVgCache[D][x]
  except KeyError:
    result = gammaln(x+ 0.5*(1 - np.arange(1,D+1)) ).sum() + 0.25*D*(D-1)*LOGPI
    MVgCache[D][x] = result
  return result

def MVdigamma(x, D):
  ''' Compute the first-derivative of the log of the D-dim. Gamma function
  '''
  return digamma(x + 0.5 * (1 - np.arange(1,D+1))).sum()

def logsumexp(logA, axis=None):
  ''' Efficiently compute log(sum(exp(...))) for input matrix "logA"
      Computation is both vectorized and numerically stable.
  '''
  logA = np.asarray(logA)
  logAmax = logA.max(axis=axis)
  if axis is None:
    logA = logA - logAmax
  elif axis==1:
    logA = logA - logAmax[:,np.newaxis]
  elif axis==0:
    logA = logA - logAmax[np.newaxis,:]
  assert np.allclose( logA.max(), 0.0 )
  logA = np.log( np.sum( np.exp(logA), axis=axis )  )
  return logA + logAmax


#==============================================================================
# Photon Diffusion
# Calculates the diffusion of photons in a non-absorbing medium (lanex phosphor) using Fick's laws of diffusion.
#==============================================================================

#Imports
from numpy import *
import matplotlib as mpl
import matplotlib.pyplot as plt
import seaborn as sns
from scipy.integrate import quad
from scipy.special import erf
import sys
import os

#Import custom modules
from physics import *

sns.set(font_scale=2.0)
sns.set_style("ticks")
sns.set_palette(palette='deep')
sns.set_color_codes(palette='deep')
mpl.rcParams.update({'font.family': 'serif', 'font.serif':'DejaVu Serif'})

#%%
#==============================================================================
# Calculation of the phosphor layer thickness of lanex regular given its areal density:
#==============================================================================

Dcell = 55*10**-6
DPET = 175*10**-6
Dcell2 = 13*10**-6
rhocell = 1.44*10**3
rhoPET = 1.38*10**3
rhophos = 4.48*10**3
sigma = 70*10**-2

Dphos = (sigma - Dcell*rhocell - DPET*rhoPET - Dcell2*rhocell)/rhophos
print(Dphos*10**6)

#%%
#==============================================================================
#  Define functions for photon density and photon current density
#  These functions were derived from Fick's laws of diffusion:
#
#  $$ \frac{\partial n(z,t)}{\partial t} = D \frac{\partial^2 n(z,t)}{\partial z^2}, \: \: \phi (z,t) = -D \frac{\partial n(z,t)}{\partial z}$$
#
#  where n(z,t) is the photon density, D is the diffusion constant $D=\lambda_s c /6$ with $\lambda_s$ corresponding to the mean photon scattering length, and $\phi (z,t)$ is the photon current density. The fluorescence of the lanex phosphor was modeled as the instantaneous generation of light within a infinitely thin segment within a rectangular slab. The slab thickness $L$ is taken to be small compared to the other dimensions so that the problem can be treated one-dimensionally. The initial condition is taken to be
#
#  $$ n(z,0) = \frac{N_0}{A} \delta (z) \: \: \mathrm{with} \: \: n(z,t)=0 \: \: \mathrm{for} \: t<0 $$
#
#  where $N_0$ is the number of photons generated and $A$ is the cross-sectional area of the slab. In other words, at $t=0$, $N_0$ photons are generated and modeled as a dirac delta function at $z=0$. Fick's laws are then solved with absorbing boundary conditions at the edges of the lanex:
#
#  $$ n(d,t)=0, \: n(-l,t)=0$$
#
#  where $z=d$ is the location of the CCD and $z=-l$ is the location of the top edge of the phosphor. This yields
#
#  $$ n(z,t) = \frac{N_0}{2 A \sqrt{\pi D t}} \sum\limits_{m=-\infty}^\infty \left[ e^{-(z-2mL)^2/4Dt} - e^{-(z+2mL-2d)^2/4Dt} \right] $$
#
#  The photon current density then follows by differentiation with respect to $z$.
#==============================================================================

N0 = 10**6 #Number of photons emitted at t=0
lambdas = 2.85*10**-6 #Diffusion length in m
D = lambdas*c/6 #Diffusion constant
A = 100*10**-6*100*10**-6 #Area of segment in m^2
L = 81*10**-6 #Depth of lanex in m
l = 10.0*10**-6 #Distance from top lanex edge to segment in m
d = L-l #Distance from bottom lanex edge to segment

def n(z,t):
    '''Returns the photon density at position z and time t'''
    n0 = N0/(2*A*sqrt(pi*D*t))
    Sum = 0
    maxm = 10
    for m in range(-maxm,maxm+1):
        Sum += exp(-(z-2*m*(l+d))**2/(4*D*t))-exp(-(z+2*m*(l+d)-2*d)**2/(4*D*t))
    return n0*Sum

def particlecurrent(t):
    '''Returns the particle current (photons per second per meter^2) at the boundary z=d at time t'''
    Sum = 0
    maxm = 10
    for m in range(-maxm,maxm+1):
        am = d-2*m*L
        Sum += am*exp(-am**2/(4*D*t))
    return N0/(A*sqrt(4*pi*D*t**3))*Sum

#%%
#==============================================================================
# Plot photon density
# The function $n(z,t)$ is calculated from 1 fs to 10 ps and plotted below.
# The boundary conditions are visibly satisfied and the function approaches a
# dirac delta function for short times. It then spreads out with the total number
# of photons decreasing as they're absorbed at the boundaries.
#==============================================================================

narray = []
zarray = np.linspace(-l,d,1000)
time = [1,10,10**2,10**3,10**4]
time = np.multiply(time,10**-15) #convert to s

for i in range(len(time)):
    narray.append([])
    for z in zarray:
        narray[i].append(n(z,time[i])*10**-6)

zarray = np.multiply(zarray,10**6)

plt.subplots(figsize=(12,6))

color = ['r','g','b','c','m','y','k']
legend = []
for i in range(5):
    legend.append(str(int(time[i]*10**15))+' fs')
    plt.plot(zarray,narray[i],color=color[i],linewidth=2,label=legend[i])
plt.xlim(np.min(zarray),np.max(zarray))
plt.ylim(1.0*10**6,np.max(narray[0]))
plt.xlabel('Position (um)')
plt.ylabel('Photon Density (m^-3)')
#plt.semilogy()
plt.legend(loc=1)

#%%
#==============================================================================
# Plot photon current density
# Photon current density is then calculated at $z=d$ and plotted below as a function of time
#==============================================================================

particlecurrentarray = []
tarray = []
for t in linspace(10**-15,50*10**-12,1000):
    tarray.append(t*10**12)
    particlecurrentarray.append(particlecurrent(t))

plt.subplots(figsize=(12,6))

plt.plot(tarray,particlecurrentarray,linewidth=2)
plt.xlim(np.min(tarray),np.max(tarray))
plt.ylim(0)
plt.xlabel('time (ps)')
plt.ylabel('Photon Current at $z=d$ $(s^{-1} \cdot m^{-2})$')
#plt.semilogy()
plt.legend(loc=4)

#%%
#==============================================================================
# Integrate photon current density
# The photon current density at $z=d$ is then integrated over large times and
# multiplied by the area $A$ to determine the total number of photons absorbed
# by the CCD. This is done numerically and analytically with the function
# defined below. A plot of the fraction of photons absorbed is plotted as a
# function of time to ensure that the integral converges.
#==============================================================================

Nabs = A*quad(particlecurrent,0,400*10**-12)[0] #Total number of photons absorbed at the boundary z=d
print(Nabs/N0)

def F(t,maxm,distance):
    Sum1 = 0
    Sum2 = 0
    for m in range(-maxm,1):
        am = distance-2*m*L
        Sum1 += 1 - erf(am/sqrt(4*D*t))
    for m in range(1,maxm+1):
        am = distance-2*m*L
        Sum2 += 1 + erf(am/sqrt(4*D*t))
    return (Sum1 - Sum2)

FractionAbsArray = []
FractionAbsArrayAnalytic = []
tarray = []
for t in linspace(10**-12,50*10**-12,10000):
    tarray.append(t*10**12)
    #FractionAbsArray.append(A*quad(particlecurrent,0,t)[0]/N0)
    FractionAbsArrayAnalytic.append(F(t,100,d))

plt.subplots(figsize=(12,6))

plt.plot(tarray,FractionAbsArrayAnalytic,linewidth=2)
plt.xlim(np.min(tarray),np.max(tarray))
plt.ylim(0,1.0)
plt.xlim(0,50)
plt.xlabel('time (ps)')
plt.ylabel('Fraction Absorbed at $z=d$')
#plt.semilogy()
plt.legend(loc=4)

#%%
#==============================================================================
# Calculate number of photons absorbed as a function of d
# A function is defined to calculate the total number of photons absorbed at $z=d$ after all time as a function of $d$. The results are then plotted and found to be linear. The rather unpleasant expression defined above evidently can be approximated as (or is exactly equal to)
#
# $$N_{abs}(d) = N_0 (1 - d/L) $$
#==============================================================================

FractionAbsArrayAnalytic = []
distancearray = []

#Find the fraction of photons absorbed at z=d for various values of d ranging from 0 to L - 1 um (to avoid division by zero errors)
for distance in linspace(0,L-10**-6,100):
    Integrationtime = 10**-12
    TargetError = 10**-3
    Error = 1.0
    FractionAbsAnalytic=0
    while Error>TargetError:
        Error = abs(FractionAbsAnalytic-F(Integrationtime,100,distance))/F(Integrationtime,100,distance)
        FractionAbsAnalytic = F(Integrationtime,100,distance)
        Integrationtime *= 2
    FractionAbsArrayAnalytic.append(FractionAbsAnalytic)
    distancearray.append(distance*10**6)

plt.subplots(figsize=(12,6))

plt.plot(distancearray,FractionAbsArrayAnalytic,linewidth=2)
#plt.xlim(np.min(tarray),np.max(tarray))
#plt.ylim(0,1.0)
#plt.xlim(0,50)
plt.xlabel('Segment Distance (um)')
plt.ylabel('Fraction Absorbed by CCD')
#plt.semilogy()


#! /usr/bin/env python
# -*- coding: utf-8 -*-

import numpy as np
import sktensor as skt

def rmse(Y1,Y2):
    return np.sqrt(np.linalg.norm(Y1-Y2)**2/Y2.size)

class Kernel(object):
    def __init__(self,name,param=None, normalize=False):
        assert name in ['rbf','linear','poly'], 'kernel not implemented'
        if name == 'rbf':
            self.f = lambda x,y: np.exp(-1*param * np.linalg.norm(x-y)**2)
            self.param = param
            self.name = name

        if name == 'linear':
            self.f = np.dot
            self.param = param
            self.name = name

        if name == 'poly':
            self.f = lambda x,y: (param[1] + x.T.dot(y))**param[0]
            self.param = param
            self.name = name
        self.normalize = normalize

    def __call__(self,x,y):
        if self.normalize:
            z = np.sqrt(self.f(x,x)*self.f(y,y))
        else:
            z = 1
        return self.f(x,y) / z

    def __str__(self):
        return self.name + ' (param=%s, norm=%s)' % (str(self.param), str(self.normalize))

    def gram_matrix(self,X,X_test=None):
        if isinstance(X,skt.dtensor):
            X = X.unfold(0)
        if X_test is None:
            n = X.shape[0]
            K = np.zeros([n,n])
            for i in range(n):
                for j in range(i,n):
                    tmp = self(X[i,:],X[j,:])
                    K[i,j] = tmp
                    K[j,i] = tmp
        else:
            if isinstance(X_test,skt.dtensor):
                X_test = X_test.unfold(0)
            K = np.zeros([X_test.shape[0], X.shape[0]])
            for i in range(X_test.shape[0]):
                for j in range(X.shape[0]):
                    K[i,j] = self(X_test[i,:],X[j,:])
        return K

def rbf_kernel(gamma):
    return lambda x,y: np.exp(-1*gamma * np.linalg.norm(x-y)**2)


#!/usr/bin/env python

"""
This example shows how to work with coordinate transformations, curvilinear
coordinates and a little bit with differential geometry.

It takes polar, cylindrical, spherical, rotating disk coordinates and others
and calculates all kinds of interesting properties, like Jacobian, metric
tensor, Laplace operator, ...
"""

from sympy import var, sin, cos, pprint, Matrix, eye, trigsimp, Eq, \
    Function, simplify, sinh, cosh, expand, symbols

def laplace(f, g_inv, g_det, X):
    """
    Calculates Laplace(f), using the inverse metric g_inv, the determinant of
    the metric g_det, all in variables X.
    """
    r = 0
    for i in range(len(X)):
        for j in range(len(X)):
            r += g_inv[i, j]*f.diff(X[i]).diff(X[j])
    for sigma in range(len(X)):
        for alpha in range(len(X)):
            r += g_det.diff(X[sigma]) * g_inv[sigma, alpha] * \
                f.diff(X[alpha]) / (2*g_det)
    return r

def transform(name, X, Y, *, g_correct=None, recursive=False):
    """
    Transforms from cartesian coordinates X to any curvilinear coordinates Y.

    It printing useful information, like Jacobian, metric tensor, determinant
    of metric, Laplace operator in the new coordinates, ...

    g_correct ... if not None, it will be taken as the metric --- this is
                  useful if sympy's trigsimp() is not powerful enough to
                  simplify the metric so that it is usable for later
                  calculation. Leave it as None, only if the metric that
                  transform() prints is not simplified, you can help it by
                  specifying the correct one.

    recursive ... apply recursive trigonometric simplification (use only when
                  needed, as it is an expensive operation)
    """
    print("_"*80)
    print("Transformation:", name)
    for x, y in zip(X, Y):
        pprint(Eq(y, x))
    J = X.jacobian(Y)
    print("Jacobian:")
    pprint(J)
    g = J.T*eye(J.shape[0])*J

    g = g.applyfunc(expand)
    print("metric tensor g_{ij}:")
    pprint(g)
    if g_correct is not None:
        g = g_correct
        print("metric tensor g_{ij} specified by hand:")
        pprint(g)
    print("inverse metric tensor g^{ij}:")
    g_inv = g.inv(method="ADJ")
    g_inv = g_inv.applyfunc(simplify)
    pprint(g_inv)
    print("det g_{ij}:")
    g_det = g.det()
    pprint(g_det)
    f = Function("f")(*list(Y))
    print("Laplace:")
    pprint(laplace(f, g_inv, g_det, Y))

def main():
    mu, nu, rho, theta, phi, sigma, tau, a, t, x, y, z, w = symbols(
        "mu, nu, rho, theta, phi, sigma, tau, a, t, x, y, z, w")

    transform("polar", Matrix([rho*cos(phi), rho*sin(phi)]), [rho, phi])

    transform("cylindrical", Matrix([rho*cos(phi), rho*sin(phi), z]),
              [rho, phi, z])

    transform("spherical",
              Matrix([rho*sin(theta)*cos(phi), rho*sin(theta)*sin(phi),
                      rho*cos(theta)]),
              [rho, theta, phi],
              recursive=True
              )

    transform("rotating disk",
              Matrix([t,
                      x*cos(w*t) - y*sin(w*t),
                      x*sin(w*t) + y*cos(w*t),
                      z]),
              [t, x, y, z])

    transform("parabolic",
              Matrix([sigma*tau, (tau**2 - sigma**2) / 2]),
              [sigma, tau])

    transform("bipolar",
            Matrix([a*sinh(tau)/(cosh(tau)-cos(sigma)),
                a*sin(sigma)/(cosh(tau)-cos(sigma))]),
            [sigma, tau]
            )

    transform("elliptic",
              Matrix([a*cosh(mu)*cos(nu), a*sinh(mu)*sin(nu)]),
              [mu, nu]
              )

if __name__ == "__main__":
    main()


import numpy as np

def second_differences(xs, *, direction):
    """Compute second differences. Returns three numpy arrays: the first
    and second differences, as well as a truncated version of the input
    array so that the positions at each index match up.
    """
    xs = np.array(xs)
    if direction == 'forward':
        trunc_xs = xs[:-2]
        dxs = xs[1:] - xs[:-1]
        d2xs = dxs[1:] - dxs[:-1]
        return trunc_xs, dxs[:-1], d2xs
    elif direction == 'central':
        trunc_xs = xs[1:-1]
        dxs = (xs[2:] - xs[:-2]) / 2
        d2xs = xs[2:] - 2*xs[1:-1] + xs[:-2]
        return trunc_xs, dxs, d2xs
    else:
        raise NotImplementedError('direction must be forward or central')

def accelerate(xs, **kwargs):
    iterations = kwargs.pop('iterations', 1)
    direction = kwargs.setdefault('direction', 'forward')
    strength = kwargs.setdefault('strength', 1)

    if not isinstance(iterations, int) or iterations < 1:
        raise TypeError('The number of iterations must be a positive integer.')

    if iterations > 1:
        return accelerate(
            accelerate(xs, **kwargs), iterations=iterations-1, **kwargs
        )

    xs, dxs, d2xs = second_differences(xs, direction=direction)
    correction = np.where(
        (dxs == 0) & (d2xs == 0),
        0,
        np.divide(dxs ** 2, d2xs)
    )
    return xs - strength * correction


# implementation of linear kalman filter using 2D constant acceleration model

# state matrix:             2D x-y position, velocity and acceleration in x and y axis (6 x 1)
# input matrix:             --None--
# measurement matrix:       2D x-y position and imu acceleration in x and y axis (4 x 1)

import math
import matplotlib.pyplot as plt
import numpy as np
from scipy.sparse.linalg import expm
from scipy.linalg import sqrtm

# initalize global variables
dt = 0.01  # seconds
N = int(input('Enter number of samples : '))  # number of samples
qc = 0.1  # process noise magnitude

z_noise = 1  # measurement noise magnitude

# prior mean
x_0 = np.array([[0.0],  # x position
                [0.0],  # y position
                [0.0],  # x velocity
                [0.0],  # y velocity
                [0.0],  # x acceleration
                [0.0]  # y acceleration
                ])

# prior covariance
p_0 = np.array([[1.0, 0.0, 0.0, 0.0, 0.0, 0.0],
                [0.0, 1.0, 0.0, 0.0, 0.0, 0.0],
                [0.0, 0.0, 1.0, 0.0, 0.0, 0.0],
                [0.0, 0.0, 0.0, 1.0, 0.0, 0.0],
                [0.0, 0.0, 0.0, 0.0, 1.0, 0.0],
                [0.0, 0.0, 0.0, 0.0, 0.0, 1.0]])

# a matrix - continuous time motion model
a = np.array([[0.0, 0.0, 1.0, 0.0, 0.0, 0.0],
              [0.0, 0.0, 0.0, 1.0, 0.0, 0.0],
              [0.0, 0.0, 0.0, 0.0, 1.0, 0.0],
              [0.0, 0.0, 0.0, 0.0, 0.0, 1.0],
              [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
              [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]])

a = expm(dt * a)
gamma = np.array([[0.0], [0.0], [0.0], [0.0], [1.0], [1.0]])

# q matrix - continuous time process noise covariance
q = qc * gamma @ np.transpose(gamma)
q_euler = dt * q

# h matrix - measurement model
h = np.array([[1.0, 0.0, 0.0, 0.0, 0.0, 0.0],
              [0.0, 1.0, 0.0, 0.0, 0.0, 0.0],
              [0.0, 0.0, 0.0, 0.0, 1.0, 0.0],
              [0.0, 0.0, 0.0, 0.0, 0.0, 1.0]])

# r matrix - measurement noise covariance
r = np.array([[0.010, 0.0, 0.0, 0.0],
              [0.0, 0.015, 0.0, 0.0],
              [0.0, 0.0, 0.1, 0.0],
              [0.0, 0.0, 0.0, 0.1]])**2

# main program
def main():
    show_final = int(input('Display final result? (No/Yes = 0/1) : '))
    show_animation = int(
        input('Show animation of filter working? (No/Yes = 0/1) : '))
    if show_animation == 1:
        show_ellipse = int(
            input('Display covariance ellipses in animation? (No/Yes = 0/1) : '))
    else:
        show_ellipse = 0
    x_est = x_0
    p_est = p_0
    x_true_cat = np.array([x_0[0, 0], x_0[1, 0]])
    x_est_cat = np.array([x_0[0, 0], x_0[1, 0]])
    z_cat = np.array([x_0[0, 0], x_0[1, 0]])
    for i in range(N):
        z, x_true = gen_measurement(i)
        if i == (N - 1) and show_final == 1:
            show_final_flag = 1
        else:
            show_final_flag = 0
        postpross(x_true, x_true_cat, x_est, p_est,
                  x_est_cat, z_cat, z, show_animation, show_ellipse, show_final_flag)
        x_est, p_est = kalman_filter(x_est, p_est, z)
        x_true_cat = np.vstack((x_true_cat, np.transpose(x_true[0:2])))
        z_cat = np.vstack((z_cat, np.transpose(z[0:2])))
        x_est_cat = np.vstack((x_est_cat, np.transpose(x_est[0:2])))
    print('KF Over')

# generate ground truth position x_true and noisy position z
def gen_measurement(i):
    x_true = np.array([[i], [i], [0.0], [0.0]])
    z = x_true + z_noise * np.random.randn(4, 1)
    return z, x_true

# linear kalman filter prediction step
def linear_prediction(a, x_hat, p_hat, q):
    x_pred = a @ x_hat
    p_pred = a @ p_hat @ np.transpose(a) + q
    return x_pred, p_pred

# linear kalman filter update step
def linear_update(x_hat, p_hat, y, h, r):
    s = h @ p_hat @ np.transpose(h) + r
    k = p_hat @ np.transpose(h) @ np.linalg.pinv(s)
    v = y - h @ x_hat

    x_upd = x_hat + k @ v
    p_upd = p_hat - k @ s @ np.transpose(k)
    return x_upd, p_upd

# linear kalman filter
def kalman_filter(x, p, z):
    x_pred, p_pred = linear_prediction(a, x, p, q_euler)
    x_upd, p_upd = linear_update(x_pred, p_pred, z, h, r)
    return x_upd, p_upd

# postprocessing
def plot_ellipse(x_est, p_est):
    phi = np.linspace(0, 2 * math.pi, 100)
    p_ellipse = np.array(
        [[p_est[0, 0], p_est[0, 1]], [p_est[1, 0], p_est[1, 1]]])
    x0 = 3 * sqrtm(p_ellipse)
    xy_1 = np.array([])
    xy_2 = np.array([])
    for i in range(100):
        arr = np.array([[math.sin(phi[i])], [math.cos(phi[i])]])
        arr = x0 @ arr
        xy_1 = np.hstack([xy_1, arr[0]])
        xy_2 = np.hstack([xy_2, arr[1]])
    plt.plot(xy_1 + x_est[0], xy_2 + x_est[1], 'r')
    plt.pause(0.00001)

def plot_final(x_true_cat, x_est_cat, z_cat):
    fig = plt.figure()
    f = fig.add_subplot(111)
    f.plot(x_true_cat[0:, 0], x_true_cat[0:, 1], 'r', label='True Position')
    f.plot(x_est_cat[0:, 0], x_est_cat[0:, 1], 'b', label='Estimated Position')
    f.plot(z_cat[0:, 0], z_cat[0:, 1], '+g', label='Noisy Measurements')
    f.set_xlabel('x [m]')
    f.set_ylabel('y [m]')
    f.set_title('Linear Kalman Filter - Constant Acceleration Model')
    f.legend(loc='upper left', shadow=True, fontsize='large')
    plt.grid(True)
    plt.show()

def plot_animation(x_true, x_est, z):
    plt.plot(x_true[0], x_true[1], '.r')
    plt.plot(x_est[0], x_est[1], '.b')
    plt.plot(z[0], z[1], '+g')
    plt.grid(True)
    plt.pause(0.001)

def postpross(x_true, x_true_cat, x_est, p_est, x_est_cat, z_cat, z, show_animation, show_ellipse, show_final_flag):
    if show_animation == 1:
        plot_animation(x_true, x_est, z)
        if show_ellipse == 1:
            plot_ellipse(x_est[0:2], p_est)
    if show_final_flag == 1:
        plot_final(x_true_cat, x_est_cat, z_cat)

main()


"""
Procrustes analysis with Python
https://en.wikipedia.org/wiki/Procrustes_analysis
"""

# Get [proper] multi-dimensional array support
import numpy as np

# Ordinary Procrustes Analysis (alignment to reference)
def opa(a, b):

	# Compute centroid (ori_avg/ori_len)
	aT = a.mean(0)
	bT = b.mean(0)

	# Translate point cloud around own origin (0)
	A = a - aT
	B = b - bT

	# Compute quadratic mean of point cloud
	aS = np.sum(A * A)**.5
	bS = np.sum(B * B)**.5

	# Get scale invariant unit vector (ori/ori_rms=0_to_1_rmsd_norm_coords)
	A /= aS
	B /= bS

	# Compute the covariance matrix from escalar product of sets
	C = np.dot(B.T, A)

	# Decompose point cloud into orthonormal bases (U, V being U transposed as in 'np.fliplr(np.rot90(U))') and singular values (S/_)
	U, _, V = np.linalg.svd(C)

	# Compute optimal rotation matrix from unitary matrices (left U and transposed right V singular vectors)
	aR = np.dot(U, V)

	# Enforce clockwise rotation matrix by disabling svd 'best-fit' reflections (the UV matrix R determinant should always be +1)
	if np.linalg.det(aR) < 0:

		# Flip sign on second row
		V[1] *= -1

		# Rebuild rotation matrix (no need to rerun if condition skipped)
		aR = np.dot(U, V)

	# Get scale multiplier factor so we can zoom in mutable shape into reference (rotation is already computed so we can leave the norm space)
	aS = aS / bS

	# Compute translation distance from mutable shape to reference (aT - bT_fitted_to_a_space)
	aT = aT - (bT.dot(aR) * aS)

	# Rotate mutable B unto fixed A (svd helps but the method for optimal rotation matrix U is Kabsch's)
	B_ = B.dot(aR)

	# Compute Procrustes distance from RMSD (as per Kabsch 'A-BR' instead of dummy 'A-B')
	aD = (np.sum((A - B_)**2) / len(a))**.5

	# Output full rst transformation tuple and Kabsch RMSD
	return aR, aS, aT, aD

# Generalized Procrustes Analysis (mean shape)
def gpa(v, n=-1):

	# Compute unknown mean shape prototype or fit everything to nth shape
	p = avg(v) if n < 0 else v[n]

	# Get shape collection length
	l = len(v)

	# Initialize storage arrays (one per shape)
	r, s, t, d = np.ndarray((4, l), object)

	# Get procrustes components for every shape against the reference (first one)
	for i in range(l):
		r[i], s[i], t[i], d[i] = opa(p, v[i])

	# Return per-shape transforms collection (including average shape and disparity)
	return r, s, t, d

# Average shape from n point clouds
def avg(v):

	# Make a shape collection copy (we don't want to overwrite the original data) while making sure is a well-formatted one (numpy)
	v = np.copy(v)

	# Get shape collection length
	l = len(v)

	# Initialize storage arrays (one per shape) with type-agnostic lists for flexible assignment
	R, S, T = [list(np.zeros(l)) for _ in range(3)]

	# Avg 1/2 (sum): Iterate every possible match between shapes
	for i, j in np.ndindex(l, l):

		# Compute transform components for every permutation
		r, s, t, _ = opa(v[i], v[j])

		# Add up [A]ngle (scalar) from all [R]otations (matrix) carried against every other shape
		R[j] += np.arccos(min(1, max(-1, np.trace(r[:1])))) * np.sign(r[1][0])

		# Get combined scale and translation values for every shape
		S[j] += s
		T[j] += t

	# Avg 2/2 (div): Apply average transformations to shapes
	for i in range(l):

		# Average rotation 'theta' angle for every shape and rebuild rotation matrix
		a = R[i] / l
		r = [np.cos(a), -np.sin(a)], [np.sin(a), np.cos(a)]

		# Transform shape collection (so we can compute the prototype average from it)
		v[i] = v[i].dot(r) * (S[i] / l) + (T[i] / l)

	# Return components and shape prototype (useful as virtual reference) from 'procrustes mean' (shapes are optimally fitted so this is a safe estimation)
	return v.mean(0)


import numpy as np
import matplotlib.pyplot as plt
from matplotlib.backends.backend_agg import FigureCanvasAgg as FigureCanvas
from scipy.spatial import Voronoi
import math
# https://stackoverflow.com/questions/48253800/shading-a-map-according-to-data-for-a-set-of-coordinates

def generate_voronoi_diagram(width, height, sparse_locations, sparse_data):
    '''
    sparse_locations should be list of centers with associated measurement locations (so for example one row could be [10,24])
    sparse_data should be measurements at sensor locations
    '''
    arr = np.zeros((width, height))
    imgx,imgy = width, height
    num_cells=np.shape(sparse_locations)[0]

    nx = sparse_locations[:,0]
    ny = sparse_locations[:,1]
    nr = list(range(num_cells))
    ng = nr
    nb = nr

    for y in range(imgy):
        for x in range(imgx):
            dmin = math.hypot(imgx-1, imgy-1)
            j = -1
            for i in range(num_cells):
                d = math.hypot(nx[i]-x, ny[i]-y)
                if d < dmin:
                    dmin = d
                    j = i
            arr[x, y] = sparse_data[j]

    plt.imshow(arr)
    plt.show()

    return arr

def generate_weighted_voronoi_diagram(width, height, sparse_locations, sparse_data):
    '''
    sparse_locations should be list of centers with associated measurement locations (so for example one row could be [10,24])
    sparse_data should be measurements at sensor locations
    '''
    arr = np.zeros((width, height))
    imgx,imgy = width, height
    num_cells=np.shape(sparse_locations)[0]

    nx = sparse_locations[:,0]
    ny = sparse_locations[:,1]
    nr = list(range(num_cells))
    ng = nr
    nb = nr

    weighted_sparse_data = sparse_data/np.max(sparse_data)

    for y in range(imgy):
        for x in range(imgx):
            dmin = math.hypot(imgx-1, imgy-1)
            j = -1
            for i in range(num_cells):
                d = math.hypot(nx[i]-x, ny[i]-y)
                if d < weighted_sparse_data[i]*dmin:
                    dmin = d
                    j = i
            arr[x, y] = sparse_data[j]

    plt.imshow(arr)
    plt.show()

    return arr

if __name__ == '__main__':
    # get random points
    np.random.seed(1234)

    # Using other (slower method)
    sparse_locations = np.random.randint(256,size=(15,2)) # 15 sensors
    sparse_data = np.random.uniform(low=0.0, high=1.0, size=(15,))

    input_array = generate_voronoi_diagram(256,256,sparse_locations,sparse_data)
    input_array = generate_weighted_voronoi_diagram(256,256,sparse_locations,sparse_data)


from numpy import *
from sigmoid import sigmoid

def computeCost(theta, X, y):
	# Computes the cost of using theta as the parameter
	# for logistic regression.

    m = X.shape[0] # number of training examples
    J = 0

    for i in range(m):
        val = dot(X[i,:], theta)
        J += -y[i]*log(sigmoid(val))-(1-y[i])*log(1-sigmoid(val))

    J /= m

	# =============================================================

    return J


'''
The np.random module and Bernoulli trials
100xp
You can think of a Bernoulli trial as a flip of a possibly biased coin. Specifically, each coin flip has a probability p
 of landing heads (success) and probability 1−p
 of landing tails (failure). In this exercise, you will write a function to perform n Bernoulli trials, perform_bernoulli_trials(n, p), which returns the number of successes out of n Bernoulli trials, each of which has probability p of success. To perform each Bernoulli trial, use the np.random.random() function, which returns a random number between zero and one.

Instructions
-Define a function with signature perform_bernoulli_trials(n, p).
    -Initialize to zero a variable n_success the counter of Trues, which are Bernoulli trial successes.
    -Write a for loop where you perform a Bernoulli trial in each iteration and increment the number of success if the result is True. Perform n iterations by looping over range(n).
    -To perform a Bernoulli trial, choose a random number between zero and one using np.random.random(). If the number you chose is less than p, increment n_success (use the += 1 operator to achieve this).
    -The function returns the number of successes n_success.
'''
import numpy as np

def perform_bernoulli_trials(n, p):
    """Perform n Bernoulli trials with success probability p
    and return number of successes."""
    # Initialize number of successes: n_success
    n_success = 0

    # Perform trials
    for i in range(n):
        # Choose random number between zero and one: random_number
        random_number = np.random.random()

        # If less than p, it's a success so add one to n_success
        if random_number < p:
            n_success += 1

    return n_success

print(perform_bernoulli_trials(10000, 0.65))


###________________________ 2nd-Order-Free-Response ________________________###

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

matplotlib.rcParams.update({'font.size': 22})

##__________ Functions for dumping characteristic cases __________##
def undumped(wn, x0, x_dot0, t):
    return x0 * np.cos(wn * t) + x_dot0 / wn * np.sin(wn * t)

def underdumped(xi, wn, x0, x_dot0, t):

    wd = wn * np.sqrt(1 - xi**2)

    x = (
        np.exp(-xi*wn*t) *
        (
            x0 * np.cos(wd * t) +
            (xi*wn*x0 + x_dot0) / wd * np.sin(wd * t)
        )
    )
    return x

def critically_dumped(wn, x0, x_dot0, t):
    return np.exp(-wn*t) * (x0 * (1 + wn * t) + x_dot0 * t)

def overdumped(xi, wn, x0, x_dot0, t):
    a = xi * wn + wn * np.sqrt(xi**2 - 1)
    b = xi * wn - wn * np.sqrt(xi**2 - 1)

    x = (
        (a * x0 + x_dot0) / (a - b) * np.exp(-b*t) -
        (b * x0 + x_dot0) / (a - b) * np.exp(-a*t)
        )
    return x

##__________ 2nd order free response __________##
def plot_2order_free_resp(xi, wn, x0, x_dot0):

    t = np.linspace(0, 10, 1000)

    # Calculate selected
    if np.isclose(xi, 0):
        x = undumped(wn, x0, x_dot0, t)
    elif np.isclose(xi, 1):
        x = critically_dumped(wn, x0, x_dot0, t)
    elif 0 < xi < 1:
        x = underdumped(xi, wn, x0, x_dot0, t)
    else:
        x = overdumped(xi, wn, x0, x_dot0, t)

    # Plot selected
    plt.figure(figsize=(15, 8))
    plt.plot(t, x, ls='-', lw=2, c='#b30000', label=f"$\\xi={xi:.2f}$")

    # Critical dumping
    x = critically_dumped(wn, x0, x_dot0, t)
    plt.plot(t, x, ls='--', lw=4, alpha=0.7, c='#02818a', label="$\\xi=1.00$")

    # No dumping
    x = undumped(wn, x0, x_dot0, t)

    # Formatting
    plt.title("$\omega_n$ = {}, $x_0$ = {}, $\\dotx_0$ = {}".format(wn, x0, x_dot0))
    plt.plot(t, x, ls='-', lw=4, alpha=0.5, c='#0570b0', label="$\\xi=0.00$")

    plt.ylim(-1, 1)
    plt.xlim(0,10)
    plt.grid()
    plt.xlabel('t')
    plt.ylabel('x')
    plt.legend(loc='upper right')

for xii in [1.5, 1.85, 2.5]:
# for xii in [0.00, 1.00, 2.50]:
    for wnn in [1, 3, 5]:
        for x00 in [-1, -0.5, 0]:
            for x_dot00 in [1.5, 3.6, 4.5]:
                plot_2order_free_resp(xii, wnn, x00, x_dot00)


import numpy as np
import matplotlib.pyplot as plt
import matplotlib.gridspec as gridspec  # for unequal plot boxes
import scipy.optimize

# define fitting function
def GaussPolyBase(f, a, b, c, P, fp, fw):
    return a + b*f + c*f*f + P*np.exp(-0.5*((f-fp)/fw)**2)

# read in spectrum from data file
# f=frequency, s=signal, ds=s uncertainty
f, s, ds = np.loadtxt("Spectrum.txt", skiprows=4, unpack=True)

# initial guesses for fitting parameters
a0, b0, c0 = 60., -3., 0.
P0, fp0, fw0 = 80., 11., 2.

# fit data using SciPy's Levenberg-Marquart method
nlfit, nlpcov = scipy.optimize.curve_fit(GaussPolyBase,
                f, s, p0=[a0, b0, c0, P0, fp0, fw0], sigma=ds)

# unpack fitting parameters
a, b, c, P, fp, fw = nlfit
# unpack uncertainties in fitting parameters from diagonal
# of covariance matrix
da, db, dc, dP, dfp, dfw = \
          [np.sqrt(nlpcov[j,j]) for j in range(nlfit.size)]

# create fitting function from fitted parameters
f_fit = np.linspace(0.0, 25., 128)
s_fit = GaussPolyBase(f_fit, a, b, c, P, fp, fw)

# Calculate residuals and reduced chi squared
resids = s - GaussPolyBase(f, a, b, c, P, fp, fw)
redchisqr = ((resids/ds)**2).sum()/float(f.size-6)

# Create figure window to plot data
fig = plt.figure(1, figsize=(8,8))
gs = gridspec.GridSpec(2, 1, height_ratios=[6, 2])

# Top plot: data and fit
ax1 = fig.add_subplot(gs[0])
ax1.plot(f_fit, s_fit)
ax1.errorbar(f, s, yerr=ds, fmt='or', ecolor='black')
ax1.set_xlabel('frequency (THz)')
ax1.set_ylabel('absorption (arb units)')
ax1.text(0.7, 0.95, 'a = {0:0.1f}$\pm${1:0.1f}'
         .format(a, da), transform = ax1.transAxes)
ax1.text(0.7, 0.90, 'b = {0:0.2f}$\pm${1:0.2f}'
         .format(b, db), transform = ax1.transAxes)
ax1.text(0.7, 0.85, 'c = {0:0.2f}$\pm${1:0.2f}'
         .format(c, dc), transform = ax1.transAxes)
ax1.text(0.7, 0.80, 'P = {0:0.1f}$\pm${1:0.1f}'
         .format(P, dP), transform = ax1.transAxes)
ax1.text(0.7, 0.75, 'fp = {0:0.1f}$\pm${1:0.1f}'
         .format(fp, dfp), transform = ax1.transAxes)
ax1.text(0.7, 0.70, 'fw = {0:0.1f}$\pm${1:0.1f}'
         .format(fw, dfw), transform = ax1.transAxes)
ax1.text(0.7, 0.60, '$\chi_r^2$ = {0:0.2f}'
         .format(redchisqr),transform = ax1.transAxes)
ax1.set_title('$s(f) = a + bf + cf^2 + P\, e^{-(f-f_p)^2/2f_w^2}$')

# Bottom plot: residuals
ax2 = fig.add_subplot(gs[1])
ax2.errorbar(f, resids, yerr = ds, ecolor="black", fmt="ro")
ax2.axhline(color="gray", zorder=-1)
ax2.set_xlabel('frequency (THz)')
ax2.set_ylabel('residuals')
ax2.set_ylim(-20, 20)
ax2.set_yticks((-20, 0, 20))

plt.show()
plt.savefig("FitSpectrum.pdf")


__author__ = 'dixon'
from scipy.stats import norm
import scipy.stats as st
import numpy as np
import pandas as pd
from math import *

def bsformula( callput, S0, K, r, T, sigma, q=0.):
    """
    :param callput: Indicates if the option is a Call or Put option
    :param S0: Stock price
    :param K: Strike price
    :param r: Risk-free rate
    :param T: Time to expiration
    :param sigma: Volatility
    :param q: Dividend rate
    :return: Value of the option, its Delta, its Vega
    """
    d1=(log(float(S0)/K)+((r-q)+sigma*sigma/2.)*T)/(sigma*sqrt(T))
    d2=d1-sigma*sqrt(T)
    if callput==1:
        optionValue=S0*exp(-q*T)*norm.cdf(d1)-K*exp(-r*T)*norm.cdf(d2)
        delta=norm.cdf(d1)
    elif callput==-1:
        optionValue=K*exp(-r*T)*norm.cdf(-d2)-S0*exp(-q*T)*norm.cdf(-d1)
        delta=-norm.cdf(-d1)

    vega=S0*sqrt(T)*norm.pdf(d1)
    return optionValue,delta,vega

#Density derivative
def dpdf(x):
    v = 1
    return -x*np.exp(-x**2/(2.0*v**2))/(v**3*np.sqrt(2.0*np.pi))

def generalizedGreeks(cp, s, k, t, v, rfInt, divInt):
        """ Price an option using the Black-Scholes model.
        cp: +1/-1 for call/put
        s: initial stock price
        k: strike price
        t: expiration time
        v: volatility
        rfInt: deterministic risk-free rate integrated between 0 and t
        divInt: deterministic dividend integrated between 0 and t
        """

        d1 = (np.log(s/k)+(rfInt-divInt+0.5*v*v*t))/(v*np.sqrt(t))
        d2 = d1 - v*np.sqrt(t)

        Nd1 = st.norm.cdf(cp*d1)
        Nd2 = st.norm.cdf(cp*d2)

        discountFactor = np.exp(-rfInt)
        forwardFactor = np.exp(-divInt)
        avgDiv = divInt/t
        avgRf = rfInt/t

        optprice = (cp*s*forwardFactor*Nd1) - (cp*k*discountFactor*Nd2)

        delta = cp*Nd1
        vega  = s*np.sqrt(t)*st.norm.pdf(d1)
        delta_k = -s*forwardFactor*Nd1/(v*np.sqrt(t)*k) - cp*discountFactor*Nd2 + k*discountFactor*Nd2/(v*np.sqrt(t)*k)

        gamma_k = s*forwardFactor/((v*np.sqrt(t)*k)**2)*(Nd1*v*np.sqrt(t) + cp*dpdf(cp*d1)) - k*discountFactor/((v*np.sqrt(t)*k)**2)*(Nd2*v*np.sqrt(t) + cp*dpdf(cp*d2)) +  2.0*discountFactor*Nd2/(v*np.sqrt(t)*k)

        dd1_dt = (avgRf-avgDiv+0.5*v*v)/(v*np.sqrt(t)) - 0.5*(np.log(s/k)+(rfInt-divInt+0.5*v*v*t))/(v*v*t**(3/2))
        dd2_dt = dd1_dt - 0.5*v/np.sqrt(t)
        delta_T = avgRf*cp*k*discountFactor*Nd2 - avgDiv*cp*s*forwardFactor*Nd1 + s*forwardFactor*Nd1*dd1_dt- k*discountFactor*Nd2*dd2_dt

        return optprice, delta, vega, delta_k, gamma_k, delta_T

#Change of variable for constant discount and dividend short rate
def changeOfVariable_BS(s, t, q, r):

  factorPrice = np.exp( - q*t )

  divSpread = (q-r)*t

  factorStrike = np.exp( divSpread )
  adjustedStrike = np.multiply(s, factorStrike)
  return adjustedStrike, factorPrice

def bs_price(cp, s, k, rf, t, v, div):
        """ Price an option using the Black-Scholes model.
        cp: +1/-1 for call/put
        s: initial stock price
        k: strike price
        t: expiration time
        v: volatility
        rf: risk-free rate
        div: dividend
        """

        d1 = (np.log(s/k)+(rf-div+0.5*v*v)*t)/(v*np.sqrt(t))
        d2 = d1 - v*np.sqrt(t)

        optprice = (cp*s*np.exp(-div*t)*st.norm.cdf(cp*d1)) - (cp*k*np.exp(-rf*t)*st.norm.cdf(cp*d2))

        return optprice

def bissectionMethod(S0, r, q, implied_vol0, maturity, Strike, refPrice, epsilon, optionType):
    calibratedSigma = implied_vol0
    #Call black-scholes price function for initial value
    priceBS = bs_price(optionType , S0, Strike, r, maturity, calibratedSigma, q)
    sigmaUp = 2.0
    sigmaInf = epsilon
    lossSerie = []

    priceMax = bs_price(optionType ,S0, Strike, r, maturity, sigmaUp, q)
    if priceMax < refPrice:
        return priceMax, sigmaUp, pd.Series(lossSerie)

    priceMin = bs_price(optionType ,S0, Strike, r, maturity, sigmaInf, q)
    if priceMin > refPrice:
        return priceMin, sigmaInf, pd.Series(lossSerie)

    #Stop the optimization when the error is less than epsilon
    while(abs(priceBS - refPrice) > epsilon):
        #Update the upper bound or the lower bound
        #by comparing calibrated price and the target price
        if priceBS < refPrice :
            sigmaInf = calibratedSigma
        else :
            sigmaUp = calibratedSigma
        #Update calibratedSigma
        calibratedSigma = (sigmaUp + sigmaInf) / 2
        #Update calibrated price
        priceBS = bs_price(optionType ,S0, Strike, r, maturity, calibratedSigma, q)
        #Record the calibration error for this step
        lossSerie.append(abs(priceBS - refPrice))

    return priceBS, calibratedSigma, pd.Series(lossSerie)

def vectorizedImpliedVolatilityCalibration(S0,
                                           bootstrap,
                                           maturity,
                                           strike,
                                           optionType,
                                           marketPrice):
    epsilon = 1e-6
    discountRate = bootstrap.discountIntegral(maturity) / maturity
    dividendRate = bootstrap.dividendIntegral(maturity) / maturity
    data = np.vstack([np.array(maturity),  np.array(strike), np.array(optionType),
                      np.array(marketPrice), np.array(discountRate), np.array(dividendRate)]).T
    dataSet = pd.DataFrame(data, columns = ["Maturity", "Strike", "OptionType", "Price", "r", "q"])

    calibFunction = lambda x : bissectionMethod(S0, x["r"], x["q"], 0.2,
                                                x["Maturity"],
                                                x["Strike"],
                                                x["Price"],
                                                epsilon,
                                                x["OptionType"])[1]
    impVol = dataSet.apply(calibFunction, axis=1)

    return np.ravel(impVol)


"""

Path Planning with 4 point Beizer curve

author: Atsushi Sakai(@Atsushi_twi)

"""

import scipy.special
import numpy as np
import matplotlib.pyplot as plt
import math

show_animation = True

def calc_4point_bezier_path(sx, sy, syaw, ex, ey, eyaw, offset):
    D = math.sqrt((sx - ex)**2 + (sy - ey)**2) / offset
    cp = np.array(
        [[sx, sy],
         [sx + D * math.cos(syaw), sy + D * math.sin(syaw)],
         [ex - D * math.cos(eyaw), ey - D * math.sin(eyaw)],
         [ex, ey]])

    traj = []
    for t in np.linspace(0, 1, 100):
        traj.append(bezier(3, t, cp))
    P = np.array(traj)

    return P, cp

def bernstein(n, i, t):
    return scipy.special.comb(n, i) * t**i * (1 - t)**(n - i)

def bezier(n, t, q):
    p = np.zeros(2)
    for i in range(n + 1):
        p += bernstein(n, i, t) * q[i]
    return p

def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
    u"""
    Plot arrow
    """

    if not isinstance(x, float):
        for (ix, iy, iyaw) in zip(x, y, yaw):
            plot_arrow(ix, iy, iyaw)
    else:
        plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw),
                  fc=fc, ec=ec, head_width=width, head_length=width)
        plt.plot(x, y)

def main():
    start_x = 10.0  # [m]
    start_y = 1.0  # [m]
    start_yaw = math.radians(180.0)  # [rad]

    end_x = -0.0  # [m]
    end_y = -3.0  # [m]
    end_yaw = math.radians(-45.0)  # [rad]
    offset = 3.0

    P, cp = calc_4point_bezier_path(
        start_x, start_y, start_yaw, end_x, end_y, end_yaw, offset)

    assert P.T[0][0] == start_x, "path is invalid"
    assert P.T[1][0] == start_y, "path is invalid"
    assert P.T[0][-1] == end_x, "path is invalid"
    assert P.T[1][-1] == end_y, "path is invalid"

    if show_animation:
        plt.plot(P.T[0], P.T[1], label="Bezier Path")
        plt.plot(cp.T[0], cp.T[1], '--o', label="Control Points")
        plot_arrow(start_x, start_y, start_yaw)
        plot_arrow(end_x, end_y, end_yaw)
        plt.legend()
        plt.axis("equal")
        plt.grid(True)
        plt.show()

def main2():
    start_x = 10.0  # [m]
    start_y = 1.0  # [m]
    start_yaw = math.radians(180.0)  # [rad]

    end_x = -0.0  # [m]
    end_y = -3.0  # [m]
    end_yaw = math.radians(-45.0)  # [rad]
    offset = 3.0

    for offset in np.arange(1.0, 5.0, 1.0):
        P, cp = calc_4point_bezier_path(
            start_x, start_y, start_yaw, end_x, end_y, end_yaw, offset)
        assert P.T[0][0] == start_x, "path is invalid"
        assert P.T[1][0] == start_y, "path is invalid"
        assert P.T[0][-1] == end_x, "path is invalid"
        assert P.T[1][-1] == end_y, "path is invalid"

        if show_animation:
            plt.plot(P.T[0], P.T[1], label="Offset=" + str(offset))

    if show_animation:
        plot_arrow(start_x, start_y, start_yaw)
        plot_arrow(end_x, end_y, end_yaw)
        plt.legend()
        plt.axis("equal")
        plt.grid(True)
        plt.show()

if __name__ == '__main__':
    main()
    #  main2()


from .ArrayUtils import *
import numpy as np
import math

def generateDistanceMatrix(width, height):
    """
	Generates a matrix specifying the distance of each point in a window to its centre.
	"""

    # Determine the coordinates of the exact centre of the window
    originX = width / 2
    originY = height / 2

    # Generate the distance matrix
    distances = zerosFactory((height, width), dtype=np.float)
    for index, val in np.ndenumerate(distances):
        y, x = index
        distances[(y, x)] = math.sqrt(
            math.pow(x - originX, 2) + math.pow(y - originY, 2)
        )

    return distances


"""
    Computation to machine precision of the ground state energy (per site)
of 1D Transverse Field Ising Model (TFIM) for various values of the parameter g.
"""
import numpy as np
import scipy.integrate as integrate

def E0_sum(N, g):
    ks = np.linspace(-(N-1)/2, (N-1)/2, num=N) / N * 2 * np.pi
    epsilon_ks = 2 * np.sqrt(g**2 - 2 * g * np.cos(ks) + 1)
    E0 = - 0.5 * epsilon_ks.sum() / N
    return E0

def E0_integrate(g):
    f = lambda k: -1. / (2. * np.pi) * np.sqrt(g**2 - 2 * g * np.cos(k) + 1)
    E0, error = integrate.quadrature(f, -np.pi, np.pi, tol=1e-16, rtol=1e-16, maxiter=2000)
    return E0, error

if __name__ == "__main__":
    gs = np.array([0.80, 0.85, 0.90, 0.95, 1.00, 1.05, 1.10, 1.15, 1.20])
    E0s_sum = np.empty(gs.size)
    for i in range(gs.size):
        g = gs[i]
        print("g =", g)
        upper = 10 if g == 1.00 else 8
        for exponent in range(2, upper):
            E0 = E0_sum(10 ** exponent, g)
            print("N = 10^%d, \t\tE0 = %.15f" % (exponent, E0))
            if exponent == upper - 1:
                E0s_sum[i] = E0
        #print("E0_integrate = %.15f" % E0_integrate(g)[0])

    filename = "datas/E0_sum.npz"
    np.savez(filename, gs=gs, E0s=E0s_sum)


import numpy as np

def softmax(X):
    """ Softmax function """
    return np.exp(X) / np.sum(np.exp(X), axis=0)

def main():
    """ Main """
    scores = np.array([1.0, 2.0, 3.0])
    print softmax(scores)

    scores = np.array([[1, 2, 3, 6],
                      [2, 4, 5, 6],
                      [3, 8, 7, 6]])
    print softmax(scores)

if __name__ == "__main__":
    main()


import math
import matplotlib.pyplot as plt
import time
from abc import ABC, abstractmethod
from scipy import integrate
import numpy as np

from numericalODE import RK4, Euler, CalcError
from functions import fun_exp, lot_volt_f, lot_volt_g, to_1dim, exact

#warunki początkowe
t = 0.0
t_stop = 80.0

dt = 0.1
dt_e = 0.1

y = 1.0
y0 = [30,4]
func = fun_exp

params = {
    'a': 0.2,   # współczynnik nardzin ofiar
    'b': 0.04,   # czestotliwosc umierania ofiar
    'c': 0.04,   # przyrost drapieżników
    'd': 0.5    # umieranie drapieżnikow
    }
# tuple - do Scipy
params_ =[v for _, v in params.items()]
params_= tuple(params_)

# obliczenia
euler_1 = Euler(fun_exp, y, t, t_stop, dt)
rk4_1 = RK4(fun_exp, y, t, t_stop, dt)

# Lotki-Volterry, ukad równań różniczkowych
Ye_2dim, Te_2dim = euler_1.solve_system(f=lot_volt_f, g=lot_volt_g, u=y0, params=params)
Yrk_2dim, Trk_2dim = rk4_1.solve_system(f=lot_volt_f, g=lot_volt_g, u=y0, params=params)
preyRK, predRK = to_1dim(Yrk_2dim)
preyE, predE = to_1dim(Ye_2dim)

# wizualizacja
plt.figure(figsize=(15,8))
plt.plot(Te_2dim, Ye_2dim, label="E")
plt.plot(Trk_2dim, Yrk_2dim, label="RK")
plt.xlabel("czas")
plt.ylabel("liczba zwierząt")
plt.legend()
plt.show()

# równanie różniczkowe
t_stop = 40
euler_2 = Euler(fun_exp, y, t, t_stop, dt)
rk4_2 = RK4(fun_exp, y, t, t_stop, dt)

Ye, Te = euler_2.solve()
Yrk, Trk = rk4_2.solve()
exact_rk = exact(dt,t_stop,y)

#wizualizacja
plt.figure(figsize=(16,10))
plt.plot(Te,Ye, color="red", label=f"Euler dt: {dt_e}")
plt.plot(Trk,exact_rk, label="exact")
plt.xlabel("czas",fontsize=13)
plt.ylabel("y",fontsize=13)
plt.legend()
plt.show()


# Copyright 2021 Christian Schneider Pedersen <cspe18@student.aau.dk>, Helene Bach Vistisen, Julian Teule, Mikkel Filt Bengtson, Victor Büttner <beer@0x23.dk>
#
# SPDX-License-Identifier: Beerware OR MIT
import math
from typing import List
import numpy as np
from numpy.typing import ArrayLike

def gen_bits(nbits: int) -> List[int]:
    """
    example:
    >>> gen_bits(2)
    [0, 1, 3, 2]
    """
    if nbits == 1:
        return [0, 1]
    previous = gen_bits(nbits - 1)
    reverse = reversed(previous)
    # prefix bytes with 1, e.g. 01 becomes 101
    reverse = map(lambda v: (1 << (nbits - 1)) | v, reverse)
    return previous + list(reverse)

class GrayEncoder:

    def __init__(self, M: int) -> None:
        nbits = math.log2(M)
        assert nbits.is_integer(), f"M should be a power of 2, {M} is not"
        self.lookup_encode = np.array(gen_bits(int(nbits)))

        self.lookup_decode = np.empty(len(self.lookup_encode))
        for i, element in enumerate(self.lookup_encode):
            self.lookup_decode[element] = i

    def encode(self, a: ArrayLike) -> ArrayLike:
        return self.lookup_encode[a]

    def decode(self, a: ArrayLike) -> ArrayLike:
        return self.lookup_decode[a]


import numpy as np

def cal_similarity(v1, v2, similarity='cos'):
    """
    计算余弦相似度
    :param v1: 第一个向量
    :param v2: 第二个向量
    :return: 余弦相似度
    """

    if similarity == 'cos':
        cos = np.dot(v1, v2) / (np.linalg.norm(v1) * np.linalg.norm(v2))
        return cos
    elif similarity == 'Euclidean':
        Euclidean = np.linalg.norm(v2 - v1)
        return Euclidean
    else:
        raise ValueError('mode should be cos or Euclidean')

def cal_similarities(v, v_list, similarity='cos', modify=False):
    '''

    :param v: array,向量
    :param v_list: iter,向量迭代器
    :param similarity: str,'cos' or 'Euclidean',相似度计算方法
    :param modify: bool,是否进行余弦修正
    :return:
    '''
    similarity_all = []
    if modify:
        texts_vec_use = np.array([i for i in v_list if type(i) == np.ndarray])
        texts_vec_mean = texts_vec_use.mean(axis=0)
        for i in v_list:
            # 出现nan则相似度记为-999
            if type(v) != np.ndarray or type(i) != np.ndarray:
                similarity_one = -999.0
            else:
                similarity_one = cal_similarity(v1=v - texts_vec_mean,
                                                v2=i - texts_vec_mean,
                                                similarity=similarity)
            similarity_all.append(similarity_one)
    else:
        for i in v_list:
            # 出现nan则相似度记为-999
            if type(v) != np.ndarray or type(i) != np.ndarray:
                similarity_one = -999.0
            else:
                similarity_one = cal_similarity(v1=v, v2=i, similarity=similarity)
            similarity_all.append(similarity_one)
    return similarity_all

if __name__ == '__main__':
    x = np.array([1, 2])
    y = np.array([1, 1])
    print('cos:', cal_similarity(x, y, similarity='cos'))
    print('Euclidean:', cal_similarity(x, y, similarity='Euclidean'))


# Project Euler 11 - Largest product in a grid
# GitHub: urtuba

import numpy as np

def load_2d_array(filename: str) -> np.ndarray:
    '''
    Reads a txt file into 2d numpy integer array.

    :param filename: name of the txt file.
    :return: 2d numpy array
    '''
    with open(filename, 'r') as file:
        array_2d = np.loadtxt(filename, dtype=np.int32, ndmin=2)
    return array_2d

def largest_product_of_quarts(array_2d: np.ndarray) -> int:
    '''
    Returns largest product of four adjacent integers in the given 2D array.
    Adjacency can occur horizontally, vertically or diagonally.

    :param array_2d: 2d array, n x n.
    :return: largest product of 4 adjacent numbers
    '''

    # hidden helper functions
    def _max_quart_product_of_rows(array_2d: np.ndarray) -> int:
        dim = len(array_2d)
        maximum = 0
        for row in array_2d:
            for i in range(dim-3):
                temp = np.product(row[i:i+4])
                if maximum < temp: maximum = temp
        return maximum

    def _max_quart_product_of_cols(array_2d: np.ndarray) -> int:
        dim = len(array_2d)
        maximum = 0
        for col in array_2d.transpose():
            for i in range(dim-3):
                temp = np.product(col[i:i+4])
                if maximum < temp: maximum = temp
        return maximum

    def _max_quart_product_of_diags(array_2d: np.ndarray) -> int:
        dim = len(array_2d)
        diags = [array_2d.diagonal(i) for i in range(-dim+1, dim)]
        diags.extend([array_2d[:,::-1].diagonal(i) for i in range(-dim+1, dim)])
        diags = list(filter(lambda n: len(n) > 3, diags))

        maximum = 0
        for diag_line in diags:
            diag_len = len(diag_line)
            for i in range(diag_len-3):
                temp = np.product(diag_line[i:i+4])
                if maximum < temp: maximum = temp
        return maximum

    # actual function body
    rows_max = _max_quart_product_of_rows(array_2d)
    cols_max = _max_quart_product_of_cols(array_2d)
    diags_max = _max_quart_product_of_diags(array_2d)
    return max(rows_max, cols_max, diags_max)

if __name__ == '__main__':
    matrix = load_2d_array('s011-input.txt')
    result = largest_product_of_quarts(matrix)
    print (result)


from functools import partial
from typing import Optional

import numpy as np

# found module but no type hints or library stubs
from scipy.special import binom  # type: ignore

def fdiff_coef(d: float, window: int) -> np.ndarray:
    """Returns sequence of coefficients in fracdiff operator.

    Parameters
    ----------
    d : float
        Order of differentiation.
    window : int
        Number of terms.

    Returns
    -------
    coef : numpy.array, shape (window,)
        Coefficients in fracdiff operator.

    Examples
    --------
    >>> fdiff_coef(0.5, 4)
    array([ 1.    , -0.5   , -0.125 , -0.0625])
    >>> fdiff_coef(1.0, 4)
    array([ 1., -1.,  0., -0.])
    >>> fdiff_coef(1.5, 4)
    array([ 1.    , -1.5   ,  0.375 ,  0.0625])
    """
    return (-1) ** np.arange(window) * binom(d, np.arange(window))

def fdiff(
    a: np.ndarray,
    n: float = 1.0,
    axis: int = -1,
    prepend: Optional[np.ndarray] = None,
    append: Optional[np.ndarray] = None,
    window: int = 10,
    mode: str = "same",
) -> np.ndarray:
    """Calculate the `n`-th differentiation along the given axis.

    Extention of ``numpy.diff`` to fractional differentiation.

    Parameters
    ----------
    a : array_like
        The input array.
    n : float, default=1.0
        The order of differentiation.
        If ``n`` is an integer, returns the same output with ``numpy.diff``.
    axis : int, default=-1
        The axis along which differentiation is performed, default is the last axis.
    prepend : array_like, optional
        Values to prepend to ``a`` along axis prior to performing the differentiation.
        Scalar values are expanded to arrays with length 1 in the direction of axis and
        the shape of the input array in along all other axes.
        Otherwise the dimension and shape must match ``a`` except along axis.
    append : array_like, optional
        Values to append.
    window : int, default=10
        Number of observations to compute each element in the output.
    mode : {"same", "valid"}, default="same"
        "same" (default) :
            At the beginning of the time series,
            return elements where at least one coefficient of fracdiff is used.
            Output size along ``axis`` is :math:`L_{\\mathrm{in}}`
            where :math:`L_{\\mathrm{in}}` is the length of ``a`` along ``axis``
            (plus the lengths of ``append`` and ``prepend``).
            Boundary effects may be seen at the at the beginning of a time-series.
        "valid" :
            Return elements where all coefficients of fracdiff are used.
            Output size along ``axis`` is
            :math:`L_{\\mathrm{in}} - \\mathrm{window} + 1` where
            where :math:`L_{\\mathrm{in}}` is the length of ``a`` along ``axis``
            (plus the lengths of ``append`` and ``prepend``).
            Boundary effects are not seen.

    Returns
    -------
    fdiff : numpy.ndarray
        The fractional differentiation.
        The shape of the output is the same as ``a`` except along ``axis``.

    Examples
    --------
    This returns the same result with ``numpy.diff`` for integer `n`.

    >>> from fracdiff import fdiff
    >>> a = np.array([1, 2, 4, 7, 0])
    >>> (np.diff(a) == fdiff(a)).all()
    True
    >>> (np.diff(a, 2) == fdiff(a, 2)).all()
    True

    This returns fractional differentiation for noninteger `n`.

    >>> fdiff(a, 0.5, window=3)
    array([ 1.   ,  1.5  ,  2.875,  4.75 , -4.   ])

    Mode "valid" returns elements for which all coefficients are convoluted.

    >>> fdiff(a, 0.5, window=3, mode="valid")
    array([ 2.875,  4.75 , -4.   ])
    >>> fdiff(a, 0.5, window=3, mode="valid", prepend=[1, 1])
    array([ 0.375,  1.375,  2.875,  4.75 , -4.   ])

    Differentiation along desired axis.

    >>> a = np.array([[  1,  3,  6, 10, 15],
    ...               [  0,  5,  6,  8, 11]])
    >>> fdiff(a, 0.5, window=3)
    array([[1.   , 2.5  , 4.375, 6.625, 9.25 ],
           [0.   , 5.   , 3.5  , 4.375, 6.25 ]])
    >>> fdiff(a, 0.5, window=3, axis=0)
    array([[ 1. ,  3. ,  6. , 10. , 15. ],
           [-0.5,  3.5,  3. ,  3. ,  3.5]])
    """
    if mode == "full":
        mode = "same"
        raise DeprecationWarning("mode 'full' was renamed to 'same'.")

    if isinstance(n, int) or n.is_integer():
        prepend = np._NoValue if prepend is None else prepend  # type: ignore
        append = np._NoValue if append is None else append  # type: ignore
        return np.diff(a, n=int(n), axis=axis, prepend=prepend, append=append)

    if a.ndim == 0:
        raise ValueError("diff requires input that is at least one dimensional")

    a = np.asanyarray(a)
    # Mypy complains:
    # fracdiff/fdiff.py:135: error: Module has no attribute "normalize_axis_index"
    axis = np.core.multiarray.normalize_axis_index(axis, a.ndim)  # type: ignore
    dtype = a.dtype if np.issubdtype(a.dtype, np.floating) else np.float64

    combined = []
    if prepend is not None:
        prepend = np.asanyarray(prepend)
        if prepend.ndim == 0:
            shape = list(a.shape)
            shape[axis] = 1
            prepend = np.broadcast_to(prepend, tuple(shape))
        combined.append(prepend)

    combined.append(a)

    if append is not None:
        append = np.asanyarray(append)
        if append.ndim == 0:
            shape = list(a.shape)
            shape[axis] = 1
            append = np.broadcast_to(append, tuple(shape))
        combined.append(append)

    if len(combined) > 1:
        a = np.concatenate(combined, axis)

    if mode == "valid":
        D = partial(np.convolve, fdiff_coef(n, window).astype(dtype), mode="valid")
        a = np.apply_along_axis(D, axis, a)
    elif mode == "same":
        # Convolve with the mode 'full' and cut last
        D = partial(np.convolve, fdiff_coef(n, window).astype(dtype), mode="full")
        s = tuple(
            slice(a.shape[axis]) if i == axis else slice(None) for i in range(a.ndim)
        )
        a = np.apply_along_axis(D, axis, a)
        a = a[s]
    else:
        raise ValueError("Invalid mode: {}".format(mode))

    return a


# -*- coding: utf-8 -*-
"""
Created on Tue Dec  7 20:49:22 2021

@author: Oliver

"""
import numpy as np
import sympy as sp
import math
import matplotlib.pyplot as plt
w_0=250
E=200000
I=300000000
delta_x =125 # mm
L = 3000 #mm

y = np.linspace(0,3000,24000)
def theta_x(x):
    return w_0/(120*E*I*L) *(-5*x**4 +6*L**2*x**2 - L**4)

dydx = theta_x(0)

"E*I*dthetadx = M(x)"
"dMdx = V(x)"
"dVdx = -w(x)"

def calculate_dx (a, b, n):
    return (b-a)/float(n)

def rect_rule (f, a, b, n):
    total = 0.0
    dx = calculate_dx(a, b, n)
    for k in range (0, n):
        total = total + f((a + (k*dx)))
    return dx*total
print(rect_rule(theta_x, 0, 1.5, 24))

deflection_values = []

for i in y:
    deflection_values.append(rect_rule(theta_x,0,i,24))
plt.title("THIS IS THE GRAPH")
plt.plot(y,deflection_values)
plt.show()

# DERIVATIVES NOW
z = sp.Symbol('z')
x = sp.Symbol('x')
f = sp.Function('f')(x)
f = w_0/(120*E*I*L) *(-5*x**4 +6*L**2*x**2 - L**4)
M_x = sp.diff(f)/(E*I)
v = M_x.evalf(subs = {x:1.5})

print(f'The Moment at midpoint is: {v}')
V_x = sp.diff(M_x)
w_x = sp.diff(V_x)
for i in y:
    if i==3:
        print(0)
        break
    print(w_x.evalf(subs = {x:i}))

print(f'Moment is given by the following equation: {M_x}')
print(f'Shear is given by the following equation: {V_x}')
print(f'Load is given by the following equation: {V_x}')

# NEXT QUESTION
# SPHERICAL TANK HAS A CIRCULAR ORIFICE IN ITS BOTTOM THROUGH
# WHICH THE LIQUID FLOWS OUT
# inputs

# functions that returns dy/dx
# i.e. the equation we want to solve: dy/dx = - y
def model(y,x):
    C=0.55
    r_ori = 0.3
    A=(r_ori/2)**2*math.pi
    g=9.81
    r=1.5
    dydx = -(C*A*(2*g*y)**0.5)/(2*math.pi*r*y-math.pi*y**2)
    return dydx

# initial conditions
x0 = 0
y0 = 2.75
# total solution interval
x_final = 100
# step size
h = 0.1
# ------------------------------------------------------

# ------------------------------------------------------
# Euler method

# number of steps
n_step = math.ceil(x_final/h)

# Definition of arrays to store the solution
y_eul = np.zeros(n_step+1)
x_eul = np.zeros(n_step+1)

# Initialize first element of solution arrays
# with initial condition
y_eul[0] = y0
x_eul[0] = x0

# Populate the x array
for i in range(n_step):
    x_eul[i+1]  = x_eul[i]  + h

# Apply Euler method n_step times
for i in range(n_step):
    # compute the slope using the differential equation
    slope = model(y_eul[i],x_eul[i])
    # use the Euler method
    y_eul[i+1] = y_eul[i] + h * slope
    if y_eul[i+1] < 0:
        break
    # print( x_eul[i+1], y_eul[i+1])
# ------------------------------------------------------

# ------------------------------------------------------
# plot results
plt.plot(x_eul, y_eul , 'b.-',)
plt.xlabel('time (s)')
plt.ylabel('y(x)')
plt.show()
# ------------------------------------------------------

# FINAL QUESTION OF THE TUTORIAL

ff,zz,EE,II,LL,zz = sp.symbols('ff,zz,EE,II,LL,zz')

z = 30 #HEIGHT AT THE END WHICH IS EQUAL TO L
f=60
E=1.25*10**8
I=0.05
L=30

g = ff*(LL-zz)**2/(2*EE*II)

Integral_1 = sp.integrate(g,zz)
Integral_2 = sp.integrate(Integral_1,zz)
print(Integral_1)
print(Integral_1.subs([(zz,z),(ff,f),(EE,E),(II,I),(LL,L)]))
print(Integral_2)
print(Integral_2.subs([(zz,z),(ff,f),(EE,E),(II,I),(LL,L)]))


"""Tools for maximum likelihood estimation associated with Lomb-Scargle"""
import numpy as np

def design_matrix(t, frequency, dy=None, bias=True, nterms=1):
    """Compute the Lomb-Scargle design matrix at the given frequency

    This is the matrix X such that the periodic model at the given frequency
    can be expressed :math:`\\hat{y} = X \\theta`.

    Parameters
    ----------
    t : array_like, shape=(n_times,)
        times at which to compute the design matrix
    frequency : float
        frequency for the design matrix
    dy : float or array_like (optional)
        data uncertainties: should be broadcastable with `t`
    bias : bool (default=True)
        If true, include a bias column in the matrix
    nterms : int (default=1)
        Number of Fourier terms to include in the model

    Returns
    -------
    X : ndarray, shape=(n_times, n_parameters)
        The design matrix, where n_parameters = bool(bias) + 2 * nterms
    """
    t = np.asarray(t)
    assert t.ndim == 1
    assert np.isscalar(frequency)

    if nterms == 0 and not bias:
        raise ValueError("cannot have nterms=0 and no bias")

    if bias:
        cols = [np.ones_like(t)]
    else:
        cols = []

    for i in range(1, nterms + 1):
        cols.append(np.sin(2 * np.pi * i * frequency * t))
        cols.append(np.cos(2 * np.pi * i * frequency * t))
    XT = np.vstack(cols)

    if dy is not None:
        XT /= dy

    return np.transpose(XT)

def periodic_fit(t, y, dy, frequency, t_fit,
                 center_data=True, fit_bias=True, nterms=1):
    """Compute the Lomb-Scargle model fit at a given frequency

    Parameters
    ----------
    t, y, dy : float or array_like
        The times, observations, and uncertainties to fit
    frequency : float
        The frequency at which to compute the model
    t_fit : float or array_like
        The times at which the fit should be computed
    center_data : bool (default=True)
        If True, center the input data before applying the fit
    fit_bias : bool (default=True)
        If True, include the bias as part of the model
    nterms : int (default=1)
        The number of Fourier terms to include in the fit

    Returns
    -------
    y_fit : ndarray
        The model fit evaluated at each value of t_fit
    """
    if dy is None:
        dy = 1

    t, y, dy = np.broadcast_arrays(t, y, dy)
    t_fit = np.asarray(t_fit)
    assert t.ndim == 1
    assert t_fit.ndim == 1
    assert np.isscalar(frequency)

    if center_data:
        w = dy ** -2.0
        y_mean = np.dot(y, w) / w.sum()
        y = (y - y_mean)
    else:
        y_mean = 0

    X = design_matrix(t, frequency, dy=dy, bias=fit_bias, nterms=nterms)
    theta_MLE = np.linalg.solve(np.dot(X.T, X),
                                np.dot(X.T, y / dy))

    X_fit = design_matrix(t_fit, frequency, bias=fit_bias, nterms=nterms)

    return y_mean + np.dot(X_fit, theta_MLE)


import numpy as np
from pydrake.autodiffutils import AutoDiffXd

from pydrake.solvers.mathematicalprogram import MathematicalProgram

def cos(theta):
  return AutoDiffXd.cos(theta)
def sin(theta):
  return AutoDiffXd.sin(theta)

def landing_constraint(vars):
  '''
  Impose a constraint such that if the ball is released at final state xf,
  it will land a distance d from the base of the robot
  '''
  l = 1
  g = 9.81
  constraint_eval = np.zeros((3,), dtype=AutoDiffXd)
  q = vars[:3]
  qdot = vars[3:6]
  t_land = vars[-1]
  pos = np.array([[-l*sin(q[0]) - l*sin(q[0] + q[1]) - l*sin(q[0] + q[1] + q[2])],
                  [-l*cos(q[0]) - l*cos(q[0] + q[1]) - l*cos(q[0] + q[1] + q[2])]])
  vel = np.array([[-l*qdot[2]*cos(q[0] + q[1] + q[2]) + qdot[0]*(-l*cos(q[0]) - l*cos(q[0] + q[1]) - l*cos(q[0] + q[1] + q[2])) + qdot[1]*(-l*cos(q[0] + q[1]) - l*cos(q[0] + q[1] + q[2]))],
                  [l*qdot[2]*sin(q[0] + q[1] + q[2]) + qdot[0]*(l*sin(q[0]) + l*sin(q[0] + q[1]) + l*sin(q[0] + q[1] + q[2])) + qdot[1]*(l*sin(q[0] + q[1]) + l*sin(q[0] + q[1] + q[2]))]])

  # TODO: Express the landing constraint as a function of q, qdot, and t_land
  # constraint_eval[0]: Eq (15)
  # constraint_eval[1]: Eq (16)
  # constraint_eval[2]: Eq (18)

  return constraint_eval

def AddFinalLandingPositionConstraint(prog, xf, d, t_land):

  # TODO: Add the landing distance equality constraint as a system of inequality constraints
  # using prog.AddConstraint(landing_constraint, lb, ub, vars)

  # TODO: Add a constraint that t_land is positive
  pass


import scipy as sc
import scipy.sparse as sparse
import scipy.sparse.linalg
import numpy as np
import pylab as pl
import matplotlib.pyplot as plt
import time

t=time.time()
# Number of internal points
N = 512

# Calculate Spatial Step-Size
h = 1/(N+1.0)

# Create Temporal Step-Size, TFinal, Number of Time-Steps
k = h/2
TFinal = 0.2
NumOfTimeSteps = int(TFinal/k)

# Create grid-points on x axis
x = np.linspace(0,1,N+2)
x = x[1:-1]

# Initial Conditions
u = np.transpose(np.mat(10*np.sin(2*np.pi*x)))

# Second-Derivative Matrix
data = np.ones((3, N))
data[1] = -2*data[1]
diags = [-1,0,1]
L = sparse.spdiags(data,diags,N,N)/(h**2)

# Identity Matrix
I = sparse.identity(N)

# Data for each time-step
data = []

for i in range(NumOfTimeSteps):
    # Solve the System: (I - k/2*L) u_new = (I + k/2*L)*u_old
    A = (I -k/2*L)
    b = ( I + k/2*L )*u
    u = np.transpose(np.mat( sparse.linalg.spsolve(A,b)))
    data.append(u)

print(time.time()-t)
plt.clf()
plt.imshow(np.squeeze(np.asarray(data))) #transforme le format matrix en format array pour imshow
plt.xlabel('x')
plt.ylabel('t (temps)')
plt.colorbar()

#http://jkwiens.com/2010/01/02/finite-difference-heat-equation-using-numpy/