#%% Load libraries
import matplotlib.pyplot as plt
%matplotlib inline
import seaborn as sns

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

#%% Population and samples
data = pd.DataFrame()
data['population'] = [47, 48, 49, 50, 19, 20, 21, 22, 25, 30, 60]

sample_noreplacement = data['population'].sample(10, replace=False)
sample_withreplacement = data['population'].sample(10, replace=True)

display(sample_noreplacement)  # no duplicates in index or values column
display(sample_withreplacement)  # duplicates could be observed

# parameters - population
# statistics - sample

# Print parameters of population:
print('Population mean, var, std, size are: {0}'.format([
    data['population'].mean(), data['population'].var(ddof=0),
    data['population'].std(ddof=0), data['population'].shape[0]
]))

# Print statistics os sample
a_sample = data['population'].sample(10, replace=True)
print('Sample mean, var, std, size are: {0}'.format([
    a_sample.mean(), a_sample.var(ddof=1), a_sample.std(ddof=1), a_sample.shape[0]
]))

#%% Explain why ddof=1
sample_length = 500
sample_variance_collection_0 = [
    data['population'].sample(50, replace=True).var(ddof=0)
    for i in range(sample_length)
]
sample_variance_collection_1 = [
    data['population'].sample(50, replace=True).var(ddof=1)
    for i in range(sample_length)
]

print('population variance is {0}'.format(data['population'].var(ddof=0)))
print('average of sample variance with n is {0}'.format(
    pd.DataFrame(sample_variance_collection_0).mean()
))
print('average of sample variance with n-1 is {0}'.format(
    pd.DataFrame(sample_variance_collection_1).mean()  # closer than using n to real population variance
))

# on average, the average of estimator using division n-1 equals population variance

# Degrees of Freedom == the number of values in calculation that are free to variate

#%% Variation and distribution of the sample mean

# Sampling fromm normal distribution
norm_dist_sample = pd.DataFrame(np.random.normal(10, 5, size=100))
print('Sample mean, std are {0}'.format([
    norm_dist_sample[0].mean(), norm_dist_sample[0].std(ddof=1)
]))

# Empirical distribution of sample mean and variance
mean_lst = []
var_lst = []
for t in range(1000):
    sample = pd.DataFrame(np.random.normal(10, 5, size=10))
    mean_lst.append(sample[0].mean())
    var_lst.append(sample[0].var(ddof=1))

collection = pd.DataFrame()
collection['mean_lst'] = mean_lst
collection['var_lst'] = var_lst

collection['mean_lst'].hist(bins=500, normed=1)  # kind of normally distributed
plt.show()

collection['var_lst'].hist(bins=500, normed=1)  # non-normal, right-skewed
plt.show()

#%% Why variance of sample mean is smaller than variance of population?
population = pd.DataFrame(np.random.normal(10, 5, size=100000))
population[0].hist(bins=100, color='cyan', normed=1)
collection['mean_lst'].hist(bins=100, normed=1, color='red')
plt.show()

#%% Sampling from general distribution | sampling vs big sampling
# ЦПТ - if sample size is large enough, the distribution of sample mean is approximately NORMAL
sample_mean_lst = []
population = pd.DataFrame([1, 0, 1, 0, 1])
for t in range(10000):
    sample_100 = population[0].sample(100, replace=True)  # pretty big sample size - 100
    sample_mean_lst.append(sample_100.mean())

collected_means = pd.DataFrame()
collected_means['mean_lst'] = sample_mean_lst
collected_means.hist(bins=100, normed=1, figsize=(15, 10))

#%% Try the same stuff but with lower sample size
sample_mean_lst = []
for t in range(100000):
    sample_10 = population[0].sample(10, replace=True)  # pretty big sample size - 100
    sample_mean_lst.append(sample_10.mean())

collected_means = pd.DataFrame()
collected_means['mean_lst'] = sample_mean_lst
collected_means.hist(bins=100, normed=1, figsize=(15, 10))

#%% Confidence intervals
aapl = pd.DataFrame.from_csv('data/apple.csv')
aapl['log_return'] = np.log(
    aapl['Close'].shift(-1) / aapl['Close']
)

# sigma - std; we can replace std with sample standard deviation IF sample size is large enough.

# Calculate 80% confidence interval
z_left = norm.ppf(0.1)
z_right = norm.ppf(0.9)
sample_mean = aapl['log_return'].mean()
sample_std = aapl['log_return'].std(ddof=1) / (aapl.shape[0])**0.5  # replace std with sample std

# Calculate left and right quantiles
interval_left = sample_mean + z_left * sample_std
interval_right = sample_mean + z_right * sample_std

print('Sample mean is {0}'.format(sample_mean))
print('80%% interval is {0}'.format([interval_left, interval_right]))

# 80%% interval is [0.0004927367254936761, 0.0014581987928065012]
# Because interval is on the positive side, it implies that the average return is very likely to be POSITIVE.

#%% 90% confidence interval
# Lets build 90% confidence interval for log return
sample_size = aapl['log_return'].shape[0]
sample_mean = aapl['log_return'].mean()
sample_std = aapl['log_return'].std(ddof=1) / sample_size**0.5

# left and right quantile
z_left = norm.ppf(0.05)
z_right = norm.ppf(0.95)

# upper and lower bound
interval_left = sample_mean + z_left * sample_std
interval_right = sample_mean + z_right * sample_std
print('90%% interval is {0}'.format([interval_left, interval_right]))
# 90%% interval is [0.0003558891850999989, 0.001595046333200178]

#%% Hypothesis testing
# Making a judgement, whether the condition (some kind of forecast, like "should you invest in this project with 36 months data at hand?") is satisfied.

# Plot AAPL close price & return
fig, [axis_0, axis_1] = plt.subplots(1, 2, figsize=(15, 2))
aapl.loc[:, 'Close'].plot(ax=axis_0)
aapl.loc[:, 'log_return'].plot(ax=axis_1)
axis_0.set_title('AAPL 2017-2018 Close price')
axis_1.set_title('AAPL 2017-2018 log Return')
plt.show()

# 1st step: set hypothesis
# we have NULL HYPOTHESIS and ALTERNATIVE HYPOTHESIS
# Null hypothesis is assertion we are against
# Alternative hypothesis is a conclusion we accept whenever we reject the null hypothesis.

# In our example, the Null H is "population mean, the average daily return is 0". (mu=0)
#                 the alternative hypothesis is "average daily return does not equal to 0". (mu!=0)

# In our example, treat t as if it is z distribution when N is large

# calculate z_hat
xbar = aapl['log_return'].mean()
s = aapl['log_return'].std(ddof=1)
n = aapl['log_return'].shape[0]
z_hat = (xbar - 0) / (s / (n**0.5))
print('z_hat is {0}'.format(z_hat))  # z_hat is 2.58966618410296

#%% Set decision criteria
alpha = 0.05
z_left = norm.ppf(alpha / 2, 0, 1)
z_right = -z_left

print('z_left and z_right are {0} at the significance of {1}'.format([z_left, z_right], alpha))
# z_left and z_right are [-1.9599639845400545, 1.9599639845400545] at the significance of 0.05

print('Shall we reject? {0}'.format(z_hat>z_right or z_hat<z_left))
# Shall we reject? True


import numpy as np
import scipy.linalg

def estsimt(X1,X2):
    # from estsimt.m in MultiCameSelfCal

    # ESTimate SIMilarity Transformation
    #
    # [s,R,T] = estsimt(X1,X2)
    #
    # X1,X2 ... 3xN matrices with corresponding 3D points
    #
    # X2 = s*R*X1 + T
    # s ... scalar scale
    # R ... 3x3 rotation matrix
    # T ... 3x1 translation vector
    #
    # This is done according to the paper:
    # "Least-Squares Fitting of Two 3-D Point Sets"
    # by K.S. Arun, T. S. Huang and S. D. Blostein

    N = X1.shape[1]
    if N != X2.shape[1]:
        raise ValueError('both X1 and X2 must have same number of points')

    X1cent = np.mean(X1,axis=1)
    X2cent = np.mean(X2,axis=1)
    # normalize coordinate systems for both set of points
    x1 = X1 - X1cent[:,np.newaxis]
    x2 = X2 - X2cent[:,np.newaxis]

    # mutual distances
    d1 = x1[:,1:]-x1[:,:-1]
    d2 = x2[:,1:]-x2[:,:-1]
    ds1 = np.sqrt( np.sum( d1**2, axis=0) )
    ds2 = np.sqrt( np.sum( d2**2, axis=0) )

    #print 'ds1'
    #print ds1

    scales = ds2/ds1
    s = np.median( scales )

    #print 's', s

    # undo scale
    x1s = s*x1

    # finding rotation
    H = np.zeros((3,3))
    for i in range(N):
        tmp1 = x1s[:,i,np.newaxis]
        #print 'tmp1',tmp1
        tmp2 = x2[np.newaxis,:,i]
        #print 'tmp2',tmp2
        tmp = np.dot(tmp1,tmp2)
        #print 'tmp'
        #print tmp
        H += tmp
        #print 'H'
        #print H

    U,S,Vt = scipy.linalg.svd(H)
    #print 'U'
    #print U
    #print 'S'
    #print S
    #print 'Vt'
    #print Vt
    V = Vt.T
    X = np.dot(V,U.T)
    R=X

    T = X2cent - s*np.dot(R,X1cent)
    return s,R,T

def build_xform(s,R,t):
    T = np.zeros((4,4),dtype=np.float)
    T[:3,:3] = R
    T = s*T
    T[:3,3] = t
    T[3,3]=1.0
    return T

def align_points( s,R,T, X ):
    assert X.ndim==2
    assert X.shape[0] in [3,4] # either 3D or 3D homogeneous
    T = build_xform(s,R,T)
    if X.shape[0]==3:
        # make homogeneous
        Xnew = np.ndarray((4,X.shape[1]),dtype=X.dtype)
        Xnew[3,:].fill(1)
        Xnew[:3,:] = X
        X = Xnew
    X = np.dot(T,X)
    return X

def align_pmat( s,R,T, P ):
    T = build_xform(s,R,T)
    P = np.dot(P,scipy.linalg.inv(T))
    return P

def align_pmat2( M, P ):
    P = np.dot(P,np.dual.inv(M))
    return P

def test_align():
    orig_points = np.array([
        [3.36748406,  1.61036404,  3.55147255],
        [3.58702265,  0.06676394,  3.64695356],
        [0.28452026, -0.11188296,  3.78947735],
        [0.25482713,  1.57828256,  3.6900808],

        [3.54938525,  1.74057692,  5.13329681],
        [3.6855626 ,  0.10335229,  5.26344841],
        [0.25025385, -0.06146044,  5.57085135],
        [0.20742481,  1.71073272,  5.41823085],
        ]).T

    ft2inch = 12.0
    inch2cm = 2.54
    cm2m = 0.01
    ft2m = ft2inch * inch2cm * cm2m

    x1,y1,z1=0,0,0
    x2,y2,z2=np.array([10,5,5])*ft2m

    new_points = np.array([
        [x2, y2, z2],
        [x2, y1, z2],
        [x1, y1, z2],
        [x1, y2, z2],

        [x2, y2, z1],
        [x2, y1, z1],
        [x1, y1, z1],
        [x1, y2, z1],
        ]).T

    print orig_points.T
    print new_points.T

    s,R,t = estsimt(orig_points,new_points)
    print 's=%s'%repr(s)
    print 'R=%s'%repr(R.tolist())
    print 't=%s'%repr(t.tolist())
    Xnew = align_points( s,R,t, orig_points )

    # measure distance between elements
    mean_absdiff = np.mean( abs(Xnew[:3]-new_points).flatten() )
    assert mean_absdiff < 0.05

    pmat_orig = np.array([[1,2,3,4],
                          [5,6,7,8],
                          [9,10,11,12]],dtype=np.float)
    print 'Xnew.T'
    print Xnew.T

    pmat_new = align_pmat( s,R,t, pmat_orig )
    print 'pmat_new'
    print pmat_new

    ## print 's',s
    ## print 'R'
    ## print R
    ## print 'T'
    ## print T


#!/usr/bin/python

