from scipy.stats import multivariate_normal
import pandas as pd
import numpy as np

def generate_bank_customer_data(n_common=200, n_debtors=150, n_good=50, include_labels=True):
    """
    Function to generate bank customers data characterized by income and debt.
    :param int n_common: Number of common customers.
    :param int n_debtors: Number of debtors.
    :param int n_good: Number of good customers.
    :param bool include_labels: Whether to include or not ground truth labels.
    :return: pd.DataFrame with generated data.
    """
    # Parameters
    mu1 = np.array([3, 2.5])
    mu2 = np.array([2, 6])
    mu3 = np.array([6, 5])
    cov1 = np.array([[1.2, -0.2],
                     [-0.2, 1]])
    cov2 = np.array([[0.6, 0],
                     [0, 0.6]])
    cov3 = np.array([[1, 0.9],
                     [0.9, 1]])

    # Ground truth random variables
    p1 = multivariate_normal(mean=mu1, cov=cov1)
    p2 = multivariate_normal(mean=mu2, cov=cov2)
    p3 = multivariate_normal(mean=mu3, cov=cov3)

    # Data generation
    data = np.concatenate([p1.rvs(size=n_common),
                           p2.rvs(size=n_debtors),
                           p3.rvs(size=n_good)],
                          axis=0)
    data = pd.DataFrame({"income": data[:, 0], "debt": data[:, 1]})

    # Include ground truth labels
    if include_labels:
        data["labels"] = np.concatenate([np.zeros((n_common, 1)),
                                         np.ones((n_debtors, 1)),
                                         2 * np.ones((n_good, 1))],
                                        axis=0)

    return data.sample(frac=1).reset_index(drop=True)


import cv2
import numpy as np
img = cv2.imread("imori_noise.jpg")
H,W,C=img.shape

#gray scale
b = img[:,:,0].copy()
g = img[:,:,1].copy()
r = img[:,:,2].copy()

gray = 0.2126 * r + 0.7152 * g + 0.0722 * b #0.2126+0.7152+0.0722 = 1
gray = gray.astype(np.uint8)

#LoGfiltersize
LoGfiltersize=5
s=3
pad=LoGfiltersize//2
gray_=np.zeros((H+pad*2,W+pad*2),dtype=np.float)
gray_[pad:pad+H, pad:pad+W] = gray.copy().astype(np.float)

#What is LoGfilter?
#LoG(x,y) = (x**2 + y**2 - s**2) / (2 * pi * s^6) * np.exp(-(x**2+y**2) / (2*s**2))
#LoGfilter producer
LoGfilter=np.zeros((LoGfiltersize,LoGfiltersize),dtype=np.float)
for x in range (-pad,-pad+LoGfiltersize):
    for y in range(-pad,-pad+LoGfiltersize):
        LoGfilter[x+pad,y+pad]=(x**2 + y**2 - s**2) / (2 * np.pi * s**6) * np.exp(-(x**2+y**2) / (2*s**2))
LoGfilter/=LoGfilter.sum()

#LoGfilter
temp=gray_.copy()

for y in range (H):
    for x in range(W):
            gray_[y+pad][x+pad]= np.sum(LoGfilter * temp[y:y+LoGfiltersize, x:x+LoGfiltersize])

gray_[gray_<0]=0
gray_[gray_>255]=255
out = gray_[pad:pad+H, pad:pad+W].astype(np.uint8)
cv2.imwrite("question19.jpg",out)
cv2.imshow("result", out)
cv2.waitKey(0)
cv2.destroyAllWindows()


import numpy as np

class LinearRegression(object):
	def __init__(self, x = None, y = None):
		super(LinearRegression, self).__init__()
		# Data.
		# X E R(mxn) m=data points ; n=features.
		# Y E R(mx1) m=data points ; single column.
		self.x = x
		self.y = y
		# Weights. W X R(nx1) n=features ; single column.
		self.w = self.initializeWeights(n = x.shape[1])
		# Hyperparameters
		self.epochs = 500
		self.learningRate = 0.01

	def initializeWeights(self, n = None, seed = None):
		if (seed != None):
			np.random.seed(seed)
		return np.random.rand(n).reshape(-1, 1)

	def costFunction(self, hwx = None, deriv = None):
		# Local variables.
		m = self.x.shape[0]
		# Logic
		if (deriv == True):
			jw = (1/m) * np.dot(self.x.T, (hwx - self.y))
		else:
			jw = (1/(2*m)) * np.sum((hwx - self.y)**2)
		return jw

	def train(self):
		# BGD
		for e in range(self.epochs):
			hwx = np.dot(self.x, self.w)
			self.w = self.w - (self.learningRate * self.costFunction(hwx = hwx, deriv = True))
			if (e%10 == 0):
				jw = self.costFunction(hwx = hwx, deriv = False)
				print("INFO: Cost function: ", jw)


import json
import numpy as np
import os
from scipy import stats
from sklearn.metrics import r2_score

class HobStatistics:

    def __init__(self, model_ws, name):
        self._model_ws = model_ws
        self._name = name
        self._input_file = os.path.join(model_ws, name) + '.hob.out'
        self._output_file = os.path.join(model_ws, name) + '.hob.stat'

    def write_files(self):
        with open(self._output_file, 'w') as outfile:
            try:
                result = self.calculate()
            except:
                result = {"error": 'Error in Hob-Calculation.'}
            finally:
                json.dump(result, outfile)

    @staticmethod
    def calculate_npf(x, n):
        a = 0.5
        if x < 11:
            a = 3 / 8

        return stats.norm.ppf((x - a) / (n + 1 - 2 * a))

    def calculate(self):
        if not os.path.isfile(self._input_file):
            return {"error": 'File ' + self._input_file + ' not found.'}

        f = open(self._input_file)

        header = False

        names = []
        observed = []
        simulated = []

        for line in f:
            if line.startswith('#'):
                continue

            if not header:
                header = line.split('"')[1::2]
                continue

            values = line.split()
            simulated.append(float(values[0]))
            observed.append(float(values[1]))
            names.append('_'.join(values[2].split('_')[:-1]))

        simulated = np.array(simulated)
        observed = np.array(observed)

        # Write to statistics object
        statistics = dict(
            observed=list(observed),
            simulated=list(simulated),
            n=len(observed),
            rMax=np.max(np.abs(simulated - observed)),
            rMin=np.min(np.abs(simulated - observed)),
            rMean=np.mean(simulated - observed),
            absRMean=np.mean(np.abs(simulated - observed)),
            sse=stats.sem(simulated - observed),
            rmse=np.sqrt(((simulated - observed) ** 2).mean()),
            R=stats.pearsonr(observed, simulated)[0],
            R2=r2_score(observed, simulated)
        )

        statistics["nrmse"] = statistics["rmse"] / (np.max(observed) - np.min(observed))

        # Plot simulated vs. observed values
        statistics["Z"] = 1.96
        statistics["stdObserved"] = np.std(observed)
        statistics["deltaStd"] = statistics["Z"] * statistics["stdObserved"] / np.sqrt(statistics["n"])

        # Plot (weighted) residuals vs. simulated heads
        statistics["weightedResiduals"] = list(simulated - observed)
        statistics["linRegressSW"] = stats.linregress(simulated, statistics["weightedResiduals"])

        # Check for Normal Distribution¶
        statistics["rankedResiduals"] = list(np.sort(simulated - observed))

        n = statistics["n"]
        npf = np.linspace(1, n, num=n)
        npf = list(map(lambda x, n: self.calculate_npf(x, n), npf, np.ones(n) * n))

        statistics["npf"] = npf
        statistics["linRegressRN"] = stats.linregress(statistics["rankedResiduals"], statistics["npf"])

        return statistics


import numpy as np
import modern_robotics as mr
import math

print("Ex 1: Determina the end-effector zero configuration M")
M = np.array([[1, 0, 0, 3.73],
              [0, 1, 0,    0],
              [0, 0, 1, 2.73],
              [0, 0, 0,    1]])
print(M)
# [[1,0,0,3.73],[0,1,0,0],[0,0,1,2.73],[0,0,0,1]]

print("Ex 2: Determine the screw axes Si")
Ss = np.array([[ 0, 0,    0,     0, 0,     0],
               [ 0, 1,    1,     1, 0,     0],
               [ 1, 0,    0,     0, 0,     1],
               [ 0, 0,    1, -0.73, 0,     0],
               [-1, 0,    0,     0, 0, -3.73],
               [ 0, 1, 2.73,  3.73, 1,     0]])
print(Ss)
# [[0,0,0,0,0,0],[0,1,1,1,0,0],[1,0,0,0,0,1],[0,0,1,-0.73,0,0],[-1,0,0,0,0,-3.73],[0,1,2.73,3.73,1,0]]

print("Ex 3: Determine the screw aes Bi")
Bs = np.array([[   0,     0,    0, 0, 0, 0],
               [   0,     1,    1, 1, 0, 0],
               [   1,     0,    0, 0, 0, 1],
               [   0,  2.73, 3.73, 2, 0, 0],
               [2.73,     0,    0, 0, 0, 0],
               [   0, -2.73,   -1, 0, 1, 0]])
print(Bs)
# [[0,0,0,0,0,0],[0,1,1,1,0,0],[1,0,0,0,0,1],[0,2.73,3.73,2,0,0],[2.73,0,0,0,0,0],[0,-2.73,-1,0,1,0]]

print("Ex 4: Find the end-effector configuration T using FKinSpace")
theta = np.array([-math.pi/2, math.pi/2, math.pi/3, -math.pi/4, 1, math.pi/6])
T_space = np.around(mr.FKinSpace(M, Ss, theta), 2)
print(T_space)
# [[0.5,0.87,0,1],[0.22,-0.13,-0.97,-1.9],[-0.84,0.48,-0.26,-4.5],[0,0,0,1]]

print("Ex 5: Find the end-effector configuration T using FKinBody")
T_body = np.around(mr.FKinBody(M, Bs, theta), 2)
print(T_body)
# [[0.5,0.87,0,1],[0.22,-0.13,-0.97,-1.9],[-0.84,0.48,-0.26,-4.5,],[0,0,0,1]]


import numpy as np

# Inner (or dot) product
a = np.array([1,2])
b = np.array([3,4])
np.inner(a, b)
a.dot(b)

# Outer product
a = np.array([1,2])
b = np.array([3,4])
np.outer(a, b)

# Inverse
m = np.array([[1,2], [3,4]])
np.linalg.inv(m)

# Inner (or dot) product
m = np.array([[1,2], [3,4]])
minv = np.linalg.inv(m)
m.dot(minv)

# Diagonal
m = np.array([[1,2], [3,4]])
np.diag(m)

m = np.array([1,2])
np.diag(m)

# Determinant
m = np.array([[1,2], [3,4]])
np.linalg.det(m)

# Trace - sum of elements of the diagonal
m = np.array([[1,2], [3,4]])
np.diag(m)
np.diag(m).sum()
np.trace(m)

# Transpose
m = np.array([ [1,2], [3,4] ])
m.T

# Gaussian distribution
m = np.random.randn(2,3)
m

# Covariance
X = np.random.randn(100,3)
np.cov(X.T)

# Eigen vectors and values
# For symmetric matrix (m == m.T) and hermitian matrix (m = m.H) we use eigh.
m = np.array([
  [ 0.89761228,  0.00538701, -0.03229084],
  [ 0.00538701,  1.04860676, -0.25001666],
  [-0.03229084, -0.25001666,  0.81116126]])

# The first tuple contains three Eigen values.
# The second tuple contains Eigen vectors stored in columns.
np.linalg.eigh(m)

# Solving linear systems
# The admissions fee at a small far is $1.50 for children an $4.00 for adults.
# On a certain day 2,200 people enter the fair and $5050 is collected.
# How many children and how many adults attended.
#
# Let X1 = number of children
# Let X2 = number of adults
# X1 + X2 = 2200
# 1.5X1 + 4X2 = 5050
a = np.array([ [1,1], [1.5,4] ])
b = np.array( [ 2200, 5050] )
np.linalg.solve(a, b)


# -*- coding: utf8 -*-
import numpy as np
import numpy.linalg as la

# Datos iniciales
k1 = 1000.0
k2 = 2000.0
k3 = 3000.0
P = 5000.0

# Matrices por elemento
K1 = np.array([[k1,-k1],[-k1,k1]])
K2 = np.array([[k2,-k2],[-k2,k2]])
K3 = np.array([[k3,-k3],[-k3,k3]])

# Matriz global
K = np.array([[  K1[0,0],        0,           K1[0,1],                0],
               [      0 ,  K3[0,0],                 0,          K3[0,1]],
               [ K1[1,0],        0,   K1[1,1]+K2[0,0],          K2[0,1]],
               [       0,  K3[1,0],           K2[1,0],  K2[1,1]+K3[1,1]]])

F = np.array([0, 0, 0, P])

# Condiciones de frontera
# Nodos 1 y 2 conocidos -> UX = 0
KS = K[2:,2:]
FS = F[2:]

# Resolviendo
USOL = la.solve(KS, FS)

# Vector de desplazamientos
USOL = np.concatenate(([0,0],USOL))

# Obteniendo las fuerzas nodales
NF = np.dot(K,USOL)

# Presentando los resultados
for nodo in range(4):
    print("%g  UX = %-8.4f    FX = %-8.4f"%(nodo+1, USOL[nodo], NF[nodo]))


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

df = pd.read_excel('mlr02.xls')

X = df.values

plt.scatter(X[:,1], X[:,0])
plt.show()
plt.scatter(X[:,2], X[:,0])
plt.show()
plt.scatter(X[:,1], X[:,2])
plt.show()

df['ones'] = 1 #np.random.rand(11)*100
Y = df['X1']
X = df[['X2','X3','ones']]
X2only = df[['X2','ones']]
X3only = df[['X3','ones']]

