#!/usr/bin/env python3

import csv
import numpy as np
import matplotlib.pyplot as plt
from sklearn.preprocessing import MinMaxScaler
from math import sqrt
import argparse

def		train(mileage, price, learning_rate, n_epochs):
	m = float(len(mileage))
	theta0, theta1 = 0, 0
	for _ in range(n_epochs):
		price_estimate = mileage * theta1 + theta0
		diff = price_estimate - price
		tmp_theta0 = np.sum(diff)*learning_rate/m
		tmp_theta1 = np.sum(diff*mileage)*learning_rate/m
		theta0 -= tmp_theta0
		theta1 -= tmp_theta1
	return theta0, theta1

def		main():
	# Parse arguments
	my_parser = argparse.ArgumentParser(description='Train a linear regression model on a univariate dataset.')
	my_parser.add_argument('Path', metavar='path', type=str, help='The path to the csv file containing the data.')
	my_parser.add_argument('-n', '-Number of epochs', type=int, default=1000)
	my_parser.add_argument('-l', '-Learning rate', type=float, default=0.1)
	args = my_parser.parse_args()
	if (args.n < 1):
		print("The number of epochs must be larger than zero.")
		exit(1)
	if (args.l <= 0):
		print("The learning rate must be larger than zero.")
		exit(1)

	# Read csv file
	try:
		with open(args.Path) as data_file:
			csv_reader = csv.reader(data_file, delimiter=',')
			mileage, price = [], []
			count = 0
			for row in csv_reader:
				if (count):
					mileage.append(int(row[0]))
					price.append(int(row[1]))
				count += 1
	except Exception as e:
		print(f"Can't open {args.Path} or it's not a valid csv file.")
		print(e)
		exit(1)
	mileage = np.array(mileage)
	price = np.array(price)

	# Data normalization
	mileage_scaler = MinMaxScaler()
	mileage = mileage.reshape(-1, 1)
	mileage_norm = mileage_scaler.fit_transform(mileage)

	price_scaler = MinMaxScaler()
	price = price.reshape(-1, 1)
	price_norm = price_scaler.fit_transform(price)

	# Training
	theta0_norm, theta1_norm = train(mileage_norm, price_norm, args.l, args.n)
	predicted_price_norm = theta1_norm * mileage_norm + theta0_norm

	# Rescaling
	predicted_price = price_scaler.inverse_transform(predicted_price_norm)

	# Calculating root mean squared error
	rmse = sqrt(np.sum((predicted_price - price) * (predicted_price - price))/(count - 1))
	print(f"RMSE: {rmse}")

	# Plotting
	plt.scatter(mileage, price)
	plt.plot(mileage, predicted_price, color='red')
	plt.xlabel("Mileage (in Km)")
	plt.ylabel("Price (in Euros)")
	plt.show()

	# Calculating real values of theta0 and theta1
	theta0 = (theta0_norm - (theta1_norm * mileage.min()) / (mileage.max() - mileage.min())) * (price.max() - price.min()) + price.min()
	theta1 = (theta1_norm / (mileage.max() - mileage.min())) * (price.max() - price.min())

	# Saving theta0 and theta1 to a file
	f = open("parameters.txt", 'w')
	f.write(f"{theta0}\n{theta1}\n")

if __name__ == "__main__":
    main()


# A Gabriel's Horn, brought to you by PharaohCola13

import mpl_toolkits.mplot3d.axes3d as p3
import matplotlib.pyplot as plt
from matplotlib import *
from numpy import *
from mpl_toolkits.mplot3d.art3d import *
from matplotlib.animation import *

name = "Gabriel's-Horn"

def shape(fig, alpha, color, edge_c, edge_w, grid, sides, edges, multi_pi, radius, height):
	# Definition of x
	def x_(u, v):
		x = u
		return x

	# Definition of y
	def y_(u, v):
		y = (a * cos(v)) / u
		return y

	# Definition of z
	def z_(u, v):
		z = (a * sin(v)) /u
		return z

	a = radius # changes radius of the entire thing

	h = height

	# Value of the angles
	s = sides
	u = linspace(1, h, s + 1)
	v = linspace(0, 2 * pi, edges)

	u, v = meshgrid(u, v)

	# Symbolic representation
	x = x_(u, v)
	y = y_(u, v)
	z = z_(u, v)

	# Figure Properties
	ax = p3.Axes3D(fig)
	ax.set_facecolor('black')  # Figure background turns black

	# Axis Properties
	plt.axis(grid)  # Turns off the axis grid
	plt.axis('equal')

	# Axis Limits
	ax.set_xlim(-4, 4)
	ax.set_ylim(-4, 4)
	ax.set_zlim(-4, 4)

	# Surface Plot
	horn = ax.plot_surface(x, y, z)

	horn.set_alpha(alpha)  # Transparency of figure
	horn.set_edgecolor(edge_c)  # Edge color of the lines on the figure
	horn.set_linewidth(edge_w)  # Line width of the edges
	horn.set_facecolor(color)  # General color of the figure


from numpy import *

def rmserr( set1, set2 ):
    """Compute and return RMS error for two sets of equal length involving the same set of samples."""
    tot = 0.
    for i in range(len(set1)):
        tot+= (set1[i] - set2[i])**2
    return sqrt(tot/float(len(set1)))

def correl(x,y ):
    """For two data sets x and y of equal length, calculate and return r, the product moment correlation. """
    if len(x)!=len(y):
        print("ERROR: Data sets have unequal length.\n")
        raise LengthError

    #Compute averages (try not to require numerical library)
    avgx=sum(x)/float(len(x))
    avgy=sum(y)/float(len(y))

    #Compute standard deviations
    sigmax_sq=0
    for elem in x:
        sigmax_sq+=(elem-avgx)**2
    sigmax_sq=sigmax_sq/float(len(x))
    sigmay_sq=0
    for elem in y:
        sigmay_sq+=(elem-avgy)**2
    sigmay_sq=sigmay_sq/float(len(y))

    sigmax=sqrt(sigmax_sq)
    sigmay=sqrt(sigmay_sq)

    #Compute numerator of r
    num=0
    for i in range(len(x)):
        num+=(x[i]-avgx)*(y[i]-avgy)
    #Compute denominator of r
    denom=len(x)*sigmax*sigmay

    corr = num/denom
    return corr