import scipy
import sys
if sys.version_info[0] == 2:
  # get rid of 2.x range that produced list instead of iterator
  range = xrange

def getGrid(fileName):
  grid = []
  with open(fileName, 'r') as fIn:
    for line in fIn:
      grid.append([int(n) for n in line.split()])
  return scipy.array(grid)

def genNegSlopeDiagonal(grid, numElements):
  numRows, numCols = grid.shape
  nEM1 = numElements - 1
  for row in range(numRows - nEM1):
    for col in range(numCols - nEM1):
      diag = grid[range(row, row + numElements),
                  range(col, col + numElements)]
      yield diag

def genPosSlopeDiagonal(grid, numElements):
  numRows, numCols = grid.shape
  nEM1 = numElements - 1
  for row in range(nEM1, numRows):
    for col in range(numCols - nEM1):
      diag = grid[range(row, row - numElements, -1),
                  range(col, col + numElements)]
      yield diag

def genHoriz(grid, numElements):
  numRows, numCols = grid.shape
  nEM1 = numElements - 1
  for row in range(numRows):
    for col in range(0, numCols - nEM1):
      horiz = grid[row, range(col, col + numElements)]
      yield horiz

def genVert(grid, numElements):
  numRows, numCols = grid.shape
  nEM1 = numElements - 1
  for row in range(numRows - nEM1):
    for col in range(numCols):
      vert = grid[range(row, row + numElements), col]
      yield vert

def genLine(grid, numElements):
  for seg in genNegSlopeDiagonal(grid, numElements):
    yield (scipy.prod(seg), seg, 'negative slope')
  for seg in genPosSlopeDiagonal(grid, numElements):
    yield (scipy.prod(seg), seg, 'positive slope')
  for seg in genHoriz(grid, numElements):
    yield (scipy.prod(seg), seg, 'horizontal')
  for seg in genVert(grid, numElements):
    yield (scipy.prod(seg), seg, 'vertical')

def euler11(gridFile='data/euler011.txt', numElements=4):
  grid = getGrid(gridFile)
  maxSeg = max(genLine(grid, numElements), key=lambda x: x[0])

  print('Greatest product is %d, From %s line with elements: %s'
        % (maxSeg[0], maxSeg[2], str(maxSeg[1])))

if __name__ == "__main__":
  euler11()


import cv2
import numpy as np
from numpy.fft import *
import matplotlib.pyplot as plt

def DFT(sig):
    N = sig.size
    V = np.array([[np.exp(-1j * 2 * np.pi * v * y / N) for v in range(N)] for y in range(N)])
    return sig.dot(V)

def FFT(x):
    N = x.shape[1]  # 只需考虑第二个维度，然后在第一个维度循环
    if N % 2 > 0:
        raise ValueError("size of x must be a power of 2")
    elif N <= 8:  # this cutoff should be optimized
        return np.array([DFT(x[i, :]) for i in range(x.shape[0])])
    else:
        X_even = FFT(x[:, ::2])
        X_odd = FFT(x[:, 1::2])
        factor = np.array([np.exp(-2j * np.pi * np.arange(N) / N) for i in range(x.shape[0])])
        return np.hstack([X_even + np.multiply(factor[:, :int(N / 2)], X_odd),
                          X_even + np.multiply(factor[:, int(N / 2):], X_odd)])

def FFT2D(img):
    return FFT(FFT(img).T).T

def FFT_SHIFT(img):
    M, N = img.shape
    M = int(M / 2)
    N = int(N / 2)
    return np.vstack((np.hstack((img[M:, N:], img[M:, :N])), np.hstack((img[:M, N:], img[:M, :N]))))

def paint_my_square(img, size=4, bias=None):
    if bias is None:
        bias = [0, 0]
    img2 = np.zeros(img.shape, img.dtype)
    for i in range(img.shape[0]):
        for j in range(img.shape[1]):
            img[i][j] = 0
    d = int(img.shape[0] / 2 - size / 2)
    for i in range(size):
        for j in range(size):
            img[d + i + bias[0]][d + j + bias[1]] = 255
            # print("img[{}][{}] = 255".format(d + i, d + j))

    for i in range(img.shape[0]):
        for j in range(img.shape[1]):
            img2[i][j] = img[i][j]

    return img2

# img = cv2.imread("Lenna.png", cv2.IMREAD_COLOR)
# gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)

gray = np.zeros((128, 128), "uint8")

# gray = paint_my_square(gray)
# gray = paint_my_square(gray, bias=[-40, -40])
# gray = paint_my_square(gray,32)
gray = paint_my_square(gray, 2)

plt.imshow(gray, cmap='gray')
plt.show()

# gray = cv2.imread("./image/lena256.bmp", cv2.IMREAD_GRAYSCALE)

my_fft = abs(FFT_SHIFT(FFT2D(gray)))
target = abs(fftshift(fft2(gray)))
print('distance from numpy.fft: ', np.linalg.norm(my_fft - target))

print(type(my_fft))
print(my_fft.shape)
# print("my_fft=", my_fft)

plt.subplot(2, 2, 1)
plt.title('original')
plt.imshow(gray, cmap='gray')
plt.subplot(2, 2, 2)
plt.title('my FFT2D')
my_fft = np.log(1 + my_fft)
# print("my_fft", my_fft)
print("my_fft", my_fft.dtype)

plt.imshow(my_fft, cmap='gray')
plt.subplot(2, 2, 3)
plt.title('numpy.fft2')
plt.imshow(np.log(1 + target), cmap='gray')
plt.show()


#Universidad Autónoma de San Luis Potosí Enero 2020

#----- INTRODUCCION -----
#Trata de ejecutar paso a paso cada una de las siguientes instrucciones y analizalas con ayuda de tu instructor de laboratorio.

#Multiplicación de matrices
import numpy.matlib
import numpy as np

a = np.array([[1,2],[3,4]])
b = np.array([[11,12],[13,14]])
np.dot(a,b)

#Determinante de una matriz
import numpy as np

b = np.array([[6,1,1], [4, -2, 5], [2,8,7]])
print(b)
print(np.linalg.det(b))
print(6*(-2*7 - 5*8) - 1*(4*7 - 5*2) + 1*(4*8 - -2*2)) #Así harías el determinante a mano

#Solución de sistema de ecuaciones y cálculo de la inversa de una matriz
import numpy as np
a = np.array([[1,1,1],[0,2,5],[2,5,-1]])

print('matriz a:')
print(a)
ainv = np.linalg.inv(a)

print('Inversa de a:')
print(ainv)

print('Matriz B:')
b = np.array([[6],[-4],[27]])
print(b)

print('Solucion:')
x = np.linalg.solve(a,b)
print(x)
# esta es la solució de la ecuacion
#Se puede encontrar la misma solución mediante
x = np.dot(ainv,b)


# Exercise 7.31
# Author: Noah Waterfield Price

import numpy

class Polynomial:

    def __init__(self, coefficients):
        self.coeff = coefficients

    def __call__(self, x):
        """Evaluate the polynomial."""
        s = 0
        for i in range(len(self.coeff)):
            s += self.coeff[i] * x ** i
        return s

    def __add__(self, other):
        # Start with the longest list and add in the other
        if len(self.coeff) > len(other.coeff):
            result_coeff = self.coeff[:]   # copy!
            for i in range(len(other.coeff)):
                result_coeff[i] += other.coeff[i]
        else:
            result_coeff = other.coeff[:]  # copy!
            for i in range(len(self.coeff)):
                result_coeff[i] += self.coeff[i]
        return Polynomial(result_coeff)

    def __sub__(self, other):
        if len(self.coeff) > len(other.coeff):
            result_coeff = self.coeff[:]  # copy!
            for i in range(len(other.coeff)):
                result_coeff[i] -= other.coeff[i]
        else:
            #ensures subtraction done in right directions
            result_coeff = [-i for i in other.coeff]  # copy!
            for i in range(len(self.coeff)):
                result_coeff[i] += self.coeff[i]
        return Polynomial(result_coeff)

    def __mul__(self, other):
        c = self.coeff
        d = other.coeff
        M = len(c) - 1
        N = len(d) - 1
        result_coeff = numpy.zeros(M + N + 1)
        for i in range(0, M + 1):
            for j in range(0, N + 1):
                result_coeff[i + j] += c[i] * d[j]
        return Polynomial(result_coeff)

    def differentiate(self):
        """Differentiate this polynomial in-place."""
        for i in range(1, len(self.coeff)):
            self.coeff[i - 1] = i * self.coeff[i]
        del self.coeff[-1]

    def derivative(self):
        """Copy this polynomial and return its derivative."""
        dpdx = Polynomial(self.coeff[:])  # make a copy
        dpdx.differentiate()
        return dpdx

    def __str__(self):
        s = ''
        for i in range(0, len(self.coeff)):
            if self.coeff[i] != 0:
                s += ' + %g*x^%d' % (self.coeff[i], i)
        # Fix layout
        s = s.replace('+ -', '- ')
        s = s.replace('x^0', '1')
        s = s.replace(' 1*', ' ')
        s = s.replace('x^1 ', 'x ')
        # s = s.replace('x^1', 'x') # will replace x^100 by x^00
        if s[0:3] == ' + ':  # remove initial +
            s = s[3:]
        if s[0:3] == ' - ':  # fix spaces for initial -
            s = '-' + s[3:]
        return s

    def simplestr(self):
        s = ''
        for i in range(0, len(self.coeff)):
            s += ' + %g*x^%d' % (self.coeff[i], i)
        return s

P1 = Polynomial([2, 0,  3, 4, 1, 5])
P2 = Polynomial([3, 0, 2, 8])
P3 = P2 - P1
print P3.coeff

"""
Sample run:
python Polynomial_sub.py
[-1, 0, 1, -4, 1, 5]
"""


import numpy as np
import matplotlib.pyplot as plt
from simulation_and_utilities import *
from scipy.optimize import curve_fit

if __name__ == "__main__":

    np.random.seed(42)

    numbers_for_computing = np.arange(1, 1_001)  # Sequence [1...1.000] for plotting

    # Now we generate the first 1.000 values of the sequence nH_n-1
    # (theoretical prediction).
    harmonic_generator = first_n_harmonic_numbers_generator(numbers_for_computing[-1])
    tmp_theoretical_prediction_list = []
    for n in numbers_for_computing:
        H_prev = next(harmonic_generator)
        tmp_theoretical_prediction_list.append(n * H_prev)

    # for plotting and curve fitting we start from 10.
    numbers_to_plot = numbers_for_computing[10::]
    theoretical_prediction_arr = np.array(tmp_theoretical_prediction_list)[10::]

    # Now we generate the results obtained via the simulation we wrote in
    # simulation_and_utilities.py (with the seed imposed here).
    experimental_results_arr = np.array([number_steps_in_queue_simulation(n)
                                         for n in numbers_to_plot])

    # Finally we plot
    plt.clf()
    plt.plot(numbers_to_plot, experimental_results_arr, 'r.', markersize=3,
             label="Resultados experimentales")
    plt.plot(numbers_to_plot, theoretical_prediction_arr, linewidth=2.5,
             label="Predicción teórica ($nH_{n-1}$)")
    plt.legend()
    plt.xlabel("n (Número de elementos en la cola)")
    plt.ylabel("Número de pasos hasta\nprimer lugar de la cola")
    plt.tight_layout()
    plt.show()

    # Let's now try to fit a polynomial of degree 3 and a logarithmic curve to the
    # experimental data

    def objective_poly(x, a, b, c, d):
        return a * x**3 + b * x**2 + c * x + d

    def objective_log(x, a, b):
        return a * x * np.log(x-1) + b * x

    params_log, _ = curve_fit(objective_log, numbers_to_plot, experimental_results_arr)
    params_poly, _ = curve_fit(objective_poly, numbers_to_plot, experimental_results_arr)

    print("Fitted logarithmic curve: f(x) = {0:.2f}*xln(x-1) + {1:.2f}*x".format(params_log[0],
                                                                             params_log[1]))

    print("Fitted degree 3 polynomial is: f(x) = {0:.3e}*x^3 + {1:.3e}*x^2 + "
          "{2:.2f}*x + {3:.2f}".format(params_poly[0], params_poly[1],
                                      params_poly[2], params_poly[3]))

    fitted_log_curve_data_arr = objective_log(numbers_to_plot, params_log[0], params_log[1])
    fitted_poly_curve_data_arr = objective_poly(numbers_to_plot, params_poly[0], params_poly[1],
                                                params_poly[2], params_poly[3])

    # We now compute and plot the relative difference between the fitted curves
    # and the theoretical prediction nH_n-1.

    relative_difference_log_arr = np.abs((fitted_log_curve_data_arr
                                          - theoretical_prediction_arr)/numbers_to_plot)

    relative_difference_poly_arr = np.abs((fitted_poly_curve_data_arr
                                           - theoretical_prediction_arr)/numbers_to_plot)

    mean_error_log = np.mean(relative_difference_log_arr)
    mean_error_poly = np.mean(relative_difference_poly_arr)

    print("Mean absolute relative error between the theoretical predictions"
          " and the logarithmic fitted curve is {}".format(mean_error_log))
    print("Mean absolute relative error between the theoretical predictions"
          " and the fitted degree 3 polynomial is {}".format(mean_error_poly))

    plt.clf()
    plt.plot(numbers_to_plot, relative_difference_log_arr)
    plt.xlabel("n (Número de elementos en la cola)")
    plt.ylabel(r"$|\frac{F(n)_{log} - nH_{n-1}}{n}|$", fontsize=12)
    plt.tight_layout()
    plt.show()

    plt.clf()
    plt.plot(numbers_to_plot, relative_difference_poly_arr)
    plt.xlabel("n (Número de elementos en la cola)")
    plt.ylabel(r"$|\frac{F(n)_{poly} - nH_{n-1}}{n}|$", fontsize=12)
    plt.tight_layout()
    plt.show()