def get_r2(X,Y):
    w = np.linalg.solve(np.dot(X.T, X), np.dot(X.T, Y))
    Y_hat = np.dot(X, w)
    d1 = Y - Y_hat
    d2 = Y - Y.mean()
    SS_res = d1.dot(d1)
    SS_tot = d2.dot(d2)
    R2 = 1 - SS_res / SS_tot
    return R2

print("R2 for X2 only: ", get_r2(X2only, Y))
print("R2 for X3 only: ", get_r2(X3only, Y))
print("R2 for X2 and X3: ", get_r2(X, Y))


from matplotlib import pyplot as plt
import numpy as np
import cv2

def scaling(simg, srows, scols):
	pt1 = np.float32([[0.5, 0, 0],
				      [0, 0.5, 0],
				      [0,   0, 1]])

	simg = cv2.cvtColor(simg, cv2.COLOR_BGR2RGB)
	dst = cv2.warpPerspective(simg,pt1,(scols*1,srows*1))

	matplotting(simg, dst)
	cv2.waitKey(0)

def rotation(simg, srows, scols):
	theta = np.radians(30)
	m = np.float32([[np.cos(theta), np.sin(theta), 150],
				    [-np.sin(theta), np.cos(theta), 150],
				    [0,					0,			 1]])

	simg = cv2.cvtColor(simg, cv2.COLOR_BGR2RGB)
	dst = cv2.warpPerspective(simg,m,(srows*1,scols*1))

	matplotting(simg, dst)
	cv2.waitKey(0)

def translation(simg, srows, scols):
	m = np.float32([[1, 0, 150],
				    [0, 1, 150],
				    [0, 0,   1]])

	simg = cv2.cvtColor(simg, cv2.COLOR_BGR2RGB)
	dst = cv2.warpPerspective(simg,m,(srows*1,scols*1))

	matplotting(simg, dst)
	cv2.waitKey(0)

def h_shearing(simg, srows, scols):
	m = np.float32([[1, 0.5, 0],
				    [0,   1, 0],
				    [0,   0, 1]])

	simg = cv2.cvtColor(simg, cv2.COLOR_BGR2RGB)
	dst = cv2.warpPerspective(simg,m,(scols*2,srows*1))

	matplotting(simg, dst)
	cv2.waitKey(0)

def v_shearing(simg, srows, scols):
	m = np.float32([[1,   0, 0],
				    [0.5, 1, 0],
				    [0,   0, 1]])

	simg = cv2.cvtColor(simg, cv2.COLOR_BGR2RGB)
	dst = cv2.warpPerspective(simg,m,(scols*1,srows*2))

	matplotting(simg, dst)
	cv2.waitKey(0)

def matplotting(simg, sdst):

	plt.subplot(121)
	plt.imshow(simg)
	plt.title('Original')

	plt.subplot(122)
	plt.imshow(sdst)
	plt.title('Result')

	plt.show()

def plotting(func):
	img = cv2.imread('D:\\111.jpg',0)
	rows, colms = img.shape

	if func == 1:
		scaling(img, rows, colms)
	elif func == 2:
		rotation(img, rows, colms)
	elif func == 3:
		translation(img, rows, colms)
	elif func == 4:
		h_shearing(img, rows, colms)
	elif func == 5:
		v_shearing(img, rows, colms)
	else:
		print("Wrong input")

print("Please choose which transformation you would like to demonstrate: \n")
print("1.Scaling\n2.Rotation\n3.Translation\n4.Horizantal Shearing\n5.Vertical Shearing\n")
print("Choice: ")
choice = int(input())

plotting(choice)


"""

Functions related to STAAR

"""

import numpy as np
from scipy.stats import cauchy

c = cauchy()

def cct(pvals, weights=None):
    """
    Python port of the CCT function as defined in the STAAR R-package (https://github.com/xihaoli/STAAR/blob/2f67fafec591a45e81a54eca24564b09ce90e252/R/CCT.R)

    An analytical p-value combination method using the Cauchy distribution.

    takes in a numeric vector of p-values, a numeric vector of non-negative weights, and return the aggregated p-value using Cauchy method.

    :param np.ndarray pval: Numpy array containing the p-values
    :param np.ndarray weights: Numpy array containing the weights

    :return: The aggregated p-value
    :rtype: float

    Liu, Y., & Xie, J. (2020). Cauchy combination test: a powerful test with analytic p-value calculation under arbitrary dependency structures.
    Liu, Y., et al. (2019). Acat: A fast and powerful p value combination method for rare-variant analysis in sequencing studies.
    """

    # check for NA
    assert not np.isnan(pvals).any(), 'Error: Cannot have nan in the p-values!'
    # check range
    assert not (((pvals < 0).sum() + (pvals > 1).sum()) > 0), "Error: All p-values must be between 0 and 1"

    # check for p-values that are either exactly 0 or 1
    is_zero = (pvals == 0.).any()
    is_one = (pvals == 1.).any()

    assert not (is_zero & is_one), 'Error: Cannot have both 0 and 1 p-values'

    if is_zero:
        return 0
    if is_one:
        print('Warning: there are p-values that are exactly 1!')
        return 1.

    # check the validity of weights
    if weights is None:
        weights = np.ones_like(pvals) / len(pvals)
    else:
        assert len(weights) == len(pvals), 'Error: length of weights should be the same as that of the p-values!'
        assert not ((weights < 0).any()), 'Error: all weights must be positive!'
        weights /= weights.sum()

    # check if there are very small non-zero p-values
    is_small = pvals < 1e-16
    if not is_small.any():
        cct_stat = (weights * np.tan((0.5 - pvals) * np.pi)).sum()
    else:
        cct_stat = (weights[is_small] / pvals[is_small] / np.pi).sum()
        cct_stat += (weights[~is_small] * np.tan((0.5 - pvals[~is_small]) * np.pi)).sum()

    # check if the test statistic is very large
    if (cct_stat > 1e15):
        pval = (1. / cct_stat) / np.pi
    else:
        pval = c.sf(cct_stat)

    return pval


import numpy as np
import matplotlib.pyplot as plt

x = np.linspace(1, 100, 100)
y1 = np.log(x)
y2 = np.log2(x)

plt.plot(x, y1)
plt.plot(x, y2)

plt.grid(True)
plt.savefig('../../img/question_4_plots/c.png')


import numpy as np
import pandas as pd

def read_file(prob_num):
    df = pd.read_csv("dat/port" + str(prob_num) + ".txt", header=None,
    delimiter="\s+", names=range(3)) # info on assets
    n = int(df[0][0]) # number of assets
    r = df[1: (n + 1)][0].values.reshape(n, 1) # mean of returns
    s = df[1: (n + 1)][1].values.reshape(n, 1) # std. of returns
    df = df.values
    c = np.zeros((n, n))
    for it in np.arange(n,len(df)):
        i, j = int(df[it][0] - 1), int(df[it][1] - 1)
        c[i][j] = c[j][i] = df[it][2] # covariance between asset i, j
    return n, r, s, c

def evaluate(x, r, s, c):
    M = - np.sum(np.dot(x.T, r)) # obj. 1: -1 * mean as return
    V = np.sum(np.dot(x, x.T) * np.dot(s, s.T) * c) # obj. 2: variance as risk
    return M, V

def pf(prob_num):
    pf = np.genfromtxt("dat/portef" + str(prob_num) + ".txt") # points on pf
    M = []
    V = []
    for i in range(len(pf)):
        M += [pf[i][0]]
        V += [pf[i][1]]
    return M, V

def set(instance):
    n, r, s, c = read_file(instance)
    lb, ub = np.zeros((n, 1)), np.ones((n, 1)) # upper and lower bounds
    port = evaluate
    mp, vp = pf(instance)
    return n, r, s, c, lb, ub, port, mp, vp


import cozmo
import math
import sys
import time
import numpy as np

from cmap import *
from gui import *
from utils import *

from random import random as rd, choice
import numpy as np

MAX_NODES = 20000

from go_to_goal_cozmo import *

def step_from_to(node0, node1, limit=20):
    ########################################################################
    # TODO: please enter your code below.
    # 1. If distance between two nodes is less than limit, return node1
    # 2. Otherwise, return a node in the direction from node0 to node1 whose
    #    distance to node0 is limit. Recall that each iteration we can move
    #    limit units at most
    # 3. Hint: please consider using np.arctan2 function to get vector angle
    if get_dist(node0, node1) < limit:
    # 4. Note: remember always return a Node object
        return node1
    else:
        theta = np.arctan2(node1.y - node0.y, node1.x - node0.x)
        # print("{} -> {}".format( node0.coord ,(node0.x + limit * math.cos(theta), node0.y + limit * math.sin(theta)) ))
        return Node((node0.x + limit * math.cos(theta), node0.y + limit * math.sin(theta)))
    ############################################################################

def node_generator(cmap):
    ############################################################################
    # TODO: please enter your code below.
    # 1. Use CozMap width and height to get a uniformly distributed random node
    # 2. Use CozMap.is_inbound and CozMap.is_inside_obstacles to determine the
    #    legitimacy of the random node.
    # 3. Note: remember always return a Node object

    rand_node = None
    while rand_node is None or not cmap.is_inbound(rand_node) or cmap.is_inside_obstacles(rand_node):
        if rd() < 0.05:
            # print("generating a target node")
            target_coord = choice(cmap.get_goals()).coord
        else:
            # print('generate a random node')
            target_coord = (rd()*cmap.width, (rd()*cmap.height))
        rand_node = Node(target_coord)
    #temporary cod below to be replaced
    return rand_node
    ############################################################################

def RRT(cmap, start):
    cmap.add_node(start)
    map_width, map_height = cmap.get_size()
    while (cmap.get_num_nodes() < MAX_NODES):
        ########################################################################
        # TODO: please enter your code below.
        # 1. Use CozMap.get_random_valid_node() to get a random node. This
        #    function will internally call the node_generator above
        # 2. Get the nearest node to the random node from RRT
        # 3. Limit the distance RRT can move
        # 4. Add one path from nearest node to random node
        #

        #temporary code below to be replaced
        rand_node = cmap.get_random_valid_node()
        nearest_node = None
        min_dist = float('inf')
        for node in cmap.get_nodes():
            dist = get_dist(node, rand_node)
            if dist < min_dist:
                min_dist = dist
                nearest_node = node
        new_node = step_from_to(nearest_node, rand_node)

        if cmap.is_inbound(new_node) and not cmap.is_inside_obstacles(new_node):
            if not cmap.is_collision_with_obstacles((nearest_node, new_node)):
                cmap.add_node(new_node)
                cmap.add_path(nearest_node, new_node)
        ########################################################################

        # time.sleep(0.01)
        if cmap.is_solved():
            break

    path = cmap.get_path()
    smoothed_path = cmap.get_smooth_path()

    if cmap.is_solution_valid():
        print("A valid solution has been found :-) ")
        print("Nodes created: ", cmap.get_num_nodes())
        print("Path length: ", len(path))
        print("Smoothed path length: ", len(smoothed_path))
    else:
        print("Please try again :-(")

async def dock_with_cube(robot: cozmo.robot.Robot):

    print("Cozmo is waiting until he sees a cube.")
    cube = await robot.world.wait_for_observed_light_cube()
    await robot.dock_with_cube(cube, approach_angle=cozmo.util.degrees(0), num_retries=2).wait_for_completed()
    await robot.set_lift_height(1).wait_for_completed()
    await robot.turn_in_place(cozmo.util.degrees(45),
                                speed=cozmo.util.Angle(degrees=60)).wait_for_completed()

async def init(robot: cozmo.robot.Robot):

    await robot.set_lift_height(0).wait_for_completed()
    await robot.set_head_angle(cozmo.util.degrees(0)).wait_for_completed()

async def CozmoPlanning(robot: cozmo.robot.Robot):
    # Allows access to map and stopevent, which can be used to see if the GUI
    # has been closed by checking stopevent.is_set()
    global stopevent, cmap
    ########################################################################
    # TODO: please enter your code below.
    # Description of function provided in instructions. Potential pseudcode is below

    #assume start position is in cmap and was loaded from emptygrid.json as [50, 35] already
    #assume start angle is 0
    #Add final position as goal point to cmap, with final position being defined as a point that is at the center of the arena
    #you can get map width and map weight from cmap.get_size()
    map_width, map_height = cmap.get_size()
    final_pos = (550, 370)

    #reset the current stored paths in cmap
    cmap.reset_paths()
    cmap.add_goal(Node(final_pos))
    #call the RRT function using your cmap as input, and RRT will update cmap with a new path to the target from the start position
    RRT(cmap, cmap.get_start())
    cozmo_pos = cmap.get_start()
    cozmo_angle = 0
    #get path from the cmap
    # paths = cmap.get_path()
    paths = cmap.get_smooth_path()
    #marked and update_cmap are both outputted from detect_cube_and_update_cmap(robot, marked, cozmo_pos).
    #and marked is an input to the function, indicating which cubes are already marked
    #So initialize "marked" to be an empty dictionary and "update_cmap" = False
    marked = {}
    last_subpath = cmap.get_start()
    #while the current cosmo position is not at the goal:
    while cozmo_pos != final_pos:
        #break if path is none or empty, indicating no path was found
        if paths is None or len(paths) <= 0:
            break
        # Get the next node from the path
        #drive the robot to next node in path. #First turn to the appropriate angle, and then move to it
        #you can calculate the angle to turn through a trigonometric function
        subpath = paths.pop(0) # of type (from_node, to_node)
        theta = np.arctan2(subpath.y - last_subpath.y, subpath.x - last_subpath.x)
        distance = get_dist(subpath, last_subpath)
        last_subpath = subpath

        await robot.turn_in_place(angle=cozmo.util.Angle(radians=theta - cozmo_angle), speed=cozmo.util.Angle(radians=1.5)).wait_for_completed()
        cozmo_angle = theta
        await robot.drive_straight(cozmo.util.distance_mm(distance_mm=distance), cozmo.util.speed_mmps(100)).wait_for_completed()
        # await robot.turn_in_place(angle=cozmo.util.Angle(radians=-theta), speed=cozmo.util.Angle(radians=1)).wait_for_completed()

        # Update the current Cozmo position (cozmo_pos and cozmo_angle) to be new node position and angle
        cozmo_pos = subpath.coord
        # Set new start position for replanning with RRT
        cmap.set_start(Node(coord=cozmo_pos))
    await robot.turn_in_place(angle=cozmo.util.Angle(radians=-cozmo_angle), speed=cozmo.util.Angle(radians=1.5)).wait_for_completed()

    await robot.drive_straight(cozmo.util.distance_mm(distance_mm=15), cozmo.util.speed_mmps(70)).wait_for_completed()
    await robot.set_lift_height(0).wait_for_completed()
    await robot.drive_straight(cozmo.util.distance_mm(distance_mm=-15), cozmo.util.speed_mmps(70)).wait_for_completed()
    ########################################################################