def percent_within_half( x, y):
    """Takes two sets, x and y, of equal length, and returns the percentage of values within 0.5 units."""

    diff = x - y
    indices = where( abs(diff) < 0.5 )
    return 100.*float(len(indices[0]) )/float(len(x))


import numpy as np
import math

def makeTranslation(x, y, z):
    """
    return the 4x4 numpy array transpose of the
    3x4
    """
    m = np.eye(4)
    m[:3,3] = x,y,z
    return m

def makeRotationX(theta):
    c = math.cos(theta)
    s = math.sin(theta)
    m = np.eye(4)
    m[1,1] = c
    m[1,2] = -s
    m[2,1] = s
    m[2,2] = c
    return m

def makeRotationY(theta):
    c = math.cos(theta)
    s = math.sin(theta)
    m = np.eye(4)
    m[0,0] = c
    m[0,2] = s
    m[2,0] = -s
    m[2,2] = c
    return m

def makeRotationZ(theta):
    c = math.cos(theta)
    s = math.sin(theta)
    m = np.eye(4)
    m[0,0] = c
    m[0,1] = -s
    m[1,0] = s
    m[1,1] = c
    return m

def makeQuaternionMatrix(x, y, z, w):
    n = math.sqrt(x**2 + y**2 + z**2 + w**2)
    qx = x/n
    qy = y/n
    qz = z/n
    qw = w/n
    m = np.eye(3)
    m[0, 0] =  1 - 2*qy**2 - 2*qz**2
    m[0, 1] = 2*qx*qy - 2*qz*qw
    m[0, 2] = 2*qx*qz + 2*qy*qw
    m[1, 0] = 2*qx*qy + 2*qz*qw
    m[1, 1] = 1 - 2*qx**2 - 2*qz**2
    m[1, 2] = 2*qy*qz - 2*qx*qw
    m[2, 0] = 2*qx*qz - 2*qy*qw
    m[2, 1] = 2*qy*qz + 2*qx*qw
    m[2, 2] = 1 - 2*qx**2 - 2*qy**2
    return m
# end def

def applyQuaternion(coords, m3):
    """ assume we are applying rows of vectors
    [
    [x1, y1, z1],
        ...             * m3.T
    [xn, yn, zn]
    ]
    """
    return np.dot(coords, m3.T)
# end def

def applyGeomStack(coords, stack):
    pass
# end def

def applyTransform(coords, m4):
    """ assume we are applying rows of vectors
    [
    [x1, y1, z1, 1.],
        ...             * m4[:3, :].T
    [xn, yn, zn, 1.]
    ]

    where m4 is the total 4x4 transfomation matrix
    ['n11','n12', 'n13', 'n14',
     'n21', 'n22', 'n23', 'n24',
     'n31', 'n32', 'n33', 'n34',
     'n41', 'n42', 'n43', 'n44'])
    """

    rows, cols = coords.shape
    # print(rows, cols, coords.dtype)
    stacked_coords = np.hstack((coords, np.ones((rows, 1))))
    m4x3 = m4[:3,:].T   # cut off last row so we end up with a (rows , 3) product
    return np.dot(stacked_coords, m4x3)


import math
import numpy as np
from skimage.metrics import structural_similarity

def psnr(label, outputs, max_val = 1.0):
    """
    Computes Peak Signal to Noise Ratio
    PSNR = 20 * log_10(max_val / sqrt(MSE))

    Definitions
        PSNR
            Peak Signal to Noise Ratio (the higher the better)
            https://en.wikipedia.org/wiki/Peak_signal-to-noise_ratio#Definition

        MSE
            Mean Square Error

    Inputs
        :label: <torch.tensor> the ground truth image
        :outputs: <torch.tensor> the resulting super resoluted image
        :max_val: <float> the highest pixel value

    Outputs
        :returns: psnr(label, outputs)
    """
    label = label.cpu().detach().numpy()
    outputs = outputs.cpu().detach().numpy()
    img_diff = outputs - label
    rmse = math.sqrt(np.mean((img_diff) ** 2))
    if rmse == 0:
        return 100
    else:
        PSNR = 20 * math.log10(max_val / rmse)
        return PSNR

def ssim(label, outputs):
    """
    Calculates the Structural Similairty Index Measure
    SSIM = L(x, y)^a C(x, y)^b S(x, y)^c

    Definitions
        SSIM
            Structural Similairty Index Measure (the higher the better)
            https://en.wikipedia.org/wiki/Structural_similarity

    Inputs
        :label: <torch.tensor> the ground truth image
        :outputs: <torch.tensor> the resulting super resoluted image

    Outputs
        :returns: ssim(label, outputs)
    """
    score, loss = structural_similarity(label, outputs, full=True, multichannel=True)
    return score


'''
data_normalization.py
https://aimldl.blog.me/221627895429

https://livebook.manning.com/book/deep-learning-with-python/chapter-3/190
'''
from keras.datasets import boston_housing
import numpy as np

# Load data
(train_data, train_targets), (test_data, test_targets) = boston_housing.load_data()
print(f'train_data.shape = {train_data.shape}' )  # (404, 13)
print(f'test_data.shape = {test_data.shape}' )    # (102, 13)
print( train_data[0] )
#[  1.23247   0.        8.14      0.        0.538     6.142    91.7
#   3.9769    4.      307.       21.      396.9      18.72   ]
print( train_targets[0] )
# 15.2

# Prepare data
#   Normalize the data with respect to the features
mean = train_data.mean( axis=0 )
std  = train_data.std( axis=0 )

# Option 1
x_train = (train_data - mean) / std
x_test  = (test_data - mean) / std

# Option 2
train_data -= mean
train_data /= std
test_data  -= mean
test_data  /= std

print( 'Compare the normalized values.' )
print( x_train[0] )
print( train_data[0] )
#Compare the normalized values.
#[-0.27224633 -0.48361547 -0.43576161 -0.25683275 -0.1652266  -0.1764426
#  0.81306188  0.1166983  -0.62624905 -0.59517003  1.14850044  0.44807713
#  0.8252202 ]
#[-0.27224633 -0.48361547 -0.43576161 -0.25683275 -0.1652266  -0.1764426
#  0.81306188  0.1166983  -0.62624905 -0.59517003  1.14850044  0.44807713
#  0.8252202 ]