# -*- coding: utf-8 -*-

import numpy as np
from matplotlib import pyplot

def abstand(x1,y1,x2,y2):
    return np.sqrt((x2-x1)**2+(y2-y1)**2)

def punktmenge(N):
    xi = np.linspace(a,b,N)
    yi = [f(x) for x in xi]
    return [xi, yi]

def bogenlaenge(N):
    [xi,yi] = punktmenge(N)
    s = 0.
    for i in range(N-1):
        s = s + abstand(xi[i],yi[i],xi[i+1],yi[i+1])
    return s

def dump(N,s,e):
    print "{:6d} {:16.8e} {:16.8e}".format(N,s,e)

def dumpHeadline():
    print
    print "{:>6s} {:>17s} {:>16s}".format('N','Bogenlänge s','Fehler e')
    print "----------------------------------------"

def drawPlot(hi,ei):
    fig = pyplot.figure()
    fig.suptitle("sin(x)")
    pyplot.plot(hi,ei,'o-')
    pyplot.xscale('log')
    pyplot.yscale('log')
    pyplot.xlabel('Gitterweite h')
    pyplot.ylabel('Fehler e')
    pyplot.grid()

def f(x):
    return np.sin(x)

a = np.double(0)
b = np.double(np.pi)

N = 4096
sref = bogenlaenge(N)

Ni = [2,4,8,16,32,64,128,256,512,1024]
hi = []
ei = []
dumpHeadline()
for N in Ni:
    s = bogenlaenge(N)
    h = (b-a)/(N-1)
    e = np.fabs(s-sref)
    hi.append(h)
    ei.append(e)
    dump(N,s,e)

drawPlot(hi,ei)


# In NumPy, universal functions are instances of the numpy.ufunc class.
# NumPy provides mathematical functions (such as sin, exp, log, ...), which are called "universal functions"(ufunc).
# Within NumPy, these functions operate elementwise on an array, producing an array as output.
# All trigonometric functions use radians when an angle is called for.
import math
import numpy as np

def _fill_trigonometry_array():
    """
    Utility function to fill the trigonometry array
    Returns
    -------
    The array filled with useful angle measures
    """
    trig_array = [
        0,  # 0°
        math.pi / 6,  # 30°
        math.pi / 4,  # 45°
        math.pi / 3,  # 60°
        math.pi / 2,  # 90°
        (2 * math.pi) / 3,  # 120°
        (3 * math.pi) / 4,  # 135°
        (5 * math.pi) / 6,  # 150°
        math.pi,  # 180°
        (7 * math.pi) / 6,  # 210°
        (5 * math.pi) / 4,  # 225°
        (4 * math.pi) / 3,  # 240°
        (3 * math.pi) / 2,  # 270°
        (5 * math.pi) / 3,  # 300°
        (7 * math.pi) / 4,  # 315°
        (11 * math.pi) / 6,  # 330°
        (2 * math.pi),  # 360°
    ]
    return trig_array

def _fill_arc_trigonometry_array():
    """
    Utility function to fill the trigonometry array used by some arc* functions (arcsin, arccos, ...)
    Returns
    -------
    The array filled with useful angle measures
    """
    arc_trig_array = [
        -1,
        math.pi / 4,  # -45°
        math.pi / 6,  # -30°
        0,  # 0°
        math.pi / 6,  # 30°
        math.pi / 4,  # 45°
        1
    ]
    return arc_trig_array

def np_sin(array):
    return np.sin(array)

def np_cos(array):
    return np.cos(array)

def np_tan(array):
    return np.tan(array)

def np_arcsin(array):
    return np.arcsin(array)

def np_arccos(array):
    return np.arccos(array)

def np_arctan(array):
    return np.arctan(array)

def np_sinh(array):
    return np.sinh(array)

def np_cosh(array):
    return np.cosh(array)

def np_tanh(array):
    return np.tanh(array)

def np_arcsinh(array):
    return np.arcsinh(array)

def np_arccosh(array):
    return np.arccosh(array)

def np_arctanh(array):
    return np.arctanh(array)

def angles_from_radians_to_degrees(value):
    return np.degrees(value)

def angles_from_degrees_to_radians(value):
    return np.radians(value)

if __name__ == '__main__':
    trigonometry_array = _fill_trigonometry_array()
    arc_trigonometry_array = _fill_arc_trigonometry_array()

    # trigonometry arrays

    # base
    sin_array = np_sin(trigonometry_array)
    cos_array = np_cos(trigonometry_array)
    tan_array = np_tan(trigonometry_array)

    # inverse
    arcsin_array = np_arcsin(arc_trigonometry_array)
    arccos_array = np_arccos(arc_trigonometry_array)
    arctan_array = np_arctan(trigonometry_array)

    # hyperbolic
    sinh_array = np_sinh(trigonometry_array)
    cosh_array = np_cosh(trigonometry_array)
    tanh_array = np_tanh(trigonometry_array)

    # inverse hyperbolic
    arcsinh_array = np_arcsinh(trigonometry_array)
    arccosh_array = np_arccosh(trigonometry_array)
    arctanh_array = np_arctanh(arc_trigonometry_array)

    # print out to the user
    print('Numpy Array - Universal Functions - Trigonometric\n')
    print('--- base ---')
    print('sin:\n' + str(sin_array))
    print('cos:\n' + str(cos_array))
    print('tan:\n' + str(tan_array))
    print('--- inverse ---')
    print('arcsin:\n' + str(arcsin_array))
    print('arccos:\n' + str(arccos_array))
    print('arctan:\n' + str(arctan_array))
    print('--- hyperbolic ---')
    print('sinh:\n' + str(sinh_array))
    print('cosh:\n' + str(cosh_array))
    print('tanh:\n' + str(tanh_array))
    print('--- inverse hyperbolic ---')
    print('arcsinh:\n' + str(arcsinh_array))
    print('arccosh:\n' + str(arccosh_array))
    print('arctanh:\n' + str(arctanh_array))
    print('--- Convert angles from radians to degrees ---')
    angles_trigonometry_array = []

    for i in trigonometry_array:
        angles_trigonometry_array.append(angles_from_radians_to_degrees(i))
    print(angles_trigonometry_array)

    print('--- Convert angles from degrees to radians ---')
    radiant_trigonometry_array = []

    for i in angles_trigonometry_array:
        radiant_trigonometry_array.append(angles_from_degrees_to_radians(i))
    print(radiant_trigonometry_array)

    # # plotting cos
    # numpy_test_array = np.arange(10)
    # f_x = np.linspace(-math.pi, math.pi, 100)
    # f_y = np.zeros((1, 100))
    #
    # for i in range(f_x.size):
    #     f_y[0][i] = np.cos(f_x[i])
    #
    # plt.figure(1)
    # plt.plot(f_x, f_y[0])
    # plt.xlim(-math.pi, math.pi)
    # plt.ylim(-1.5, 1.5)
    #
    # # Add a horizontal line across the axis.
    # plt.axhline(y=0, color='k')
    #
    # # Add a vertical line across the axes.
    # plt.axvline(x=0, color='k')
    # plt.show()

# https://realpython.com/python-import/ (import issue)
# https://numpy.org/doc/stable/reference/ufuncs.html
# https://numpy.org/doc/stable/reference/ufuncs.html#trigonometric-functions


# SPDX-FileCopyrightText: 2014-2020 Martin Roelfs
#
# SPDX-License-Identifier: MIT

"""
Some common distributions are defined in this module. That way, users can easily build
more complicated expressions without making them look hard.

I have deliberately chosen to start these function with a capital, e.g.
Gaussian instead of gaussian, because this makes the resulting expressions more
readable.
"""
import sympy

def Gaussian(x, mu, sig):
    """
    .. math::

        f(x) = \\frac{1}{\\sqrt{2 \\pi \\sigma^2}} e^{- \\frac{(x - \\mu)^2}{2 \\sigma^2}}

    Gaussian pdf.

    :param x: free variable.
    :param mu: mean of the distribution.
    :param sig: standard deviation of the distribution.
    :return: sympy.Expr for a Gaussian pdf.
    """
    return sympy.exp(-(x - mu)**2/(2*sig**2))/sympy.sqrt(2*sympy.pi*sig**2)

def BivariateGaussian(x, y, mu_x, mu_y, sig_x, sig_y, rho):
    """
    `Bivariate Gaussian pdf
    <http://mathworld.wolfram.com/BivariateNormalDistribution.html>`_.

    :param x: :class:`symfit.core.argument.Variable`
    :param y: :class:`symfit.core.argument.Variable`
    :param mu_x: :class:`symfit.core.argument.Parameter` for the mean of `x`
    :param mu_y: :class:`symfit.core.argument.Parameter` for the mean of `y`
    :param sig_x: :class:`symfit.core.argument.Parameter` for the standard
        deviation of `x`
    :param sig_y: :class:`symfit.core.argument.Parameter` for the standard
        deviation of `y`
    :param rho: :class:`symfit.core.argument.Parameter` for the correlation
        between `x` and `y`.
    :return: sympy expression for a Bivariate Gaussian pdf.
    """
    exponent = - 1 / (2 * (1 - rho**2))
    exponent *= (x - mu_x)**2 / sig_x**2 + (y - mu_y)**2 / sig_y**2 \
                - 2 * rho * (x - mu_x) * (y - mu_y) / (sig_x * sig_y)
    return sympy.exp(exponent) / (2 * sympy.pi * sig_x * sig_y * sympy.sqrt(1 - rho**2))

def Exp(x, l):
    """
    .. math::

        f(x) = l e^{- l x}

    Exponential Distribution pdf.

    :param x: free variable.
    :param l: rate parameter.
    :return: sympy.Expr for an Exponential Distribution pdf.
    """
    return l * sympy.exp(- l * x)

# def Beta():
#     sympy.stats.Beta()


# -*- coding: utf-8 -*-
"""
Created on Fri Jan  5 18:06:25 2018

@author: Alankar
"""

import numpy as np

ME, RE = 5.972e24, 6371 #in SI
G = 6.67e-11

def height(T):
    return (((G*(ME*1e3)*T**2)/(4*np.pi**2))**(1/3))*1e-3-RE # in km