async def CozmoPlanning_back(robot: cozmo.robot.Robot):
    # Allows access to map and stopevent, which can be used to see if the GUI
    # has been closed by checking stopevent.is_set()
    global stopevent, cmap_back
    ########################################################################
    # TODO: please enter your code below.
    # Description of function provided in instructions. Potential pseudcode is below

    #assume start position is in cmap and was loaded from emptygrid.json as [50, 35] already
    #assume start angle is 0
    #Add final position as goal point to cmap, with final position being defined as a point that is at the center of the arena
    #you can get map width and map weight from cmap.get_size()
    map_width, map_height = cmap.get_size()
    final_pos = (50, 440)

    #reset the current stored paths in cmap
    cmap_back.reset_paths()
    cmap_back.add_goal(Node(final_pos))
    #call the RRT function using your cmap as input, and RRT will update cmap with a new path to the target from the start position
    RRT(cmap_back, cmap_back.get_start())

    cozmo_pos = cmap_back.get_start()
    cozmo_angle = 0
    #get path from the cmap
    # paths = cmap.get_path()
    paths = cmap_back.get_smooth_path()
    #marked and update_cmap are both outputted from detect_cube_and_update_cmap(robot, marked, cozmo_pos).
    #and marked is an input to the function, indicating which cubes are already marked
    #So initialize "marked" to be an empty dictionary and "update_cmap" = False
    marked = {}
    last_subpath = cmap_back.get_start()
    #while the current cosmo position is not at the goal:
    while cozmo_pos != final_pos:
        #break if path is none or empty, indicating no path was found
        if paths is None or len(paths) <= 0:
            break
        # Get the next node from the path
        #drive the robot to next node in path. #First turn to the appropriate angle, and then move to it
        #you can calculate the angle to turn through a trigonometric function
        subpath = paths.pop(0) # of type (from_node, to_node)
        print("path: {}".format(paths))
        theta = np.arctan2(subpath.y - last_subpath.y, subpath.x - last_subpath.x)
        distance = get_dist(subpath, last_subpath)
        last_subpath = subpath
        await robot.turn_in_place(angle=cozmo.util.Angle(radians=theta - cozmo_angle), speed=cozmo.util.Angle(radians=-1.5)).wait_for_completed()
        cozmo_angle = theta
        await robot.drive_straight(cozmo.util.distance_mm(distance_mm=distance), cozmo.util.speed_mmps(100)).wait_for_completed()
        # await robot.turn_in_place(angle=cozmo.util.Angle(radians=-theta), speed=cozmo.util.Angle(radians=1)).wait_for_completed()

        # Update the current Cozmo position (cozmo_pos and cozmo_angle) to be new node position and angle
        cozmo_pos = subpath.coord

        # Set new start position for replanning with RRT
        cmap.set_start(Node(coord=cozmo_pos))

    await robot.turn_in_place(angle=cozmo.util.degrees(205), speed=cozmo.util.Angle(radians=1)).wait_for_completed()

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

def get_global_node(local_angle, local_origin, node):
    """Helper function: Transform the node's position (x,y) from local coordinate frame specified by local_origin and local_angle to global coordinate frame.
                        This function is used in detect_cube_and_update_cmap()
        Arguments:
        local_angle, local_origin -- specify local coordinate frame's origin in global coordinate frame
        local_angle -- a single angle value
        local_origin -- a Node object
        Outputs:
        new_node -- a Node object that decribes the node's position in global coordinate frame
    """
    ########################################################################
    # TODO: please enter your code below.
    t = np.zeros((3,3))
    t[0, 0] = math.cos(local_angle)
    t[0, 1] = -math.sin(local_angle)
    t[0, 2] = local_origin.x
    t[1, 0] = math.sin(local_angle)
    t[1, 1] = math.cos(local_angle)
    t[1, 2] = local_origin.y
    t[2, 2] = 1

    new_coord = t @ (np.array(node.x, node.y, 1).T)

    #temporary code below to be replaced
    new_node = Node((new_coord.reshape(-1)[0], new_coord.reshape(-1)[1]))
    return new_node
    ########################################################################
class RobotThread(threading.Thread):
    """Thread to run cozmo code separate from main thread
    """

    def __init__(self):
        threading.Thread.__init__(self, daemon=True)

    def run(self):
        cozmo.run_program(init)
        cozmo.run_program(localization, use_viewer=False)

        for i in range(5):
            cozmo.run_program(init)
            cozmo.run_program(dock_with_cube)

            # Please refrain from enabling use_viewer since it uses tk, which must be in main thread
            cozmo.run_program(CozmoPlanning,use_3d_viewer=False, use_viewer=False)

            cozmo.run_program(CozmoPlanning_back,use_3d_viewer=False, use_viewer=False)
            print("*" * 30, "LOOP COMPLETE", "*" * 30)

        stopevent.set()

class DeliveryThread(threading.Thread):
    """Thread to run RRT separate from main thread
    """

    def __init__(self):
        threading.Thread.__init__(self, daemon=True)

    def run(self):
        while not stopevent.is_set():
            RRT(cmap, cmap.get_start())
            time.sleep(100)
            cmap.reset_paths()
        stopevent.set()

if __name__ == '__main__':
    global cmap, cmap_back, stopevent
    stopevent = threading.Event()
    robotFlag = False
    for i in range(0,len(sys.argv)): #reads input whether we are running the robot version or not
        if (sys.argv[i] == "-robot"):
            robotFlag = True
    if (robotFlag):
        cmap = CozMap("maps/path_planning_map.json", node_generator)
        cmap_back = CozMap("maps/path_planning_map_back.json", node_generator)
        robot_thread = RobotThread()
        robot_thread.start()
    else:
        cmap = CozMap("maps/path_planning_map.json", node_generator)
        sim = DeliveryThread()
        sim.start()
    localization_gui.show_particles(pf.particles)
    localization_gui.show_mean(0, 0, 0)
    localization_gui.start()
    visualizer = Visualizer(cmap)
    visualizer.start()
    stopevent.set()


import numpy as np
import matplotlib.pyplot as plt
from scipy.optimize import minimize

class ModelPredictiveControl:
    def __init__(self):
        self.horizon = 40

    def plant_model(self, u, prev_temp):
        knob_angle = u # control input
        # Knob angle to temperature
        knob_temp = knob_angle * 0.5
        # Calculate dT or change in temperature.
        tau = 6
        dT = (knob_temp - prev_temp) / tau
        # new temp = current temp + change in temp.
        return prev_temp + dT

    def cost_function(self, u):
        cost = 0.0
        temp = 0.0 #initial temp
        ideal_temp = 40
        #within a fixed horizon
        for i in range(0, self.horizon):
            temp = self.plant_model(u[i], temp)
            cost += abs(ideal_temp - temp)

        return cost #cost summation in a fixed horizon

mpc = ModelPredictiveControl()

# Set bounds, or constraints
bounds = []
for i in range(mpc.horizon):
    bounds += [[0, 180]] #hard constraint

# Create Inputs to be filled.
u = np.ones(mpc.horizon)

# Non-linear optimization.
u_solution = minimize(mpc.cost_function,
                      x0=u,
                      method='SLSQP',
                      bounds=bounds,
                      tol = 1e-8) #threshold

# # --------------------------
# # Calculate data for Plot 1, which has constant control input: knob angle
# knob_angle_list = []
# water_temp_list = []
# t_list = []
# knob_angle = 80
# water_temp = 0.0
# for t in range(40):
#     t_list += [t]
#     knob_angle_list += [knob_angle]
#     water_temp_list += [water_temp]
#     water_temp = mpc.plant_model(knob_angle, water_temp)

# # Create Plot 1 - Constant Input
# # Subplot 1
# plt.figure(figsize=(8,8))
# plt.subplot(211)
# plt.title("Constant Input")
# plt.ylabel("Knob Angle")
# # Enter Data
# plt.plot(t_list, knob_angle_list, 'k')
# plt.ylim(0,180)

# # Subplot 2
# plt.subplot(212)
# plt.ylabel("Water Temp")
# # Enter Data
# plt.plot(t_list, water_temp_list, 'ro')
# plt.ylim(0,50)
# plt.show()

# --------------------------
# Calculate data for Plot 2.
knob_angle_list = []
water_temp_list = []
t_list = []
water_temp = 0.0
for t in range(40):
    t_list += [t]
    knob_angle = u_solution.x[t] # get the sequence of control inputs that minimizes cost
    knob_angle_list += [knob_angle]
    water_temp_list += [water_temp]
    water_temp = mpc.plant_model(knob_angle, water_temp)

# Plot 2 - MPC
# Subplot 1
plt.figure(figsize=(8,8))
plt.subplot(211)
plt.title("MPC")
plt.ylabel("Knob Angle")
# Enter data
plt.plot(t_list, knob_angle_list, 'k')
plt.ylim(0,180)

# Subplot 2
plt.subplot(212)
plt.ylabel("Water Temp")
# Enter data
plt.plot(t_list, water_temp_list, 'ro')
plt.ylim(0,50)
plt.show()


import numpy as np

class Householder:
  """
  Classe que representa o algoritmo utilizado para fazer transformações de Householder
  para reduzir uma matriz simétrica a uma tridiagonal simetrica semelhante.

  Args:
      A (numpy.ndarray): Array n por n que representa a matriz completa a ser reduzida.
      print_steps (bool, optional): Indica se deve ou não imprimir os passos intermediários. Padrão é False.

  Attributes:
    n (int): Variável que indica a dimensão da matriz do sistema.
    k (int): Guarda o número da iteração atual do algorítmo.
    print_steps (bool): Indica se deve imprimir informações de cada iteração na tela.
    A (numpy.ndarray): Guarda o valor atual da matriz A.
    Ht (numpy.ndarray): Guarda o valor atual da matriz H transposta.
  """
  def __init__(self, A, print_steps=False):
    assert A.shape[0] == A.shape[1]

    self.n = A.shape[0]
    self.k = 0
    self.print_steps = print_steps

    self.A = np.ndarray(A.shape, dtype=float)

    self.Ht = np.identity(self.n, dtype=float)

    np.copyto(self.A, A)

  def _get_alfa_beta(self):
    """
    Retorna a diagonal pricipal e subdiagonal da matriz tridiagonalizada pelo algoritmo.

    Returns:
        dict: Dicionário com dois elementos, um numpy.ndarray representando a diagonal principal
                cuja chave é "alfa", e um representando as subdiagonais, com chave "beta".
    """
    alfa = np.ndarray(self.n, dtype=float)
    beta = np.ndarray(self.n-1, dtype=float)

    for i in range(self.n-1):
      alfa[i] = self.A[i][i]
      beta[i] = self.A[i+1][i]

    alfa[self.n-1] = self.A[self.n-1][self.n-1]

    result = {
      'alfa' : alfa,
      'beta' : beta,
    }

    return result

  def _iterate_once(self):
    """
    Método que representa uma iteração de transformações de Householder para uma matriz simétrica.
    """
    if self.print_steps:
      print(f'Iteração {self.k+1}:')

    # Calcula o valor do wk da iteracao atual
    ak_ = self.A[self.k+1 :, [self.k]]

    wk_ = np.copy(ak_)
    wk_[0] += np.sign(ak_[0]) * np.linalg.norm(ak_)

    if self.print_steps:
      print(f'w{self.k+1}_:\n{wk_}')

    # Multiplica a matriz H_wk a esquerda de A
    for i in range(self.k, self.n):
      ai_ = self.A[self.k+1 :, [i]]

      self.A[self.k+1 :, [i]] = ai_ - 2 * np.dot(ai_.T, wk_) / np.dot(wk_.T, wk_) * wk_

    if self.print_steps:
      print(f'H_w{self.k+1} A:\n{self.A}')

    # Multiplica a matriz H_wk a direita de A
    for i in range(self.k+1, self.n):
      bi_ = self.A[[i], self.k+1 :]

      self.A[[i], self.k+1 :] = bi_ - 2 * np.dot(bi_, wk_) / np.dot(wk_.T, wk_) * wk_.T

    if self.print_steps:
      print(f'H_w{self.k+1} A H_w{self.k+1}:\n{self.A}')

    # Multiplica a matriz H_wk a direita de Ht
    for i in range(0, self.n):
      bi_ = self.Ht[[i], self.k+1 :]

      self.Ht[[i], self.k+1 :] = bi_ - 2 * np.dot(bi_, wk_) / np.dot(wk_.T, wk_) * wk_.T

    if self.print_steps:
      print(f'Ht H_w{self.k+1}:\n{self.Ht}\n')

    self.k += 1

  def iterate(self):
    """
    Método que executa as transformações de Householder para a matriz fornecida no construtor.

    Returns:
        dict: Dicionário contendo a matriz tridiagonal e a matriz tranposta de H, para serem
                utilizados pelo método QR.
    """
    for m in range(self.n-2):
      self._iterate_once()

    result = {}
    result['T'] = self._get_alfa_beta()
    result['Ht'] = self.Ht

    return result