print( x_test[0] )
print( test_data[0] )
#[ 1.55369355 -0.48361547  1.0283258  -0.25683275  1.03838067  0.23545815
#  1.11048828 -0.93976936  1.67588577  1.5652875   0.78447637 -3.48459553
#  2.25092074]
#[ 1.55369355 -0.48361547  1.0283258  -0.25683275  1.03838067  0.23545815
#  1.11048828 -0.93976936  1.67588577  1.5652875   0.78447637 -3.48459553
#  2.25092074]


from scipy.stats import lognorm, norm
import numpy as np

def make_normal_from_interval(lb, ub, alpha):
    ''' Creates a normal distribution SciPy object from intervals.

    This function is a helper to create SciPy distributions by specifying the
    amount of wanted density between a lower and upper bound. For example,
    calling with (lb, ub, alpha) = (2, 3, 0.95) will create a Normal
    distribution with 95% density between 2 a 3.

    Args:
        lb (float): Lower bound
        ub (float): Upper bound
        alpha (float): Total density between lb and ub

    Returns:
        scipy.stats.norm

    Examples:
        >>> dist = make_normal_from_interval(-1, 1, 0.63)
        >>> dist.mean()
        0.0
        >>> dist.std()
        1.1154821104064199
        >>> dist.interval(0.63)
        (-1.0000000000000002, 1.0)

    '''
    z = norm().interval(alpha)[1]
    mean_norm = (ub + lb) / 2
    std_norm = (ub - lb) / (2 * z)
    return norm(loc=mean_norm, scale=std_norm)

def make_lognormal_from_interval(lb, ub, alpha):
    ''' Creates a lognormal distribution SciPy object from intervals.

    This function is a helper to create SciPy distributions by specifying the
    amount of wanted density between a lower and upper bound. For example,
    calling with (lb, ub, alpha) = (2, 3, 0.95) will create a LogNormal
    distribution with 95% density between 2 a 3.

    Args:
        lb (float): Lower bound
        ub (float): Upper bound
        alpha (float): Total density between lb and ub

    Returns:
        scipy.stats.lognorm

    Examples:
        >>> dist = make_lognormal_from_interval(2, 3, 0.95)
        >>> dist.mean()
        2.46262863041182
        >>> dist.std()
        0.25540947842844575
        >>> dist.interval(0.95)
        (1.9999999999999998, 2.9999999999999996)

    '''
    z = norm().interval(alpha)[1]
    mean_norm = np.sqrt(ub * lb)
    std_norm = np.log(ub / lb) / (2 * z)
    return lognorm(s=std_norm, scale=mean_norm)

class EmpiricalDistribution:
    def __init__(self, observations, method='sequential'):
        self.observations = np.array(observations)
        self.method = 'sequential'
        self.rvs = (self._sequential_rvs if method == 'sequential' else
                    self._uniform_rvs)

    def _sequential_rvs(self, size):
        assert size <= len(self.observations)
        return self.observations[:size]

    def _uniform_rvs(self, size):
        return np.random.choice(self.observations, size, replace=True)


import numpy as np

class PCA:

    def __init__(self,n_components):
        self.n_components=n_components
        self.components=None
        self.mean=None

    def fit(self,X):
        #mean
        self.mean=np.mean(X,axis=0)
        X=X-self.mean

        #covariance
        cov = np.cov(X.T)
        #eigen values and eigen vectors
        eigenvalues, eigenvectors = np.linalg.eig(cov)

        #sort eigenvectors
        eigenvectors=eigenvectors.T
        idxs=np.argsort(eigenvalues)[::-1]
        eigenvalues=eigenvalues[idxs]
        eigenvectors=eigenvectors[idxs]

        # store first n eigenvectors
        self.components=eigenvectors[0:self.n_components]
        return eigenvalues[0:self.n_components],self.components,self.mean,cov
    def transform(self,X):
        # project data on vector found
        X=X-self.mean
        return np.dot(X,self.components.T)


import numpy as np
from numpy.linalg import inv,norm
import matplotlib.pyplot as plt

def solve_lambda_x(A,y,lambs_exponent=np.arange(-10,5,0.01)):
    '''
    Minimize the Lagrangian L(x,lambda) = || A*x - y||**2 + lambda*||x||**2 based on the idea of Tikhonov Regularization.
    This program is used to roughly estimate the parameters x and the Lagrange multiplier lambda.
    The L-curve method is applied to get the proper Lagrangian multiplier.

    Usage:
    estimate_lamb,estimate_x,log10_residual_norm,log10_solution_norm,curvature = solve_lambda_x(A,y)

    Inputs:
    A -> [float 2d array] Design matrix
    y -> [float array] Measurements

    Parameters:
    lambs_exponent -> [optional, float 3d/4d array, default = np.arange(-10,5,0.01)] Exponent for lambda with base of 10

    Outputs:
    estimate_lamb -> [float] Lagrange multiplier
    estimate_x -> [float array] Estimated parameters
    log10_residual_norm -> [float array] log10(||A*x-y||) with lambda taking 10**lambs_exponent
    log10_solution_norm -> [float array] log10(||x||) with lambda taking 10**lambs_exponent
    curvature -> [float array] curvature of the L-curve, where the ordinate of the curve is log10_solution_norm and the abscissa is log10_residual_norm.

    For more information, please refer to
    (1) [NumPy/SciPy Recipes for Data Science: Regularized Least Squares Optimization](https://www.researchgate.net/publication/274138835_NumPy_SciPy_Recipes_for_Data_Science_Regularized_Least_Squares_Optimization)
    (2) [Choosing the Regularization Parameter](http://www2.compute.dtu.dk/~pcha/DIP/chap5.pdf)
    '''
    np.seterr(divide='ignore',invalid='ignore')
    m = A.shape[1]

    # set a series of Lagrangian multiplier
    log10_residual_norm,log10_solution_norm = [],[]
    lambs = np.float_power(10,lambs_exponent)
    for lamb in lambs:
        x = np.dot(inv(np.dot(A.T,A)+lamb*np.eye(m)),np.dot(A.T,y))
        residual_norm = norm(np.dot(A,x)-y)
        solution_norm = norm(x)
        log10_residual_norm.append(np.log10(residual_norm))
        log10_solution_norm.append(np.log10(solution_norm))
    log10_residual_norm = np.array(log10_residual_norm)
    log10_solution_norm = np.array(log10_solution_norm)

    # calculate the curvature of the L-curve
    g1 = np.gradient(log10_solution_norm, log10_residual_norm)
    g1[np.isnan(g1)] = -np.inf
    g2 = np.gradient(g1,log10_residual_norm)
    g2[np.isnan(g2)] = np.inf
    curvature = np.abs(g2)/(1+g1**2)**1.5
    curvature[np.isnan(curvature)] = 0
    curvature[np.isinf(curvature)] = 0
    index_curvature_max = np.argmax(curvature)
    estimate_lamb = lambs[index_curvature_max]
    estimate_x = np.dot(inv(np.dot(A.T,A)+estimate_lamb*np.eye(m)),np.dot(A.T,y))
    return estimate_lamb,estimate_x,log10_residual_norm,log10_solution_norm,curvature