print('-----------------------------------------------------------')
print('Demo Output:')
print('The satellite with T = %d day has to be approximately %d km above the Earth\'s surface.'%(1,height(1*24*60**2)))
print('The satellite with T = %d minutes has to be approximately %d km above the Earth\'s surface.'%(90,height(90*60)))
print('The satellite with T = %d minutes has to be approximately %d km above the Earth\'s surface.'%(45,height(45*60)))
print('Faster satellites are at lower heights.')
print('-----------------------------------------------------------')
h = height(float(input('Enter the desired revolution time period of satellite in seconds: ')))
print('The satellite has to be approximately %d km above the Earth\'s surface.'%h)

del_h = height(24*60**2)-height(23.93*60**2)
print('Difference in heights of satellites from Earth\'s surface with time period T = 24 hrs and T = 23.93 hrs (sidereal day) is %d km'%del_h)


'''
Matrix Pencil Method

@author: zbb
@date: 20190811
@updates:2020-09-17
@ref: Tapan K. Sakar and Odilon Pereira, Using the Matrix Pencil Method to Estimate the Parameters of Sum of Complex Exponetials,
IEEE Antennas and Propagation Magazine, Vol. 37, No. 1, February 1995.
'''

import numpy as np
import matplotlib.pyplot as plt

def _constructY(y, N, L):
    '''
    y: complex signal sequence.
    N: len(y)
    L: L<N, pencil parameter, N/3 < L < N/2 recommended.
    return: constructed Y matrix,
    [
        [y[0], y[1], ..., y[L-1]],
        [y[1], y[1, 0], ..., y[L]],
        ...
        [y[N-L-1], y[N-L], ..., y[N-1]]
    ]
    (N-L)*(L+1) matrix.
    '''
    Y = np.zeros((N-L, L+1), dtype=np.complex_)
    for k in range(N-L):
        Y[k, :] = y[k:(k+L+1)]
    return Y

def _constructZM(Z, N):
    '''
    Z: 1-D complex array.
    return N*M complex matrix (M=len(Z)):
    [
        [1,  1,  1, ..., 1 ],
        [z[0], z[1], .., z[M-1]],
        ...
        [z[0]**(N-1), z[1]**(N-1), ..., z[M-1]**(N-1)]
    ]
    '''
    M = len(Z)
    ZM = np.zeros( (N, M), dtype=np.complex_)
    for k in range(N):
        ZM[k, :] = Z**k
    return ZM

def _SVDFilter(Sp, p=3.0):
    '''
    Sp: 1-D normed eigenvalues of Y after SVD, 1-st the biggest
    p: precise ditigits, default 3.0.
    return: M, M is the first integer that S[M]/S_max <= 10**(-p)
    '''
    Sm = np.max(Sp)
    pp = 10.0**(-p)
    for m in range(len(Sp)):
        if Sp[m]/Sm <= pp:
            return m+1
    return m+1

def pencil(y, dt, M=None, p=8.0, Lfactor=0.40):
    '''
    Purpose:
      Complex exponential fit of a sampled complex waveform by Matrix Pencil Method.
    Authors:
      Zbb
    Arguments:
      N    - number of data samples. ==len(y)       [INPUT]
      y    - 1-D complex array of sampled signal.   [INPUT]
      dt   - sample interval.                       [INPUT]
      M    - pencil parameter.
             if None: use p to determin M.
             if given in range(0, Lfractor*N), then use it
             if given out of range, then use p to determin M.
      p    - precise digits of signal, default 8.0, corresponding to 10**(-8.0).
    Returns: (Z, R, M, (residuals, rank, s))
      Z    - 1-D Z array.
      R    - 1-D R array.
      M    - M in use.
      (residuals, rank, s)   - np.linalg.lstsq further results.
    Method:
      y[k] = y(k*dt) = sum{i=0--> M} R[i]*( Z[i]**k )
      Z[i] = exp(si*dt)

    Comment:
      To some extent, it is a kind of PCA method.
    '''
    N = len(y)
    # better between N/3~N/2, pencil parameter:
    L = int(N*Lfactor)
    # construct Y matrix (Hankel data matrix) from signal y[i], shape=(N-L, L+1):
    Y = _constructY(y, N, L)
    # SVD of Y:
    _, S, V = np.linalg.svd(Y, full_matrices=True)
    #results: U.shape=(N-L, N-L), S.shape=(L+1, ), V.shape=(L+1, L+1)

    # find M:
    if M is None:
        M = _SVDFilter(np.abs(S), p=p)
    elif M not in range(0, L+1):
        M = _SVDFilter(np.abs(S), p=p)
    else:
        pass
    # matrix primes based on M:
    #Vprime = V[0:M, :] # remove [M:] data set. only 0, 1, 2, ..., M-1 remains
    #Sprime = S[0:M]
    V1prime = V[0:M, 0:-1] # remove last column
    V2prime = V[0:M, 1:] # remove first column
    #smat = np.zeros((U.shape[0], M), dtype=np.complex_)
    #smat[:M, :M] = np.diag(Sprime)
    #Y1 = np.dot(U[:-1, :], np.dot(smat, V1prime))

    V1prime_H_MPinv = np.linalg.pinv(V1prime.T) # find V1'^+ , Moore-Penrose pseudoinverse
    V1V2 = np.dot(V1prime_H_MPinv, V2prime.T) # construct V1V2 = np.dot(V1'^+, V2')
    Z = np.linalg.eigvals(V1V2) # find eigenvalues of V1V2. Zs.shape=(M,)
    #print(V1V2.shape, Z)

    # find R by solving least-square problem: Y = np.dot(ZM, R)
    ZM = np.row_stack([Z**k for k in range(N)]) # N*M
    R, residuals, rank, s = np.linalg.lstsq(ZM, y)
    return (Z, R, M, (residuals, rank, s))

if __name__ == '__main__':
    import random
    import matplotlib.pyplot as plt
    fig, axes = plt.subplots(1, 2)
    '''
    @test 1: matrixpencil for complex, sampled data.
    '''
    dt = 0.005
    x = np.arange(0.0, 1.0, dt,  dtype=np.float_)
    N = len(x) # sapmle size

    # sampled data: [complex]
    y = 10.0 * np.exp(-x*(5.04 + 8.0j)) + 1.0*np.exp(-x*(1.0 - 6.28J*10.3))
    y *= 1.0 + 0.05*np.array([random.random() for i in range(len(x))])

    # lets fit it:
    Z, R, M, _ =  pencil(y, dt, M=6, p=3.0, Lfactor=0.33)
    print('@test2 result: ')
    print('expontial factors:\t', np.log(Z)/dt)
    print('amplitudes:\t', R)
    print('M =\t', M)

    # display:
    y_from_result = np.zeros(shape=x.shape, dtype=np.complex_)
    ks = np.linspace(0.0, N*1.0, N)
    for idx in range(len(Z)):
        y_from_result += R[idx] * Z[idx]**ks

    axes[0].scatter(x, np.real(y), c='k', s=2, label='real part of y')
    axes[0].plot(x, np.real(y_from_result), 'r', linewidth=1, label='real part of y_result')
    axes[0].set_xlabel('x')
    tmp = np.abs(y)
    axes[0].set_ylim((-np.amax(tmp)*1.0, np.amax(tmp)*1.5))
    axes[0].set_ylabel('y.re')
    axes[0].set_title('@test1: real part compare')
    axes[0].legend()

    axes[1].scatter(x, np.imag(y), c='k', s=2, label='real part of y')
    axes[1].plot(x, np.imag(y_from_result), 'r', linewidth=1, label='imag part of y_result')
    axes[1].set_xlabel('x')
    axes[1].set_ylim((-np.amax(tmp)*1.0, np.amax(tmp)*1.5))
    axes[1].set_ylabel('y.imag')
    axes[1].set_title('@test1: imag part compare')
    axes[1].legend()

    '''
    @test2: matrixpencil for real, sampled data.  Find its envelope first.
    '''

    plt.show()


# ELEMENTARY FLOW - SOURCE/SINK FLOW
# Written by: JoshTheEngineer
# YouTube   : www.youtube.com/joshtheengineer
# Website   : www.joshtheengineer.com
# Started: 02/19/19
# Updated: 02/19/19 - Transferred from MATLAB to Python
#                   - Works as expected

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

# %% KNOWNS

lmbda = 1                                                                       # Source/sink strength (+: Source, -: Sink)
X0    = 0                                                                       # Source/sink X coordinate
Y0    = 0                                                                       # Source/sink Y coordinate

# %% CALCULATIONS

# Create grid
numX   = 100                                                                    # Number of X points
numY   = 100                                                                    # Number of Y points
X      = np.linspace(-10,10,numX)                                               # X-point array
Y      = np.linspace(-10,10,numY)                                               # Y-point array
XX, YY = np.meshgrid(X,Y)                                                       # Create the meshgrid

# Solve for velocities
Vx = np.zeros([numX,numY])                                                      # Initialize X velocity component
Vy = np.zeros([numX,numY])                                                      # Initialize Y velocity component
V  = np.zeros([numX,numY])                                                      # Initialize velocity magnitude
Vr = np.zeros([numX,numY])                                                      # Initialize radial velocity component
r  = np.zeros([numX,numY])                                                      # Initialize radius
for i in range(numX):                                                           # Loop over X points
    for j in range(numY):                                                       # Loop over Y points
        x       = XX[i,j]                                                       # X coordinate
        y       = YY[i,j]                                                       # Y coordinate
        dx      = x - X0                                                        # X distance from source/sink
        dy      = y - Y0                                                        # Y distance from source/sink
        r       = np.sqrt(dx**2 + dy**2)                                        # Distance from source/sink
        Vx[i,j] = (lmbda*dx)/(2*np.pi*r**2)                                     # Compute X velocity component
        Vy[i,j] = (lmbda*dy)/(2*np.pi*r**2)                                     # Compute Y velocity component
        V[i,j]  = np.sqrt(Vx[i,j]**2 + Vy[i,j]**2)                              # Compute velocity
        Vr[i,j] = lmbda/(2*np.pi*r)                                             # Compute radial velocity component

# %% COMPUTE CIRCULATIONS

a    = 1.5                                                                      # Horizontal axis half-length
b    = 1.5                                                                      # Vertical axis half-length
x0   = 0                                                                        # Ellipse center X coordinate
y0   = 0                                                                        # Ellipse center Y coordinate
numT = 100                                                                      # Number of points along ellipse
Gamma, xC, yC, VxC, VyC = COMPUTE_CIRCULATION(a,b,x0,y0,numT,Vx,Vy,X,Y)         # Call circulation calculation
print("Circulation: ", Gamma)                                                   # Display circulation result

# %% PLOTTING

# Plot quiver
fig = plt.figure(1)                                                             # Create figure
plt.cla()                                                                       # Get ready for plotting
plt.quiver(X,Y,Vx,Vy)                                                           # Plot velocity vectors
plt.plot(xC,yC,'b-')                                                            # Plot ellipse
plt.title('Source/Sink Flow')                                                   # Set title
plt.xlabel('X-Axis')                                                            # Set X-label
plt.ylabel('Y-Axis')                                                            # Set Y-label
plt.xlim([-2, 2])                                                               # Set X-limits
plt.ylim([-2, 2])                                                               # Set Y-limits
plt.gca().set_aspect('equal')                                                   # Set axes equal
plt.show()                                                                      # Display plot

## Save the figure
#saveFlnm = 'Sink_Flow'
#savePth = 'C:/Users/Josh/Documents/Latex/YouTube/Panel_Methods/Figures_Python/' + saveFlnm + '.pdf'
#plt.savefig(savePth,bbox_inches='tight',facecolor='w',edgecolor='w')


from typing import Optional, List, Tuple

import numpy as np
from scipy import stats

def get_moments(
    methods: Optional[List[str]], np_2d_image: np.ndarray
) -> Tuple[np.ndarray, List[str]]:
    if methods is None:
        methods = ["mean", "median", "var", "skew", "kurtosis"]

    moments = np.array([METHODS[method](np_2d_image) for method in methods])

    return moments, methods

def mean(np_2d_image: np.ndarray) -> np.ndarray:
    return np.mean(np_2d_image, axis=0)

def median(np_2d_image: np.ndarray) -> np.ndarray:
    return np.median(np_2d_image, axis=0)

def var(np_2d_image: np.ndarray) -> np.ndarray:
    return np.var(np_2d_image, axis=0)