if __name__ == '__main__':
  print('Esse arquivo contem apenas o algoritmo Householder, mas não os testes.')
  print('Para rodar os testes corretamente, por favor consulte o LEIAME.txt')


from collections import defaultdict
import numpy as np

class DGraph():
    def __init__(self, num_vertices):
        self.num_vertices = num_vertices
        self.graph = defaultdict(list)
        self.tc_graph = defaultdict(list)
        # Transitive closure.
        self.tc = np.zeros((num_vertices, num_vertices))

    def add_edge(self, u, v):
        self.graph[u].append(v)

    def dfs_traverse(self, s, v):
        self.tc[s][v] = 1
        self.tc_graph[s].append(v)

        for i in self.graph[v]:
            if self.tc[s][i] < 1:
                self.dfs_traverse(s, i)

    def transitive_closure(self):
        for i in range(self.num_vertices):
            self.dfs_traverse(i, i)

def transitive_closure(g):
    '''
    Parameters:
        g (dict), a DAG with adjacent list representation
    Return:
        trans_path(dict), where key = vertice v, value = a dict maps reachable
        nodes u to a path of v to u
        trans_dict(dict), the adjacent list representation of the transitive
        closure of g
    '''
    trans_path = {}
    trans_dict = {}
    for v in g:
        transitive_closure_helper(g, trans_path, v)

    for v in trans_path:
        reachables = []
        reachable_dict = trans_path[v]
        for u in reachable_dict:
            reachables.append(u)
        trans_dict[v] = reachables
    return trans_path, trans_dict

def transitive_closure_helper(g, trans_path, v):
    '''
    Helper function of the 'transitive_closure'
    Parameters:
        g (dict), a DAG with adjacent list representation
        trans_path(dict), where key = vertice v, value = a dict maps reachable
        nodes u to a path of v to u
        v: a vertex v
    Return:
        trans_path(dict)
    '''
    if v in trans_path:
        return trans_path
    if len(g[v]) == 0:
        trans_path[v] = {}
        return trans_path
    nbrs = g[v]
    v_dict = {}
    for u in nbrs:
        recur = transitive_closure_helper(g, trans_path, u)
        u_dict = recur[u]
        for node in u_dict:
            if node not in v_dict:
                # TODO: O(n), will fix later to amortized O(1).
                v_dict[node] = [v] + u_dict[node]
        v_dict[u] = [v, u]
    trans_path[v] = v_dict
    return trans_path

def tst():
    g = DGraph(4)
    g.add_edge(0, 1)
    g.add_edge(0, 2)
    g.add_edge(1, 2)
    g.add_edge(2, 0)
    g.add_edge(2, 3)
    g.add_edge(3, 3)
    g.transitive_closure()
    print(g.tc)

if __name__ == "__main__":
    print("Running tests for transitive closure.")
    tst()

# References
# [1] https://www.geeksforgeeks.org/transitive-closure-of-a-graph-using-dfs/
# [2] https://www.cs.princeton.edu/courses/archive/spr03/cs226/lectures/digraph.4up.pdf


import numpy as np
import matplotlib.pyplot as plt

def hoocneNhan(A,xk):
    A.append(1)
    for i in range(len(A)-2,0,-1):
        A[i] = A[i - 1] - A[i] * xk
    A[0] = - A[0] * xk
    return A

def hoocneChia(A,xk):
    B = np.ones(len(A) - 1)
    for i in range(len(B) - 2,-1,-1):
        B[i] = A[i + 1] + B[i + 1] * xk
    return B

def init():
    global x,y,n
    x = []
    y = []
    with open('nsLagrange.txt','r+') as f:
            for line in f.readlines():
                xt = float(line.split(' ')[0])
                yt = float(line.split(' ')[1])
                check = True
                for x_check in x:
                    if x_check == xt:
                        check = False
                        break
                if check:
                    x.append(xt)
                    y.append(yt)
                plt.scatter(xt,yt)
    x = np.asarray(x)
    y = np.asarray(y)
    n = len(x)

def PolyCoefficients(xt, coeffs):
    """ Returns a polynomial for ``x`` values for the ``coeffs`` provided.

    The coefficients must be in ascending order (``x**0`` to ``x**o``).
    """
    o = len(coeffs)
    yt = 0
    for i in range(o):
        yt += coeffs[i] * xt ** i
    return yt

def main():
    init()

    # tinh D
    D = []
    for i in range(n):
        D.append(1)
        for j in range(n):
            if(i != j):
                D[i] *= (x[i] - x[j])
    D = np.asarray(D)

    # tinh w
    w = [1]
    for i in range(n):
        w = hoocneNhan(w,x[i])
    w = np.asarray(w)

    # tinh wi
    wi = []
    for i in range(n):
        wi.append(hoocneChia(w,x[i]))
    wi = np.asarray(wi)

    # tinh px
    px = np.zeros(n)
    for i in range(n):
        for j in range(n):
            px[i] += wi[j,i] * y[j] / D[j]

    print(px)

    plt.scatter(x,y)

    xt = np.linspace(min(x)-0.5,max(x) + 0.5,1000)
    plt.plot(xt,PolyCoefficients(xt,px))
    plt.savefig("mygraph.png")

    #plt.show()

if __name__=='__main__':
    main()

Topic 3 - Function Approximation/16.Lagrange_Interpolation/nsLagrange.py


import numpy as np
import scipy.signal

def discount(x, gamma):
    """
    computes discounted sums along 0th dimension of x.

    inputs
    ------
    x: ndarray
    gamma: float

    outputs
    -------
    y: ndarray with same shape as x, satisfying

        y[t] = x[t] + gamma*x[t+1] + gamma^2*x[t+2] + ... + gamma^k x[t+k],
                where k = len(x) - t - 1

    """
    assert x.ndim >= 1
    return scipy.signal.lfilter([1],[1,-gamma],x[::-1], axis=0)[::-1]

def explained_variance(ypred,y):
    """
    Computes fraction of variance that ypred explains about y.
    Returns 1 - Var[y-ypred] / Var[y]

    interpretation:
        ev=0  =>  might as well have predicted zero
        ev=1  =>  perfect prediction
        ev<0  =>  worse than just predicting zero

    """
    ##assert y.ndim == 1 and ypred.ndim == 1
    vary = np.var(y)
    return np.nan if vary==0 else 1 - np.var(y-ypred)/vary

def explained_variance_2d(ypred, y):
    assert y.ndim == 2 and ypred.ndim == 2
    vary = np.var(y, axis=0)
    out = 1 - np.var(y-ypred)/vary
    out[vary < 1e-10] = 0
    return out

def ncc(ypred, y):
    return np.corrcoef(ypred, y)[1,0]

def flatten_arrays(arrs):
    return np.concatenate([arr.flat for arr in arrs])

def unflatten_vector(vec, shapes):
    i=0
    arrs = []
    for shape in shapes:
        size = np.prod(shape)
        arr = vec[i:i+size].reshape(shape)
        arrs.append(arr)
        i += size
    return arrs

def discount_with_boundaries(X, New, gamma):
    """
    X: 2d array of floats, time x features
    New: 2d array of bools, indicating when a new episode has started
    """
    Y = np.zeros_like(X)
    T = X.shape[0]
    Y[T-1] = X[T-1]
    for t in range(T-2, -1, -1):
        Y[t] = X[t] + gamma * Y[t+1] * (1 - New[t+1])
    return Y

def test_discount_with_boundaries():
    gamma=0.9
    x = np.array([1.0, 2.0, 3.0, 4.0], 'float32')
    starts = [1.0, 0.0, 0.0, 1.0]
    y = discount_with_boundaries(x, starts, gamma)
    assert np.allclose(y, [
        1 + gamma * 2 + gamma**2 * 3,
        2 + gamma * 3,
        3,
        4
    ])


import numpy as np
from matplotlib import pyplot as plt

# 一维
a = np.array([1, 2, 3]); print(a) # [1 2 3]
# 等间隔数字的数组
b = np.arange(10); print(b) # [0 1 2 3 4 5 6 7 8 9]
b = np.arange(3,7,2); print(b) # array([3, 5])
# 二维
c = np.array([[1, 2], [3, 4]]); print(c) # [[1 2]
										 #  [3 4]]

# 调整数组shape
a = np.array([[1, 2, 3], [4, 5, 6]]); print(a); # [[1 2 3]
												#  [4 5 6]]
a.shape = (3, 2); print(a)                      # [[1 2]
												#  [3 4]
												#  [5 6]]

# ndim:返回数组的维数
a = np.arange(24);
print(a)
print(a.ndim) # 1

# numpy.reshape: 在不改变数据的条件下修改形状
b = a.reshape(2, 4, 3); print(b.ndim) # 3

# 空数组
x = np.empty([3, 2], dtype='i1'); print(x) # 数组x的元素为随机值，因为它们未初始化

# 含有5个0的数组，若不指定类型，则默认为float
x = np.zeros(5, dtype=np.int); print(x) # [0 0 0 0 0]
# 含有6个1的二维数组，若不指定类型，则默认为float
x = np.ones([2, 3], dtype=int); print(x) # [[1 1 1]
										 #  [1 1 1]]

# 使用内置的range()函数创建列表对象
x = range(5); print(x) # range(0, 5)

# numpy.linspace，此函数类似于arange，在此函数中，指定了范围之间的均匀间隔数量，而不是步长
x = np.linspace(10, 20, 5); print(x) # [10. 12.5 15. 17.5  20.]

# 算数运算：add, subtract, multiply, divide, reciprocal, power, mod 输入数组必须具有相同的形状或符合数组广播规则
a, b = [5, 6], [7, 10]
c = np.subtract(a, b); print(c) # [-2 -4]

# 统计函数：用于从数组中给定的元素中查找最小，最大，百分标准差和方差等, amin, amax, ptp, percentile, median, mean, average, std
a = np.array([1, 2, 3, 4, 5])
print(np.amin(a)) # 1
print(np.mean(a)) # 3.0


import numpy as np

def extract_band(frequency_band, fft_obj, epsilon=1e-7):
    """
    TODO: THis may want to be a method of fft_obj, or it may want to be a
    method of frequency band.  For now leave as stand alone.
    Parameters
    ----------
    interval
    fft_obj: xr.DataArray
    epsilon: use this when you are worried about missing a frequency due to
    round off error.  THis is in general not needed if we use a df/2 pad
    around true harmonics.

    Returns xr.DataArray
    -------

    """
    cond1 = fft_obj.frequency >= frequency_band.lower_bound - epsilon
    cond2 = fft_obj.frequency <= frequency_band.upper_bound + epsilon

    band = fft_obj.where(cond1 & cond2, drop=True)
    return band

def frequency_band_edges(f_lower_bound, f_upper_bound,
                                  num_bands_per_decade=None, num_bands=None):
    """
    Provides logarithmically spaced fenceposts acoss lowest and highest
    frequencies. This is a lot like calling logspace.  The resultant gates
    have constant  Q, i.e. deltaF/f_center=Q=constant.
    where f_center is defined geometircally, i.e. sqrt(f2*f1) is the center freq
    between f1 and f2.

    TODO: Add a linear spacing option?

    Parameters
    ----------
    f_lower_bound : float
        lowest frequency under consideration
    f_upper_bound : float
        highest frequency under consideration
    num_bands_per_decade : int (TODO test, float maybe ok also.. need to test)
        number of bands per decade
    num_bands : int
        total number of bands.  This supercedes num_bands_per_decade if supplied

    Returns :
    fence_posts : array
        logarithmically spaced fenceposts acoss lowest and highest
        frequencies.  These partition the frequency domain between
        f_lower_bound and f_upper_bound
    -------

    """
    if (num_bands is None) & (num_bands_per_decade is None):
        print("Specify either number_of_bands or numnerbands_per_decade")
        raise Exception

    if num_bands is None:
        number_of_decades = np.log10(f_upper_bound/ f_lower_bound)
        # The number of decades spanned (use log8 for octaves)
        num_bands = round(number_of_decades * num_bands_per_decade) #floor or ceiling here?

    base = np.exp((1.0 / num_bands) * np.log(f_upper_bound / f_lower_bound))
    # log - NOT log10!

    print(f"base = {base}")
    bases = base * np.ones(num_bands + 1);
    print(f"bases = {bases}")
    exponents = np.linspace(0, num_bands, num_bands + 1)
    print(f"exponents = {exponents}")
    fence_posts = f_lower_bound * (bases ** exponents)
    print(f"fence posts = {fence_posts}")
    return fence_posts


"""
Laboratory Experiment 3 - Script
 - Rootlocus project
 @author Rafael Lima
"""

from sympy import *

def simplifyFraction(G,s):
    """
    Expand numerator and denominator from given fraction
    """
    num,den = fraction(G.expand().simplify())
    num = Poly(num,s)
    den = Poly(den,s)

    return (num/den)

def partfrac(G,s):
    """
    Split Fraction into several factors using residues theorem
    """
    # Find Poles
    poles = solve(sympy.fraction(G.expand().simplify())[1],s)

    # Find Resudues
    Gp = 0
    for p in poles:
        Gp = Gp + (G*(s-p)).subs(s,p)/(s-p)

    return Gp

def roundExpr(expr, num_digits=4):
    """
    Round Every Number in an expression
    """
    return expr.xreplace({n : round(n, num_digits) for n in expr.atoms(Number)})