def L_curve(A,y,visible=None):
    '''
    Minimize the Lagrangian L(x,lambda) = || A*x - y||**2 + lambda*||x||**2 based on the idea of Tikhonov Regularization.
    This program is used to accurately estimate the parameters x and the Lagrange multiplier lambda.
    The final Lagrange multiplier is determained by the L-curve method. The L-curve can be visualized by outputing an image.

    Usage:
    accu_lamb,accu_x = L_curve(A,y)

    Inputs:
    A -> [float 2d array] Design matrix
    y -> [float array] Measurements

    Parameters:
    visible -> [optional, str, default = None] If None, the visualization of L-vurve will be closed. If 'visible', the L-curve will be visualized by outputing an image.

    Outputs:
    accu_lamb -> [float] Lagrange multiplier
    accu_x -> [float array] Estimated parameters

    For more information, please refer to
    (1) [NumPy/SciPy Recipes for Data Science: Regularized Least Squares Optimization](https://www.researchgate.net/publication/274138835_NumPy_SciPy_Recipes_for_Data_Science_Regularized_Least_Squares_Optimization)
    (2) [Choosing the Regularization Parameter](http://www2.compute.dtu.dk/~pcha/DIP/chap5.pdf)
    '''
    m = A.shape[1]

    # Estimate the Lagrange multiplier roughly
    appr_lamb,appr_x,log10_residual_norm,log10_solution_norm,appr_curvature = solve_lambda_x(A,y)

    # Estimate the Lagrange multiplier accurately
    lambs_exponent = np.linspace(np.log10(appr_lamb)-2,np.log10(appr_lamb)+2,2000)
    accu_lamb,accu_x,log10_residual_norm,log10_solution_norm,accu_curvature = solve_lambda_x(A,y,lambs_exponent)

    if visible is not None:
        fig_dir = 'figures/'
        if not os.path.exists(fig_dir): os.makedirs(fig_dir)
        # plot
        plt.clf()
        fig, (ax1, ax2) = plt.subplots(1, 2,dpi=200)
        # make a little extra space between the subplots
        fig.subplots_adjust(wspace=0.4)
        ax1.plot(log10_residual_norm,log10_solution_norm)
        ax1.set_xlabel(r'$\log \parallel A x_{\lambda}-y \parallel_2$')
        ax1.set_ylabel(r'$\log \parallel x_{\lambda} \parallel_2$')
        ax1.set_title('L-Curve')
        ax2.plot(lambs_exponent,accu_curvature)
        ax2.set_xlabel(r'$\log \parallel \lambda \parallel_2$')
        ax2.set_ylabel('Curvature')
        ax2.set_title('curvature of L-Curve')
        plt.savefig(fig_dir+'L-Curve.png')
    return accu_lamb,accu_x


import logging

import numpy as np

def sigmoid(z):
    return 1 / (1 + np.exp(-z))

def propagate(w, b, X, Y):
    m = X.shape[1]

    # forward propagation
    A = sigmoid(np.dot(w.T, X) + b)
    cost = -(1/m) * np.sum(Y*np.log(A) + (1-Y)*np.log(1-A))

    # backward propagation
    dw = (1/m) * np.dot(X, (A - Y).T)
    db = (1/m) * np.sum(A - Y)

    cost = np.squeeze(cost)

    grads = { "dw": dw, "db": db }

    return grads, cost

def optimize(w, b, X, Y, iterations, learning_rate):
    costs = []

    for i in range(iterations):
        grads, cost = propagate(w, b, X, Y)

        w = w - (learning_rate * grads['dw'])
        b = b - (learning_rate * grads['db'])

        if i % 100 == 0:
            logging.info(f"iteration: {i}, cost: {cost}")
            costs.append(cost)

        params = { "w": w, "b": b }

        grads = { "dw": grads["dw"], "db": grads["db"] }

    return params, grads, costs

def predict(w, b, X):
    m = X.shape[1]
    w = w.reshape(X.shape[0], 1)

    Y_prediction = np.zeros((1,m))

    A = sigmoid(np.dot(w.T, X) + b)

    for i in range(A.shape[1]):
        if A[0,i] > 0.5:
            Y_prediction[0, i] = 1

    return Y_prediction


import numpy
from matplotlib import pyplot

def euler_pc(f, x_end, y0, N):
    x, dx = numpy.linspace(0, x_end, N+1, retstep=True)
    y = numpy.zeros((len(y0),N+1))
    y[:,0] = y0
    for n in range(N):
        fn = f(x[n], y[:,n])
        yp = y[:,n] + dx * fn
        y[:,n+1] = y[:,n] + dx / 2 * (fn + f(x[n+1], yp))
    return x, dx, y

def ab2(f, x_end, y0, N):
    x, dx = numpy.linspace(0, x_end, N+1, retstep=True)
    y = numpy.zeros((len(y0),N+1))
    fn = numpy.zeros((len(y0),N+1))
    y[:,0] = y0
    fn[:,0] = f(x[0], y[:,0])
    x_epc, dx_epc, y_epc = euler_pc(f, dx, y0, 1)
    y[:,1] = y_epc[:,1]
    for n in range(1,N):
        fn[:,n] = f(x[n], y[:,n])
        y[:,n+1] = y[:,n] + dx * (3 * fn[:,n] - fn[:,n-1]) / 2
    return x, dx, y