def skew(np_2d_image: np.ndarray) -> np.ndarray:
    return stats.skew(np_2d_image, axis=0)

def kurtosis(np_2d_image: np.ndarray) -> np.ndarray:
    return stats.kurtosis(np_2d_image, axis=0)

METHODS = {
    "mean": mean,
    "median": median,
    "var": var,
    "skew": skew,
    "kurtosis": kurtosis,
}


"""
Created on Wed April 17 2019
@author: oliver.mirat
"""
import numpy as np
import matplotlib.pyplot as plt
from scipy.optimize import curve_fit

def expFunc(x, a, b, c):
    return a * np.exp( -(1/b) * x) + c

def getExponentialPart(x, y, nbPointsForFit):
  minIndex = y.argmin(axis=0)
  if nbPointsForFit == 0:
    nbPointsForFit = len(y)
  else:
    nbPointsForFit = minIndex + nbPointsForFit
  xExpPart = x[minIndex:nbPointsForFit]
  yExpPart = y[minIndex:nbPointsForFit]
  return [xExpPart, yExpPart]

def exponentialFitGetTau(x, y, showPlot=0, nbPointsForFit=0):
    """
    This function outputs a monoexponential fit to points denoted by x and y.
    It works for downward slopes as it finds the min of y as starting point.
    3rd term = show plot of monoexponential fit (1) or not (0)
    4th term = number of points for which to do the fit
    """
    [xExpPart, yExpPart] = getExponentialPart(x, y, nbPointsForFit)
    popt, pcov = curve_fit(expFunc, xExpPart, yExpPart, p0=[np.amin(yExpPart), 200, 0])
    if showPlot:
        print('Monoexponential fit is superimposed (red) on raw data (blue)')
        plt.plot(xExpPart, yExpPart)
        plt.plot(xExpPart, expFunc(xExpPart, *popt), 'r-',label='fit: a=%5.3f, b=%5.3f, c=%5.3f' % tuple(popt))
        plt.show()
        return popt[1]


"""
fibonacci_algorithm.py

Returns the reduced uncertainty interval containing the minimizer of the function
func - anonimous function
interval0 - initial uncertainty interval
N_iter - number of iterations
"""

import numpy as np

def fibonacci_algorithm_calc_N_iter(interval0, uncertainty_range_desired):

    F = np.empty(shape = (10000), dtype = int);
    F[0] = 0;
    F[1] = 1;
    F[2] = 1;

    N = 1;
    epsilon = 10e-6;
    while F[N + 1] < (1 + 2 * epsilon) * (interval0[1] - interval0[0]) / uncertainty_range_desired:
        F[N + 2] = F[N + 1] + F[N];
        N += 1;

    N_iter = N - 1;
    return (N_iter, F);

def fibonacci_algorithm(func, interval0, N_iter, F):

    epsilon = 10e-6;
    left_limit = interval0[0];
    right_limit = interval0[1];

    smaller = 'a';
    a = left_limit + (F[N_iter] / F[N_iter + 1]) * (right_limit - left_limit);
    f_at_a = func(a);

    for iter_no in range(N_iter):
        if (iter_no != N_iter):
            rho = 1 - F[N_iter + 1 - iter_no] / F[N_iter + 2 - iter_no];
        else:
            rho = 0.5 - epsilon;

        if (smaller == 'a'):
            c = a;
            f_at_c = f_at_a;
            a = left_limit + rho * (right_limit - left_limit);
            f_at_a = func(a);
        else:
            a = c;
            f_at_a = f_at_c;
            c = left_limit + (1 - rho) * (right_limit - left_limit);
            f_at_c = func(c);
        if (f_at_a < f_at_c):
            right_limit = c;
            smaller = 'a';
        else:
            left_limit = a;
            smaller = 'c';

    interval = (left_limit, right_limit);
    return interval;


# This python function shows how to implement back (retro) propagation
# in a regression model.

import numpy as np
import tensorflow as tf
from tensorflow.python.framework import ops
ops.reset_default_graph()

# Create graph and start a session
sess = tf.Session()

# Here in this example we discuss regression:
# The sample data are created as follows:

# x-sample: 300 random samples from a normal ~ N(2, 0.1)
# target: 300 values of the value 18.

# We will fit the model:
# x-sample * W = target. In theroy, W = 9.

# Now we create 300 data for x-values.
x_input = np.random.normal(2, 0.2, 300)

###Now we create 300 target data for y-values.
y_out = np.repeat(18., 300)
##print(y_out) ###y_out are 300 numbers of 18.

###Introduce 2 placeholders for the training data
x_sample = tf.placeholder(shape=[1], dtype=tf.float32)
y_target = tf.placeholder(shape=[1], dtype=tf.float32)

# Create variable W. In this model we only have one parameter W.
W = tf.Variable(tf.random_normal(shape=[1]))

# Add a multiplication operation to graph
y_output = tf.multiply(x_sample, W)

# Add L2 loss operation to graph
loss = tf.square(y_output - y_target)

# Create 2 Optimizers
theOptimizer1 = tf.train.GradientDescentOptimizer(0.0002)
theOptimizer2=tf.train.AdamOptimizer(0.002)
train_step1 = theOptimizer1.minimize(loss)
train_step2 = theOptimizer2.minimize(loss)

# Initialize variables
init = tf.global_variables_initializer()
sess.run(init)

# Now we run the for-Loop for 3500 times.
for i in range(3500): ##repeat this for-loop 3500 times, 0, 1, 2,....3499.
    rand_index = np.random.choice(300)
    ###In every for-loop, create a random number between 0 and 299
   # print(rand_index)
    rand_x = [x_input[rand_index]]
    rand_y = [y_out[rand_index]]
    sess.run(train_step1, feed_dict={x_sample: rand_x, y_target: rand_y})
    if (i+1)%5==0:
        print('Step #' + str(i+1) + 'GD W = ' + str(sess.run(W)))
        print('GDOpt Loss = ' + str(sess.run(loss, feed_dict={x_sample: rand_x, y_target: rand_y})))
    sess.run(train_step2, feed_dict={x_sample: rand_x, y_target: rand_y})
    if (i+1)%5==0:
        print('Step #' + str(i+1) + 'Adam W = ' + str(sess.run(W)))
        print('AdamOpt Loss = ' + str(sess.run(loss, feed_dict={x_sample: rand_x, y_target: rand_y})))


from __future__ import print_function, division

import numpy as np

# print(np.__version__)

''' Rotation Matrix (implementation)

 ref: https://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions

   Xnew = [[ cos(q),-sin(q), 0 ],
           [ sin(q), cos(q), 0 ],
           [      0,      0, 1 ]]    *   X

 ref: https://math.stackexchange.com/questions/1137745/proof-of-the-extrinsic-to-intrinsic-rotation-transform

    Extrinsic : T R
        xyz_world = TR * xyz_body

    Intrinsic : Rinv Tinv
        xyz_body = Rinv Tinv * xyz_world

 ref: http://danceswithcode.net/engineeringnotes/rotations_in_3d/rotations_in_3d_part1.html

'''

''' cf) Rotation of Axes

 ref: https://en.wikipedia.org/wiki/Rotation_of_axes

    Xnew = [[ cos(q), sin(q), 0 ],
            [-sin(q), cos(q), 0 ],
            [      0,      0, 1 ]]    *   X
'''

''' New Interfaces
    2017.12.08
    TODO: add test funtions
'''

def RotationMatrixFromAxisAngle(axis, theta):
    '''
    parameter:

        axis  [ string ] - 'x', 'y', 'z'
        theta [ float  ] - angle in radian

    return:

        R [ numpy array 3x3 ] - rotation matrix

    description:

        Rotation matrix 'R' means as below

        v_world = R * v_body

        [x]              =  [         ]                   [x]
        [y]              =  [    R    ]                *  [y]
        [z] world frame  =  [         ] body to world     [z] body frame

    '''

    if   axis=='x':
        return np.array([
            [  1,              0,              0 ],
            [  0,  np.cos(theta), -np.sin(theta) ],
            [  0,  np.sin(theta),  np.cos(theta) ]])
    elif axis=='y':
        return np.array([
            [  np.cos(theta), 0,  np.sin(theta) ],
            [              0, 1,              0 ],
            [ -np.sin(theta), 0,  np.cos(theta) ]])
    elif axis=='z':
        return np.array([
            [  np.cos(theta), -np.sin(theta), 0 ],
            [  np.sin(theta),  np.cos(theta), 0 ],
            [              0,              0, 1 ]])
    else:
        raise Exception('invalid axis value : '+str(axis))

def RotationMatrixFromEulerAngles(r,p,y):
    '''
    parameter:

        r [ float ] - roll  in radian
        p [ float ] - pitch in radian
        y [ float ] - yaw   in radian

    return:

        R [ numpy array 3x3 ] - rotation matrix

    description:

        Rotation matrix 'R' means as below

        v_world = R * v_body

        [x]              =  [         ]                   [x]
        [y]              =  [    R    ]                *  [y]
        [z] world frame  =  [         ] body to world     [z] body frame

    '''
    return np.dot( RotationMatrixFromAxisAngle('z', y),
            np.dot( RotationMatrixFromAxisAngle('y', p),
                RotationMatrixFromAxisAngle('x', r)))

def RotationMatrixFromQuaternion(q):
    '''
    parameter:

        q [ array 4 ] - quaternion in (w,x,y,z) order

    return:

        R [ numpy array 3x3 ] - rotation matrix

    description:

        https://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions

    '''
    r,i,j,k = q

    return np.array([
        [ 1-2*j*j-2*k*k, 2*(i*j-k*r), 2*(i*k+j*r) ],
        [ 2*(i*j+k*r), 1-2*i*i-2*k*k, 2*(j*k-i*r) ],
        [ 2*(i*k-j*r), 2*(i*r+j*k), 1-2*i*i-2*j*j ]])

def QuaternionFromRotationMatrix(R):
    '''
    parameter:

        R [ numpy array 3x3 ] - rotation matrix

    return:

        q [ array 4 ] - quaternion in (w,x,y,z) order

    description:

        https://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions
    '''

    r = 0.5*np.sqrt(1.0+R[0,0]+R[1,1]+R[2,2])
    i = (R[2,1]-R[1,2])/(4.0*r)
    j = (R[0,2]-R[2,0])/(4.0*r)
    k = (R[1,0]-R[0,1])/(4.0*r)

    return np.array([r,i,j,k])

def QuaternionFromAxisAngle(v, theta):
    '''
    parameter:

        v     [ array 3 ] - rotation axis as unit vector
        theta [ float   ] - rotation angle in radian

    return:

        q [ array 4 ] - quaternion in (w,x,y,z) order

    '''
    q = np.zeros(4)

    q[0] = np.cos(theta/2)
    q[1] = np.sin(theta/2) * v[0]
    q[2] = np.sin(theta/2) * v[1]
    q[3] = np.sin(theta/2) * v[2]

    return q

def QuaternionFromTwoVectors(v1, v2):
    '''
    CAUTION: Rotation from two vector cannot represent Full Rotation
             ... it cannot detect rotation about own axis(vector)

    parameter:

        v1    [ array 3 ] - vector 1
        v2    [ array 3 ] - vector 2

    return:

        q [ array 4 ] - quaternion in (w,x,y,z) order
                      - rotation v1 -> v2

    '''

    u1 = v1/np.linalg.norm(v1)
    u2 = v2/np.linalg.norm(v2)

    # theta = np.arccos(np.dot(u1,u2))
    v3 = np.cross(u1, u2)
    n3 = np.linalg.norm(v3)
    theta = np.arcsin(n3)
    if theta < 1e-9:
        u3 = np.array([1,0,0])
        theta = 0.0
    else:
        u3 = v3/n3

    # print(theta, np.arcsin(n3))

    return QuaternionFromAxisAngle(u3, theta)