printing.printer.Printer().set_global_settings(precision=3)

# Symbols
s = symbols("s",complex=True)
z = symbols("z",complex=True)
K,a1,a0,T,b = symbols("K alpha_1 alpha_0 T beta",real=True)

# Constants
na0 = 1
nT = 0.5

# Open Loop Transfer Function
sGo = 1/(s+na0)

# Z Transform (From table)
sGz = (1/na0)*(1-exp(-T))/(z-exp(-T))

# Controler TF
sGc = K*z/(z-1)
sGma = simplify(expand(sGc*sGz))
sGmf = simplify(expand(sGma/(1+sGma)))

# Characterist Equation
_,poly = fraction(sGmf)

# Find Critical Value for K
sK = solve(poly,K)[0]
Kmax = sK.subs([(T,nT),(z,-1)])

poles2 = solve(poly.subs([(T,nT),(K,2)]),z)

# Part 2
# Constants
na1 = 2
Ts = 0.2

# Open Loop Transfer Function
sGo2 = 1/(s+2)

# TODO find Z Transform (From table)
# BUG: not matching ZOH discretization from matlab
sGz2 = (z-1)*(1/(4*(z-exp(-2*T))) - 1/(4*(z-1)) + T*(1/((z-1)*(z-1))))

# Controler TF
sGc2 = K*(z-exp(-na1*T))/(z-b)
sGma2 = simplifyFraction(sGc2*sGz2,z)
sGmf2 = simplify(expand(sGma2/(1+sGma2)))

# Expression from matlab
mGz2 = ((2533546664982251*z)/144115188075855872 + 554410548014771/36028797018963968)/(z**2 - (3761226368457787*z)/2251799813685248 + 6037706219090157/9007199254740992)

# Controler TF from Matlab
mGc2 = K*(z-exp(-na1*T))/(z-b)
mGma2 = simplifyFraction(mGc2*mGz2,z)
mGmf2 = simplify(expand(mGma2/(1+mGma2)))

# Characterist Equation
_,poly2 = fraction(sGmf2)

# Request Conditions
desiredDamping = 0.5
desiredSettlingTime = 2
desiredOvershoot = exp(-desiredDamping*pi/sqrt(1-desiredDamping**2))
desiredPoles = [0,0]
desiredPoles[0] = -(4/desiredSettlingTime)*(1 + I*sqrt(1-desiredDamping**2)/desiredDamping)
desiredPoles[1] = -(4/desiredSettlingTime)*(1 - I*sqrt(1-desiredDamping**2)/desiredDamping)
desiredPolesZ = [exp(desiredPoles[0]*Ts),exp(desiredPoles[1]*Ts)]

# Solve Linear System to find K and b
sysKb = [K,b]
sysKb[0] = poly2.subs([(z,desiredPolesZ[0]),(T,Ts)]).evalf().collect(K).collect(b)
sysKb[1] = poly2.subs([(z,desiredPolesZ[1]),(T,Ts)]).evalf().collect(K).collect(b)
resp = list(linsolve(sysKb,(K,b)))[0]
nK = resp[0]
nb = resp[1]

# Find TF
nGmf2 = sGmf2.subs([(K,nK),(b,nb),(T,Ts)])

# Find Critical Value for K
sK2 = solve(poly2,K)[0]
#Kmax = sK.subs([(T,nT),(z,-1)])


# %%
import numpy as np
import matplotlib.pyplot as plt
from scipy.stats import norm
from matplotlib.animation import FuncAnimation
from matplotlib.collections import PolyCollection
# matplotlib parameters to ensure correctness of Chinese characters
plt.rcParams["font.family"] = 'sans-serif'
plt.rcParams['font.sans-serif']=['Arial Unicode MS', 'SimHei'] # Chinese font
plt.rcParams['axes.unicode_minus']=False # correct minus sign

plt.rcParams["font.size"] = 16
plt.rcParams["xtick.labelsize"] = 16
plt.rcParams["ytick.labelsize"] = 16
plt.rcParams["axes.spines.top"] = False
plt.rcParams["axes.spines.right"] = False
#%%
prefix = 'Hypothesis_movie'
class UpdateFigure:
    def __init__(self,
        ax:plt.Axes, dx:float=0.05):
        """Plot the first frame for the animation.

        Args:
            ax (plt.Axes): axes of flight icons.
        """

        self.ax = ax
        self.ax.spines["left"].set_visible(False)
        self.dx = dx
        self.mean = 7.0
        self.sigma = 0.04
        self.th = 0.0784
        self.x = np.linspace(-0.25,0.25,1000) + self.mean

        self.ax.set_xlim(self.x[0],self.x[-1])
        self.ax.set_ylim(0, 12.0)
        self.norm = norm(loc=self.mean, scale=self.sigma)
        self.gauss = self.norm.pdf
        self.isf = self.norm.isf(0.025)
        self.ppf = self.norm.ppf(0.025)
        self.ax.set_yticks([])

        # start ploting
        plt.savefig(prefix+'_step1.pdf')
        self.line = self.ax.plot(self.x,self.gauss(self.x), color='k', zorder=1)
        plt.savefig(prefix+'_step2.pdf')
        self.ax.axvline(self.mean, ymax=0.95, ls='--', color='#C00000')

        vline0 = self.ax.axvline(norm.isf(0.025, loc=self.mean, scale=self.sigma), ymax=0.8, ls='--', color='#002060')
        vline1 = self.ax.axvline(norm.ppf(0.025, loc=self.mean, scale=self.sigma), ymax=0.8, ls='--', color='#002060')
        self.vlines = [vline0, vline1]
        plt.savefig(prefix+'_step3.pdf')

        shade_verts = [
            self._shading_verts(self.isf, 'right'),
            self._shading_verts(self.ppf, 'left'),
        ]
        self.shades = [
            PolyCollection(verts, fc='#0096FE', alpha=0.8, zorder=0)
            for verts in shade_verts
        ]
        [self.ax.add_collection(shade) for shade in self.shades]
        stick0, text0 = self.create_text(self.isf, 'right')
        stick1, text1 = self.create_text(self.ppf, 'left')
        self.texts = [text0, text1]
        self.sticks = [stick0, stick1]
        plt.savefig(prefix+'_step4.pdf')

    def create_text(self, x, direction='right'):
        height = 1.2
        width = 0.06
        offset = 0.005
        if direction == 'right':
            stick, = self.ax.plot(x+np.array([offset, offset+width]),self.gauss(x+offset)/2+np.array([0., height]), color='k', zorder=1)
            text = self.ax.text(x+offset+0.005+width, height+self.gauss(x+offset)/2, r'$\frac{\alpha}{2}$', ha='left')
        elif direction == 'left':
            stick, = self.ax.plot(x-np.array([offset, offset+width]),self.gauss(x-offset)/2+np.array([0., height]), color='k', zorder=1)
            text = self.ax.text(x-offset-0.005-width, height+self.gauss(x-offset)/2, r'$\frac{\alpha}{2}$', ha='right')
        return stick, text

    def update_text(self, x, stick, text, direction='right'):
        height = 1.2
        width = 0.06
        offset = 0.005
        if direction == 'right':
            stick.set_data(x+np.array([offset, offset+width]),self.gauss(x+offset)/2+np.array([0., height]))
            text.set_position((x+offset+0.005+width, height+self.gauss(x+offset)/2))
        elif direction == 'left':
            stick.set_data(x-np.array([offset, offset+width]),self.gauss(x-offset)/2+np.array([0., height]))
            text.set_position((x-offset-0.005-width, height+self.gauss(x-offset)/2))
        return stick, text

    def update_gauss(self):
        self.line[0].set_data(self.x,self.gauss(self.x))

    def update_vlines(self):
        [self.vlines[i].set_data(np.ones(2)*val, [0,0.8]) for i, val in enumerate((self.isf, self.ppf))]

    def _shading_verts(self, x, direction='right'):
        if direction == 'right':
            _x = self.x[self.x>=x]
            _y = self.gauss(self.x[self.x>=x])
        elif direction == 'left':
            _x = self.x[self.x<=x]
            _y = self.gauss(self.x[self.x<=x])
        verts = [[_xx, _yy] for _xx, _yy in zip(_x, _y)]
        verts += [[_x[-1], 0],[_x[0], 0]]
        return np.array([verts])

    # def __call__(self, i):
    #     # This way the plot can continuously run and we just keep
    #     # watching new realizations of the process
    #     depsilon=1.5e-4
    #     eps = 0.025-i*depsilon
    #     self.isf = self.norm.isf(eps)
    #     self.ppf = self.norm.ppf(eps)
    #     self.update_vlines()
    #     shade_verts = [
    #         self._shading_verts(self.isf, 'right'),
    #         self._shading_verts(self.ppf, 'left' ),
    #     ]
    #     [shade.set_verts(verts) for shade, verts in zip(self.shades, shade_verts)]
    #     self.update_text(self.isf, self.sticks[0], self.texts[0], 'right')
    #     self.update_text(self.ppf, self.sticks[1], self.texts[1], 'left')
    #     return self.line

    def __call__(self, i):
        # This way the plot can continuously run and we just keep
        # watching new realizations of the process
        self.norm = norm(loc=self.mean, scale=self.sigma-i*self.dx)
        self.gauss = self.norm.pdf
        self.isf = self.norm.isf(0.025)
        self.ppf = self.norm.ppf(0.025)
        self.update_gauss()
        self.update_vlines()
        shade_verts = [
            self._shading_verts(self.isf, 'right'),
            self._shading_verts(self.ppf, 'left'),
        ]
        [shade.set_verts(verts) for shade, verts in zip(self.shades, shade_verts)]
        self.update_text(self.isf, self.sticks[0], self.texts[0], 'right')
        self.update_text(self.ppf, self.sticks[1], self.texts[1], 'left')
        return self.line
fig, ax = plt.subplots(1,1, figsize=(10,3),dpi=400)

# create a figure updater
ud = UpdateFigure(ax, dx=0.00014)
# user FuncAnimation to generate frames of animation
anim = FuncAnimation(fig, ud, frames=121, blit=True)
# save animation as *.mp4
anim.save('hypothesis_movie.mp4', fps=60, dpi=400, codec='libx264', bitrate=-1, extra_args=['-pix_fmt', 'yuv420p'])
# %%


import scipy as sp
from scipy.stats import norm
import matplotlib.pyplot as plt
from sklearn.cluster import KMeans

seed = 2
sp.random.seed(seed)  # to reproduce the data later on

numClusters = 3

xw1 = norm(loc=.3, scale=.15).rvs(20)
yw1 = norm(loc=.3, scale=.15).rvs(20)

xw2 = norm(loc=.7, scale=.15).rvs(20)
yw2 = norm(loc=.7, scale=.15).rvs(20)

xw3 = norm(loc=.2, scale=.15).rvs(20)
yw3 = norm(loc=.8, scale=.15).rvs(20)

x = sp.append(sp.append(xw1, xw2), xw3)
y = sp.append(sp.append(yw1, yw2), yw3)

def plot(x, y, title, km=None):
    if km:
        plt.scatter(x, y, s=50, c=km.predict(sp.vstack((x, y)).T))
    else:
        plt.scatter(x, y, s=50)
    plt.title(title)
    plt.autoscale(tight=True)
    plt.grid()
    plt.xlim(0, 1)
    plt.ylim(0, 1)

plot(x, y, 'vectors')
plt.savefig('./pics/figure1.png')
plt.clf()

def plot_kmean_iter(i):
    km = KMeans(init='random', n_clusters=numClusters, verbose=1,
                n_init=1, max_iter=i,
                random_state=seed)
    km.fit(features)
    plot(x, y, 'iter %d' % i, km)
    mx, my = sp.meshgrid(sp.arange(0, 1, 0.001), sp.arange(0, 1, 0.001))
    Z = km.predict(sp.vstack((mx.ravel(), my.ravel())).T).reshape(mx.shape)
    plt.imshow(Z, interpolation='nearest',
                 extent=(mx.min(), mx.max(), my.min(), my.max()),
                 cmap=plt.cm.Blues,
                 aspect='auto', origin='lower')
    plt.scatter(km.cluster_centers_[:, 0], km.cluster_centers_[:, 1],
                  marker='x', linewidth=2, s=100, color='black')
    return km.cluster_centers_

features = sp.vstack((x, y)).T

centers1 = plot_kmean_iter(1)
plt.savefig('./pics/figure2.png')
plt.clf()

def plot_arrow(centers1, centers2):
    for i in range(centers1.shape[0]):
        x1, y1 = centers1[i, :]
        x2, y2 = centers2[i, :]
        plt.arrow(x1, y1, x2 - x1, y2 - y1, length_includes_head=True, head_width=.01, lw=2)

centers2 = plot_kmean_iter(2)
plot_arrow(centers1, centers2)
plt.savefig('./pics/figure3.png')
plt.clf()

centers3 = plot_kmean_iter(3)
plot_arrow(centers2, centers3)
plt.savefig('./pics/figure4.png')
plt.clf()

centers3 = plot_kmean_iter(10)
plt.savefig('./pics/figure5.png')
plt.clf()


#!/usr/bin/env python3
from scipy.interpolate import CubicSpline
import numpy as np

x = np.arange(21)*np.pi/5.0
y = np.sin(x)
y[0] = 0.01
y[-1] = y[0]
xq = np.linspace(min(x), max(x), 101)
cs_not_a_knot = CubicSpline(x, y, bc_type='not-a-knot')
cs_clamped    = CubicSpline(x, y, bc_type='clamped')
cs_natural    = CubicSpline(x, y, bc_type='natural')
cs_periodic   = CubicSpline(x, y, bc_type='periodic')
yq_knot    = cs_not_a_knot(xq)
yq_clamped = cs_clamped(xq)
yq_natural = cs_natural(xq)
yq_period  = cs_periodic(xq)
ofl = open('cubicSplineInterpolation.txt', 'w')
for i in range(len(xq)):
    ofl.write("%e, %e, %e, %e, %e\n"%(xq[i], yq_knot[i], yq_clamped[i],
                                      yq_natural[i], yq_period[i]))