if __name__=="__main__":

    def f_sin(x, y):
        return -numpy.sin(x)
    print("Euler Predictor-Corrector")
    x, dx, y = euler_pc(f_sin, 0.5, [1], 5)
    print("dx=", dx, "y(0.5)=", y[0,-1])
    x, dx, y = euler_pc(f_sin, 0.5, [1], 50)
    print("dx=", dx, "y(0.5)=", y[0,-1])
    Npoints = 5*2**numpy.arange(1,10)
    dx_all = 0.5/Npoints
    errors = numpy.zeros_like(dx_all)
    for i, N in enumerate(Npoints):
        x, dx, y = euler_pc(f_sin, 0.5, [1], N)
        errors[i] = abs(y[0,-1] - numpy.cos(0.5))
        dx_all[i] = dx
    pyplot.figure(figsize=(12,6))
    pyplot.loglog(dx_all, errors, 'kx')
    pyplot.loglog(dx_all, errors[0]*(dx_all/dx_all[0])**2, 'b-',
                  label=r"$\propto \Delta x^2$")
    pyplot.legend(loc='upper left')
    pyplot.xlabel(r"$\Delta x$")
    pyplot.ylabel("Error")
    pyplot.show()

    print("Adams-Bashforth 2")
    x, dx, y = ab2(f_sin, 0.5, [1], 5)
    print("dx=", dx, "y(0.5)=", y[0,-1])
    x, dx, y = ab2(f_sin, 0.5, [1], 50)
    print("dx=", dx, "y(0.5)=", y[0,-1])
    Npoints = 5*2**numpy.arange(1,10)
    dx_all = 0.5/Npoints
    errors = numpy.zeros_like(dx_all)
    for i, N in enumerate(Npoints):
        x, dx, y = ab2(f_sin, 0.5, [1], N)
        errors[i] = abs(y[0,-1] - numpy.cos(0.5))
        dx_all[i] = dx
    pyplot.figure(figsize=(12,6))
    pyplot.loglog(dx_all, errors, 'kx')
    pyplot.loglog(dx_all, errors[0]*(dx_all/dx_all[0])**2, 'b-',
                  label=r"$\propto \Delta x^2$")
    pyplot.legend(loc='upper left')
    pyplot.xlabel(r"$\Delta x$")
    pyplot.ylabel("Error")
    pyplot.show()


# Question 1, Lab 6
# AB Satyaprakash, 180123062

# imports
from sympy.abc import t,y
import numpy as np
import sympy as sp
import pandas as pd
import matplotlib.pyplot as plt

# functions
def getEulerApproximation(f,X,Y,h):
    for i in range(1,Y.shape[0]):
        Y[i]=Y[i-1] + (f.subs({t:X[i-1], y:Y[i-1]})*h)

def getActualValues(g,X):
    for i in range(X.shape[0]):
        Z[i]=g.subs(t,X[i])

def phi(n, k, x, z):

    prod = 1
    for i in range(n):
        if i != k:
            prod = prod * (z - x[i])

    return prod

def lagrangeInterpolation(z, x, fx):

    n = len(x)
    l = np.empty(n)
    for i in range(n):
        l[i] = phi(n,i,x,z)/phi(n,i,x,x[i])

    ans = np.dot(l, fx)

    return ans

# program body
# t belongs to [1,2] and y(1)=-1, with h = 0.05
a, b, h = 1, 2, 0.05
X = np.arange(a,b+h/2,h)
Y = np.zeros(X.shape[0])
Z = np.zeros(X.shape[0])
Y[0] = -1

f = 1/(t**2) - y/t - (y**2) # from question
g = -1/t

getEulerApproximation(f,X,Y,h)
getActualValues(g,X)

print('(a)')
table={'Evaluate':[], 'Approx':[], 'Actual':[], 'Error':[]}

for i in range(X.shape[0]):
    table['Evaluate'].append('y({})'.format(round(X[i],2)))
    table['Approx'].append(Y[i])
    table['Actual'].append(Z[i])
    table['Error'].append(abs(Y[i]-Z[i]))

df = pd.DataFrame(table,columns=['Evaluate', 'Approx', 'Actual','Error'])
print(df)

print('\n(b)')
print('(I)')
x = 1.052
intepolatedVal = lagrangeInterpolation(x,X,Y)
print('Estimated value of y({}) from interpolation ={}'.format(x,intepolatedVal))
print('Actual value of y = {}'.format(g.subs(t,x)))
print('The error between them = {}'.format(abs(intepolatedVal-g.subs(t,x))))

print('(II)')
x = 1.555
intepolatedVal = lagrangeInterpolation(x,X,Y)
print('Estimated value of y({}) from interpolation ={}'.format(x,intepolatedVal))
print('Actual value of y = {}'.format(g.subs(t,x)))
print('The error between them = {}'.format(abs(intepolatedVal-g.subs(t,x))))

print('(III)')
x = 1.978
intepolatedVal = lagrangeInterpolation(x,X,Y)
print('Estimated value of y({}) from interpolation ={}'.format(x,intepolatedVal))
print('Actual value of y = {}'.format(g.subs(t,x)))
print('The error between them = {}'.format(abs(intepolatedVal-g.subs(t,x))))

# plotting the values in the table.
plt.plot(X,Y, label='Euler Approximation')
plt.plot(X,Z, label='Actual value')
plt.xlabel('Value of t')
plt.ylabel('Value of y')
plt.title('Comparision between Euler Approx and Actual values of y')
plt.legend()
plt.show()

Semester 6/MA 322 (Scientific Computing Theory)/Labs/Lab 6/Code/q3.py


#!/usr/bin/python
#   X = GAUSS(N) returns N Legendre points X in (-1,1).
#
#   X, C = GAUSS(N) returns also a vector C of weights for
#   Gauss-Legendre quadrature.
#
#   X, C = GAUSS(N, [A, B]) returns the nodes and weeights
#   for the interval [A, B].

import numpy as np