def AxisAngleFromQuaternion(q):
    '''
    parameter:

        q [ array 4 ] - quaternion in (w,x,y,z) order

    return:

        unit_vector [ array 3 ] - rotation axis as unit vector
        theta       [ float   ] - rotation angle in radian

    '''
    if abs(q[0]-1.0) < 1e-8:
        unit_vector = np.array([1.0,0.0,0.0])
        theta       = 0.0
    else:
        theta_d2 = np.arccos(q[0])
        v = q[1:]/np.sin(theta_d2)

        unit_vector = v / np.linalg.norm(v)
        theta       = theta_d2*2.0

    return unit_vector, theta

def EulerAnglesFromRotationMatrix(R):
    '''
    parameter:

        R [ numpy array 3x3 ] - rotation matrix

    return:

        rpy [ numpy array 3 ] - (roll, pitch, yaw) order in radian

    description:

        https://www.learnopencv.com/rotation-matrix-to-euler-angles/
    '''

    sy = np.sqrt(R[0,0] * R[0,0] +  R[1,0] * R[1,0])

    singular = sy < 1e-6

    if  not singular :
        r = np.arctan2(R[2,1] , R[2,2])
        p = np.arctan2(-R[2,0], sy)
        y = np.arctan2(R[1,0], R[0,0])
    else :
        r = np.arctan2(-R[1,2], R[1,1])
        p = np.arctan2(-R[2,0], sy)
        y = 0

    return np.array([r, p, y])

def TransformationMatrix(r,p,y, tx,ty,tz):
    '''
    parameter:

        r  [ float ] - roll  in radian
        p  [ float ] - pitch in radian
        y  [ float ] - yaw   in radian
        tx [ float ] - x translation
        ty [ float ] - y translation
        tz [ float ] - z translation

    return:

        T [ numpy array 4x4 ] - transformation matrix

            [ R11 R12 R13   tx]
            [ R21 R22 R23   ty]
            [ R31 R32 R33   tz]
            [   0   0   0   1 ]

    '''
    T = np.eye(4)
    T[0,3] = tx
    T[1,3] = ty
    T[2,3] = tz
    T[0:3,0:3] = RotationMatrixFromEulerAngles(r,p,y)

    return T

def TransformationMatrixFromRotationMatrixTranslation(R,t):
    '''
    parameter:

        R [ numpy array 3x3 ] - rotation matrix
        t [ numpy array 3   ] - translation vector

    return:

        T [ numpy array 4x4 ] - transformation matrix

            [ R11 R12 R13   tx]
            [ R21 R22 R23   ty]
            [ R31 R32 R33   tz]
            [   0   0   0   1 ]

    '''
    T = np.eye(4)
    T[:3,:3] = R
    T[:3,3] = t

    return T

def TransformationMatrixFromEulerAngleTranslation(rpy, tsl):
    '''
    parameter:
        rpy [ numpy array 3] - roll, pitch, yaw
        tsl [ numpy array 3] - translation x, y, z

    return:

        T [ numpy array 4x4 ] - transformation matrix

            [ R11 R12 R13   tx]
            [ R21 R22 R23   ty]
            [ R31 R32 R33   tz]
            [   0   0   0   1 ]

    '''
    T = np.eye(4)
    T[:3,:3] = RotationMatrixFromEulerAngles(*rpy)
    T[:3, 3] = tsl
    return T

def EulerAngleTranslationFromTransformationMatrix(T):
    '''
    parameter:

        T [ numpy array 4x4 ] - transformation matrix

            [ R11 R12 R13   tx]
            [ R21 R22 R23   ty]
            [ R31 R32 R33   tz]
            [   0   0   0   1 ]

    return:

        rpy [ numpy array 3] - roll, pitch, yaw
        tsl [ numpy array 3] - translation x, y, z

    '''
    return EulerAnglesFromRotationMatrix(T[:3,:3]), T[:3,3]

def SphericalLinearInterpolation(q0, q1, t):

    '''
    spherical linear interpolation
    parameter:

        q0 [ w, x, y, z] - intial rotation in quaternion
        q1 [ w, x, y, z] - final rotation in quaternion
        t  [ float, 0<=t<=1] - amount of interpolation

    return:
        qi [ w, x, y, z] - interpolated quaternion

    ref: https://en.wikipedia.org/wiki/Slerp
    '''

    # normalize
    v0 = np.array(q0)/np.linalg.norm(q0)
    v1 = np.array(q1)/np.linalg.norm(q1)

    # amount of rotation
    dot = np.dot(v0, v1)
    if dot < 0:
        v1 = -v1
        dot = -dot

    if dot > 0.9995:
        qi = v0 + t*(v1 - v0)        # linear interpolation

    else:
        theta = np.arccos(dot)
        theta_i = theta*t
        sin_theta = np.sin(theta)
        sin_theta_i = np.sin(theta_i)

        s0 = np.cos(theta_i) - dot * sin_theta_i / sin_theta
        s1 = sin_theta_i / sin_theta

        qi = s0*v0 + s1*v1

    return qi/np.linalg.norm(qi)

def Rxyz(theta, axis):

    '''Deprecated ...
       Rotation matrix
       theta [radian]
       axis - 'x','y','z'
    '''

    if   axis=='x':
        return np.array([
            [  1,              0,              0 ],
            [  0,  np.cos(theta), -np.sin(theta) ],
            [  0,  np.sin(theta),  np.cos(theta) ]])
    elif axis=='y':
        return np.array([
            [  np.cos(theta), 0,  np.sin(theta) ],
            [              0, 1,              0 ],
            [ -np.sin(theta), 0,  np.cos(theta) ]])
    elif axis=='z':
        return np.array([
            [  np.cos(theta), -np.sin(theta), 0 ],
            [  np.sin(theta),  np.cos(theta), 0 ],
            [              0,              0, 1 ]])
    else:
        raise Exception('invalid axis value : '+str(axis))

def Rrpy(r,p,y):

    '''Deprecated ...
    Rotation matrix for Euler angle (intrinsic)
    v_world = Rrpy * v_body

    explain 1) rotaion of axes
       -> first, rotate axis z with amount of yaw
        v1 = Rz(yaw).T * v_world
       -> next, rotate axis y with amount of pitch
        v2 = Ry(pitch).T * v_world
       -> last, rotate axis x with amount of roll
        v_body = Rx(roll).T * v_world

       => v_body = Rx(roll).T * Ry(pitch).T * Rz(yaw).T * v_world

    explain 2) rotaion of points

       inverse of explain 1

       => v_world = Rz(yaw) * Ry(pitch) * Rx(roll) * v_body

    '''

    return np.dot( Rxyz(y, 'z'), np.dot( Rxyz(p, 'y'), Rxyz(r, 'x') ) )

###def Rz(psi):
###    ''' Axis Roataion Matrix for Z '''
###    return np.array([
###        [  np.cos(psi), np.sin(psi), 0 ],
###        [ -np.sin(psi), np.cos(psi), 0 ],
###        [            0,           0, 1 ]])
###
###def Ry(theta):
###    ''' Axis Roataion Matrix for Y '''
###    return np.array([
###        [ np.cos(theta), 0, -np.sin(theta) ],
###        [             0, 1,              0 ],
###        [ np.sin(theta), 0,  np.cos(theta) ]])
###
###def Rx(phi):
###    ''' Axis Roataion Matrix for Z '''
###    return np.array([
###        [ 1,            0,           0 ],
###        [ 0,  np.cos(phi), np.sin(phi) ],
###        [ 0, -np.sin(phi), np.cos(phi) ]])
###
###
###def Rxyz(r,p,y):
###    ''' Axes Rotation Matrix for XYZ (Euler Angle)
###        v_body = R_xyz * v_global  (x for roll, y for pitch, z for yaw)
###        Rxyz : C^b_g ( g to b )
###    '''
###    return np.dot(np.dot(Rx(r), Ry(p)), Rz(y))
###

def Cb2g(r,p,y):
    return Rrpy(r,p,y)

def T(r,p,y, tx,ty,tz):
    '''Deprecated ...
    transform matrix  (rotation + translation)
    xyz_world = T * xyz_body
    '''

    T = np.eye(4)
    T[0,3] = tx
    T[1,3] = ty
    T[2,3] = tz
    T[0:3,0:3] = Rrpy(r,p,y)
    return T

def Q2R(q):
    '''Deprecated ...
    Quaternion to rotation matrix
    q - ( w, x, y, z)

    ref: https://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions
    '''

    r,i,j,k = q

    return np.array([
        [ 1-2*j*j-2*k*k, 2*(i*j-k*r), 2*(i*k+j*r) ],
        [ 2*(i*j+k*r), 1-2*i*i-2*k*k, 2*(j*k-i*r) ],
        [ 2*(i*k-j*r), 2*(i*r+j*k), 1-2*i*i-2*j*j ]])

def R2Q(R):
    '''Deprecated ...
    Rotation matrix to quaternion

    R - 3x3 rotation matrix

    return quaternion (w,x,y,z)

    ref: https://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions
    '''

    r = 0.5*np.sqrt(1.0+R[0,0]+R[1,1]+R[2,2])
    i = (R[2,1]-R[1,2])/(4.0*r)
    j = (R[0,2]-R[2,0])/(4.0*r)
    k = (R[1,0]-R[0,1])/(4.0*r)

    return np.array([r,i,j,k])

def R2RPY(R):
    '''Deprecated ...
    Roataion matrix to roll, pitch, yaw (X-Y-Z)

    return numpy array [r,p,y]

    ref: https://www.learnopencv.com/rotation-matrix-to-euler-angles/
    '''

    sy = np.sqrt(R[0,0] * R[0,0] +  R[1,0] * R[1,0])

    singular = sy < 1e-6

    if  not singular :
        x = np.arctan2(R[2,1] , R[2,2])
        y = np.arctan2(-R[2,0], sy)
        z = np.arctan2(R[1,0], R[0,0])
    else :
        x = np.arctan2(-R[1,2], R[1,1])
        y = np.arctan2(-R[2,0], sy)
        z = 0

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

def Q2ThetaV(q):
    '''Deprecated ...
    extract angle, unit vector from quaternion (w,x,y,z)
    return theta(rad), unit_vector
    '''
    if abs(q[0]-1.0) < 1e-8:
        return 0, np.array([0.0,0.0,1.0])
    else:
        theta_d2 = np.arccos(q[0])

        v = q[1:]/np.sin(theta_d2)
        unit_vector = v / np.linalg.norm(v)

        return theta_d2*2, unit_vector

def ThetaV2Q(theta,v):
    '''Deprecated ...
    quaternion from angle and unit vector
    return quaternion (w,x,y,z)
    '''
    q = np.zeros(4)

    q[0] = np.cos(theta/2)
    q[1] = np.sin(theta/2) * v[0]
    q[2] = np.sin(theta/2) * v[1]
    q[3] = np.sin(theta/2) * v[2]

    return q

def SPH2XYZ(rng,rho,phi):
    ''' Spherical to XYZ '''
    return np.array([
        rng*np.cos(rho)*np.cos(phi),
        rng*np.cos(rho)*np.sin(phi),
        rng*np.sin(rho)             ])

def SPH2XYZ1(rng,rho,phi):
    ''' Spherical to XYZ1 (corresponding to matrix T) '''
    return np.array([
        rng*np.cos(rho)*np.cos(phi),
        rng*np.cos(rho)*np.sin(phi),
        rng*np.sin(rho),
        1.0])

def SPH2XYZ1A(rng,rho,phi):
    ''' Spherical to XYZ1 Array (corresponding to matrix T) '''
    return np.vstack((
        rng*np.cos(rho)*np.cos(phi),
        rng*np.cos(rho)*np.sin(phi),
        rng*np.sin(rho),
        np.ones_like(rng)))

def get_ground_range(rho, phi, T, ground_height=0.0):
    return (ground_height-T[2,3])/(
        np.cos(rho)*np.cos(phi)*T[2,0] +
        np.cos(rho)*np.sin(phi)*T[2,1] +
        np.sin(rho)            *T[2,2])

def param_form_string_from_transformation_matrix(T):
    eu = EulerAnglesFromRotationMatrix(T[:3,:3])
    s1 = ' -o %.6f %.6f %.6f' % (eu[0], eu[1], eu[2])
    s2 = ' -p %.6f %.6f %.6f' % (T[0,3], T[1,3], T[2,3])
    return s1 + s2