ofl.close()
print(cs_not_a_knot.integrate(0, 1))
print(cs_natural.integrate(max(x), 0))
print(cs_clamped.integrate(4.08, 6))
print(cs_periodic.integrate(3.99, max(x)))
print(cs_periodic.integrate(3.007729,2.541008))


import math
import numpy as np
#from fastdist import fastdist

class Proximity:
    def __init__(self):
        pass

    @staticmethod
    def compute(a, b, proxtype):
        p = 0.0;
        # euclidean
        if proxtype=="euclidean":
            for i in range(len(a)):
                c = a[i]-b[i]
                c = c**2
                p+=c

            # p = math.sqrt(p)
            p = np.sqrt(p)

            # a = np.array(a)
            # b = np.array(b)
            # d = [(ai - bi)**2 for ai, bi in zip(a, b)]
            # p = math.sqrt(sum(d))

        return p

    # @staticmethod
    # def computeFE(XFEi=None, XFEj=None, i=None, j=None, proxtype):
    #     p = 0.0;
    #     # euclidean
    #     if proxtype=="euclidean":
    #         #pi = ProximityMatrix.getProximityType()["Euclidean"];
    #         p = XFEi.proximity_two(XFEi, XFEj, i, j, 0)

    #     return p

class PMatrix:
    def __init__(self, n):
        self.dist = [ [] for i in range(n)]
        for i in range(n):
            self.dist[i] = [ None for j in range(n)];

    def get(self, i, j):
        mi = i
        mj = j
        if j<i:
            mi = j
            mj = i

        return self.dist[mi][mj]

    def set(self, i, j, d):
        mi = i
        mj = j
        if j<i and i!=j:
            mi = j
            mj = i
        self.dist[mi][mj] = d


# -*- coding: utf-8 -*-
"""
Created on Sat Jan 30 21:39:29 2021

@author: TaeKen
"""
from math import sin, cos, sqrt, atan2, radians, degrees
import numpy as np

def distance(loc1, loc2):

    R = 3958.8

    lat1 = radians(loc1[0])
    long1 = radians(loc1[1])
    lat2 = radians(loc2[0])
    long2 = radians(loc2[1])

    dlong = long2 - long1
    dlat = lat2 - lat1

    a = sin(dlat / 2)**2 + cos(lat1) * cos(lat2) * sin(dlong / 2)**2
    c = 2 * atan2(sqrt(a), sqrt(1 - a))

    distance = R * c

    return distance

def midpoint(loc1, loc2,pop1=1,pop2=1):

    lat1 = radians(loc1[0])
    long1 = radians(loc1[1])
    lat2 = radians(loc2[0])
    long2 = radians(loc2[1])

    x = (cos(lat1) * cos(long1) *pop1 + cos(lat2) * cos(long2) *pop2)/(pop1+pop2)
    y = (cos(lat1) * sin(long1) *pop1 + cos(lat2) * sin(long2) *pop2)/(pop1+pop2)
    z = (sin(lat1) *pop1 + sin(lat2) *pop2)/(pop1+pop2)

    central_longitude = atan2(y, x)
    central_square_root = sqrt(x * x + y * y)
    central_latitude = atan2(z, central_square_root)

    return np.array([degrees(central_latitude),degrees(central_longitude)])


from scipy.stats import entropy
from scipy.special import rel_entr
import numpy as np

def ent(l):
    return entropy(l, base=2)

# relative entropy or Kullback-Leibler Divergence
# id not a distance metric because it is not symmetric
def rel_ent(p, q, e=1e-15):
    assert len(p) == len(q)
    pn = [i / sum(p) for i in p]
    qn = [i / sum(q) for i in q]

    return sum(
        pn[i] * np.log2(np.clip(pn[i], e, 1.0) / np.clip(qn[i], e, 1.0))
        for i in range(len(pn))
    )

def js_div(p, q):
    assert len(p) == len(q)
    pn = [i / sum(p) for i in p]
    qn = [i / sum(q) for i in q]

    m = 0.5 * (np.array(pn) + np.array(qn))
    return 0.5 * rel_ent(pn, m) + 0.5 * rel_ent(qn, m)


import numpy as np
def cosine_similarity(a,b):
    a=np.mat(a)#把list转换为matrix
    b=np.mat(b)
    num=float(a * b.T)
    #linalg.norm()计算向量的范数
    denom=np.linalg.norm(a)*np.linalg.norm(a)
    return 0.5+0.5*(num/denom)#cosine similarity的范围为[-1,1],先把他归一化至[0,1]
if __name__ == '__main__':
    cosine_similarity([1,2],[2,1])


from pydoc import plain
import numpy as np

MATRIX_SIZE = 3

def get_random_key(m_size):
    A = np.random.randint(10, size = (m_size, m_size))
    b = np.random.randint(10, size = (m_size, 1))
    assert np.linalg.det(A)
    return A, b

def enc(s, A, b, n):
    l = []
    for i in s:
        l.append(ord(i) - ord('a'))
    M = np.array(l)
    C = A * M + b
    res = ''
    for idx in range(np.shape(A)[0]):
        C[0][idx] = C[0][idx] % n
        res += chr(C[0][idx] + ord('a'))
    return res

def dec(s, A, b, n):
    l = []
    for i in s:
        l.append(ord(i) - ord('a'))
    M = np.array(l)
    C = np.linalg.inv(A) * (M - b)
    ## Get the invert in mod
    res = ''
    for idx in range(np.shape(A)[0]):
        C[0][idx] = C[0][idx] % n
        res += chr(int(C[0][idx] + ord('a')))
    return res

def main():
    plain_text = 'abc'
    m_size = len(plain_text)
    A, b = get_random_key(m_size)
    cypher_text = enc(plain_text, A, b, 26)
    print('Cypher Text: {}'.format(cypher_text))
    plain_text = dec(cypher_text, A, b, 26)
    print('Plain Text: {}'.format(plain_text))

if __name__ == '__main__':
    main()


import numpy as np
import cv2

# Exercise 1: Create a vector of 10 floats (zeros)

def ex1():
    arr = np.zeros(shape = (1,10))
    print(arr)

# Exercise 2: Create a vector of 10 float (zeros) whose 5th element is one

def ex2():
    arr = np.zeros(shape = (1,10))
    arr[0,4] = 1.0
    print(arr)

# Exercise 3: Create a vector of integers going from 10 to 49

def ex3():
    arr = np.arange(10, 50) # ints from 0 to 9
    print(arr)

# Exercise 4: Create a matrix of 3x3 floats going from 1 to 9

def ex4():
    arr = np.arange(1,10)
    arr = arr.reshape(3, 3)
    print(arr)

# Exercise 5: Create a matrix of 3x3 floats going from 1 to 9 and flip it horizontally

def ex5():
    arr = np.arange(1,10)
    arr = arr.reshape(3,3)
    arr = np.flip(arr,1)
    print(arr)

# Exercise 6: Create a matrix of 3x3 floats going from 1 to 9 and flip it vertically

def ex6():
    arr = np.arange(1,10)
    arr = arr.reshape(3,3)
    arr = np.flip(arr,0)
    print(arr)

# Exercise 7: Create a 3x3 identity matrix

def ex7():
    arr = np.identity(3)
    print(arr)

# Exercise 8: Create a 3x3 matrix of random values

def ex8():
    arr = np.random.random_sample((3, 3)) # 2x3 array of random floats from 0 to 1
    print(arr)

# Exercise 9: Create a random vector of 10 numbers and compute the mean value

def ex9():
    arr = np.random.randint(0,10,11)
    avg = arr.mean()
    print(arr)
    print(avg)

# Exercise 10: Create a 10x10 array of zeros surrounded/framed by ones

def ex10():
    arr = np.ones((5,5))
    arr[1:-1, 1:-1] = 0
    print(arr)

# Exercise 11:  Create a 5x5 matrix of rows from 1 to 5

def ex11():
    arr = np.zeros(shape = (5,5))
    arr[:, :] = np.arange(1,6)
    print(arr)

# Exercise 12:  Create an array of 9 random integers and reshape it to a 3x3 matrix of floats

def ex12():
    arr = np.random.randint(0, 100, 9)
    arr = arr.reshape(3,3)
    print(arr)

# Exercise 13:  Create a 5x5 matrix of random values and subtract its average from it

def ex13():
    arr = np.random.randint(0, 100, 25)
    arr = np.float64(arr)
    arr = arr.reshape(5,5)
    avg = arr.mean()
    arr -= avg
    print(avg)
    print(arr)

# Exercise 14:  Create a 5x5 matrix of random values and subtract the average of each row to each row

def ex14():
    arr = np.random.randint(0, 100, 25)
    arr = np.float64(arr)
    arr = arr.reshape(5,5)
    avg = np.zeros(shape=(1, 5))
    arr -= avg
    print(avg)
    print(arr)

#14 is not finished!!

# Exercise 15: Create an array of 5x5 random values and return the value that is closer to 0.5

def ex15():
    arr = np.random.uniform(0.0,1.0,(5,5))
    index = np.absolute(arr-0.5).argmin()
    arr = arr.flatten()
    print(arr[index])

# Exercise 16: . Make a 3x3 matrix of random numbers from 0 to 10 and count how many of them are > 5

def ex16():
    arr = np.random.randint(0,10,(3,3))
    print(arr)
    print(arr > 5)
    result = arr[arr>5]
    print(result)
    print(len(result))

# Exercise 17:  Create a horizontal gradient image of 64x64 that goes from black to white

def ex17():
    img = np.zeros((64,64))
    grad = np.arange(0.0,1.0,1.0/64.0)
    img = img + grad
    cv2.imshow("image", img)
    cv2.waitKey(0)

# Exercise 18: Create a vertical gradient image of 64x64 that goes from black to white

def ex18():
    img = np.zeros((64,64))
    grad = np.arange(0.0,1.0,1.0/64.0)
    grad = grad.reshape((64,1))
    img = img + grad
    cv2.imshow("image", img)
    cv2.waitKey(0)

# Exercise 19:Create a 3-component white image of 64x64 pixels and set the blue component to zero
# (the result should be yellow)

def ex19():
    img = 255 * np.ones((64,64,3), np.uint8)
    img[:, :, 0] = 0.0
    cv2.imshow("image", img)
    cv2.waitKey(0)

# Exercise 20: Create a 3-component white image of 64x64 pixels, set the blue component of the
# top-left part to zero (the result should be yellow) and the red component of the
# bottom-right part to zero (the result should be cyan

def ex20():
    img = 255 * np.ones((64,64,3), np.uint8)
    img[:32, :32, 0] = 0.0
    img[32:, 32:, 2] = 0.0
    cv2.imshow("image", img)
    cv2.waitKey(0)

# Exercise 21: Open an image and insert black horizontal scan lines at 50%

def ex21():
    img = cv2.imread("vegetto.png", cv2.IMREAD_ANYCOLOR)
    img[::2, :] = 0.0
    cv2.imshow("image", img)
    cv2.waitKey(0)

# Exercise 22: Open an image and insert black vertical scan lines at 50%

def ex22():
    img = cv2.imread("vegetto.png", cv2.IMREAD_ANYCOLOR)
    img[:, ::2] = 0.0
    cv2.imshow("image", img)
    cv2.waitKey(0)

# Exercise 23: Open an image, convert it to float64, normalize it, darken it 50%, and show it
def ex23():
    img = cv2.imread("vegetto.png", cv2.IMREAD_ANYCOLOR)
    # normalize it by diving between mas (find max)
    img[:, ::2] = (img[:, ::2]/2)
    cv2.imshow("image", img)
    cv2.waitKey(0)

ex23()


# Play with Kalman filters from Kalman-and-Bayesian-Filters-in-Python
# Press the green button in the gutter to run the script.
# install packages using 'python -m pip install numpy, matplotlib, scipy, pyfilter
# References:
#   [2] Roger Labbe. "Kalman and Bayesian Filters in Python"  -->kf_book
#   https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Pythonimport numpy as np
# Dependencies:  python3.10, numpy, matplotlib, math, filterpy, ./kf_book
from numpy.random import randn
import numpy as np
import matplotlib.pyplot as plt
import math
from kf_book.nonlinear_plots import plot_gaussians
from filterpy.kalman import KalmanFilter
from filterpy.common import Q_discrete_white_noise
from math import sqrt

def univariate_filter(x0, P, R, Q):
    kf1 = KalmanFilter(dim_x=1, dim_z=1, dim_u=1)
    kf1.x = np.array([[x0]])
    kf1.P *= P
    kf1.H = np.array([[1.]])
    kf1.F = np.array([[1.]])
    kf1.B = np.array([[1.]])
    kf1.Q *= Q
    kf1.R *= R
    return kf1

def pos_vel_filter(x, P, R, Q=0., dt=1.0):
    """ Returns a KalmanFilter which implements a
    constant velocity model for a state [x dx].T
    """

    kf = KalmanFilter(dim_x=2, dim_z=1)
    kf.x = np.array([x[0], x[1]]) # location and velocity
    kf.F = np.array([[1., dt],
                     [0.,  1.]])  # state transition matrix
    kf.H = np.array([[1., 0]])    # Measurement function
    kf.R *= R                     # measurement uncertainty
    if np.isscalar(P):
        kf.P *= P                 # covariance matrix
    else:
        kf.P[:] = P               # [:] makes deep copy
    if np.isscalar(Q):
        kf.Q = Q_discrete_white_noise(dim=2, dt=dt, var=Q)
    else:
        kf.Q[:] = Q
    return kf