def gauss(N, dom=[-1,1]):
    x = np.zeros(shape=(N,1))
    for k in range(N):
        x[k] = -np.cos((0.5+k)*np.pi/N)
    tol = 1e-10
    dx = 1 + 0 *x
    # Loop until convergence:
    while np.linalg.norm(dx, np.inf) > tol:
        # Recurrence relation for Legendre polynomials:
        Pm2 = 1
        Pm1 = x
        for n in range(1,N):
            P = ( (2*n+1)*Pm1*x - n*Pm2 ) / (n+1)
            Pm2 = Pm1
            Pm1 = P

        # Derivative.  See NIST (18.9.17)
        dPdx = -N * (x*P - Pm2) / (1 - x**2)

        # Newton step:
        dx = -P / dPdx;
        # Newton update:
        x = x + dx;

    # Weights
    c = 2 / ( (1-x**2) * dPdx**2 );

    # Scale
    a = dom[0];
    b = dom[1];
    x = .5*(b-a)*x + .5*(b+a);
    c = .5*(b-a)*c;

    return (x, c)

if __name__ == "__main__":
    x, c = gauss(5, [-1, 1])
    print 'x = ', x
    print 'c = ', c


import numpy as np

def dot_product(vector1, vector2):
    """ Implement dot product of the two vectors.
    Args:
        vector1: numpy array of shape (x, n)
        vector2: numpy array of shape (n, x)

    Returns:
        out: numpy array of shape (x,x) (scalar if x = 1)
    """
    out = None
    ### YOUR CODE HERE

    if(len(np.shape(vector1))==1):
        vector1.shape=(1,vector1.shape[0]);
    if(len(np.shape(vector2))==1):
        vector2.shape=(1,vector2.shape[0]);

    input_size1=np.shape(vector1);

    row=input_size1[0];

    out = np.ones((row,row));

    for i in range(row):
        for j in range(row):
            x1=vector1[i,];
            x2=vector2.T[j];
            out[i,j]=x1.dot(x2);

    if(np.shape(out)==(1,1)):
        out=out[0,0];

    ### END YOUR CODE
    return out

def matrix_mult(M, vector1, vector2):
    """ Implement (vector1.T * vector2) * (M * vector1)
    Args:
        M: numpy matrix of shape (x, n)
        vector1: numpy array of shape (1, n)
        vector2: numpy array of shape (n, 1)

    Returns:
        out: numpy matrix of shape (1, x)
    """
    out = None
    ### YOUR CODE HERE
    x=vector1.T*vector2;
    y=M*vector1;
    out=x.T.dot(y.T);
    ### END YOUR CODE

    return out

def svd(matrix):
    """ Implement Singular Value Decomposition
    Args:
        matrix: numpy matrix of shape (m, n)

    Returns:
        u: numpy array of shape (m, m)
        s: numpy array of shape (k)
        v: numpy array of shape (n, n)
    """
    u = None
    s = None
    v = None
    ### YOUR CODE HERE
    u,s,v=np.linalg.svd(matrix,1,1);
    ### END YOUR CODE

    return u, s, v

def get_singular_values(matrix, n):
    """ Return top n singular values of matrix
    Args:
        matrix: numpy matrix of shape (m, w)
        n: number of singular values to output

    Returns:
        singular_values: array of shape (n)
    """
    singular_values = None
    u, s, v = svd(matrix)
    ### YOUR CODE HERE
    singular_values=s[0:n];
    ### END YOUR CODE
    return singular_values

def eigen_decomp(matrix):
    """ Implement Eigen Value Decomposition
    Args:
        matrix: numpy matrix of shape (m, )

    Returns:
        w: numpy array of shape (m, m) such that the column v[:,i] is the eigenvector corresponding to the eigenvalue w[i].
    """
    w = None
    v = None
    ### YOUR CODE HERE
    w,v=np.linalg.eig(matrix);
    ### END YOUR CODE
    return w, v

def get_eigen_values_and_vectors(matrix, num_values):
    """ Return top n eigen values and corresponding vectors of matrix
    Args:
        matrix: numpy matrix of shape (m, m)
        num_values: number of eigen values and respective vectors to return

    Returns:
        eigen_values: array of shape (n)
        eigen_vectors: array of shape (m, n)
    """
    w, v = eigen_decomp(matrix)
    eigen_values = []
    eigen_vectors = []
    ### YOUR CODE HERE
    m=np.shape(matrix)[0];
    value_sorted=np.argsort(w);
    eigen_values=w[sorted(value_sorted[-num_values:])];
    eigen_vectors=np.zeros((m,num_values));
    for i in range(m):
        for j in range(num_values):
            eigen_vectors[i,j]=v[i,value_sorted[-1-j]];
    ### END YOUR CODE
    return eigen_values, eigen_vectors


#!/usr/bin/env python
# -*- coding: utf-8 -*-
# @Time     : 2018/11/25 14:27
# @Author   : Iydon
# @File     : 7.3.py

import numpy as np

def jacobi_method(A, b, x0=None):
    """
    A = L(ow) + D(iag) + U(pper)
    |ρ(inv(D)*(L+U))| < 1
    => convergent.
    """
    A_,b_ = A.copy(),b.copy()
    diag  = np.diag(A_)
    shap  = A.shape
    A_ = np.diag(diag) - A_
    if x0 is not None:
        x_ = x0.copy()
    else:
        x_ = np.zeros(b.shape)
    for i in range(shap[0]):
        A_[i,:] /= diag[i]
        b_[i,:] /= diag[i]
    while True:
        yield x_
        x_ = A_*x_ + b_

def gauss_seidel(A, b, x0=None):
    """
    A = L(ow) + D(iag) + U(pper)
    |ρ(inv(D-L)*U)| < 1
    => convergent.
    """
    A_,b_ = A.copy(),b.copy()
    DL_   = np.tril(A_)
    U_    = np.triu(A_, 1)
    if x0 is not None:
        x_ = x0.copy()
    else:
        x_ = np.zeros(b.shape)
    invDL_ = np.linalg.inv(DL_)
    A_ = np.matmul( invDL_, U_ )
    b_ = np.matmul( invDL_, b_ )
    while True:
        yield x_
        x_ = np.matmul(-A_, x_) + b_

A  = np.matrix([[3.,-1,1],[3,6,2],[3,3,7]])
b  = np.matrix([[1.],[0],[4]])
x0 = np.matrix([[0.],[0],[0]])
result = jacobi_method(A, b, x0)
t = 0
for r in result:
    print(t, r.T)
    t += 1
    if t > 10: break

result = gauss_seidel(A, b, x0)
t = 0
for r in result:
    print(t, r.T)
    t += 1
    if t > 10: break


#!/usr/bin/python