def param_form_string_ndt_from_transformation_matrix(T):
    eu = EulerAnglesFromRotationMatrix(T[:3,:3])
    s1 = ' -ab %.6f,%.6f,%.6f' % (eu[0], eu[1], eu[2])
    s2 = ' -pb %.6f,%.6f,%.6f' % (T[0,3], T[1,3], T[2,3])
    return s1 + s2

def transformation_matrix_from_ssv_rotation_matrix_translation(ssv):

    ss = ssv.split()

    R = np.array( [ float(s) for s in ss[:9]]).reshape((3,3))
    t = np.array( [ float(s) for s in ss[9:12]])

    return TransformationMatrixFromRotationMatrixTranslation(R,t)

def transformation_matrix_from_param_form_string(param_str):

    ss = param_str.split()

    euler = [float(s) for s in ss[1:1+3]]
    tsl = [float(s) for s in ss[5:5+3]]

    return TransformationMatrixFromEulerAngleTranslation(euler,tsl)

def bursa_wolf_transformation(pa, pb, b_scale=True):
    '''
    Transformation A->B

    input :
        pa      - numpy array
        pb      - numpy array
        b_scale - boolean

    return:
        rpy, tsl, ds, residuals, rank, singular_values

    ref:
        https://www.researchgate.net/publication/228757515_A_NOTE_ON_THE_BURSA-WOLF_AND_MOLODENSKY-BADEKAS_TRANSFORMATIONS
    '''

    assert len(pa) == len(pb)

    n = len(pa)

    ##
    ##    0 1 2     3   4   5    6
    ##  ------------------------------------------------------------------------
    ##  [ 1 0 0 |   0  z1 -y1 | x1 ]  [tx ty tz | ex ey ez | ds].T = [ x2 -x1 ]
    ##  [ 0 1 0 | -z1   0  x1 | y1 ]                                 [ y2 -y1 ]
    ##  [ 0 0 1 |  y1 -x1   0 | z1 ]                                 [ z2 -z1 ]
    ##

    if b_scale:
        k = 7
    else:
        k = 6

    A = np.zeros((3*n, k))
    #
    A[0::3,0] = 1.0
    A[1::3,1] = 1.0
    A[2::3,2] = 1.0
    #
    A[0::3,4] =  pa[:,2] #  z
    A[0::3,5] = -pa[:,1] # -y
    #
    A[1::3,3] = -pa[:,2] # -z
    A[1::3,5] =  pa[:,0] #  x
    #
    A[2::3,3] =  pa[:,1] #  y
    A[2::3,4] = -pa[:,0] # -x
    #
    if b_scale:
        A[0::3,6] =  pa[:,0] #  x
        A[1::3,6] =  pa[:,1] #  y
        A[2::3,6] =  pa[:,2] #  z
    else:
        pass

    if b_scale:
        B = (pb-pa).flatten()
    else:
        B = pb.flatten()

    x, residual, rank, sv = np.linalg.lstsq(A,B)

    translation = x[:3]
    euler = np.arcsin(x[3:6])
    if b_scale:
        ds = x[6]
    else:
        ds = 1.0

    return euler, translation, ds, residual, rank, sv

def open3d_icp(pa, pb, trans_init=np.eye(4), max_distance=0.02, b_graph=False):
    '''
    (Transformation A->B)

    pa : numpy array
    pb : numpy array

    return:
        transformation
            - 4x4 numpy array
        result
            - transformation
            - fitness
            - inlier_rmse
            - correspondence_set
    '''

    import open3d as op3
    import copy

    def draw_registration_result(source, target, transformation):
        source_temp = copy.deepcopy(source)
        target_temp = copy.deepcopy(target)
        source_temp.paint_uniform_color([1, 0.706, 0])
        target_temp.paint_uniform_color([0, 0.651, 0.929])
        source_temp.transform(transformation)
        op3.draw_geometries([source_temp, target_temp])

    pc1 = op3.PointCloud()
    pc1.points = op3.Vector3dVector(pa)

    pc2 = op3.PointCloud()
    pc2.points = op3.Vector3dVector(pb)

    # op3.draw_geometries([pc1, pc2])

    # max_distance = 0.2
    # trans_init = np.asarray(
                # [[0.862, 0.011, -0.507,  0.5],
                # [-0.139, 0.967, -0.215,  0.7],
                # [0.487, 0.255,  0.835, -1.4],
                # [0.0, 0.0, 0.0, 1.0]])
    # trans_init = np.eye(4)
    # tsl = pb.mean(axis=0) - pa.mean(axis=0)
    # trans_init = TransformationMatrixFromEulerAngleTranslation(euler, tsl)
    # trans_init = transform

    # print("Apply point-to-point ICP")
    reg_p2p = op3.registration_icp(pc1, pc2, max_distance, trans_init,
            # op3.TransformationEstimationPointToPoint())
            op3.TransformationEstimationPointToPoint(),
            op3.ICPConvergenceCriteria(max_iteration = 2000))
    # print(reg_p2p)
    # print("Transformation is:")
    # print(reg_p2p.transformation)
    # print(EulerAngleTranslationFromTransformationMatrix(reg_p2p.transformation))
    # print("")
    if b_graph:
        draw_registration_result(pc1, pc2, reg_p2p.transformation)

    # print(reg_p2p.transformation)
    # print(reg_p2p.fitness)
    # print(reg_p2p.inlier_rmse)
    # print(reg_p2p.correspondence_set)

    return reg_p2p.transformation, reg_p2p

def open3d_icp_normal(pa, pb, trans_init=np.eye(4), max_distance=0.02, b_graph=False):
    '''
    (Transformation A->B)

    pa : numpy array
    pb : numpy array

    return:
        transformation
            - 4x4 numpy array
        result
            - transformation
            - fitness
            - inlier_rmse
            - correspondence_set
    '''

    import open3d as op3
    import copy

    def draw_registration_result(source, target, transformation):
        source_temp = copy.deepcopy(source)
        target_temp = copy.deepcopy(target)
        source_temp.paint_uniform_color([1, 0.706, 0])
        target_temp.paint_uniform_color([0, 0.651, 0.929])
        source_temp.transform(transformation)
        op3.draw_geometries([source_temp, target_temp])

    pc1 = op3.PointCloud()
    pc1.points = op3.Vector3dVector(pa)

    pc2 = op3.PointCloud()
    pc2.points = op3.Vector3dVector(pb)

    op3.estimate_normals(pc1,
            search_param = op3.KDTreeSearchParamHybrid(radius = 10.0, max_nn = 30))
    op3.estimate_normals(pc2,
            search_param = op3.KDTreeSearchParamHybrid(radius = 10.0, max_nn = 30))

    # op3.draw_geometries([pc1, pc2])

    # max_distance = 0.2
    # trans_init = np.asarray(
                # [[0.862, 0.011, -0.507,  0.5],
                # [-0.139, 0.967, -0.215,  0.7],
                # [0.487, 0.255,  0.835, -1.4],
                # [0.0, 0.0, 0.0, 1.0]])
    # trans_init = np.eye(4)
    # tsl = pb.mean(axis=0) - pa.mean(axis=0)
    # trans_init = TransformationMatrixFromEulerAngleTranslation(euler, tsl)
    # trans_init = transform

    #### # print("Apply point-to-point ICP")
    #### reg_p2p = op3.registration_icp(pc1, pc2, max_distance, trans_init,
    ####         # op3.TransformationEstimationPointToPoint())
    ####         op3.TransformationEstimationPointToPoint(),
    ####         op3.ICPConvergenceCriteria(max_iteration = 2000))
    #### # print(reg_p2p)
    #### # print("Transformation is:")
    #### # print(reg_p2p.transformation)
    #### # print(EulerAngleTranslationFromTransformationMatrix(reg_p2p.transformation))
    #### # print("")

    # print("Apply point-to-plane ICP")
    reg_p2l = op3.registration_icp(pc1, pc2, max_distance, trans_init,
            op3.TransformationEstimationPointToPlane(),
            op3.ICPConvergenceCriteria(max_iteration = 2000))
    # print(reg_p2l)
    # print("Transformation is:")
    # print(reg_p2l.transformation)
    # print("")

    if b_graph:
        draw_registration_result(pc1, pc2, reg_p2l.transformation)

    # print(reg_p2p.transformation)
    # print(reg_p2p.fitness)
    # print(reg_p2p.inlier_rmse)
    # print(reg_p2p.correspondence_set)

    return reg_p2l.transformation, reg_p2l