def plot_1d_2d(xs, xs1d, xs2d):
    plt.plot(xs1d, label='1D Filter')
    plt.scatter(range(len(xs2d)), xs2d, c='r', alpha=0.7, label='2D Filter')
    plt.plot(xs, ls='--', color='k', lw=1, label='track')
    plt.title('State')
    plt.legend(loc=4)
    plt.show()

def compare_1D_2D(x0, P, R, Q, vel, u=None):
    # storage for filter output
    xs, xs1, xs2 = [], [], []

    # 1d KalmanFilter
    f1D = univariate_filter(x0, P, R, Q)

    #2D Kalman filter
    f2D = pos_vel_filter(x=(x0, vel), P=P, R=R, Q=0)
    if np.isscalar(u):
        u = [u]
    pos = 0 # true position
    for i in range(100):
        pos += vel
        xs.append(pos)

        # control input u - discussed below
        f1D.predict(u=u)
        f2D.predict()

        z = pos + randn()*sqrt(R) # measurement
        f1D.update(z)
        f2D.update(z)

        xs1.append(f1D.x[0])
        xs2.append(f2D.x[0])
    plt.figure()
    plot_1d_2d(xs, xs1, xs2)
    print('example2:  done')

if __name__ == '__main__':
    compare_1D_2D(x0=0, P=50., R=5., Q=.02, vel=1.)


"""Test localization module."""
import numpy as np
from numpy import sqrt

from dapper.tools.localization import pairwise_distances

A2 = [[0, 0], [0, 1], [1, 0], [1, 1], [2, 2]]

def test_pairwise_distances_2():
    """Test with a 2-dimensional collection."""
    dists = pairwise_distances(A2)
    answer = np.array(
        [
            [0.0, 1.0, 1.0, sqrt(2), sqrt(8)],
            [1.0, 0.0, sqrt(2), 1.0, sqrt(5)],
            [1.0, sqrt(2), 0.0, 1.0, sqrt(5)],
            [sqrt(2), 1.0, 1.0, 0.0, sqrt(2)],
            [sqrt(8), sqrt(5), sqrt(5), sqrt(2), 0.0],
        ]
    )
    assert np.all(dists == answer)

A3 = np.array(
    [
        [0, 0, 0],
        [1, 0, 0],
        [2, 0, 0],
        [0, 0, 2],
        [0, 0, 3],
    ]
)

def test_pairwise_distances_3():
    """Test with a 3-dimensional collection."""
    dists = pairwise_distances(A3)
    answer = np.array(
        [
            [0.0, 1.0, 2.0, 2.0, 3.0],
            [1.0, 0.0, 1.0, 2.236068, 3.162278],
            [2.0, 1.0, 0.0, 2.828427, 3.605551],
            [2.0, 2.236068, 2.828427, 0.0, 1.0],
            [3.0, 3.162278, 3.605551, 1.0, 0.0],
        ]
    )
    assert np.allclose(dists, answer)

def test_pairwise_distances_3_periodic():
    """Test with a 3-dimensional collection, assuming a periodic domain."""
    dists = pairwise_distances(A3, domain=(10, 10, 3))
    answer = np.array(
        [
            [0.0, 1.0, 2.0, 1.0, 0.0],
            [1.0, 0.0, 1.0, 1.414214, 1.0],
            [2.0, 1.0, 0.0, 2.236068, 2.0],
            [1.0, 1.414214, 2.236068, 0.0, 1.0],
            [0.0, 1.0, 2.0, 1.0, 0.0],
        ]
    )
    assert np.allclose(dists, answer)


# load the data for time-series
import numpy as np
from scipy import signal

from load_time_series import load_data

np.random.seed(231)

x = np.array([1, 2, 3, 4])
# print("train_set_x[0]: ", x)
print("len of x: ", len(x))

filter_size = 2
corr_filter = np.array([1, 2])

standard_corr = signal.correlate(x, corr_filter, 'valid')
print("len of standard corr: ", len(standard_corr))

# print("standard_corr:", standard_corr)

def fft_cross_correlation(x, output_len, preserve_energy_rate=0.95):
    xfft = np.fft.fft(x)
    squared_abs = np.abs(xfft) ** 2
    full_energy = np.sum(squared_abs)
    current_energy = 0.0
    preserve_energy = full_energy * preserve_energy_rate
    index = 0
    while current_energy < preserve_energy and index < len(squared_abs):
        current_energy += squared_abs[index]
        index += 1
    xfft = xfft[:index]
    filterfft = np.conj(np.fft.fft(corr_filter, len(xfft)))
    # element-wise multiplication in the frequency domain
    out = xfft * filterfft
    # take the inverse of the output from the frequency domain and return the modules of the complex numbers
    out = np.fft.ifft(out)
    output = np.array(out, np.double)
    # output = np.absolute(out)

    output = output[:output_len]
    return output

# print("output of cross-correlation via fft: ", output)
output = fft_cross_correlation(x, len(standard_corr), preserve_energy_rate=0.95)
print("is the fft cross_correlation correct: ", np.allclose(output, standard_corr, atol=1e-12))
print("absolute error: ", np.sum(np.abs(output - standard_corr)))


"""
This module stores all the utility functions
that are required for the implementation of
of any Machine Learning logic but not associated
to any particular technique.
"""

import numpy as np

def euclidean_distance(data_points):
    """
    It calculates euclidean distance between two
    set of points.

    :param data_points: np.narray for two set of points
    :return: Euclidean Distance
    """
    assert len(data_points) == 2
    # Sum of Squared distance as in Euclidean distance
    return np.sqrt(np.sum((data_points[0] - data_points[1]) ** 2))


# Step 1: Load dataset, split into training and test sets, and scale features
import numpy as np
from sklearn.datasets import load_boston

# load boston housing price dataset
boston = load_boston()
x = boston.data
y = boston.target

# split into training and test sets, namely 80 percent of examples goes for the training, 20 percent goes for the test set
N_train = int(0.8 * x.shape[0])
x_train = x[:N_train,:]
y_train = y[:N_train]
x_test = x[N_train:,:]
y_test = y[N_train:]

# scale features by removing mean and dividing by the standard deviation
x_train_scaled = # YOUR CODE GOES HERE
x_test_scaled = # YOUR CODE GOES HERE

print(x_train_scaled.shape)
print(y_train.shape)
print(x_test_scaled.shape)
print(y_test.shape)

# Step 2: Add intercept terms and initialize parameters
# Note: If you run this step again, please run from step 1 because notebook keeps the value from the previous run
x_train_scaled = # YOUR CODE GOES HERE

x_test_scaled = # YOUR CODE GOES HERE

print(x_train_scaled.shape)
print(x_test_scaled.shape)

# init parameters using random values
theta = # YOUR CODE GOES HERE
print(theta)

# Step 3: Implement the gradient and the cost function
# In this step, you have to calculate the gradient. You can use the provided formula but the best way is to vectorize
# that formula for efficiency
def compute_gradient(x,y,theta):
    # YOUR CODE GOES HERE

def compute_cost(x,y,theta):
    # YOUR CODE GOES HERE

# Step 4: Verify the gradient value
# In this step, you need to verify that the computed gradient is correct. The difference betweet the gradient and the
# approximate gradient should be very small (~10^-18)
def approximate_gradient(x,y,theta,epsilon):
    n_features = x.shape[1]
    app_grad = np.zeros(n_features)
    for i in range(n_features):
        app_grad[i] = # YOUR CODE GOES HERE
    return app_grad

grad = compute_gradient(x_train_scaled,y_train,theta)
epsilon = 1e-4
app_grad = approximate_gradient(x_train_scaled,y_train,theta,epsilon)
print('Sum of gradient squared error: ',np.sum((grad - app_grad)**2))

# Step 5: Try gradient descent algorithm with different learning rates
import matplotlib.pyplot as plt
import copy

# try different values for the learning rate
learning_rates = [0.001,0.003,0.01,0.03,0.1,0.3]

# this matrix keeps the learned parameters
theta_matrix = np.zeros((len(learning_rates),x_train_scaled.shape[1]))

# number of training iterations
N_iterations = 100

# prepare to plot
plt.subplot(111)

# calculate cost value and update theta
for indx,alpha in enumerate(learning_rates):
    # keep the cost value for each training step
    J = np.zeros(N_iterations)

    # initialize new parameters using random distribution
    theta = 0.5 * np.random.randn(x_train_scaled.shape[1])
    for step in range(N_iterations):
        # update theta
        theta = # YOUR CODE GOES HERE

        # save the value of theta
        theta_matrix[indx,:] = theta

        # calculate the cost on traing set
        J[step] = # YOUR CODE GOES HERE
    # plot cost function
    plt.plot(J)
plt.xlabel('Training step')
plt.ylabel('Cost')
plt.legend(('0.001','0.003','0.01','0.03','0.1','0.3'), loc='upper right')
plt.show()

# Step 6: Predict the price of house
# You have to select the best theta you found
theta = # YOUR CODE GOES HERE
predict_price = # YOUR CODE GOES HERE

# calculate the cost for the test set
test_cost = # YOUR CODE GOES HERE
print('test cost: ',test_cost)

# plot the ground truth and the predicted
x_axis = np.linspace(1,len(y_test),len(y_test))
plt.plot(x_axis,y_test,'b',x_axis,predict_price,'r')
plt.legend(('Ground truth','Predicted'))
plt.show()


import numpy as np
import scipy.io
import matplotlib.pyplot as plt
import random

def generate_heat_equation_data(amplitude=1.0, n=1, phase=0):
    """
    @param amplitude: maximum temperature of initial condition
    @param n: [int] no of harmonics of wave of initial condition
    @param phase: phase shift of the initial condition
    @return: numpy array
    """

    L=1             # length of rod in cm
    max_t = 2       # maximum time steps in seconds
    c = 0.03       # intrinsic property of metal used - fixed for same type of metal
    exponential_const = (c * n * np.pi / L) ** 2

    t = np.linspace(0, max_t, 200)
    x = np.linspace(0, L, 200)
    X, T = np.meshgrid(x, t)
    # X = X.reshape(-1,1)
    # T = T.reshape(-1,1)
    U = amplitude*(np.exp(-exponential_const*T))*np.sin((n * np.pi * X / L) + phase)
    U = U.reshape(200,200)

    # [DEBUG]
    # to see results
    # Domain bounds
    lb = np.array([0, 0.0])
    ub = np.array([L, max_t])
    def show_plot():
        plt.imshow(U.T, interpolation='nearest', cmap='YlGnBu',
                   extent=[lb[1], ub[1], lb[0], ub[0]],
                   origin='lower', aspect='auto')

        plt.ylabel('x (cm)')
        plt.xlabel('t (seconds)')
        plt.axis()
        plt.colorbar().set_label('Temperature (°C)')
        plt.show()
    # show_plot()
    return U.T

def generate_dataset():
    heat_dataset = np.zeros((1100, 200, 200))
    cnt = 0
    for amplitude in range(11):
        for harmonics in range(1,11):
            for phase_shift in range(10):
                phase_shift = random.random() * 2 * np.pi
                heat_dataset[cnt] = generate_heat_equation_data(amplitude=random.random(), n=harmonics, phase=phase_shift)
                cnt += 1
    np.random.shuffle(heat_dataset)
    return heat_dataset

if __name__ == '__main__':
    # generate range of data of heat equation
    dataset = generate_dataset()
    # save the matrix
    scipy.io.savemat('data/heat_N1100_T200_r200.mat', mdict={ 'u': dataset})


from veidt.elsie.preprocessing import Preprocessing
from pymatgen.core.spectrum import Spectrum
import numpy as np
import unittest
from copy import deepcopy

class PreprocessingTest(unittest.TestCase):
    def setUp(self):
        self.x_range = np.linspace(-np.pi, np.pi, 201)
        self.spectrum = Spectrum(self.x_range, np.sin(self.x_range))

    def test_derivative(self):
        deri_preprocessing = Preprocessing(deepcopy(self.spectrum))
        deri_preprocessing.first_derivative()
        self.assertTrue(np.allclose(deri_preprocessing.spectrum.y, np.cos(self.x_range)[:-1], atol=2e-2))

        deri_2nd_pre = Preprocessing(deepcopy(self.spectrum))
        deri_2nd_pre.second_derivative()
        self.assertTrue(np.allclose(deri_2nd_pre.spectrum.y, np.negative(np.sin(self.x_range))[:-2], atol=3.5e-2))

    def test_weighted_derivative(self):
        deri_preprocessing = Preprocessing(Spectrum(self.x_range, 2 * self.x_range))
        deri_preprocessing.weighted_first_derivative()
        self.assertTrue(np.allclose(deri_preprocessing.spectrum.y, 4 * self.x_range[:-1]))

        deri_preprocessing = Preprocessing(Spectrum(self.x_range, 2 * self.x_range))
        deri_preprocessing.weighted_second_derivative()
        self.assertTrue(np.allclose(deri_preprocessing.spectrum.y, 0))

    def test_normalization(self):
        normal_pre = Preprocessing(Spectrum(self.x_range, np.abs(np.sin(self.x_range))))
        normal_pre.intensity_normalize()
        self.assertAlmostEqual(np.sum(normal_pre.spectrum.y), 1)

        normal_max = Preprocessing(Spectrum(self.x_range, np.abs(np.sin(self.x_range))))
        normal_max.maximum_intensity_norm()
        self.assertEqual(max(normal_max.spectrum.y), 1)

        vector_norm = Preprocessing(Spectrum(self.x_range, np.abs(np.sin(self.x_range))))
        vector_norm.vector_norm_normalize()
        self.assertTrue(np.allclose(vector_norm.spectrum.y.sum() * np.linalg.norm(np.abs(np.sin(self.x_range))),
                                    np.abs(np.sin(self.x_range)).sum()))

        area_norm = Preprocessing(Spectrum(self.x_range, np.abs(np.sin(self.x_range))))
        area_norm.area_normalize()
        self.assertTrue(np.allclose(np.trapz(area_norm.spectrum.y, area_norm.spectrum.x), 1))

    def test_spec_preprocessing(self):
        x_range = np.linspace(0, np.pi / 2, 100)
        to_compare = np.cos(x_range)[:-1]
        to_compare /= to_compare.sum()
        spec_preprocessing = Preprocessing(Spectrum(x_range, np.sin(x_range)))
        spec_preprocessing.spectrum_process(('1st_der', 'intnorm'))

        self.assertAlmostEqual(spec_preprocessing.spectrum.y.sum(), 1)
        self.assertTrue(np.allclose(spec_preprocessing.spectrum.y, to_compare, atol=1.5e-4))

    def test_property(self):
        spec_preprocessing = Preprocessing(self.spectrum)
        self.assertTrue(set(spec_preprocessing.preprocessing_method) == set(
            ['first_derivative', 'second_derivative', 'vector_norm_normalize', 'maximum_intensity_norm',
             'area_normalize',
             'snv_norm', 'square_root_squashing', 'sigmoid_squashing', 'weighted_first_derivative',
             'weighted_second_derivative', 'intensity_normalize']))