'''
mjtsai1974@20180606, v1.0, MLE for https://mjtsai1974.github.io/DevBlog/2018/06/06/bayesian-ml-beyes-to-practice/

http://www.astroml.org/book_figures/chapter3/fig_gaussian_distribution.html

dist = scipy.stats.norm(...)

Where ... should be filled in with the desired distribution parameters Once we have defined the distribution parameters in this way, these distribution objects have many useful methods; for example:

    dist.pmf(x) computes the Probability Mass Function at values x in the case of discrete distributions
    dist.pdf(x) computes the Probability Density Function at values x in the case of continuous distributions
    dist.rvs(N) computes N random variables distributed according to the given distribution
'''

import numpy as np
from scipy.stats import norm
from matplotlib import pyplot as plt

'''
#----------------------------------------------------------------------
# This function adjusts matplotlib settings for a uniform feel in the textbook.
# Note that with usetex=True, fonts are rendered with LaTeX.  This may
# result in an error if LaTeX is not installed on your system.  In that case,
# you can set usetex to False.
from astroML.plotting import setup_text_plots
setup_text_plots(fontsize=8, usetex=False)
'''

# Define the given measured weights and the prior(the most current value)
prior = 14.2
weights = [13.9, 14.1, 17.5]
n = len(weights)

# Define the distributions to be plotted
mu = prior
sample_variance = np.var(weights, ddof=1)
sample_std_deviation = np.sqrt(sample_variance)

population_variance = np.var(weights, ddof=0)
population_std_deviation = np.sqrt(population_variance)

sigma_values = [sample_std_deviation, population_std_deviation]
linestyles = ['-', '--', ':']
colours = ['red', 'blue']
colours_sample = ['black', 'purple', 'green']
x_axis = np.linspace(mu - 2 * np.max(sigma_values), mu + 2 * np.max(sigma_values), 100) #use the

# plot the distributions
fig, ax = plt.subplots(figsize=(5, 3.75))

for sigma, ls, color in zip(sigma_values, linestyles, colours):
    # create a gaussian / normal distribution
    dist = norm(mu, sigma)

    plt.plot(x_axis, dist.pdf(x_axis), ls=ls, c=color,
             label=r'$\mu=%.3f,\ \sigma=%.3f$' % (mu, sigma))

plt.xlim(mu - 2 * np.max(sigma_values), mu + 2 * np.max(sigma_values))
plt.ylim(0, 0.45)

plt.xlabel('$x$')
plt.ylabel(r'$p(x|\mu,\sigma)$')
plt.title('Gaussian Distribution')

plt.legend()
plt.show()

# plot the new distribution v.s. mother N(prior, sample_std_deviation)
fig, ax = plt.subplots(figsize=(5, 3.75))

mother_dist = norm(mu, sample_std_deviation)
weight_sample_sigmas = []

for w in weights:
    weight_sigma = 1/(mother_dist.pdf(w) * np.sqrt(2 * np.pi))
    weight_sample_sigmas.append(weight_sigma)

    print('P({0:5.3f}) of N({1:5.3f},{2:5.3f}) = {3:5.3f} for N({0:5.3f}), the std error {4:5.3f}'.format(w, prior, sample_std_deviation, mother_dist.pdf(w), weight_sigma))

# plot the mother N(prior, sample_std_deviation)
plt.plot(x_axis, mother_dist.pdf(x_axis), ls='-', c='red',
             label=r'$\mu=%.3f,\ \sigma=%.3f$' % (mu, sample_std_deviation))

# plot the new N(w_sample, weight_sigma)
for w, sigma, ls, color in zip(weights, weight_sample_sigmas, linestyles, colours_sample):
    dist = norm(w, sigma)  # create a gaussian / normal distribution

    plt.plot(x_axis, dist.pdf(x_axis), ls=ls, c=color,
             label=r'$\mu=%.3f,\ \sigma=%.3f$' % (w, sigma))

plt.xlim(mu - 2 * np.max(sigma_values), mu + 2 * np.max(sigma_values))
plt.ylim(0, 0.45)

plt.xlabel('$x$')
plt.ylabel(r'$p(x|\mu,\sigma)$')
plt.title('Gaussian Distribution')

plt.legend()
plt.show()

# plot the new distribution v.s. mother N(prior, population_std_deviation)
fig, ax = plt.subplots(figsize=(5, 3.75))

mother_dist = norm(mu, population_std_deviation)
weight_population_sigmas = []

for w in weights:
    weight_sigma = 1/(mother_dist.pdf(w) * np.sqrt(2 * np.pi))
    weight_population_sigmas.append(weight_sigma)

    print('P({0:5.3f}) of N({1:5.3f},{2:5.3f}) = {3:5.3f} for N({0:5.3f}), the std error {4:5.3f}'.format(w, prior, population_std_deviation, mother_dist.pdf(w), weight_sigma))

# plot the mother N(prior, sample_std_deviation)
plt.plot(x_axis, mother_dist.pdf(x_axis), ls='-', c='blue',
             label=r'$\mu=%.3f,\ \sigma=%.3f$' % (mu, population_std_deviation))

# plot the new N(w_sample, weight_sigma)
for w, sigma, ls, color in zip(weights, weight_population_sigmas, linestyles, colours_sample):
    dist = norm(w, sigma)  # create a gaussian / normal distribution

    plt.plot(x_axis, dist.pdf(x_axis), ls=ls, c=color,
             label=r'$\mu=%.3f,\ \sigma=%.3f$' % (w, sigma))

plt.xlim(mu - 2 * np.max(sigma_values), mu + 2 * np.max(sigma_values))
plt.ylim(0, 0.45)

plt.xlabel('$x$')
plt.ylabel(r'$p(x|\mu,\sigma)$')
plt.title('Gaussian Distribution')

plt.legend()
plt.show()

# MLE to ask for possible real weight with N(prior, sample std deviation)
MLE_max = 0
mu_max = 0

mother_dist = norm(mu, sample_std_deviation)

for w, sigma in zip(weights, weight_sample_sigmas):
    dist = norm(w, sigma)

    MLE_now = 1.0

    for j in range(len(weights)):
        MLE_now = dist.pdf(weights[j]) * mother_dist.pdf(w) * MLE_now

    if MLE_now > MLE_max:
        MLE_max = MLE_now
        mu_max = w