if __name__=='__main__':

    ### TODO add unit test

    epsilon = 1e-10

    R1 = Rxyz(np.pi/2, 'x')
    R2 = np.array([[ 1,0,0], [0,0,-1], [0,1,0]], dtype=np.float)
    assert np.all(np.abs(R1-R2)<epsilon)

    R1 = Rxyz(np.pi/2, 'y')
    R2 = np.array([[ 0,0,1], [0,1,0], [-1,0,0]], dtype=np.float)
    assert np.all(np.abs(R1-R2)<epsilon)

    R1 = Rxyz(np.pi/2, 'z')
    R2 = np.array([[ 0,-1,0], [1,0,0], [0,0,1]], dtype=np.float)
    assert np.all(np.abs(R1-R2)<epsilon)

    R1 = Rxyz(np.pi/2, 'x')
    # print(R1)
    # print(R1-Q2R(R2Q(R1)))
    assert np.all(np.abs(R1-Q2R(R2Q(R1)))<epsilon)

    theta = np.radians(10)
    unitv = np.array([1,0,0])
    q = ThetaV2Q(theta, unitv)
    t,u = Q2ThetaV(q)
    # print(theta, t)
    # print(unitv, u)
    assert abs(theta-t)<epsilon
    assert np.all(np.abs(unitv-u)<epsilon)

    ''' New Interfaces '''
    R1 = RotationMatrixFromAxisAngle('x', np.pi/2)
    R2 = np.array([[ 1,0,0], [0,0,-1], [0,1,0]], dtype=np.float)
    assert np.all(np.abs(R1-R2)<epsilon)

    R1 = RotationMatrixFromAxisAngle('y', np.pi/2)
    R2 = np.array([[ 0,0,1], [0,1,0], [-1,0,0]], dtype=np.float)
    assert np.all(np.abs(R1-R2)<epsilon)

    R1 = RotationMatrixFromAxisAngle('z', np.pi/2)
    R2 = np.array([[ 0,-1,0], [1,0,0], [0,0,1]], dtype=np.float)
    assert np.all(np.abs(R1-R2)<epsilon)

    R1 = RotationMatrixFromAxisAngle('x', np.pi/2)
    assert np.all(np.abs(R1-RotationMatrixFromQuaternion(QuaternionFromRotationMatrix(R1)))<epsilon)

    theta = np.radians(10)
    unitv = np.array([1,0,0])
    q = QuaternionFromAxisAngle(unitv, theta)
    u,t = AxisAngleFromQuaternion(q)
    assert abs(theta-t)<epsilon
    assert np.all(np.abs(unitv-u)<epsilon)

    R1 = RotationMatrixFromEulerAngles(np.pi/2, np.pi/2, np.pi/2)
    Rx = np.array([[ 1,0,0], [0,0,-1], [0,1,0]], dtype=np.float)
    Ry = np.array([[ 0,0,1], [0,1,0], [-1,0,0]], dtype=np.float)
    Rz = np.array([[ 0,-1,0], [1,0,0], [0,0,1]], dtype=np.float)
    R2 = np.dot(Rz, np.dot(Ry, Rx))
    assert np.all(np.abs(R1-R2)<epsilon)

    rpy1 = [0.1, 0.2, 0.3]
    R1 = RotationMatrixFromEulerAngles(*rpy1)
    rpy2 = EulerAnglesFromRotationMatrix(R1)
    assert np.all(np.abs(rpy2-rpy1)<epsilon)

    # TransformationMatrix(r,p,y, tx,ty,tz):
    rpy = [0.1, 0.2, 0.3]
    tsl = [0.5, 0.6, 0.7]
    R = RotationMatrixFromEulerAngles(*rpy)
    p   = np.array([1,1,1,1])
    p1  = np.dot(R,p[:3])+tsl
    p2  = np.dot(TransformationMatrixFromEulerAngleTranslation(rpy,tsl), p)[:3]
    assert np.all(np.abs(p1-p2)<epsilon)
    p3  = np.dot(TransformationMatrix(rpy[0],rpy[1],rpy[2],tsl[0],tsl[1],tsl[2]), p)[:3]
    assert np.all(np.abs(p1-p3)<epsilon)
    p4  = np.dot(TransformationMatrixFromRotationMatrixTranslation(R,tsl), p)[:3]
    assert np.all(np.abs(p1-p4)<epsilon)

    # SphericalLinearInterpolation (SLERP)
    u = [1,0,0]
    thu = 10*np.pi/180
    N = 8
    vv = [0,1,0]
    for i in range(N):
        qr = QuaternionFromAxisAngle(u, thu*i)
        q0 = QuaternionFromAxisAngle(u, thu*N)
        qs = SphericalLinearInterpolation([1,0,0,0], q0, i/N)

        assert np.all(np.abs(
            np.dot(RotationMatrixFromQuaternion(qr), vv)
            -np.dot(RotationMatrixFromQuaternion(qs), vv))<epsilon)

    #
    r1 = RotationMatrixFromEulerAngles(0,0,np.radians(30))
    r2 = RotationMatrixFromEulerAngles(0,0,np.radians(90))
    r3 = RotationMatrixFromEulerAngles(0,0,np.radians(60))
    q1 = QuaternionFromRotationMatrix(r1)
    q2 = QuaternionFromRotationMatrix(r2)
    #
    q = SphericalLinearInterpolation(q1, q2, 0.5)
    print(q)
    print(QuaternionFromRotationMatrix(r3))

    # Quaternion from two vectors
    rpy = np.array([0.1,0.2,0.3])
    R = RotationMatrixFromEulerAngles(*rpy)
    v1 = np.array([2,0,0])
    v2 = np.dot(R,v1)
    q_ = QuaternionFromTwoVectors(v1,v2)
    R_ = RotationMatrixFromQuaternion(q_)
    # rpy_ = EulerAnglesFromRotationMatrix(R_)
    v2_ = np.dot(RotationMatrixFromQuaternion(q_),v1)
    # print(v2)
    # print(v2_)
    # print(R)
    # print(R_)
    # print(rpy)
    # print(rpy_)
    assert np.all(np.abs(v2 - v2_)<np.radians(0.1))

    '''
    Calculate Transformation

    '''
    euler = np.array([0.5, 0.2, 0.3])
    translation = np.array([ 7, 10, 20])
    print('TRUE euler:', euler)
    print('TRUE translation:', translation)

    T = TransformationMatrixFromEulerAngleTranslation(euler, translation)

    pa = np.random.randn(1000,3)*1.0
    paa = np.c_[pa, np.ones(len(pa))]
    pbb = np.dot(T, paa.T).T
    pb = pbb[:,:3] + np.random.randn(1000,3)*0.01

    b_scale = True
    # b_scale = False
    euler1, translation1, ds1, res, rank, sv  = bursa_wolf_transformation(pa, pb, b_scale)
    print('Bursa-Wolf euler:', euler1)
    print('Bursa-Wolf translation:', translation1)

    ##T1 = TransformationMatrixFromEulerAngleTranslation(euler1, translation1)
    ##pc = np.dot(T1,paa.T).T
    ##plt.figure()
    ##ax = plt.subplot(1,1,1, projection='3d')
    ##ax.plot( pb[:,0], pb[:,1], pb[:,2], 'b.', label='true')
    ##ax.plot( pc[:,0], pc[:,1], pc[:,2], 'r.', label='fitted')
    ##ax.grid()

    ##plt.show()

    eu = np.array(euler)*0.6
    tl = np.array(translation)*0.9
    Ti = TransformationMatrixFromEulerAngleTranslation(eu,tl)
    Tr, res = open3d_icp(pa, pb[:-10], Ti, 2.0)

    euler2, translation2 = EulerAngleTranslationFromTransformationMatrix(Tr)

    print('ICP(open3d) euler:', euler2)
    print('ICP(open3d) translation:', translation2)


import numpy as np
from random import shuffle

def softmax_loss_naive(W, X, y, reg):
  """
  Softmax loss function, naive implementation (with loops)
  Inputs:
  - W: C x D array of weights
  - X: D x N array of data. Data are D-dimensional columns
  - y: 1-dimensional array of length N with labels 0...K-1, for K classes
  - reg: (float) regularization strength
  Returns:
  a tuple of:
  - loss as single float
  - gradient with respect to weights W, an array of same size as W
  """
  # Initialize the loss and gradient to zero.
  loss = 0.0
  X = X.T
  W = W.T
  dW = np.zeros_like(W)
  #############################################################################
  # TODO: Compute the softmax loss and its gradient using explicit loops.     #
  # Store the loss in loss and the gradient in dW. If you are not careful     #
  # here, it is easy to run into numeric instability. Don't forget the        #
  # regularization!                                                           #
  #############################################################################

  num_train = X.shape[0]
  num_classes = W.shape[1]
  for i in range(num_train):
      f = X[[i], :].dot(W)
      f -= np.max(f)
      denom = np.sum(np.exp(f))
      nom = np.exp(f[:,y[i]])
      loss += -np.log(nom / denom)
      for j in range(num_classes):
          if j == y[i]:
              dW[:, [y[i]]] += (np.exp(f[:, j]) / denom - 1) * X[[i], :].T
          else:
              dW[:, [j]] += (np.exp(f[:, j]) / denom) * X[[i], :].T
  loss /= num_train
  loss += 0.5 * reg * np.sum(W * W)
  dW /= num_train
  dW += reg * W
  #############################################################################
  #                          END OF YOUR CODE                                 #
  #############################################################################

  return loss, dW.T

def softmax_loss_vectorized(W, X, y, reg):
  """
  Softmax loss function, vectorized version.

  Inputs and outputs are the same as softmax_loss_naive.
  """
  # Initialize the loss and gradient to zero.
  loss = 0.0
  X = X.T
  W = W.T
  dW = np.zeros_like(W)
  m = X.shape[0]

  # #############################################################################
  # # TODO: Compute the softmax loss and its gradient using no explicit loops.  #
  # # Store the loss in loss and the gradient in dW. If you are not careful     #
  # # here, it is easy to run into numeric instability. Don't forget the        #
  # # regularization!                                                           #
  # #############################################################################
  classes = 10

  y_one_hot = np.eye(classes)[y.reshape(-1)] # one-hot vector

  score = np.matmul(X, W)

  score -= np.max(score, axis=1, keepdims=True)

  softmax = np.exp(score) / np.sum(np.exp(score), axis=1, keepdims=True)

  loss = -1./ m * np.sum(y_one_hot * np.log(softmax)) + 0.5 * reg * np.sum(W[1:,:]*W[1:,:])

  dW = np.dot(X.T, softmax - y_one_hot) / m

  dW += reg * W

  #############################################################################
  #                          END OF YOUR CODE                                 #
  #############################################################################

  return loss, dW.T


import numpy as np

x1 = np.array([3 , 13 , 19 , 24 , 29])
print('x1.mean : ',np.mean(x1))
mean_x1 = np.mean(x1)

x2 = np.array([12 , 10 , 29, 33 , 38])
print('x2.mean : ',np.mean(x2))
mean_x2 = np.mean(x2)

num_measurements = x1.size
covariance = np.dot(x1 - mean_x1 , x2 - mean_x2) / (num_measurements - 1)

print('covariance : ' , covariance)


from sage.all import is_square, sqrt, Integer, floor, ceil

def pell_solve(D, m):
    """
    Description:

        Solves the Pell equation x^2 - D*y^2 = m

    Input:

        D - nonsquare integer
        m - integer

    Output:

        (x0, y0) - minimal solution to x^2 - D*y^2 = 1
        prim_sols - list of primitive solutions for each solution class to x^2 - D*y^2 = m

    """
    assert not is_square(D), 'D cannot be a perfect square'
    if m*m >= D:
        return _pell_solve_2(D,m)
    else:
        return _pell_solve_1(D,m)

def _pell_solve_1(D,m): # m^2 < D
    root_d = Integer(floor(sqrt(D)))
    a = Integer(floor(root_d))
    P = Integer(0)
    Q = Integer(1)
    p = [Integer(1),Integer(a)]
    q = [Integer(0),Integer(1)]
    i = Integer(1)
    x0 = Integer(0)
    y0 = Integer(0)
    prim_sols = []
    test = Integer(0)
    while not (Q == 1 and i%2 == 1) or i == 1:
        test = p[i]**2 - D* (q[i]**2)
        if test == 1:
            x0 = p[i]
            y0 = q[i]
        test = (m/test)
        if is_square(test) and test >= 1:
            test = Integer(test)
            prim_sols.append((test*p[i],test*q[i]))
        i+=1
        P = a*Q - P
        Q = (D-P**2)/Q
        a = Integer(floor((P+root_d)/Q))
        p.append(a*p[i-1]+p[i-2])
        q.append(a*q[i-1]+q[i-2])
    return (x0,y0), prim_sols

def _pell_solve_2(D,m): # m^2 >= D
    prim_sols = []
    t,u = _pell_solve_1(D,1)[0]
    if m > 0:
        L = Integer(0)
        U = Integer(floor(sqrt(m*(t-1)/(2*D))))
    else:
        L = Integer(ceil(sqrt(-m/D)))
        U = Integer(floor(sqrt(-m*(t+1)/(2*D))))
    for y in range(L,U+1):
        y = Integer(y)
        x = (m + D*(y**2))
        if is_square(x):
            x = Integer(sqrt(x))
            prim_sols.append((x,y))
            if not ((-x*x - y*y*D) % m == 0 and (2*y*x) % m == 0): # (x,y) and (-x,y) are in different solution classes, so add both
                prim_sols.append((-x,y))
    return (t,u),prim_sols


import numpy as np
import pandas as pd

def solution():
    r = np.random.uniform(size=1)
    if r <= 1/3:
        return "A"
    if r > 1/3 and r <= 2/3:
        return "B"
    if r > 2/3:
        return "C"

def selection():
    r = np.random.uniform(size=1)
    if r <= 1/3:
        return "A"
    if r > 1/3 and r <= 2/3:
        return "B"
    if r > 2/3:
        return "C"

def open_door(selection):
        if selection == "A":
            r = np.random.uniform(size=1)
            if r < 0.5:
                return "B"
            if r >= 0.5:
                return "C"
        if selection == "B":
            r = np.random.uniform(size=1)
            if r < 0.5:
                return "C"
            if r >= 0.5:
                return "A"
        if selection == "C":
            r = np.random.uniform(size=1)
            if r < 0.5:
                return "B"
            if r >= 0.5:
                return "A"

def reshuffle(prior):
    if prior == "A":
        r = np.random.uniform(size=1)
        if r < 0.5:
            return "B"
        if r >= 0.5:
            return "C"
    if prior == "B":
        r = np.random.uniform(size=1)
        if r < 0.5:
            return "A"
        if r >= 0.5:
            return "C"
    if prior == "C":
        r = np.random.uniform(size=1)
        if r < 0.5:
            return "A"
        if r >= 0.5:
            return "B"

def score(n):
    tbl = [['Trial', 'Solution', 'Selection', 'Open Door', 'Reshuffle', 'Result']]
    for x in range(n):
        a = solution()
        b = selection()
        c = open_door(b)
        d = reshuffle(c)
        if d == a:
            e = "Win"
        else:
            e = "Loss"
        tbl.append([x+1, a, b, c, d, e])
    return tbl

def montey_hall_simple(n):
    print('Simple Monty Hall Problem Simulation')
    print('What does the behavioral data look like versus the simulation?')
    print('https://behavioralscientist.org/steven-pinker-rationality-why-you-should-always-switch-the-monty-hall-problem-finally-explained/')

    print('Begin Simulation w/ Reshuffle')
    print('n = {}'.format(n))
    tbl = score(n)
    df = pd.DataFrame(tbl[1:n+2], columns = tbl[0])
    print(df)
    numerator = sum(df['Result'] == 'Win')
    print(numerator/n)

montey_hall_simple(1000000)