from functools import reduce
from hw1.ex12 import solve_ex1

import numpy as np
import random

def solve_diagonal_system(system, result):
    assert len(system.shape) == 2
    assert system.shape[0] == system.shape[1], "Must be a square matrix"

    solution = np.zeros((system.shape[1], 1))
    lines, columns = system.shape
    for idx in reversed(range(0, lines)):
        line_offset = reduce(lambda accumulator, comb: accumulator + comb[0] * comb[1],
                             zip(system[idx, idx + 1:], solution[idx + 1:, 0]),
                             0)

        solution[idx, 0] = (result[idx, 0] - line_offset) / system[idx][idx]

    return solution

def reduce_system(system, result, column=0):
    if column == system.shape[1]:
        return

    idx = column + system[column:, column].argmax()
    system[[column, idx]] = system[[idx, column]]
    result[[column, idx]] = result[[idx, column]]

    for line in range(column + 1, system.shape[0]):
        epsilon = solve_ex1()
        if -epsilon <= system[line, column] <= epsilon:
            continue

        normalization_factor = system[column, column] / system[line, column]
        system[line] *= normalization_factor
        system[line] -= system[column]

        result[line, 0] *= normalization_factor
        result[line, 0] -= result[column, 0]

    reduce_system(system, result, column=column + 1)

def solve_system(system, result):
    internal_system = np.copy(system)
    internal_result = np.copy(result)

    reduce_system(internal_system, internal_result)
    return solve_diagonal_system(internal_system, internal_result)

def generate_random_system(size):
    system = []
    for i in range(0, size):
        current_line = []
        for j in range(0, size):
            current_line.append(random.random() * 10)
        system.append(current_line)

    result = []
    for i in range(0, size):
        result.append([random.random() * 10])
    return np.array(system), result

def flip(vali_list):
    result = []
    for x in vali_list:
        result.append(x[0])
    return result

if __name__ == '__main__':
    sys_result_pair = generate_random_system(100)

    system = sys_result_pair[0]
    result = sys_result_pair[1]

    try:
        determinant = np.linalg.det(system)
        if determinant == 0:
            raise ValueError("Error: Determinant is 0")
    except ValueError as error:
        print(repr(error))
        exit(1)

    solution = solve_system(
        system,
        result
    )

    np_solution = np.linalg.solve(system, result)

    """
    print("Solution =", solution)
    print("NP Solution =", np_solution)
    """

    solution_norm = solution - np_solution
    print("Norma solutia noastra - solutia biblioteca =", np.linalg.norm(solution_norm))

    solution_mul_sys = np.matmul(system, solution) - result
    print("Norma solutia noastra * sistemul - rezultatul =", np.linalg.norm(solution_mul_sys))

    np_solution_mul_sys = np.matmul(system, np_solution) - result
    print("Norma solutia biblioteca * sistemul - rezultatul =", np.linalg.norm(np_solution_mul_sys))
    """
    solution_result = np.matmul(system, solution)
    print("A * x_sol =", solution_result)
    print("Result = ", result)
    print("Norm: ", np.linalg.norm(result - solution_result))
    """


import numpy as np
from tqdm import tqdm
from numpy import linalg as LA

class KernelBalancing:
    def __init__(self,
                 X,
                 A,
                 Y,
                 _lambda
                 ):
        self.X = X
        self.A = A
        self.Y = Y
        self._lambda = _lambda
        self.N = X.shape[0]
        self.M = Y.shape[0]
        self.KXX = np.zeros((self.N,self.N))
        self.KXY = np.zeros((self.N,self.M))
        self.KYY = np.zeros((self.M,self.M))
    def K(self,
          x,
          y):
        """
        x,y : np.array
        """
        return np.exp(-LA.norm(x - y)**2*.5)
        #return LA.norm(x - y)**2

    def getGramMatrix(self,
                      KXX = None,
                      KXY = None,
                      KYY = None):
        if KXX==None:
            print("Calculating Gram Matrix...")
            for i in range(self.N):
                for j in range(self.N):
                    self.KXX[i,j] = self.K(self.X[i],self.X[j])
            for i in range(self.N):
                for j in range(self.M):
                    self.KXY[i,j] = self.K(self.X[i],self.Y[j])
            for i in range(self.M):
                for j in range(self.M):
                    self.KYY[i,j] = self.K(self.Y[i],self.Y[j])
    def H(self,
          W):
        return -2.*self.A.dot(self.KXY).dot(W[:-1]) + W[:-1].dot(self.KYY).dot(W[:-1]) + self._lambda*np.sum(W[:-1]**2)

    def nabla_k(self,
                k,
                W):
        k = int(k)
        return -2.*np.dot(np.transpose(self.A),self.KXY[:,k]) + 2.*self._lambda  * W[k] + 2.*np.sum(self.KYY[k,:]*W[:-1]) + 2.*(np.sum(W[:-1])-1.)*W[-1]*W[k]

    def nabla_H(self, W):
        gradH = np.zeros(self.M+1)
        for i in range(self.M):
            gradH[i] = self.nabla_k(i,W)
        gradH[-1] = 1. - np.sum(W[:-1])
        return gradH
    def getOptimalWeights(self,
                          learning_rate = 0.001,
                          max_iter = 1000,
                          epsilon = 1e-7):
        W = np.zeros(self.M+1)
        W[:-1] = np.zeros(self.M) + 1./self.M
        W[-1] = 1.
        H_trace = []
        H_old = self.H(W)
        for i in tqdm(range(max_iter)):
            H_trace += [H_old]
            W -= learning_rate* self.nabla_H(W = W)
            for d in range(self.M+1):
                W[d] = np.max([W[d],0.])

            H_new = self.H(W)
            if  np.abs(H_new- H_old) <= epsilon:
                print("The preset tolorance is reached.")
                break
            H_old = H_new
        print("final objective value:", H_new)
        return W[:-1], np.array(H_trace)


# Matplotlib Plotting Tutorials
# Plot Animation
# 1D Heat equation

import numpy as np
import matplotlib.pyplot as plt

L = 10                          # Length of rod
nx = 100                        # number of points on the rod
T = 500                         # Total time for simulation
nt = 500                        # number of time steps
alpha = 5e-3                    # Diffusion Coefficient

x = np.linspace(0, L, nx+1)     # points on the rod
dx = x[1] - x[0]                # Spacing between points
t = np.linspace(0, T, nt+1)     # instances in time
dt = t[1] - t[0]                # time step
F = alpha*dt/dx**2              # diffusion parameter
fac = 1.0 - 2.0*F               # constant in the equation

# Set initial condition: a sine wave
Temp_init = 100*np.sin(2*np.pi*x/L)

Temp_old = np.copy(Temp_init)   # known T at old time level
Temp_new = np.zeros(nx+1)       # unknown T at the present time level

# To enable interactive plotting
plt.ion()
plt.figure()

for n in range(0, nt):
    print('time t=',t[n])
    # Compute temperature at all the points
    Temp_new[1:-1] = fac*Temp_old[1:-1] + F*(Temp_old[0:-2] + Temp_old[2:])

    # Update temperature before next step
    Temp_old[:]= Temp_new

    plt.clf()  # Clear the figure
    plt.plot(x, Temp_init, color='xkcd:black', label='Initial Temperature profile')
    plt.plot(x, Temp_old, color='xkcd:salmon', label='Current Temperature profile')  # Contour plotting the values
    plt.xlabel('Length', fontsize=10)  # X axis label
    plt.ylabel('Temperature in $^o C$', fontsize=10)  # Y axis label
    plt.title('Temperature Diffusion in a 1D rod')
    # string = " ".join(['Time $t$ = ', str(t[n]), ' s, $T_{max}$ = ', str(np.amax(Temp_old)), ' $^o C$, $T_{min}$ = ', str(np.amin(Temp_old)), '$^o C$'])
    string = 'Time $t$ = '+str(t[n])+' s, $T_{max}$ = '+str(np.amax(Temp_old))+' $^o C$, $T_{min}$ = '+str(np.amin(Temp_old))+'$^o C$'
    plt.suptitle(string)
    plt.legend()
    plt.grid(True)  # Enabling gridding
    plt.axis((-0.1, 10.1, -105, 105))  # Making axis rigid
    plt.pause(0.01)


import numpy as np
import matplotlib.pyplot as plt

mu = 0.0
sigma = 1.

x = np.linspace(-3.0, 3.0, 100)
y = np.exp(-((x - mu) ** 2) / (2 * sigma ** 2)) / np.sqrt(2 * np.pi * sigma ** 2)

plt.plot(x,y,color='black')
plt.fill_between(x, y, np.zeros_like(y),color='#add8e6')
plt.tick_params(bottom=False, left=False, right=False, top=False)
plt.tick_params(labelbottom=False, labelleft=False, labelright=False, labeltop=False)
plt.gca().spines["right"].set_visible(False)
plt.gca().spines["top"].set_visible(False)
plt.gca().spines["left"].set_visible(False)
plt.ylim(0.,0.5)
# plt.show()
# plt.savefig('gaussian.pdf')
plt.savefig('gaussian.svg')


import numpy as np
import matplotlib.pyplot as plt
from astropy.visualization import astropy_mpl_style
from scipy.special import factorial
plt.style.use(astropy_mpl_style)

def thermal(n, n_bar):
  return ((n_bar/(1+nbar)) ** n / (1 + n_bar))

def coherent(n, n_bar):
  return (np.exp(-n_bar) * n_bar ** n / factorial(n))

ns = np.arange(0, 40)
nbars = [6]
width = 0.35
alpha = .8

for nbar in nbars:
  plt.bar(ns, coherent(ns, nbar), label='coherent ' + str(nbar), alpha = alpha, width=width)
  plt.bar(ns+width, thermal(ns, nbar), label='thermal ' + str(nbar), alpha = alpha, width=width)
plt.legend()
plt.show()

def moment(b, v, n):
  m = np.average(b, weights=v)
  if (n == 1):
    return(m)
  return (np.sum((b - m)**n * v) / np.sum(v))

def analyze(dist, ns, nbar):
  bins = dist(ns, nbar)
  moments = {'mean': 1, 'variance': 2, 'skewness': 3, 'kurtosis': 4}
  for m, num in moments.items():
    print(m, ' = ', moment(ns, bins, num))

"""
For a Poissonian (coherent): mean=variance=skewness=nu
and kurtosis = nu*(1+3*nu)

For the thermal distribution: nth moment is something like exp(n)
"""

import numpy,from scipy,import astropy,from astropy


import numpy

def LUDecompose (table):
    #table that contains our data
    #table has to be a square array so we need to check first
    rows,columns=numpy.shape(table)
    L=numpy.zeros((rows,columns))
    U=numpy.zeros((rows,columns))
    if rows!=columns:
        return
    for i in range (columns):
        for j in range(i-1):
            sum=0
            for k in range (j-1):
                sum+=L[i][k]*U[k][j]
            L[i][j]=(table[i][j]-sum)/U[j][j]
        L[i][i]=1
        for j in range(i-1,columns):
            sum1=0
            for k in range(i-1):
                sum1+=L[i][k]*U[k][j]
            U[i][j]=table[i][j]-sum1
    return L,U

matrix =numpy.array([[2,-2,1],[0,1,2],[5,3,1]])
L,U = LUDecompose(matrix)
print(L)
print(U)


# Compute pi value by Monte Carlo sampling
# circle area within a square with unit length
# pi = Area / r^2
# Area = Area of the square * number of points within circle / total number of points
# Area of the square = 4 * r^2
# pi = 4.0 * number of points within circle / total number of points
import random
import numpy as np
import math

# number of points for sampling
N = 100000
r = 1.0
r2 = 1.0 * 1.0
A = 0
ii = 10000
for i in range(ii):
    xy = np.random.rand(2, N)
    xy2 = np.square(xy)
    A += np.sum(np.add(xy2[0,:], xy2[1,:]) < r2)
    pi = 4.0 * A / N / (i + 1)
    print "{0} samples, calculated pi value = {1} with error {2} compared to math.pi {3}".format(N * (i + 1), pi, abs(pi - math.pi), math.pi)
#pi = 4.0 * A / N / ii
#print "{0} samples, calculated pi value = {1} with error {2} compared to math.pi {3}".format(N * ii, pi, abs(pi - math.pi), math.pi)