print('using N(prior, sample std deviation), the MLE for weight {0: 5.3f}'.format(mu_max))

# MLE to ask for possible real weight with N(prior, population std deviation)
MLE_max = 0
mu_max = 0

mother_dist = norm(mu, population_std_deviation)

for w, sigma in zip(weights, weight_sample_sigmas):
    dist = norm(w, sigma)

    MLE_now = 1.0

    for j in range(len(weights)):
        MLE_now = dist.pdf(weights[j]) * mother_dist.pdf(w) * MLE_now

    if MLE_now > MLE_max:
        MLE_max = MLE_now
        mu_max = w

print('using N(prior, population std deviation), the MLE for weight {0: 5.3f}'.format(mu_max))


import math

import numpy as np

# Calculates Rotation Matrix given euler angles.
def euler_angle_to_rotation_matrix(theta):
    """
    Converts from theta [yaw, pitch, roll] to a rotation matrix
    """

    R_x = np.array(
        [
            [1, 0, 0],
            [0, math.cos(theta[0]), -math.sin(theta[0])],
            [0, math.sin(theta[0]), math.cos(theta[0])],
        ]
    )

    R_y = np.array(
        [
            [math.cos(theta[1]), 0, math.sin(theta[1])],
            [0, 1, 0],
            [-math.sin(theta[1]), 0, math.cos(theta[1])],
        ]
    )

    R_z = np.array(
        [
            [math.cos(theta[2]), -math.sin(theta[2]), 0],
            [math.sin(theta[2]), math.cos(theta[2]), 0],
            [0, 0, 1],
        ]
    )

    R = np.dot(R_z, np.dot(R_y, R_x))

    return R

def rotation_matrix_to_euler_angle(R):
    """
    Converts a rotation matrix to [yaw, pitch, roll]
    """
    yaw = math.atan2(R[1, 0], R[0, 0])
    pitch = math.atan2(-R[2, 0], math.sqrt(R[2, 1] ** 2 + R[2, 2] ** 2))
    roll = math.atan2(R[2, 1], R[2, 2])

    return yaw, pitch, roll

def static_transform_to_extrinsic(transform):
    """
    Calculates the extrinsic matrix for a ROS static_transform_publisher

    Parameters:
    - transform: a list with [x, y, z yaw, pitch, roll]
    """
    assert len(transform) == 6

    x, y, z, yaw, pitch, roll = transform
    rotation = euler_angle_to_rotation_matrix([yaw, pitch, roll])
    translation = np.array([x, y, z]).reshape(-1, 1)
    homogeneous = get_homogeneous_transformation(rotation, translation)
    return homogeneous

def extrinsic_to_static_transform(mat):
    """Converts an extrinsic matrix into rotational and translational components.

    Args:
        mat (np.array): a 4x4 extrinsic matrix

    Returns:
        np.array: a 1x6 np.array of form x, y, z, yaw, pitch, roll
    """
    x, y, z = mat[:3, 3]
    yaw, pitch, roll = rotation_matrix_to_euler_angle(mat)

    return np.array([x, y, z, yaw, pitch, roll])

def get_homogeneous_transformation(rotation, translation):
    """
    Parameters:
    - rotation: a 3x3 rotation np array
    - translation: a 3x1 translation np array
    """
    homogeneous = np.zeros((4, 4))
    homogeneous[-1][-1] = 1
    homogeneous[:3, :3] = rotation
    homogeneous[:3, -1:] = translation
    return homogeneous

def get_relative_transformation(a_to_base, b_to_base):
    """
    Finds the relative transformation from a to b given transformations for both
    relative to base_link.

    See: https://stackoverflow.com/a/55169091/6942666 for more details

    Parameters:
    - a_to_base: 4x4 np.array representing homoegenous transformation of a to base_link
    - b_to_base: 4x4 np.array representing homoegenous transformation of b to base_link
    """

    base_to_a = np.linalg.inv(a_to_base)
    return base_to_a @ b_to_base

if __name__ == "__main__":
    """
    <node pkg="tf" type="static_transform_publisher"
        respawn="true"
        name="camera_static_transform_publisher"
        args="0.5080 0.0 0.1778 0 0.05 0 base_link camera 100"
    />

    <node pkg="tf" type="static_transform_publisher"
        respawn="true"
        name="velodyne_static_transform_publisher"
        args="0.4445 0.0 0.09525 0.0 0.06981 0.0 base_link velodyne 100"
    />

    Camera static transform:
    [[ 0.99875026  0.          0.04997917  0.508     ]
    [ 0.          1.          0.          0.        ]
    [-0.04997917  0.          0.99875026  0.1778    ]
    [ 0.          0.          0.          1.        ]]

    Velodyne static transform:
    [[ 0.99756427  0.          0.06975331  0.4445    ]
    [ 0.          1.          0.          0.        ]
    [-0.06975331  0.          0.99756427  0.09525   ]
    [ 0.          0.          0.          1.        ]]

    Camera relative to Velodyne
    [[ 0.99980379  0.          0.0198087  -0.05929486]
    [ 0.          1.          0.          0.        ]
    [-0.0198087   0.          0.99980379 -0.08562051]
    [ 0.          0.          0.          1.        ]]

    Overall camera to velodyne transform
    Translation (x, y, z): -0.059294861111785835 0.0 -0.08562051124429254
    Rotation (y, p, r): 0.0 0.019810000000000008 0.0
    """

    print("Camera static transform: ")
    # Add values for the camera to base here
    camera_to_base = static_transform_to_extrinsic([0.5080, 0.0, 0.1778, 0, 0.05, 0])
    print(camera_to_base)

    print("Velodyne static transform: ")
    # Add values for the velodyne to base here
    velodyne_to_base = static_transform_to_extrinsic(
        [0.4445, 0.0, 0.09525, 0.0, 0.06981, 0.0]
    )
    print(velodyne_to_base)

    print("Camera relative to Velodyne")
    # Add values for the camera to velodyne here
    camera_to_velodyne = get_relative_transformation(camera_to_base, velodyne_to_base)
    print(camera_to_velodyne)

    print("Overall camera to velodyne transform")
    x, y, z, yaw, pitch, roll = extrinsic_to_static_transform(camera_to_velodyne)
    print("Translation (x, y, z):", x, y, z)
    print("Rotation (y, p, r):", yaw, pitch, roll)