#Align a tomography tilt series using cross correlation
#
#
#developed as part of the tomviz project (www.tomviz.com)

def transform_scalars(dataset):

    #----USER SPECIFIED VARIABLES-----#
    TILT_AXIS  = []  #Specify Tilt Axis Dimensions x=0, y=1, z=2
    #---------------------------------#

    from tomviz import utils
    import numpy as np

    data_py = utils.get_array(dataset) #get data as numpy array

    if data_py is None: #Check if data exists
        raise RuntimeError("No data array found!")

    if TILT_AXIS == []: #If tilt axis is not given, find it
    #Find smallest array dimension, assume it is the tilt angle axis
        if data_py.ndim == 3:
            TILT_AXIS = np.argmin( data_py.shape )
        elif data_py.ndim == 2:
            raise RuntimeError("Data Array is 2 dimensions, it should be 3!")
        else:
            raise RuntimeError("Data Array is not 2 or 3 dimensions!")

    print('Aligning Images by Cross Correlation')
    for i in range(1,np.size(data_py,TILT_AXIS)):#Align image to previous
        if TILT_AXIS == 2:
            im0 = np.fft.fft2(data_py[:,:,i-1])
            im1 = np.fft.fft2(data_py[:,:,i])
            xcor = abs(np.fft.ifft2((im0 * im1.conjugate())))
            shift = np.unravel_index(xcor.argmax(), xcor.shape)
            print( shift )
            data_py[:,:,i] = np.roll( data_py[:,:,i], shift[0], axis = 0)
            data_py[:,:,i] = np.roll( data_py[:,:,i], shift[1], axis = 1)
        elif TILT_AXIS == 1:
            im0 = np.fft.fft2(data_py[:,i-1,:])
            im1 = np.fft.fft2(data_py[:,i,:])
            xcor = abs(np.fft.ifft2((im0 * im1.conjugate())))
            print( np.amax(xcor) )
            shift = np.unravel_index(xcor.argmax(), xcor.shape)
            print( shift )
            data_py[:,i,:] = np.roll( data_py[:,i,:], shift[0], axis = 0)
            data_py[:,i,:] = np.roll( data_py[:,i,:], shift[2], axis = 2)
        elif TILT_AXIS == 0:
            im0 = np.fft.fft2(data_py[i-1,:,:])
            im1 = np.fft.fft2(data_py[i,:,:])
            xcor = abs(np.fft.ifft2((im0 * im1.conjugate())))
            print( np.amax(xcor) )
            shift = np.unravel_index(xcor.argmax(), xcor.shape)
            print( shift )
            data_py[i,:,:] = np.roll( data_py[i,:,:], shift[1], axis = 1)
            data_py[i,:,:] = np.roll( data_py[i,:,:], shift[2], axis = 2)
        else:
            raise RuntimeError("Python Transform Error: Unknown TILT_AXIS.")

    utils.set_array(dataset, data_py)
    print('Align Images Complete')


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

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

# stabilni softmax
def stable_softmax(x):
    max_by_row = np.max(x, axis=1).reshape((-1, 1))
    exp_x_shifted = np.exp(x - max_by_row)
    exp_sums = np.sum(exp_x_shifted, axis=1).reshape((-1, 1))
    probs = exp_x_shifted / exp_sums
    return probs

def one_hot_encode(Y, C):
    Y_encoded = []
    for y in Y:
        y_encoded = np.zeros(C)
        y_encoded[y] = 1
        Y_encoded.append(y_encoded)

    return Y_encoded

def logreg_train(X, Y_, param_niter=100_000, param_delta=0.1, print_frequency=500):
    C = int(np.max(Y_)) + 1
    N = len(X)
    n = len(X[0])
    W = np.random.randn(n, C)
    b = np.zeros((1, C)) # 1 x C
    Y_one_hot_encoded = one_hot_encode(Y_, C)

    for i in range(param_niter):
        scores = (X @ W) + b # N x C
        probs = stable_softmax(scores)
        logprobs = np.log(probs) # N x C
        # dijagnostički ispis
        if i % print_frequency == 0:
            loss = - np.sum(logprobs * Y_one_hot_encoded)  # scalar
            print(f"iteration {i}: loss {loss}")

        # derivacije komponenata gubitka po mjerama
        dL_ds = probs - Y_one_hot_encoded  # N x C
        # gradijenti parametara
        grad_W = np.transpose((1 / N) * np.transpose(dL_ds) @ X) # C x D (ili D x C)
        grad_b = (1 / N) * np.sum(dL_ds, axis=0) # C x 1 (ili 1 x C)
        # poboljšani parametri
        W -= param_delta * grad_W
        b -= param_delta * grad_b

    return W, b

def logreg_classify(X, W, b):
    return stable_softmax(X @ W + b)

def logreg_decfun(X, W, b):
    """
    Decorator for  method logreg_classify
    """

    def classify(X):
        return logreg_classify(X, W, b).argmax(axis=1)

    return classify

def sample_gauss_2d(C, N):
    """
    Method for instantiating data

    Arguments
    C - number of classes
    N - number of samples in each class

    Return Values
    x - array of features NxC
    Y - array of labels   Nx1
    """
    x = []
    Y = []
    for i in range(0, C):
        G = data.Random2DGaussian()
        samples = G.get_sample(N)
        x.append(samples.tolist())
        Y.append([i for j in range(N)])
    x = np.reshape(x, (N * C, 2))
    Y = np.reshape(Y, (N * C, 1))

    return x, Y

if __name__ == "__main__":
    np.random.seed(100)
    # get the training dataset
    X, Y_ = sample_gauss_2d(3, 100)
    W, b = logreg_train(X, Y_)
    # evaluate the model on the training dataset
    probs = logreg_classify(X, W, b)
    print(probs.shape)
    # predicted classes
    Y = np.hstack([np.argmax(probs[i][:]) for i in range(probs.shape[0])])
    # reshaping for other methods purposes
    Y_ = np.hstack(Y_)
    print(W)
    print(b)
    accuracy, pr, M = data.eval_perf_multi(Y, Y_)
    print("Accuracy: ", accuracy)
    print("Precision / Recall: ", pr)
    print("Confussion Matrix: ", M)
    # graph the decision surface
    decfun = logreg_decfun(X, W, b)
    bbox = (np.min(X, axis=0), np.max(X, axis=0))
    data.graph_surface(decfun, bbox, offset=0.5)
    # graph the data points
    data.graph_data(X, Y_, Y, special=[])
    # show the plot
    plt.show()


import numpy as np
import numba as nb

@nb.vectorize
def BoundPhi(phi):
    if phi >= np.pi:
        phi -= 2*np.pi
    elif phi < -np.pi:
        phi += 2*np.pi
    return phi

@nb.njit
def DeltaR2(deta, dphi):
    return deta**2 + BoundPhi(dphi)**2

@nb.njit
def RadToCart2D(r, phi):
    return r*np.cos(phi), r*np.sin(phi)

@nb.njit
def CartToRad2D(x, y):
    return np.sqrt(x**2+y**2), BoundPhi(np.arctan2(y, x))

@nb.njit
def PartCoorToCart3D(pt, eta, phi):
    x, y = RadToCart2D(pt, phi)
    z = pt*np.sinh(eta)
    return x, y, z

@nb.njit
def CartToPartCoor3D(x, y, z):
    pt, phi = CartToRad2D(x, y)
    eta = np.arctanh(z/np.sqrt(z**2+pt**2))
    return pt, eta, phi

@nb.njit
def LorTHPMToXYZE(t, h, p, m):
    x = t*np.cos(p)
    y = t*np.sin(p)
    z = t*np.sinh(h)
    e = np.sqrt(m**2 + t**2 + z**2)
    return x, y, z, e

@nb.njit
def LorXYZEToTHPM(x, y, z, e):
    t = np.sqrt(x**2+y**2)
    h = np.arctanh(z/np.sqrt(t**2+z**2)) if z!=0. else 0.
    p = BoundPhi(np.arctan2(y, x))
    m2 = e**2 - t**2 - z**2
    m = np.sign(m2) * np.sqrt(abs(m2))
    return t, h, p, m


import math
import scipy.integrate as integrate
from tabulate import tabulate
import functools

@functools.lru_cache
def w_sum(f, a, n, h):
    sum = 0

    for i in range(1, n):
        sum += f(a + i * h)

    return sum

@functools.lru_cache
def q_sum(f, a, n, h):
    sum = 0

    for i in range(n):
        sum += f(a + h / 2 + i * h)

    return sum

@functools.lru_cache
def z_sum(f, a, n, h):
    return f(a) + f(a + n * h)

# АСТ 0
def qf_left_rect(f, a, n, h):
    return h * (f(a) + w_sum(f, a, n, h))

# АСТ 0
def qf_right_rect(f, a, n, h):
    return h * (w_sum(f, a, n, h) + f(a + h * n))

# АСТ 1
def qf_inter_rect(f, a, n, h):
    return h * q_sum(f, a, n, h)

# АСТ 1
def qf_trap(f, a, n, h):
    return h / 2 * (z_sum(f, a, n, h) + 2 * w_sum(f, a, n, h))

# АСТ 3
def qf_simp(f, a, n, h):
    return h / 6 * (z_sum(f, a, n, h) + 2 * w_sum(f, a, n, h) + 4 * q_sum(f, a, n, h))

def quad_f(f, a, b):
    integral, _ = integrate.quad(f, a, b)
    return integral

# First derrivative of x ** 2 / (1 + x ** 2)
def df(x):
    return 2 * x / (1 + x ** 2) ** 2

# Second derrivative of x ** 2 / (1 + x ** 2)
def d2f(x):
    return (
        (8 * (x ** 2) / ((1 + x ** 2) ** 3) - 2 / ((1 + x ** 2) ** 2)) * (x ** 2)
        - (8 * (x ** 2)) / ((1 + x ** 2) ** 2)
        + 2 / (1 + x ** 2)
    )

# Fourth derrivative of x ** 2 / (1 + x ** 2)
def d4f(x):
    return (
        12 * ((8 * x ** 2) / (x ** 2 + 1) ** 3 - 2 / (x ** 2 + 1) ** 2)
        + (
            -(288 * x ** 2) / (x ** 2 + 1) ** 4
            + 24 / (x ** 2 + 1) ** 3
            + (384 * x ** 4) / (x ** 2 + 1) ** 5
        )
        * x ** 2
        + 8 * ((24 * x) / (x ** 2 + 1) ** 3 - (48 * x ** 3) / (x ** 2 + 1) ** 4) * x
    )

def find_max(f, a, n, h):
    m = 0
    for i in range(n + 1):
        m = max(m, abs(f(a + h * i)))
    return m

def theor_err(qf, a, b, n, h):
    if qf.__name__ == "qf_left_rect" or qf.__name__ == "qf_right_rect":
        m = find_max(df, a, n, h) * (b - a)
        return 1 / 2 * m * (b - a) * h

    elif qf.__name__ == "qf_inter_rect":
        m = find_max(d2f, a, n, h) * (b - a)
        return 1 / 24 * m * (b - a) * h ** 2

    elif qf.__name__ == "qf_trap":
        m = find_max(d2f, a, n, h) * (b - a)
        return 1 / 12 * m * (b - a) * h ** 2

    else:
        m = find_max(d4f, a, n, h) * (b - a)
        return 1 / 2880 * m * (b - a) * h ** 4

functions = [
    (lambda x: x * x / (1 + x * x), "x ** 2 / (1 + x ** 2)"),
    (lambda x: x * 0 + 1, "1"),
    (lambda x: x * 2, "2x"),
    (lambda x: x ** 2 * 3, "3x ** 2"),
    (lambda x: x ** 3 * 4, "4x ** 3"),
    (lambda x: math.e ** ((-x) ** 2), "e ** (-x ** 2)"),
]

quadr_formulas = [qf_left_rect, qf_right_rect, qf_inter_rect, qf_trap, qf_simp]

if __name__ == "__main__":

    def main_loop():

        while True:
            a = float(input("Введите a: "))
            b = float(input("Введите b: "))
            n = int(input("Введите число разбиений промежутка интегрирования: "))

            h = (b - a) / n
            print(f"h = (b-a)/m = {h}")

            for f, f_name in functions:
                corr_integral = quad_f(f, a, b)
                appr_integrals = [qf(f, a, n, h) for qf in quadr_formulas]
                errs = [abs(y - corr_integral) for y in appr_integrals]

                if f_name == "x ** 2 / (1 + x ** 2)":
                    theor_errs = [theor_err(qf, a, b, n, h) for qf in quadr_formulas]
                else:
                    theor_errs = ["-" for _ in range(len(quadr_formulas))]

                qf_names = [qf.__name__ for qf in quadr_formulas]

                print(f"\nf(x) = {f_name}")
                print(f"Exact integral: {corr_integral}\n")
                print(
                    tabulate(
                        [
                            (qf_name, y, err, terr)
                            for qf_name, y, err, terr in zip(
                                qf_names, appr_integrals, errs, theor_errs
                            )
                        ],
                        headers=["quad_formula", "appr", "err", "theor_err"],
                        floatfmt=".10f",
                    )
                )
                print()

            if input("\nПродолжить с новыми a, b, m? (y, n): ") == "y":
                continue
            else:
                break

    print(
        f"""Задание 4_2. Приближенное вычисление интеграла по составным квадратурным формулам
---------------------------------------------------------------------"""
    )
    main_loop()


# This file is part of the Gudhi Library - https://gudhi.inria.fr/ - which is released under MIT.
# See file LICENSE or go to https://gudhi.inria.fr/licensing/ for full license details.
# Author(s):       Hind Montassif
#
# Copyright (C) 2021 Inria
#
# Modification(s):
#   - YYYY/MM Author: Description of the modification

import numpy as np

from ._points import ctorus
from ._points import sphere

def _generate_random_points_on_torus(n_samples, dim):

    # Generate random angles of size n_samples*dim
    alpha = 2*np.pi*np.random.rand(n_samples*dim)

    # Based on angles, construct points of size n_samples*dim on a circle and reshape the result in a n_samples*2*dim array
    array_points = np.column_stack([np.cos(alpha), np.sin(alpha)]).reshape(-1, 2*dim)

    return array_points

def _generate_grid_points_on_torus(n_samples, dim):

    # Generate points on a dim-torus as a grid
    n_samples_grid = int((n_samples+.5)**(1./dim)) # add .5 to avoid rounding down with numerical approximations
    alpha = np.linspace(0, 2*np.pi, n_samples_grid, endpoint=False)

    array_points = np.column_stack([np.cos(alpha), np.sin(alpha)])
    array_points_idx = np.empty([n_samples_grid]*dim + [dim], dtype=int)
    for i, x in enumerate(np.ix_(*([np.arange(n_samples_grid)]*dim))):
        array_points_idx[...,i] = x
    return array_points[array_points_idx].reshape(-1, 2*dim)

def torus(n_samples, dim, sample='random'):
    """
    Generate points on a flat dim-torus in R^2dim either randomly or on a grid

    :param n_samples: The number of points to be generated.
    :param dim: The dimension of the torus on which points would be generated in R^2*dim.
    :param sample: The sample type of the generated points. Can be 'random' or 'grid'.
    :returns: numpy array containing the generated points on a torus.
        The shape of returned numpy array is:
        if sample is 'random' : (n_samples, 2*dim).
        if sample is 'grid' : (⌊n_samples**(1./dim)⌋**dim, 2*dim), where shape[0] is rounded down to the closest perfect 'dim'th power.
    """
    if sample == 'random':
        # Generate points randomly
        return _generate_random_points_on_torus(n_samples, dim)
    elif sample == 'grid':
        # Generate points on a grid
        return _generate_grid_points_on_torus(n_samples, dim)
    else:
        raise ValueError("Sample type '{}' is not supported".format(sample))


import autograd.numpy as np
import tensorflow as tf
import torch
from numpy import linalg as la
from numpy import random as rnd

import pymanopt
from examples._tools import ExampleRunner
from pymanopt.manifolds import Sphere
from pymanopt.solvers import SteepestDescent

SUPPORTED_BACKENDS = ("Autograd", "Callable", "PyTorch", "TensorFlow")

def create_cost_egrad(manifold, matrix, backend):
    egrad = None

    if backend == "Autograd":

        @pymanopt.function.Autograd(manifold)
        def cost(x):
            return -np.inner(x, matrix @ x)

    elif backend == "Callable":

        @pymanopt.function.Callable(manifold)
        def cost(x):
            return -np.inner(x, matrix @ x)

        @pymanopt.function.Callable(manifold)
        def egrad(x):
            return -2 * matrix @ x

    elif backend == "PyTorch":
        matrix_ = torch.from_numpy(matrix)

        @pymanopt.function.PyTorch(manifold)
        def cost(x):
            return -torch.matmul(x, torch.matmul(matrix_, x))

    elif backend == "TensorFlow":

        @pymanopt.function.TensorFlow(manifold)
        def cost(x):
            return -tf.tensordot(x, tf.tensordot(matrix, x, axes=1), axes=1)

    else:
        raise ValueError(f"Unsupported backend '{backend}'")

    return cost, egrad

def run(backend=SUPPORTED_BACKENDS[0], quiet=True):
    n = 128
    matrix = rnd.randn(n, n)
    matrix = 0.5 * (matrix + matrix.T)

    manifold = Sphere(n)
    cost, egrad = create_cost_egrad(manifold, matrix, backend)
    problem = pymanopt.Problem(manifold, cost=cost, egrad=egrad)
    if quiet:
        problem.verbosity = 0

    solver = SteepestDescent()
    estimated_dominant_eigenvector = solver.solve(problem)

    if quiet:
        return

    # Calculate the actual solution by a conventional eigenvalue decomposition.
    eigenvalues, eigenvectors = la.eig(matrix)
    dominant_eigenvector = eigenvectors[:, np.argmax(eigenvalues)]

    # Make sure both vectors have the same direction. Both are valid
    # eigenvectors, but for comparison we need to get rid of the sign
    # ambiguity.
    if np.sign(dominant_eigenvector[0]) != np.sign(
        estimated_dominant_eigenvector[0]
    ):
        estimated_dominant_eigenvector = -estimated_dominant_eigenvector

    # Print information about the solution.
    print("l2-norm of x:", la.norm(dominant_eigenvector))
    print("l2-norm of xopt:", la.norm(estimated_dominant_eigenvector))
    print(
        "Solution found:",
        np.allclose(
            dominant_eigenvector, estimated_dominant_eigenvector, atol=1e-6
        ),
    )
    error_norm = la.norm(dominant_eigenvector - estimated_dominant_eigenvector)
    print("l2-error:", error_norm)

if __name__ == "__main__":
    runner = ExampleRunner(
        run, "Dominant eigenvector of a PSD matrix", SUPPORTED_BACKENDS
    )
    runner.run()


#!/usr/bin/env python3
"""
Bayesian Probability
"""
from scipy import math, special

def posterior(x, n, p1, p2):
    """
    Calculates the posterior probability that the probability of developing
    severe side effects falls within a specific range given the data:

    x is the number of patients that develop severe side effects
    n is the total number of patients observed
    p1 is the lower bound on the range
    p2 is the upper bound on the range
    You can assume the prior beliefs of p follow a uniform distribution
    If n is not a positive integer, raise a ValueError with the message n must
    be a positive integer
    If x is not an integer that is greater than or equal to 0, raise a
    ValueError with the message x must be an integer that is greater than or
    equal to 0
    If x is greater than n, raise a ValueError with the message x cannot be
    greater than n
    If p1 or p2 are not floats within the range [0, 1], raise a ValueError with
    the message {p} must be a float in the range [0, 1] where {p} is the
    corresponding variable
    if p2 <= p1, raise a ValueError with the message p2 must be greater than p1
    The only import you are allowed to use is from scipy import special

    Returns: the posterior probability that p is within the range [p1, p2]
    given x and n
    """
    if not isinstance(n, int) or n <= 0:
        raise ValueError("n must be a positive integer")
    if not isinstance(x, (int, float)) or (x < 0):
        mg = "x must be an integer that is greater than or equal to 0"
        raise ValueError(mg)
    if x > n:
        raise ValueError("x cannot be greater than n")
    if not isinstance(p1, float) or p1 < 0 or p1 > 1:
        raise ValueError("p1 must be a float in the range [0, 1]")
    if not isinstance(p2, float) or p2 < 0 or p2 > 1:
        raise ValueError("p2 must be a float in the range [0, 1]")
    if p2 <= p1:
        raise ValueError("p2 must be greater than p1")

    num = (math.factorial(n+1))
    den = (math.factorial(x) * math.factorial(n - x))
    fct = num / den
    hp1 = special.hyp2f1(x+1, x-n, x+2, p1)
    hp2 = special.hyp2f1(x+1, x-n, x+2, p2)

    return fct * (((p2**(x+1))*hp2)-((p1**(x+1))*hp1))/(x+1)


"""

https://en.wikipedia.org/wiki/Pseudovector
https://en.wikipedia.org/wiki/Improper_rotation

"""

import numpy as np
from scipy.stats import special_ortho_group

def rotate_vectors(R, v1, v2, v3, muldet):
    d = np.linalg.det(R)
    v1p = np.dot(R, v1)
    v2p = np.dot(R, v2)
    v3p = np.dot(R, v3)
    if muldet:
        v3p *= d
    vcp = np.cross(v1p, v2p)
    return (d, v1p, v2p, v3p, vcp)

"""

A pseudovector (axial vector) is a quantity that transforms like a vector
under proper rotation but under improper rotation, the sign flips

An improper rotation is a reflection/mirror, mathematically it is a
rotation matrix but has a determinant of -1

A cross product of 2 vectors cross(a, b) gives a pseudovector c and a and b are
considered to be polar vectors

Putting the above together, this means that if we have two vectors a and b
and we take the cross product of them like so:
c = cross(a, b)
a' = R*a
b' = R*b
c' = R*c

if R was a proper rotation matrix (det(R)=1) then
cross(a', b') = c'

however if R was a improper rotation matrix (det(R)=-1) then
cross(a', b') = -c'

The way to fix this is to account for the determinant of the rotation matrix for the pseudovector
a' = R*a
b' = R*b
c' = det(R)*R*c

Here cross(a', b') = c' regardless of whether or not the rotation is proper or improper because
the det(A) will be -1 for improper rotation and fixes the sign for the pseudovector

"""

def test_random_vector_rotation():
    u = np.random.randint(1, high=3)
    v = np.random.randint(1, high=3)
    if u == v:
        v = (v + 1) % 3

    # generate a rotation matrix
    # then make another rotation matrix that
    # has a negative determinant by swapping the rows
    R = special_ortho_group.rvs(3)
    Ri = R.copy()
    Ri[[u, v]] = R[[v, u]]

    v1 = np.random.rand(3)
    v2 = np.random.rand(3)
    v3 = np.cross(v1, v2)

    # do a proper rotation, improper rotation, and a improper with the determinant sign accounted for
    # the improper rotation without the determinant sign accounted for should give a negative cross product
    # for the new transformed vector v1 and v2
    d, v1r, v2r, v3r, vcr = rotate_vectors(R, v1, v2, v3, False)
    di, v1ri, v2ri, v3ri, vcri = rotate_vectors(Ri, v1, v2, v3, False)
    _, v1pri, v2pri, v3pri, vcpri = rotate_vectors(Ri, v1, v2, v3, True)

    print("Rotation Matrices")
    print(R)
    print("det={% .4f}" % (d))
    print(Ri)
    print("det={% .4f}" % (di))
    print()
    print("Original Vectors")
    print(v1)
    print(v2)
    print(v3)
    print()
    print("Proper Rotation of Vectors")
    print(v1r)
    print(v2r)
    print(v3r)
    print(vcr)
    print()
    print("Improper Rotation of Vectors")
    print(v1ri)
    print(v2ri)
    print(v3ri)
    print(vcri)
    print()
    print("Improper Rotation of Vectors with Determinant Accounted")
    print(v1pri)
    print(v2pri)
    print(v3pri)
    print(vcpri)

"""

A negative scale value applied to one axis can be thought of a reflection over that one axis.
If we apply a uniform negative scale to all the axis, it can be thought of a sequence of reflections.
Reflections can become rotations depending on how many times we apply it.

A negative uniform scaling can be characterized as the following transformation:

A rotation in even dimensions (2D, 4D, etc)
A reflection in odd dimensions (3D, 5D, etc)

We can see this by taking the determinant of the scale matrix and look at the sign to see if the orientation flipped.
For even dimension, we have an even number of negative number multiplication so it is always positive. (rotation)
For odd dimension, we have an odd number of negative number multiplication so it is always negative. (reflection)

"""

def test_negative_scale():
    s = np.random.randint(-1000, high=-1)
    for i in range(10):
        print("Negative Scale of {}x{}".format(i+1, i+1))
        S = s*np.eye(i+1)
        print(np.linalg.det(S))
        print(S)

test_random_vector_rotation()
test_negative_scale()


import numpy as np
import csv
import math

def transform_from_pose(pose): #orientation in degree
    translation = np.asarray([pose[0],pose[1],pose[2]]).reshape(3,1)
    R = RPYeulerAnglesToRotationMatrix([np.deg2rad(pose[3]),np.deg2rad(pose[4]),np.deg2rad(pose[5])])
    T_temp = np.hstack((R,translation))
    T4 = [0.0, 0.0, 0.0, 1.0]
    T = np.vstack((T_temp,T4))

    return T

def transform_from_translation_only(translation):
    #translation in millimeter
    x = translation[0]
    y = translation[1]
    z = translation[2]
    T1 = [1.0, 0.0, 0.0, x]
    T2 = [0.0, 1.0, 0.0, y]
    T3 = [0.0, 0.0, 1.0, z]
    T4 = [0.0, 0.0, 0.0, 1.0]
    T = np.array(np.vstack((T1,T2,T3,T4)))

    return T

def transformation_matrix_to_rotation(T,key): #from transformation matrix (or rotation matrix)to Euler angles or Quaternions
    c_11 = T[0][0]
    c_12 = T[0][1]
    c_13 = T[0][2]

    c_21 = T[1][0]
    c_22 = T[1][1]
    c_23 = T[1][2]

    c_31 = T[2][0]
    c_32 = T[2][1]
    c_33 = T[2][2]

    if key == 'XYZ_euler_angles': #return in rad
        Rx = np.arctan2(-c_23,c_33)
        Ry = np.arctan2(c_13,np.sqrt(c_11**2 + c_12**2))
        Rz = np.arctan2(-c_12,c_11)
        rotation = [Rx,Ry,Rz]
    elif key == 'unit_quaternion':
        tr = c_11 + c_22 + c_33
        #print("tr: ",tr)
        if tr > 0:
            #print("case:1")
            S = np.sqrt(tr + 1.0)*2.0
            q_0 = 0.25*S #0.5*np.sqrt(tr + 1) #qw
            q_1 = (c_32 - c_23)/S #0.5*np.sign(c_32 - c_23)*np.sqrt(c_11 - c_22 - c_33 + 1) #qx
            q_2 = (c_13 - c_31)/S #0.5*np.sign(c_13 - c_31)*np.sqrt(c_22 - c_33 - c_11 + 1) #qy
            q_3 = (c_21 - c_12)/S # 0.5*np.sign(c_21 - c_12)*np.sqrt(c_33 - c_11 - c_22 + 1) #qz

            #q_0_ = 0.5*np.sqrt(tr + 1)
            #q_1_ = 0.5*np.sign(c_32 - c_23)*np.sqrt(c_11 - c_22 - c_33 + 1)
            #q_2_ = 0.5*np.sign(c_13 - c_31)*np.sqrt(c_22 - c_33 - c_11 + 1)
            #q_3_ = 0.5*np.sign(c_21 - c_12)*np.sqrt(c_33 - c_11 - c_22 + 1)

            #print("q0: ",q_0, " ",q_0_)
            #print("q1: ",q_1, " ",q_1_)
            #print("q2: ",q_2, " ",q_2_)
            #print("q3: ",q_3, " ",q_3_)
        elif (c_11 > c_22) and (c_11 > c_33):
            #print("case:2")
            S = np.sqrt(1.0 + c_11 - c_22 - c_33) * 2.0 #S = 4*qx
            q_0 = (c_32 - c_23)/S
            q_1 = 0.25*S
            q_2 = (c_12 + c_21)/S
            q_3 = (c_13 + c_31)/S
        elif c_22 > c_33:
            #print("case:3")
            S = np.sqrt(1.0 + c_22 - c_11 - c_33) * 2.0 #S = 4*qy
            q_0 = (c_13 - c_31)/S
            q_1 = (c_12 + c_21)/S
            q_2 = 0.25*S
            q_3 = (c_23 + c_32)/S
        else:
            #print("case:4")
            S = np.sqrt(1.0 + c_33 - c_11 - c_22) * 2.0 #S = 4*qz
            q_0 = (c_21 - c_12)/S
            q_1 = (c_13 + c_31)/S
            q_2 = (c_23 + c_32)/S
            q_3 = 0.25*S
    # https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
    # https://math.stackexchange.com/questions/893984/conversion-of-rotation-matrix-to-quaternion
    # https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation
        rotation = [q_0,q_1,q_2,q_3]
    else:
        print("unidentified key")
        rotation  = [0,0,0,0]

    return rotation

#def RotationMatrixToQuaternions(R) : #rotation matrix to q0,q1,q2,q3

#    tr = m00 + m11 + m22

def transform_from_MDH_parameters_analytical(alpha,beta,a,d,theta):
    #alpha, theta, beta: in radian
	cos_theta = np.cos(theta)
	sin_theta = np.sin(theta)

	cos_alpha = np.cos(alpha)
	sin_alpha = np.sin(alpha)

	T1 = [cos_theta, -sin_theta, 0.0, a]
	T2 = [cos_alpha*sin_theta, cos_alpha*cos_theta, -sin_alpha, -d*sin_alpha]
	T3 = [sin_alpha*sin_theta, sin_alpha*cos_theta, cos_alpha, d*cos_alpha]
	T4 = [0.0,0.0,0.0,1.0]
	T = np.array(np.vstack((T1,T2,T3,T4)))

	return T

def transform_from_DH_parameters_analytical(alpha,beta,a,d,theta):

	cos_theta = np.cos(theta)
	sin_theta = np.sin(theta)

	cos_alpha = np.cos(alpha)
	sin_alpha = np.sin(alpha)

	T1 = [cos_theta, -sin_theta*cos_alpha, sin_theta*sin_alpha, a*cos_theta]
	T2 = [sin_theta, cos_theta*cos_alpha, -cos_theta*sin_alpha, a*sin_theta]
	T3 = [0        , sin_alpha,            cos_alpha, d]
	T4 = [0.0,0.0,0.0,1.0]
	T = np.array(np.vstack((T1,T2,T3,T4)))

	return T

def transform_from_MDH_parameters_numerical(alpha,beta,a,d,theta):

    Rx_alpha = rotation_about_axis('x',alpha)
    Dx_a = translation_along_axis('x',a)

    Ry_beta = rotation_about_axis('y',beta)
    Dz_d = translation_along_axis('z',d)
    Rz_theta = rotation_about_axis('z',theta)

    #T = np.dot(Rx_alpha,np.dot(Dx_a,np.dot(Dz_d, np.dot(Ry_beta,Rz_theta))))
    T = np.dot(Rx_alpha,np.dot(Dx_a,np.dot(Ry_beta,np.dot(Rz_theta,Dz_d))))
    return T

def rotation_about_axis(axis_name, rotation_angle):
	#rotation_angle in radian
	theta = rotation_angle
	cos_theta = np.cos(theta)
	sin_theta = np.sin(theta)
	if axis_name == 'x':
		T1 = [1.0,        0.0,        0.0, 0.0]
		T2 = [0.0,  cos_theta, -sin_theta, 0.0]
		T3 = [0.0,  sin_theta,  cos_theta, 0.0]
		T4 = [0.0,        0.0,        0.0, 1.0]
		T = np.vstack((T1,T2,T3,T4))

	elif axis_name == 'y':
		T1 = [cos_theta ,    0.0, sin_theta, 0.0]
		T2 = [0.0       ,    1.0,       0.0, 0.0]
		T3 = [-sin_theta,    0.0, cos_theta, 0.0]
		T4 = [0.0       ,    0.0,       0.0, 1.0]
		T = np.vstack((T1,T2,T3,T4))

	elif axis_name == 'z':
		T1 = [cos_theta, -sin_theta, 0.0, 0.0]
		T2 = [sin_theta,  cos_theta, 0.0, 0.0]
		T3 = [0.0,              0.0, 1.0, 0.0]
		T4 = [0.0,              0.0, 0.0, 1.0]
		T = np.vstack((T1,T2,T3,T4))
	else:
		print('wrong axis name in rotation!')
		T = np.zeros((4,4))
	return T

def translation_along_axis(axis_name, translation):
	#translation in millimeter
	d = translation
	if axis_name == 'x':
		T1 = [1.0, 0.0, 0.0, d]
		T2 = [0.0, 1.0, 0.0, 0.0]
		T3 = [0.0, 0.0, 1.0, 0.0]
		T4 = [0.0, 0.0, 0.0, 1.0]
		T = np.vstack((T1,T2,T3,T4))

	elif axis_name == 'y':
		T1 = [1.0, 0.0, 0.0, 0.0]
		T2 = [0.0, 1.0, 0.0, d]
		T3 = [0.0, 0.0, 1.0, 0.0]
		T4 = [0.0, 0.0, 0.0, 1.0]
		T = np.vstack((T1,T2,T3,T4))

	elif axis_name == 'z':
		T1 = [1.0, 0.0, 0.0, 0.0]
		T2 = [0.0, 1.0, 0.0, 0.0]
		T3 = [0.0, 0.0, 1.0, d]
		T4 = [0.0, 0.0, 0.0, 1.0]
		T = np.vstack((T1,T2,T3,T4))

	else:
		print('wrong axis name in translation!')
		T = np.zeros((4,4))

	return T

def DH_T_analytical(DH, idx, joint_angle):
    #a,alpha,beta,d,theta
    a = DH[idx][0]
    alpha = DH[idx][1]
    beta = DH[idx][2]
    d = DH[idx][3]
    theta = DH[idx][4]+ joint_angle
    #print(a," ",alpha, " ", beta," ",d," ",theta)
    return transform_from_DH_parameters_analytical(alpha,beta,a,d,theta)

'''
def DH_T_numerical(DH, idx, joint_angle):
    #a,alpha,beta,d,theta
    a = DH[idx][0]*1000.0
    alpha = DH[idx][1]
    beta = DH[idx][2]
    d = DH[idx][3]*1000.0
    theta = DH[idx][4] + joint_angle
    #print("a, alpha,beta,d,theta, joint angle, theta + joint angle: ",a," ",alpha, " ", beta," ", d," ",DH[idx][4]," ",joint_angle," ",theta)
    return transform_from_DH_parameters_numerical(alpha,beta,a,d,theta)
'''

def MDH_T_analytical(DH, idx, joint_angle):
    #a,alpha,beta,d,theta
    a = DH[idx][0]
    alpha = DH[idx][1]
    beta = DH[idx][2]
    d = DH[idx][3]
    theta = DH[idx][4]+ joint_angle
    #print(a," ",alpha, " ", beta," ",d," ",theta)
    return transform_from_MDH_parameters_analytical(alpha,beta,a,d,theta)

def MDH_T_numerical(DH, idx, joint_angle):
    #a,alpha,beta,d,theta
    a = DH[idx][0]
    alpha = DH[idx][1]
    beta = DH[idx][2]
    d = DH[idx][3]
    theta = DH[idx][4]+ joint_angle
    #print("a, alpha,beta,d,theta, joint angle, theta + joint angle: ",a," ",alpha, " ", beta," ", d," ",DH[idx][4]," ",joint_angle," ",theta)
    return transform_from_MDH_parameters_numerical(alpha,beta,a,d,theta)

# Checks if a matrix is a valid rotation matrix.
def isRotationMatrix(R) :
    Rt = np.transpose(R)
    shouldBeIdentity = np.dot(Rt, R)
    I = np.identity(3, dtype = R.dtype)
    n = np.linalg.norm(I - shouldBeIdentity)
    return n < 1e-6

def RPYeulerAnglesToRotationMatrix(theta=[0,0,0]) : #ZYX euler angle
    roll  = theta[0]
    pitch = theta[1]
    yaw   = theta[2]
    R_x = np.array([[1,                0,               0               ],
                    [0,                math.cos(roll), -math.sin(roll) ],
                    [0,                math.sin(roll),  math.cos(roll) ]])
    R_y = np.array([[math.cos(pitch),  0,               math.sin(pitch)],
                    [0,                1,               0              ],
                    [-math.sin(pitch), 0,               math.cos(pitch)]])
    R_z = np.array([[math.cos(yaw),   -math.sin(yaw),   0],
                    [math.sin(yaw),    math.cos(yaw),   0],
                    [0,                0,               1]])

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

    assert(isRotationMatrix(R))

    return R

def RotationMatrixToRPYeulerAngles(R) : #rotation matrix to roll, pitch, yaw
    assert(isRotationMatrix(R))
    sy = math.sqrt(R[0,0] * R[0,0] +  R[1,0] * R[1,0])
    singular = sy < 1e-6
    if  not singular :
        x = math.atan2(R[2,1] , R[2,2]) #psi, rotation about x
        y = math.atan2(-R[2,0], sy) # theta, rotation about y
        z = math.atan2(R[1,0], R[0,0]) #phi, rotation about z
    else :
        x = math.atan2(-R[1,2], R[1,1])
        y = math.atan2(-R[2,0], sy)
        z = 0

    return np.array([x, y, z]) #psi,theta, phi, R = R_z(phi)*R_y(theta)*R_x(psi), return in radian

def RPYeulerAnglesToQuaternion(theta=[0,0,0]) : #ZYX euler angles to Quaternion
    roll  = theta[0] #phi
    pitch = theta[1] #theta
    yaw   = theta[2] #psi
    cos_phi_2   = math.cos(roll * 0.5)
    cos_theta_2 = math.cos(pitch * 0.5)
    cos_psi_2   = math.cos(yaw * 0.5)
    sin_phi_2   = math.sin(roll * 0.5)
    sin_theta_2 = math.sin(pitch * 0.5)
    sin_psi_2   = math.sin(yaw * 0.5)

    q0 = cos_phi_2 * cos_theta_2 * cos_psi_2 + sin_phi_2 * sin_theta_2 * sin_psi_2
    q1 = sin_phi_2 * cos_theta_2 * cos_psi_2 - cos_phi_2 * sin_theta_2 * sin_psi_2
    q2 = cos_phi_2 * sin_theta_2 * cos_psi_2 + sin_phi_2 * cos_theta_2 * sin_psi_2
    q3 = cos_phi_2 * cos_theta_2 * sin_psi_2 - sin_phi_2 * sin_theta_2 * cos_psi_2

    return np.array([q0,q1,q2,q3])

def QuaternionToRPYeulerAngles(q=[0,0,0,0]) : #Quaternion to ZYX euler angles (radian)
    q_norm = np.linalg.norm(q)
    if abs(1-q_norm) > 0.0001:
        print("norm of quaternion is not 1")

    q0 = q[0]
    q1 = q[1]
    q2 = q[2]
    q3 = q[3]

    phi = math.atan2(2.0*(q0*q1 + q2*q3), 1.0 - 2.0*(q1*q1 + q2*q2))
    theta = math.asin(2.0*(q0*q2 - q3*q1))
    psi = math.atan2(2.0*(q0*q3 + q1*q2), 1.0 - 2.0*(q2*q2 + q3*q3))

    return np.array([phi,theta,psi])

def QuaternionToRotationMatrix(Q=[0,0,0,0]) : #Quaternion to Rotation matrix
    """
    Covert a quaternion into a full three-dimensional rotation matrix.

    Input
    :param Q: A 4 element array representing the quaternion (q0,q1,q2,q3)

    Output
    :return: A 3x3 element matrix representing the full 3D rotation matrix.
             This rotation matrix converts a point in the local reference
             frame to a point in the global reference frame.
    """
    q_norm = np.linalg.norm(Q)
    if abs(1-q_norm) > 0.0001:
        print("norm of quaternion is not 1."," The norm of q is ",q_norm)

    # Extract the values from Q
    q0 = Q[0]
    q1 = Q[1]
    q2 = Q[2]
    q3 = Q[3]

    # First row of the rotation matrix
    r00 = 2 * (q0 * q0 + q1 * q1) - 1
    r01 = 2 * (q1 * q2 - q0 * q3)
    r02 = 2 * (q1 * q3 + q0 * q2)

    # Second row of the rotation matrix
    r10 = 2 * (q1 * q2 + q0 * q3)
    r11 = 2 * (q0 * q0 + q2 * q2) - 1
    r12 = 2 * (q2 * q3 - q0 * q1)

    # Third row of the rotation matrix
    r20 = 2 * (q1 * q3 - q0 * q2)
    r21 = 2 * (q2 * q3 + q0 * q1)
    r22 = 2 * (q0 * q0 + q3 * q3) - 1

    # 3x3 rotation matrix
    R = np.array([[r00, r01, r02],
                  [r10, r11, r12],
                  [r20, r21, r22]])

    assert(isRotationMatrix(R))

    return R

#https://en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations
def RotationMatrixFromAxisAngle(u,theta): #u = unit direction vector, theta = rotation angle in radian

    u_norm = np.linalg.norm(u,ord=2)
    if np.abs(u_norm - 1.0) > 1e-7:
        print("rotation axis vector is not a unit vector!")
        print("norm of the rotation axis vector is ",u_norm)
        u = np.multiply(1/u_norm,u)
    c_theta = np.cos(theta)
    s_theta = np.sin(theta)
    ux = u[0]
    uy = u[1]
    uz = u[2]
    alpha = 1-c_theta

    r00 = c_theta + ux*ux*alpha
    r01 = ux*uy*alpha - uz*s_theta
    r02 = ux*uz*alpha + uy*s_theta

    r10 = uy*ux*alpha + uz*s_theta
    r11 = c_theta + uy*uy*alpha
    r12 = uy*uz*alpha - ux*s_theta

    r20 = uz*ux*alpha - uy*s_theta
    r21 = uz*uy*alpha + ux*s_theta
    r22 = c_theta + uz*uz*alpha

    # 3x3 rotation matrix
    R = np.array([[r00, r01, r02],
                  [r10, r11, r12],
                  [r20, r21, r22]])

    assert(isRotationMatrix(R))

    return R


# encoding=utf-8
from numpy import *
''' Numpy的主要对象是同种元素的多维数组，这是一个所有的元素都是一种类型，通过一个正整数元祖索引的元素表格（通常的元素是数字）。
    在Numpy中维度（dimensions）叫做轴（axes），轴的个数叫做秩（rank）
    例如，在3D空间一个点的坐标[1,2,3]是一个秩为1的数组，因为它只有一个轴，那个轴的长度为3，
    例如，[[1.,0.,0.],[0.,1.,2.]] 的秩为2，第一个维度长度为2，第二个维度长度为3'''

''' Numpy的数组类被称作ndarry，通常被称作数组，注意numpy.array和标准python库类array.array并不相同，后者只处理一维数组和提供少量功能'''

A = array([[1.,0.,0.],[0.,1.,2.]])
# A = arange(15).reshape(3, 5)

print (A.ndim) #数组轴的个数
print (A.shape)# 数组的维度，这是一个指定数组在每个维度上大小的整数元祖。例如一个n排m列的矩阵，它的shape属性将是（2,3）
print (A.size) # 数据元素的总个数
print (A.dtype) # 描述数组中元素类型的对象
print (A.itemsize) #数组中每个元素的字节大小，一个元素类型为float64的数组itemsize属性值为8=（64/8）
print (A.data) #包含数据数组元素的缓冲区，通常我们不需要使用这个属性，因为我们总是用过索引来使用数组中的元素

'''创建数组'''
A = array([[1.,0.,0.],[0.,1.,2.]])
print (A)

B = array ([(1.5,2,3),(4,5,6)]) # 数组将序列包含序列转化成二维的数组
C = array([[1,2],[2,3]],dtype = complex) # 数字类型可以在创建时显示指定
D = zeros((3,4)) # 创建一个全是0的数组，最小化了扩展数组的需求和高昂的运算代价
E = ones((4,5))
F = empty((2,3)) # 创建一个内容随机的数组，类型为 float64
G = arange(5) # array([0, 1, 2, 3，5])
H = arange(10,30,5) # 返回array([10, 15, 20, 25])

B = arange(15).reshape(3, 5) # 打印2d array
C = arange(24).reshape(2, 3,4) # 打印3d array
set_printoptions(threshold='nan') # 设置参数来完成打印

'''基本运算'''

## **2 平方

A = array( [[1,1], [0,1]] )
B = array( [[2,0], [3,4]] )
A*B #元素相称
dot(A,B)  # 矩阵相乘
D *= 3  # 更改数组中的元素
A += B
B.sum(axis=0) # 每一列的总和 指定axis参数你可以把运算应用到数组置顶的轴上
B.min(axis=1) # 每一行的最小值
kkkkk


"""Generate random state vector."""
from typing import List, Union

import numpy as np

from toqito.states import max_entangled
from toqito.perms import swap

def random_state_vector(
    dim: Union[List[int], int], is_real: bool = False, k_param: int = 0
) -> np.ndarray:
    r"""Generate a random pure state vector.

    Examples
    ==========

    We may generate a random state vector. For instance, here is an example where we can generate a
    :math:`2`-dimensional random state vector.

    >>> from toqito.random import random_state_vector
    >>> vec = random_state_vector(2)
    >>> vec
    [[0.50993973+0.15292408j],
     [0.27787332+0.79960122j]]

    We can verify that this is in fact a valid state vector by computing the corresponding density
    matrix of the vector and checking if the density matrix is pure.

    >>> from toqito.state_props import is_pure
    >>> dm = vec.conj().T * vec
    >>> is_pure(dm)
    True

    :param dim: The number of rows (and columns) of the unitary matrix.
    :param is_real: Boolean denoting whether the returned matrix has real
                    entries or not. Default is :code:`False`.
    :param k_param: Default 0.
    :return: A :code:`dim`-by-:code:`dim` random unitary matrix.
    """
    # Schmidt rank plays a role.
    if 0 < k_param < np.min(dim):

        # Allow the user to enter a single number for dim.
        if isinstance(dim, int):
            dim = [dim, dim]

        # If you start with a separable state on a larger space and multiply
        # the extra `k_param` dimensions by a maximally entangled state, you
        # get a Schmidt rank `<= k_param` state.
        psi = max_entangled(k_param, True, False).toarray()

        a_param = np.random.rand(dim[0] * k_param, 1)
        b_param = np.random.rand(dim[1] * k_param, 1)

        if not is_real:
            a_param = a_param + 1j * np.random.rand(dim[0] * k_param, 1)
            b_param = b_param + 1j * np.random.rand(dim[1] * k_param, 1)

        mat_1 = np.kron(psi.conj().T, np.identity(int(np.prod(dim))))
        mat_2 = swap(
            np.kron(a_param, b_param),
            sys=[2, 3],
            dim=[k_param, dim[0], k_param, dim[1]],
        )

        ret_vec = mat_1 @ mat_2
        return np.divide(ret_vec, np.linalg.norm(ret_vec))

    # Schmidt rank is full, so ignore it.
    ret_vec = np.random.rand(dim, 1)
    if not is_real:
        ret_vec = ret_vec + 1j * np.random.rand(dim, 1)
    return np.divide(ret_vec, np.linalg.norm(ret_vec))


#!/usr/bin/env python2
# -*- coding: utf-8 -*-
"""
Created on Thu Jun 29 17:08:12 2017

@author: lracuna
"""
from scipy.linalg import expm
import numpy as np
from math import cos, sin

def rotation_matrix(a, alpha):
    """  Creates a 3D [R|t] matrix for rotation
    around the axis of the vector a by an alpha angle."""
    #Normalize the rotation axis a
    a = a / np.linalg.norm(a)

    #Build the skew symetric
    a_skew = np.mat([[0,-a[2],a[1]], [a[2], 0, -a[0]], [-a[1], a[0], 0]])
    R = np.eye(4)
    R[:3,:3] = expm(a_skew*alpha)
    return R

def translation_matrix(t):
  """  Creates a 3D [R|t] matrix with a translation t
  and an identity rotation """
  R = np.eye(4)
  R[:3,3] = np.array([t[0],t[1],t[2]])
  return R

def rotation_matrix_from_two_vectors(a,b):
    a = a / np.linalg.norm(a)
    b = b / np.linalg.norm(b)
    v = np.cross(a,b)
    ssc = np.matrix(np.array([[0.0, -v[2], v[1]],
                 [v[2], 0.0, -v[0]],
                 [-v[1], v[0], 0]]))
    R = np.eye(4)
    #TODO Yue: Form wrong ??? https://gist.github.com/peteristhegreat/3b76d5169d7b9fc1e333
    R[:3,:3] = np.array(np.eye(3) + ssc + (ssc**2)*(1.0/(1.0+np.dot(a,b))))
    return R

def rot_matrix_error(R0, R1, method = 'unit_quaternion_product'):
    """ R0, R1 are 3x3 or 4x4 homogeneous Rotation matrixes
        returns: the value of the error depending on the method """

    if ((R0.shape != (4,4)) and (R0.shape != (3,3))):
        print ("Error in the R0 input rotation matrix shape, must be 3x3 or 4x4")
        print R0
        return -1
    if ((R1.shape != (4,4)) and (R1.shape != (3,3))):
        print ("Error in the R1 input rotation matrix shape, must be 3x3 or 4x4")
        print R1
        return -1

    if R0.shape == (3,3):
        R = np.eye(4)
        R[:3,:3] = R0
        R0 = R

    if R1.shape == (3,3):
        R = np.eye(4)
        R[:3,:3] = R1
        R1 = R

    if(method == 'unit_quaternion_product' ):
        ## From the paper "Metrics for 3D Rotations: Comparison and Analysis" D. Huynh
        # The 3D rotation error is computed using the inner product of unit quaterions

        #We use the ros library TF to convert rotation matrix into unit quaternions
        from tf import transformations
        q0 = transformations.quaternion_from_matrix(R0)
        q1 = transformations.quaternion_from_matrix(R1)

        # We convert into unit quaternions
        q0 = q0 / np.linalg.norm(q0)
        q1 = q1 / np.linalg.norm(q1)

        #Find the error as defined in the paper
        rot_error = 1 - np.linalg.norm(np.dot(q0,q1))

    if(method == 'angle'):
        #option 2 find the angle of this rotation. In particular, the above is invalid
        #for very large error angles (error > 90 degrees) and is imprecise for large
        #error angles (angle > 45 degrees).

        E = R1.dot(R0.T)
        from cv2 import Rodrigues
        rot_vector, J = Rodrigues(E[:3,:3])

        angle = np.linalg.norm(rot_vector)

        rot_error = np.rad2deg(angle)
#        d = np.zeros(3)
#        d[0] = E[1,2] - E[2,1]
#        d[1] = E[2,0] - E[0,2]
#        d[2] = E[0,1] - E[1,0]
#
#        dmag = sqrt(d[0]*d[0] + d[1]*d[1] + d[2]*d[2])
#
#        phi = np.rad2deg(asin(dmag/2))
#
#        rot_error = phi

    return rot_error

#TODO MAYBE USE the TF package directly
#http://docs.ros.org/kinetic/api/tf/html/c++/classtf_1_1Matrix3x3.html#a9219cc537dd0a0dd8370959e5300c277

def R_matrix_from_euler_t(alpha,beta,gamma):
  """
  A=BCD. Z -> X -> Z
  1. the first rotation is by an angle phi about the z-axis using D,
  2. the second rotation is by an angle theta in [0,pi] about the former x-axis (now x^') using C, and
  3. the third rotation is by an angle psi about the former z-axis (now z^') using B.
  :param alpha:
  :param beta:
  :param gamma:
  :return:
  """
  R = np.eye(4, dtype=np.float32)
  R[0,0]=cos(alpha)*cos(gamma)-cos(beta)*sin(alpha)*sin(gamma)
  R[1,0]=cos(gamma)*sin(alpha)+cos(alpha)*cos(beta)*sin(gamma)
  R[2,0]=sin(beta)*sin(gamma)

  R[0,1]=-cos(beta)*cos(gamma)*sin(alpha)-cos(alpha)*sin(gamma)
  R[1,1]=cos(alpha)*cos(beta)*cos(gamma)-sin(alpha)*sin(gamma)
  R[2,1]=cos(gamma)*sin(beta)

  R[0,2]=sin(alpha)*sin(beta)
  R[1,2]=-cos(alpha)*sin(beta)
  R[2,2]=cos(beta)

  return R

def R_matrix_from_euler_zyx(alpha,beta,gamma):
  """
  author: Yue Hu
  A=BCD. Z -> Y -> X
  :param alpha:
  :param beta:
  :param gamma:
  :return:
  """
  R = np.eye(4, dtype=np.float32)
  R[0,0] = cos(beta) * cos(alpha)
  R[1,0] = sin(gamma) * sin(beta) * cos(alpha) - cos(gamma) * sin(alpha)
  R[2,0] = cos(gamma) * sin(beta) * cos(alpha) + sin(gamma) * sin(alpha)

  R[0,1] = cos(beta) * sin(alpha)
  R[1,1] = sin(gamma) * sin(beta) * sin(alpha) + cos(gamma) * cos(alpha)
  R[2,1] = cos(gamma) * sin(beta) * sin(alpha) - sin(gamma) * cos(alpha)

  R[0,2] = -sin(beta)
  R[1,2] = sin(gamma) * cos(beta)
  R[2,2] = cos(gamma) * cos(beta)

  return R


def tilde_w(c, k, dx, dt):
    C = dt*c/dx
    return 2/dt*asin(C*sin(k*dx/2))

def tilde_c(c, k, dx, dt):
    return tilde_w(c, k, dx, dt)/k

def r(C, p):
    return 1/(C*p)*asin(C*sin(p))  # important with 1, not 1. for sympy

def makeplot():
    n = 16
    p = linspace(0.001, pi/2, n)
    legends = []
    for C in 1.0, 0.95, 0.8, 0.3:
        plot(p, r(C, p))
        hold('on')
        legends.append('C=%g' % C)
    title('Numerical divided by exact wave velocity')
    legend(legends, fancybox=True, loc='lower left')
    axis([p[0], p[-1], 0.6, 1.1])
    xlabel('p')
    ylabel('velocity ratio')
    savefig('tmp.pdf')
    savefig('tmp.eps')
    savefig('tmp.png')
    show()

def sympy_analysis():
    C, p = symbols('C p')
    # Turn series expansion into polynomial and Python function
    # representations
    rs = r(C, p).series(p, 0, 7).removeO()
    print 'series representation of r(C, p):', rs
    print 'factored series representation of r(C, p):', factor(rs)
    rs1 = factor((rs - 1).extract_leading_order(p)[0][0])
    print 'leading order of the error:', rs1
    rs_poly = poly(rs)
    print 'polynomial representation:', rs_poly
    rs_pyfunc = lambdify([C, p], rs)  # can be used for plotting
    # Know that rs_pyfunc is correct (=1) when C=1, check that
    print rs_pyfunc(1, 0.1), rs_pyfunc(1, 0.76)

    # Alternative method for extracting terms in a series expansion:
    import itertools
    rs = [t for t in itertools.islice(r(C, p).lseries(p), 4)]
    print rs
    rs = [factor(t) for t in rs]
    print rs
    rs = sum(rs)
    print rs

    # true error
    x, t, k, w, c, dx, dt = symbols('x t k w c dx dt')
    u_n = cos(k*x - tilde_w(c, k, dx, dt)*t)
    u_e = cos(k*x - w*t)
    e = u_e-u_n
    # sympy cannot do this series expansion
    #print e.series(dx, 0, 4)

if __name__ == '__main__':
    #from scitools.std import *
    #makeplot()
    from sympy import * # erases sin and other math functions from numpy
    sympy_analysis()


import numpy as np

# change this if using K > 100
denominator_table = np.log2( np.arange( 2, 102 ))

def dcg_at_k( r, k, method = 1 ):
	"""Score is discounted cumulative gain (dcg)

	Relevance is positive real values.  Can use binary
	as the previous methods.

	Example from
	http://www.stanford.edu/class/cs276/handouts/EvaluationNew-handout-6-per.pdf
	>>> r = [3, 2, 3, 0, 0, 1, 2, 2, 3, 0]
	>>> dcg_at_k(r, 1)
	3.0
	>>> dcg_at_k(r, 1, method=1)
	3.0
	>>> dcg_at_k(r, 2)
	5.0
	>>> dcg_at_k(r, 2, method=1)
	4.2618595071429155
	>>> dcg_at_k(r, 10)
	9.6051177391888114
	>>> dcg_at_k(r, 11)
	9.6051177391888114

	Args:
		r: Relevance scores (list or numpy) in rank order
			(first element is the first item)
		k: Number of results to consider
		method: If 0 then weights are [1.0, 1.0, 0.6309, 0.5, 0.4307, ...]
				If 1 then weights are [1.0, 0.6309, 0.5, 0.4307, ...]

	Returns:
		Discounted cumulative gain
	"""
	r = np.asfarray(r)[:k]
	if r.size:
		if method == 0:
			return r[0] + np.sum(r[1:] / np.log2(np.arange(2, r.size + 1)))
		elif method == 1:
			# return np.sum(r / np.log2(np.arange(2, r.size + 2)))
			return np.sum(r / denominator_table[:r.shape[0]])
		else:
			raise ValueError('method must be 0 or 1.')
	return 0.

def get_ndcg( r, k, method = 1 ):
	"""Score is normalized discounted cumulative gain (ndcg)

	Relevance orignally was positive real values.  Can use binary
	as the previous methods.

	Example from
	http://www.stanford.edu/class/cs276/handouts/EvaluationNew-handout-6-per.pdf
	>>> r = [3, 2, 3, 0, 0, 1, 2, 2, 3, 0]
	>>> ndcg_at_k(r, 1)
	1.0
	>>> r = [2, 1, 2, 0]
	>>> ndcg_at_k(r, 4)
	0.9203032077642922
	>>> ndcg_at_k(r, 4, method=1)
	0.96519546960144276
	>>> ndcg_at_k([0], 1)
	0.0
	>>> ndcg_at_k([1], 2)
	1.0

	Args:
		r: Relevance scores (list or numpy) in rank order
			(first element is the first item)
		k: Number of results to consider
		method: If 0 then weights are [1.0, 1.0, 0.6309, 0.5, 0.4307, ...]
				If 1 then weights are [1.0, 0.6309, 0.5, 0.4307, ...]

	Returns:
		Normalized discounted cumulative gain
	"""
	dcg_max = dcg_at_k(sorted(r, reverse=True), k, method)
	dcg_min = dcg_at_k(sorted(r), k, method)
	#assert( dcg_max >= dcg_min )

	if not dcg_max:
		return 0.

	dcg = dcg_at_k(r, k, method)

	#print dcg_min, dcg, dcg_max

	return (dcg - dcg_min) / (dcg_max - dcg_min)

# ndcg with explicitly given best and worst possible relevances
# for recommendations including unrated movies
def get_ndcg_2( r, best_r, worst_r, k, method = 1 ):

	dcg_max = dcg_at_k( sorted( best_r, reverse = True ), k, method )

	if worst_r == None:
		dcg_min = 0.
	else:
		dcg_min = dcg_at_k( sorted( worst_r ), k, method )

	# assert( dcg_max >= dcg_min )

	if not dcg_max:
		return 0.

	dcg = dcg_at_k( r, k, method )

	#print dcg_min, dcg, dcg_max

	return ( dcg - dcg_min ) / ( dcg_max - dcg_min )


import numpy as np

def compute_confusion_matrix(prediction, ground_truth, num_classes):
    replace_indices = np.vstack((
        ground_truth.flatten(),
        prediction.flatten())
    ).T
    confusion_matrix, _ = np.histogramdd(
        replace_indices,
        bins=(num_classes, num_classes),
        range=[(0, num_classes), (0, num_classes)]
    )
    confusion_matrix = confusion_matrix.astype(np.uint32)
    return confusion_matrix

def iou_score(confusion_matrix, reduce_mean=True):
    ious = []
    for index in range(confusion_matrix.shape[0]):
        true_positives = confusion_matrix[index, index]
        false_positives = confusion_matrix[:, index].sum() - true_positives
        false_negatives = confusion_matrix[index, :].sum() - true_positives
        denom = true_positives + false_positives + false_negatives
        if denom == 0:
            iou = 1
        else:
            iou = float(true_positives) / denom
        ious.append(iou)
    return np.mean(ious) if reduce_mean else ious

def f1_score(confusion_matrix, reduce_mean=True):
    f1_scores = []
    for index in range(confusion_matrix.shape[0]):
        true_positives = confusion_matrix[index, index]
        false_positives = confusion_matrix[:, index].sum() - true_positives
        false_negatives = confusion_matrix[index, :].sum() - true_positives
        denom = 2 * true_positives + false_positives + false_negatives
        if denom == 0:
            f1_score = 1
        else:
            f1_score = 2 * float(true_positives) / denom
        f1_scores.append(f1_score)
    return np.mean(f1_scores) if reduce_mean else f1_scores

def precision_recall_fscore_support(confusion_matrix):
    f1_scores, precisions, recalls, supports = [], [], [], []
    for index in range(confusion_matrix.shape[0]):
        true_positives = confusion_matrix[index, index]
        false_positives = confusion_matrix[:, index].sum() - true_positives
        false_negatives = confusion_matrix[index, :].sum() - true_positives

        denom = true_positives + false_positives
        precision = 1 if denom == 0 else float(true_positives) / denom

        denom = true_positives + false_negatives
        recall = 1 if denom == 0 else float(true_positives) / denom

        denom = precision + recall
        f1_score = 1 if denom == 0 else 2 * float(precision * recall) / denom

        f1_scores.append(f1_score)
        precisions.append(precision)
        recalls.append(recall)
        supports.append(int(true_positives+false_negatives))

    return precisions, recalls, f1_scores, supports

def accuracy(confusion_matrix):
    return np.diag(confusion_matrix).sum() / confusion_matrix.sum()

class AverageMeter(object):
    """Computes and stores the average and current value"""
    def __init__(self, name, fmt=':f'):
        self.name = name
        self.fmt = fmt
        self.reset()

    def reset(self):
        self.val = 0
        self.avg = 0
        self.sum = 0
        self.count = 0

    def update(self, val, n=1):
        self.val = val
        self.sum += val * n
        self.count += n
        self.avg = self.sum / self.count

    def __str__(self):
        fmtstr = '{name} {val' + self.fmt + '} ({avg' + self.fmt + '})'
        return fmtstr.format(**self.__dict__)


import cv2,os
import numpy as np

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

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

     # FORWARD PROPAGATION (FROM X TO COST)
    z = np.dot(w.T, X)+b # tag 1
    A = sigmoid(z) # tag 2
    loss = (-np.sum(Y*np.log(A)+(1-Y)*np.log(1-A)))/m # tag 5

    # BACKWARD PROPAGATION (TO FIND GRAD)
    dw = (np.dot(X,(A-Y).T))/m # tag 6
    db = np.average(A-Y) # tag 7

    cost = np.squeeze(loss)
    grads = {"dw": dw,
             "db": db}

    return grads, loss

def optimize(w, b, X, Y, num_iterations, learning_rate, print_loss= True):
    cost = []
    for i in range(num_iterations):
        # Cost and gradient calculation
        grads, loss = propagate(w, b, X, Y)

        # Retrieve derivatives from grads
        dw = grads["dw"]
        db = grads["db"]

        # update w and b
        w = w - learning_rate*dw
        b = b - learning_rate*db

        # Record the costs
        if i % 100 == 0:
            cost.append(loss)

        # Print the cost every 100 training iterations
        if print_loss and i % 100 == 0:
            print ("Loss  %i: %f" %(i, loss))

    # update w and b to dictionary
    params = {"w": w,
              "b": b}

    # update derivatives to dictionary
    grads = {"dw": dw,
             "db": db}

    return params, grads, cost

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

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

    for i in range(A.shape[1]):
        # Convert probabilities A[0,i] to actual predictions p[0,i]
        if A[0,i] > 0.5:
            Y_prediction[[0],[i]] = 1
        else:
            Y_prediction[[0],[i]] = 0

    return Y_prediction

def initialize_with_zeros(dim):
    w = np.zeros((dim, 1))
    b = 0
    return w, b

def model(X_train, Y_train, X_test, Y_test, num_iterations = 2000, learning_rate = 0.5, print_loss = True):
    # initialize parameters with zeros
    w, b = initialize_with_zeros(X_train.shape[0])

    # Gradient descent
    parameters, grads, cost = optimize(w, b, X_train, Y_train, num_iterations, learning_rate, print_loss)

    # Retrieve parameters w and b from dictionary "parameters"
    w = parameters["w"]
    b = parameters["b"]

    # Predict test/train set examples
    Y_prediction_test = predict(w,b,X_test)
    Y_prediction_train = predict(w,b,X_train)

    # Print train/test Errors
    print("train accuracy: {} %".format(100 - np.mean(np.abs(Y_prediction_train - Y_train)) * 100))
    print("test accuracy: {} %".format(100 - np.mean(np.abs(Y_prediction_test - Y_test)) * 100))

    dict = {"loss": cost,
            "Y_prediction_test": Y_prediction_test,
            "Y_prediction_train": Y_prediction_train,
            "w": w,
            "b": b,
            "learning_rate": learning_rate,
            "num_iterations:": num_iterations}

    return dict


import numpy
from numpy import sqrt, dot, cross
from numpy.linalg import norm

thargmeter_constant = 871.02

# Find the intersection of three spheres
# P1,P2,P3 are the centers, r1,r2,r3 are the radii
# Implementaton based on Wikipedia Trilateration article.

# source:
# https://stackoverflow.com/questions/1406375/finding-intersection-points-between-3-spheres/1406415
def trilaterate(P1, P2, P3, r1, r2, r3):
    r1 *= thargmeter_constant
    r2 *= thargmeter_constant
    r3 *= thargmeter_constant
    temp1 = P2-P1
    e_x = temp1/norm(temp1)
    temp2 = P3-P1
    i = dot(e_x,temp2)
    temp3 = temp2 - i*e_x
    e_y = temp3/norm(temp3)
    e_z = cross(e_x,e_y)
    d = norm(P2-P1)
    j = dot(e_y,temp2)
    x = (r1*r1 - r2*r2 + d*d) / (2*d)
    y = (r1*r1 - r3*r3 -2*i*x + i*i + j*j) / (2*j)
    temp4 = r1*r1 - x*x - y*y
    if temp4<0:
        raise Exception("The three spheres do not intersect!");
    z = sqrt(temp4)
    p_12_a = P1 + x*e_x + y*e_y + z*e_z
    p_12_b = P1 + x*e_x + y*e_y - z*e_z
    return p_12_a, p_12_b

if __name__ == '__main__':
    center_merope = numpy.array([-78.59375, -149.625, -340.53125])
    # Col 70 Sector FY-N C21-3
    center_col70 = numpy.array([687.0625, -362.53125, -697.0625])
    # Mel 22 Sector NX-U d2-31
    center_origin = numpy.array([-64.40625, -269.5625, -373.59375])

    # distances = [0.151 * thargmeter_constant, 0.01 * thargmeter_constant, 0.937 * thargmeter_constant]
    # distances = [0.01 * thargmeter_constant, 0.151 * thargmeter_constant, 0.937 * thargmeter_constant]
    # distances = [0.01 * thargmeter_constant, 0.937 * thargmeter_constant, 0.151 * thargmeter_constant]
    distances = [0.151 * thargmeter_constant, 0.937 * thargmeter_constant, 0.01 * thargmeter_constant]
    answer = trilaterate(center_merope, center_col70, center_origin, distances[0], distances[1], distances[2])
    print(answer[1])
    # should be close to: [ -57.1, -274.7, -373.7 ]


"""
Kindly install these libraries before executing this code:
  1. numpy
"""

import math
import numpy as np
import csv

def readCSV():
    csv_file = open("data.csv", "r")
    reader = csv.reader(csv_file)

    data = []
    for row in reader:
        data.append(row[4])

    data = data[1:]
    data = [float(x) for x in data]
    return data

def compute(data):
    u = []
    for idx in range(1, len(data)):
        u.append(math.log(data[idx]/data[idx - 1]))

    E = 0
    variance = 0
    mean = 0
    n = len(u)

    for i in u:
        E += i
    E /= len(u)

    for i in u:
        variance += (i - E)*(i - E)
    variance /= (n - 1)

    mean = E + variance/2
    return mean, variance

def estimate(mean, variance, data):
    s0 = data[len(data) - 1]
    predicted = [0, 0, 0]

    for i in range(0, 1000):
        si = s0

        # Generating Normal Random Variables
        Z = np.random.normal(0, 1, 14)
        idx = 0

        # Computation for dates from 1st Oct to 7th oct
        # Holidays - 2nd Oct, 3rd Oct, 4th Oct
        for j in range(0, 4):
            s = si*math.exp((mean - 0.5*variance) + math.sqrt(variance)*Z[idx])
            si = s
            idx += 1
        predicted[0] += s

        # Computation for dates from 8th Oct to 14th Oct
        # Holidays - 10th Oct, 11th Oct
        for j in range(0, 5):
            s = si*math.exp((mean - 0.5*variance) + math.sqrt(variance)*Z[idx])
            si = s
            idx += 1
        predicted[1] += s

        # Computation for dates from 15th Oct to 21st Oct
        # Holidays - 17th Oct, 18th Oct
        for j in range(0, 5):
            s = si*math.exp((mean - 0.5*variance) + math.sqrt(variance)*Z[idx])
            si = s
            idx += 1
        predicted[2] += s

    for idx in range(0, 3):
        predicted[idx] /= 1000

    return predicted

def main():

    # Reading from csv files
    data = readCSV()

    # Computing mean and variance from historical data
    mean, variance = compute(data)

    print('Mean = {}'.format(mean))
    print('Variance = {}'.format(variance))

    # Making predictions using gBm Model
    predicted = estimate(mean, variance, data)
    actual = [190.70, 200.05, 203.75]
    percentageError = []

    # Calculating Percentage Error
    for idx in range(0, 3):
        percentageError.append(((actual[idx] - predicted[idx]) * 100)/actual[idx])

    print('\n------ Predicted closing stock prices ------')
    print('\n\n### Date = 7th Oct ###\n')
    print('Predicted price = {}'.format(predicted[0]))
    print('Actual Price = {}'.format(actual[0]))
    print('Percentage error = {} %'.format(percentageError[0]))

    print('\n\n### Date = 14th Oct ###\n')
    print('Predicted price = {}'.format(predicted[1]))
    print('Actual Price = {}'.format(actual[1]))
    print('Percentage error = {} %'.format(percentageError[1]))

    print('\n\n### Date = 21st Oct ###\n')
    print('Predicted price = {}'.format(predicted[2]))
    print('Actual Price = {}'.format(actual[2]))
    print('Percentage error = {} %'.format(percentageError[2]))

if __name__=="__main__":
    main()


import numpy as np

def p_dust(rho_0, b, rho_min, rho_vals):
    """Compute p_dust(rho | rho_0, b, rho_min)

    Parameters
    ----------
    rho_0 : `float`
        The value of rho_0
    b : `float`
        The value of b
    rho_min : `float`
        The value of rho_min
    rho_vals : `np.ndarray`
        Array of rho values to compute p_dust

    Returns
    -------
    p_dust : `np.ndarray`
        p_dust evaluated at rho_vals given rho0, b, rho_min.
    """
    lo = (rho_vals <= rho_min)
    hi = ~lo

    B = (b + 1)/(rho_0*((rho_min/rho_0)**b)*(b + 1 + (rho_min/rho_0)))

    p_dust = np.zeros(rho_vals.size)
    p_dust[lo] = B*((rho_vals[lo]/rho_0)**b)
    p_dust[hi] = B*((rho_min/rho_0)**b)*(np.exp(-(rho_vals[hi] - rho_min)/rho_0))

    return p_dust

def compute_normalized_rho_pars(u, b):
    """Compute normalized rho_0, rho_min given b, u=rho_min/rho_0

    Parameters
    ----------
    u : `float`
        u = rho_min/rho_0 parameter.
    b : `float`
        b parameter

    Returns
    -------
    rho_0, rho_min : (`float`, `float`)
    """
    rho_0 = ((b + 2)/(b + 1))*((u + b + 1)/(u**2. + (b + 2)*(u + 1)))
    rho_min = rho_0*u

    return rho_0, rho_min


import numpy as np
import math

def focal_length_from_two_points(a, b, c, d):
    """
    Brief: focal length from two points
    See appendex 6.1 Focal Length from Two Points
    :param a:
    :param b:
    :param c:
    :param d:
    :return: 0 (failed) or focal lenth in pixels
    """
    c_2 = math.pow(c, 2)  # c^2
    d_2 = math.pow(d, 2)  # d^2
    t1 = 2 * (d_2 * a * b - c_2)
    t2 = math.pow(d_2 * (a + b) - 2 * c, 2) - 4 * (d_2 * a * b - c_2) * (d_2 - 1)

    f = 0
    if t2 < 0:
        print('warning: invalid focal length')
        return f
    assert t2 >= 0
    t3 = 2 * c - d_2 * (a + b) + math.sqrt(t2)

    if t3 == 0:
        print('warning: invalid focal length')
        return f
    assert t3 != 0

    f2 = t1 / t3
    if f2 > 0:
        f = math.sqrt(f2)
    else:
        print('warning: invalid focal length')

    return f

def pan_y_tilt_x(pan, tilt):
    """
    matrix from pan Y along with axis then tilt along with X axis
    m = Q_\phi * Q_\theta in equation (1)
    :param pan:
    :param tilt:
    :return:
    """
    pan = pan * math.pi / 180.0
    tilt = tilt * math.pi / 180.0
    r_tilt = np.asarray([[1, 0, 0],
                          [0, math.cos(tilt), math.sin(tilt)],
                          [0, -math.sin(tilt), math.cos(tilt)]])
    r_pan = np.asarray([[math.cos(pan), 0, -math.sin(pan)],
                        [0, 1, 0],
                        [math.sin(pan), 0, math.cos(pan)]])
    m = r_tilt * r_pan
    return m

def ptz_from_two_point(principal_point, pan_tilt1, pan_tilt2, point1, point2):
    """
    Estimate pan, tilt and zoom from two points
    See Section 3.1 Two-point Algorithm for Data Annotation
    :param principal_point: [u, v], e.g image center, 2 x 1, numpy array
    :param pan_tilt1: pan and tile of point1, unit degress
    :param pan_tilt2: same as pan_tilt1,
    :param point1: point 1 location, unit pixel, 2 x 1, numpy array
    :param point2: same as point1
    :return: None if fail, otherwise, pan_tilt_focal_length
    """
    p1 = point1 - principal_point
    p2 = point2 - principal_point

    a = np.dot(p1, p1)
    b = np.dot(p2, p2)
    c = np.dot(p1, p2)
    z = np.asarray([0, 0, 1])

    pan1, tilt1 = pan_tilt1[0], pan_tilt1[1]
    pan2, tilt2 = pan_tilt2[0], pan_tilt2[1]

    pan_tilt_z = np.matmul(pan_y_tilt_x(pan2 - pan1, tilt2 - tilt1), z)
    d = np.dot(z, pan_tilt_z)

    f = focal_length_from_two_points(a, b, c, d) # this may fail

    if f <= 0:
        print('Warning: estimate focal length failed')
        return None
    ptz = np.zeros(3)
    pi = math.pi
    theta1 = pan1 - math.atan2(p1[0], f) * 180.0 / pi
    theta2 = pan2 - math.atan2(p2[0], f) * 180.0 / pi
    ptz[0] = (theta1 + theta2) / 2.0

    phi1 = tilt1 - math.atan2(p1[1], f) * 180.0 / pi
    phi2 = tilt2 - math.atan2(p2[1], f) * 180.0 / pi
    ptz[1] = (phi1 + phi2) / 2.0
    ptz[2] = f

    return ptz

def pan_tilt_from_principle_point(pp, pp_ptz, p):
    """
    generate pan_tilt sample points using ptz ground truth
    :param pp:  pincipal point
    :param pp_ptz: pincipal point pan, tilt zoom, eg. ptz of the image
    :param p: a point in the image, unit pixel
    :return: return value, pan and tilt
    """
    dx = p[0] - pp[0]
    dy = p[1] - pp[1]
    pan_pp = pp_ptz[0]
    tilt_pp = pp_ptz[1]
    fl = pp_ptz[2]
    delta_pan = math.atan2(dx, fl) * 180 / math.pi
    delta_tilt = math.atan2(dy, fl) * 180 / math.pi
    pt = np.zeros((2, 1))
    pt[0] = pan_pp + delta_pan
    pt[1] = tilt_pp + delta_tilt

    return pt


import numpy as np
from scipy.spatial.distance import pdist, squareform
from itertools import combinations
from scipy.spatial import Delaunay
from scipy.spatial import distance
import bats

def get_Rips_filtration(pts, rmax=np.inf):
	pts = np.array(pts)
	simplices = []
	times = []
	n = len(pts)
	dists = squareform(pdist(pts))

	for i in range(n):
		simplices.append([i])
		times.append(0)

	for i in range(n):
		for j in range(i):
			if dists[i,j] <= rmax:
				simplices.append([j,i])
				times.append(dists[i,j])

	for i in range(n):
		for j in range(i):
			for k in range(j):
				tijk = np.max([dists[i,j], dists[i,k], dists[j,k]])
				if tijk <= rmax:
					simplices.append([k, j,i])
					times.append(np.max([dists[i,j], dists[i,k], dists[j,k]]))

	return simplices, times

def unique_edges(faces):
    """
    obtain unique simplices up to dimension dim from faces
    """
    edges = []
    # loop over faces
    for face in faces:
        # loop over dimension
        for s in combinations(face, 2):
            edges.append(np.sort(list(s)))

    edges = np.unique(edges, axis=0)

    return edges

def WeakAlphaFiltration(pts, dist=distance.euclidean, maxdim=2, t0=0.):
    """
    2D weak alpha filtration
    """
    tri = Delaunay(pts)
    edges = unique_edges(tri.simplices)
    fedges = []
    for e in edges:
        fedges.append(bats.FilteredEdge(*e, dist(pts[e[0]], pts[e[1]])))

    F = bats.FlagFiltration(fedges, pts.shape[0], maxdim, t0)
    return F

def RipsFiltration(pts, maxdim=2):
	pts = np.array(pts.T, dtype=np.float, order='F')
	data = bats.DataSet(bats.DenseDoubleMatrix(pts))
	return bats.RipsFiltration(data, bats.Euclidean(), np.inf, maxdim)


import pymc as pm
import numpy as np
from scipy.optimize import nnls

def model(a_matrix, b_vector):
    x_coeffs = [pm.Uniform('x_coeffs_%i' % i, 0.0, 5.00) for i in range(len(x_true))]

    @pm.deterministic(plot=False)
    def linear_solver(x_coeffs=x_coeffs, a_matrix=a_matrix):
        solver_solution = a_matrix.dot(x_coeffs)
        return solver_solution

    @pm.stochastic(observed=True)
    def likelihood(value=b_vector, fit_results=linear_solver, sigma=err):
        chiSq = np.sum(np.square(fit_results - value) / np.square(sigma))
        return - chiSq / 2

    return locals()

x_true = np.array([3, 2, 1])
a = np.array([[10, -5, 0], [15, 20, 0],
              [12, 5, -20]])
b = a.dot(x_true)
err = 0.05

# Least squares
non_negative_least_squares = nnls(a, b)[0]

# Bayesian
MDL1 = pm.MCMC(model(a, b))
MDL1.sample(10000, 5000, 1)

# Invert matrix
x_inversion = np.dot(np.linalg.inv(np.dot(a.T, a)), np.dot(a.T, b))

# Compare
for i in range(len(b)):
    bayesian_fit_coeff = MDL1.stats()['x_coeffs_' + str(i)]['mean']
    print x_true[i], bayesian_fit_coeff, non_negative_least_squares[i], x_inversion[i]


#! /usr/bin/env python
#
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

class SphericalSampler:
    """
    Sample N points on unit sphere, to use these as camera positions in data generation process.
    """

    def __init__(self, N, sampling="LATTICE"):
        self.N = N
        if sampling == "LATTICE":
            self.points = self.sphere_fibonacci_grid_points(N)
        elif sampling == "RANDOM":
            self.points = self.sphere_sample_gaussian(N)
        elif sampling == "CURVE":
            self.points = self.sphere_sample_curve(N)
        elif sampling == "SPIRAL":
            self.points = self.sphere_sample_spiral(N)
        elif sampling == "HELIX":
            self.points = self.sphere_sample_helix(N)
        elif sampling == "ARC":
            self.points = self.sphere_sample_arc(N)

        else:
            raise NameError('Sampling of type: %s not supported. Use: LATTICE | RANDOM' % sampling)
        print("Sampled %d views from unit sphere with %s sampling." % (N, sampling))

    def sphere_fibonacci_grid_points(self, ng):
        """
         SPHERE_FIBONACCI_GRID_POINTS: Fibonacci spiral gridpoints on a sphere.

          Licensing:

            This code is distributed under the GNU LGPL license.

          Modified:

            15 May 2015
            Author:
                John Burkardt

          Reference:

            Richard Swinbank, James Purser,
            Fibonacci grids: A novel approach to global modelling,
            Quarterly Journal of the Royal Meteorological Society,
            Volume 132, Number 619, July 2006 Part B, pages 1769-1793.

          Parameters:

            Input, integer NG, the number of points.

            Output, real XG(3,N), the grid points.
        """
        phi = (1.0 + np.sqrt(5.0)) / 2.0

        theta = np.zeros(ng)
        sphi = np.zeros(ng)
        cphi = np.zeros(ng)

        for i in range(0, ng):
            i2 = 2 * i - (ng - 1)
            theta[i] = 2.0 * np.pi * float(i2) / phi
            sphi[i] = float(i2) / float(ng)
            cphi[i] = np.sqrt(float(ng + i2) * float(ng - i2)) / float(ng)

        xg = np.zeros((ng, 3))

        for i in range(0, ng):
            xg[i, 0] = cphi[i] * np.sin(theta[i])  # x
            xg[i, 1] = cphi[i] * np.cos(theta[i])  # y
            xg[i, 2] = sphi[i]  # z

        return xg

    def sphere_sample_gaussian(self, N):
        points = np.zeros((N,3))
        for i in range(N):
            x = np.random.normal()
            y = np.random.normal()
            z = np.random.normal()
            point = np.array([x,y,z])
            points[i,:] = point / np.linalg.norm(point)
            points[i,2] = abs(points[i,2])
        return points

    def sphere_sample_curve(self,N,  theta = np.pi/2):

        phi = np.linspace(0, 2*np.pi, num=N, endpoint=False, retstep=False, dtype=float, axis=0)
        points = np.zeros((N,3))

        x = np.sin(theta) * np.cos(phi)
        z = np.sin(theta) * np.sin(phi)
        y = np.cos(theta)
        points[:,0] = x
        points[:,1] = y
        points[:,2] = z

        print(points.shape)

        return points

    def sphere_sample_spiral(self,N):

        phi = np.linspace(0,1, num=N, endpoint=False, retstep=False, dtype=float, axis=0)
        points = np.zeros((N,3))

        x = phi * np.cos(16*phi)
        z = phi *  np.sin(16*phi)

        y = np.sqrt(1 - np.square(x) - np.square(z))

        #z = np.sin(theta) * np.sin(phi)
        points[:,0] = x
        points[:,1] = y
        points[:,2] = z

        for i in range(N):
            points[i,:] = points[i,:] / np.linalg.norm(points[i,:])
        print(points.shape)

        return points

    def sphere_sample_arc(self,N):

        t = np.linspace(0,1, num=N, endpoint=False, retstep=False, dtype=float, axis=0)
        points = np.zeros((N,3))
        theta_x = np.linspace(-0.5,0.5, num=N, endpoint=False, retstep=False, dtype=float, axis=0)
        theta_y = np.linspace(-0.2,0.2, num=N, endpoint=False, retstep=False, dtype=float, axis=0)
        #x = np.sin(theta_x)
        #y = np.cos(theta_y)
        #z = 0.7

        points[:,0] = theta_x
        points[:,1] = theta_y
        points[:,2] = 0.7

        # for i in range(N):
        #     points[i,:] = points[i,:] / np.linalg.norm(points[i,:])
        print(points.shape)

        return points

    def sphere_sample_helix(self,N):

        t = np.linspace(0,1, num=N, endpoint=False, retstep=False, dtype=float, axis=0)
        points = np.zeros((N,3))

        x = np.cos(3*t*np.pi)
        y = np.sin(3*t*np.pi)
        z = t

        points[:,0] = x
        points[:,1] = y
        points[:,2] = z

        # for i in range(N):
        #     points[i,:] = points[i,:] / np.linalg.norm(points[i,:])
        print(points.shape)

        return points

    def visualize(self):
        fig = plt.figure()
        ax = fig.add_subplot(111, projection='3d')
        ax.scatter(self.points[ :,0], self.points[:,1], self.points[:,2], zdir='z', s=5, c=None, depthshade=True)
        ax.set_xlabel('X Label')
        ax.set_ylabel('Y Label')
        ax.set_zlabel('Z Label')
        plt.show()

if __name__ == "__main__":
    sampler = SphericalSampler(200, 'LATTICE')
    sampler.visualize()
    print(sampler.points.shape)


'''
Functions for performing Otsu's algorithm.

3 variations:

1. the standard way Otsu's algorithm on the whole image
2. segmenting the image into blocks, and performing Otsu's algorithm on each block
3. use sliding window on the image, and use Otsu's algorithm within the window
'''
import cv2
import numpy as np

def threshold(image: np.ndarray) -> np.ndarray:
    '''
    Otsu algorithm, done the standard way. (global threshold)

    The mask that is obtained can be used to binarize the image, by doing:
    np.where(image > threshold_mask, 255, 0)

    :params:
    - image (np.ndarray): the image to find the threshold of

    :return:
    - mask of threshold (np.ndarray)
    '''
    histogram = cv2.calcHist(
        images=[image],
        channels=[0],
        mask=None,
        histSize=[256],
        ranges=[0, 256],
    )

    num_pixels = image.size

    gray_level_probabilities = [
        bin_count[0] / num_pixels
        for bin_count in histogram
    ]

    best_threshold = -1
    min_intraclass_variance = float('inf')

    for threshold in range(len(histogram)):  # 0 to 255
        lower_group_probability = sum(gray_level_probabilities[:threshold+1])
        upper_group_probability = sum(gray_level_probabilities[threshold+1:])

        if lower_group_probability == 0 or upper_group_probability == 0:
            continue  # otherwise, divide by 0

        lower_group_mean = sum([
            gray_level * probability  # gray level == pixel value
            for gray_level, probability in enumerate(gray_level_probabilities[:threshold+1])
        ]) / lower_group_probability

        upper_group_mean = sum([
            gray_level * probability
            for gray_level, probability in enumerate(gray_level_probabilities[threshold+1:])
        ]) / upper_group_probability

        lower_group_variance = sum([
            ((gray_level - lower_group_mean)**2) * probability
            for gray_level, probability in enumerate(gray_level_probabilities[:threshold+1])
        ]) / lower_group_probability

        upper_group_variance = sum([
            ((gray_level - upper_group_mean)**2) * probability
            for gray_level, probability in enumerate(gray_level_probabilities[threshold+1:])
        ]) / upper_group_probability

        intraclass_variance = lower_group_probability * lower_group_variance + \
            upper_group_probability * upper_group_variance

        if intraclass_variance < min_intraclass_variance:
            min_intraclass_variance = intraclass_variance
            best_threshold = threshold

    return np.full_like(image, fill_value=best_threshold)

def segmented_threshold(
    image: np.ndarray,
    num_vertical_segments: int,
    num_horizontal_segments: int,
) -> np.ndarray:
    '''
    Otsu threshold, done on segments of the image.

    Total number of segments = num_vertical_segments * num_horizontal_segments

    The mask that is obtained can be used to binarize the image, by doing:
    np.where(image > threshold_mask, 255, 0)

    :params:
    - image (np.ndarray): the image to find the threshold of
    - num_vertical_segments (int): number of times to segment vertically; at least 1
    - num_horizontal_segments (int): number of times to segment horizontally; at least 1

    :return:
    - mask of threshold (np.ndarray)
    '''
    if num_vertical_segments < 1:
        raise ValueError('There must be at least 1 segment in vertical direction')
    if num_horizontal_segments < 1:
        raise ValueError('There must be at least 1 segment in horizontal direction')

    image_height, image_length = image.shape

    # take ceiling to ensure entire image is covered
    segment_height = image_height // num_vertical_segments + 1
    segment_length = image_length // num_horizontal_segments + 1

    threshold_mask = np.zeros_like(image)

    for vertical_offset in range(0, image_height, segment_height):
        for horizontal_offset in range(0, image_length, segment_length):
            image_segment = image[
                vertical_offset: vertical_offset + segment_height,
                horizontal_offset: horizontal_offset + segment_length
            ]

            # standard Otsu's algorithm
            segmented_threshold_mask = threshold(image_segment)

            threshold_mask[
                vertical_offset: vertical_offset + segment_height,
                horizontal_offset: horizontal_offset + segment_length
            ] = segmented_threshold_mask

    return threshold_mask

def sliding_window_threshold(
    image: np.ndarray,
    window_height: int,
    window_length: int,
    vertical_stride: int,
    horizontal_stride: int,
) -> np.ndarray:
    '''
    Otsu threshold, done by using a sliding window over the image.

    The mask that is obtained can be used to binarize the image, by doing:
    np.where(image > threshold_mask, 255, 0)

    :params:
    - image (np.ndarray): the image to find the threshold of
    - window_height (int): height of the window i.e. vertical width
    - window_length (int): length of the window i.e. horizontal width
    - vertical_stride (int): number of pixel-wise steps to take in the vertical direction
    - horizontal_stride (int): number of pixel-wise steps to take in the horizontal direction

    :return:
    - mask of threshold (np.ndarray)
    '''
    if window_height < 1:
        raise ValueError('Window must have a height of at least 1 pixel')
    if window_length < 1:
        raise ValueError('Window must have a length of at least 1 pixel')
    if vertical_stride < 1:
        raise ValueError('Stride in the vertical direction must be of at least 1 pixel')
    if horizontal_stride < 1:
        raise ValueError('Stride in the horizontal direction must be of at least 1 pixel')

    image_height, image_length = image.shape

    threshold_mask = np.zeros_like(image, dtype=np.float64)
    num_repetitions = np.zeros_like(image)

    for vertical_offset in range(0, image_height - window_height + vertical_stride, vertical_stride):
        for horizontal_offset in range(0, image_length - window_length + horizontal_stride, horizontal_stride):
            window = image[
                vertical_offset: vertical_offset + window_height,
                horizontal_offset: horizontal_offset + window_length
            ]

            # standard Otsu's algorithm
            window_threshold_mask = threshold(window)

            threshold_mask[
                vertical_offset: vertical_offset + window_height,
                horizontal_offset: horizontal_offset + window_length
            ] += np.mean(window_threshold_mask)

            num_repetitions[
                vertical_offset: vertical_offset + window_height,
                horizontal_offset: horizontal_offset + window_length
            ] += 1

    return threshold_mask / num_repetitions  # get average


import matplotlib.pyplot as plt
import numpy as np

def laplace_function(x, b):
    result = (1/(2*b)) * np.e**(-1*(np.abs(x)/b))
    return result

#在-100到100之间等间隔的取10000个数
x = np.linspace(-100,100,10000)
y1 = [laplace_function(x_,0.5) for x_ in x]
y2 = [laplace_function(x_,1) for x_ in x]
y3 = [laplace_function(x_,2) for x_ in x]

plt.plot(x,y1,color='r',label='u:0|b:0.5')
plt.plot(x,y2,color='g',label='u:0|b:1')
plt.plot(x,y3,color='b',label='u:0|b:2')
plt.title("Laplace distribution")
plt.legend()
plt.show()


import numpy as np

def RamanujamSimultaneousEquations(a=np.array([2, 3, 16, 31, 103, 235, 674, 1669, 4526, 11595])):
    """
    This code implements the beautiful algorithm given by Ramanujam to solve a specific set of
    nonlinear simultaneous polynomial (in vectors x and y) equations. Although there is a necessary
    and sufficient condition for the existence of a solution, it is not appear to be checkable in polynomial time.
    Hence the code must be used with caution. Read more at: https://pdfs.semanticscholar.org/ae75/da0be9fb455e2c55daa5fca46ae63e6a60bd.pdf

    :param a: The RHS of the equations given as a vector. Default set to the example in Ramanujam's paper.
    :return: Two vectors x, y representing the solution.
    """
    if (len(a) % 2 != 0):
        print('There must be an even number of equations!')
        return -1, -1, -1

    n = int(len(a) / 2)

    Z = np.zeros((n, n))
    for k in range(n):
        Z[k, :] = np.flip(a[k:n + k])

    z = -a[n:2 * n]

    B = np.linalg.solve(Z,z)

    Z = np.zeros((n, n))
    for k in range(n):
        Z[k, 0:k + 1] = np.flip(a[0:k + 1])

    A = Z @ np.hstack((1, B[0:n - 1]))

    roots = np.roots(np.hstack((np.flip(B), 1)))

    if (np.linalg.norm(np.imag(roots)) != 0):
        print('There does not exist a solution!')
        return -1, -1, -1

    y = 1 / roots

    x = np.zeros(n)
    for k in range(n):
        den = np.polyval(np.flip(np.polynomial.polynomial.polyfromroots(np.delete(roots, k))), roots[k])
        num = np.polyval(np.flip(A), roots[k])
        x[k] = y[k] * num / den

    Y = np.transpose(np.vander(y, increasing=True, N=len(a)))
    diff_norm = np.linalg.norm(a - Y @ x)

    return x, y, diff_norm


import math
import numpy as np
import torch

def bitdecomp(x, n_bits):
    mods = []
    for _ in range(n_bits):
        mods.append(x % 2)
        x = x / 2

    bitrep = torch.stack(mods, dim=-1).byte()

    return bitrep

def bitrecon(x, b_source, b_target):
    bitrep = x.view(b_source * len(x) // b_target, b_target)
    exp = torch.ShortTensor([2**e for e in range(b_target)]).view(1, b_target)
    recon = (bitrep.short() * exp).sum(dim=1).view(-1)

    return recon

def numpack(n, n_bits):
    flat = n.view(-1)
    if len(flat) % 8 > 0:
        flat = torch.cat((flat, flat.new_zeros(8 - len(flat) % 8)))

    bitrep = bitdecomp(flat, n_bits)
    uint8rep = bitrecon(bitrep, n_bits, 8)

    return uint8rep.byte()

def unpack(p, n_bits, size=None):
    bitrep = bitdecomp(p, 8)
    recon = bitrecon(bitrep, 8, n_bits).short()

    if size is not None:
        nelements = np.prod(size)
        recon = recon[:nelements].view(size)

    return recon

if __name__ == '__main__':
    idx_high = 128
    a = torch.randint(low=0, high=idx_high, size=(4, 3)).long()
    p = numpack(a, int(math.log2(idx_high)))
    r = unpack(p, int(math.log2(idx_high)), a.size())
    diff = (a.short() - r).float().norm()

    print('Reconstruction error: {:.2f}'.format(diff))


from __future__ import division
import numpy as np
import seaborn as sns
import matplotlib as mpl
import matplotlib.pyplot as plt
from matplotlib.colors import NoNorm
import random
from scipy.linalg import circulant
import networkx as nx

def _distance_matrix(L):
    Dmax = L//2

    D  = list(range(Dmax+1))
    D += D[-2+(L%2):0:-1]

    return circulant(D)/Dmax

def _pd(d, p0, beta):
    return beta*p0 + (d <= p0)*(1-beta)

def watts_strogatz(L, p0, beta, directed=False, rngseed=1):
    rng = np.random.RandomState(rngseed)

    d = _distance_matrix(L)
    p = _pd(d, p0, beta)

    if directed:
        A = 1*(rng.random_sample(p.shape) < p)
        np.fill_diagonal(A, 0)
    else:
        upper = np.triu_indices(L, 1)

        A          = np.zeros_like(p, dtype=int)
        A[upper]   = 1*(rng.rand(len(upper[0])) < p[upper])
        A.T[upper] = A[upper]

    return A

def preserve_self_loops(matrix0, edges):
    # matrix0 needs to have 1's in the diagonal for this to work
    matrix1=matrix0
    for i in np.arange(0,matrix0.shape[0]):
        deg_dist = np.count_nonzero(matrix0[i,:])
        if deg_dist > edges:
            for k in np.arange(0,deg_dist-edges):
                non_zeros = (np.where(matrix0[i,:]>0)[0]).tolist()
                non_zeros.remove(i)
                random_index = non_zeros[np.random.randint(0,len(non_zeros))]
                matrix1[i,random_index] = 0
    return matrix1

def D_norm(D0):
    size = D0.shape[0]
    D1  = np.zeros((size,size)) # Preallocate matrix
    for i in np.arange(size):
        D1[:,i]  = (D0[:,i] / D0[:,i].sum()).T
    return D1

def gen_rand_matrix(seed):
    # Network size = 20
    #! Note: directed = False produces a symmetric matrix
    random.seed(seed)

    L = 20
    K = 4
    p0 = K/(L-1)
    regular = watts_strogatz(beta=0,directed=False,L=L,p0=p0)

    G = nx.Graph()
    G.add_nodes_from(np.arange(0,20))

    for i in np.arange(0,20):
        for j in np.arange(0,20):
            if regular[i,j] == 1:
                G.add_edge(i,j)

    reg_G = nx.to_numpy_matrix(G)
    di = np.diag_indices(20)

    random_G = nx.double_edge_swap(nx.from_numpy_matrix(reg_G), nswap=20, max_tries=1000)
    random_G2 = nx.to_numpy_matrix(random_G)

    di = np.diag_indices(20)
    random_G2[di]=1

    random_G2_revised = np.asarray(preserve_self_loops(random_G2,edges=5))

    random_G2_norm = D_norm(random_G2_revised)

    return random_G2_norm


import math
import numpy
import scipy
import pylab
import scipy.optimize

#function definitions.
#the 0.2 is the t-Tau moved to the other side so we can solve for x when y is 0.
def f(x):

	y = x - 0.2 * numpy.sin(x) - 0.8

	return y

def f_prime(x):

	y = 1.0 - 0.2 * numpy.cos(x)

	return y

#could also use this simple newton method.
def newt(x,n):
	for i in range(n):
		if f_prime(x) == 0:
			return x
		x = x - f(x)/f_prime(x)
	return x

#using a scipy function for computing the zero.
# Where 0.2 is the intial guess.
root = scipy.optimize.newton(f, .2, fprime = f_prime)

#could uncomment line 40 to use our own newton method. (The answer is still the same.)
#0.2 is the intial guess and 10 is the number of iterations.
#root = new(0.2, 10)

print 'this is the root for the given mean anomaly (radians):', root

#converting from radians to degrees.
root_degrees = root * ( 180.0 / numpy.pi )

print 'This is the root in degrees (E):', root_degrees

#now we want r and theta.
# r = a(1 - eccentricity * cos(E) )
# tan(theta/2) = sqrt((1+eccentricity) / (1-eccentricity)) * tan(E/2)
#We want to solve for theta
#where e = 0.2 and a = 1.1 A.U. (given)

eccentricity = 0.2
semi_major_axis = 1.1

#calculating r and theta.
r = semi_major_axis * (1.0 - eccentricity * numpy.cos(root))

theta = 2.0 * numpy.arctan(numpy.sqrt((1.0 + eccentricity) / (1.0 - eccentricity))) * (root_degrees/2.0)

print 'this is r:', r

print 'this is theta:', theta


#!/usr/bin/python3
#
# 1/4-cycle sine table
#
# 07-11-2016 E. Brombaugh

import numpy as np
import math as math
import matplotlib as mpl
import matplotlib.pyplot as plt
import scipy.signal as signal
from write_meminit import write_meminit
from write_memh import write_memh

# fixed params
sine_len = 1024
sine_bits = 16
scl = 2**(sine_bits-1)-1
LUT = np.zeros(sine_len, dtype=np.int)

# stuff into array
for i in np.arange(sine_len):
    LUT[i] = np.floor(math.sin((i+0.5)*math.pi/(2*sine_len))*scl + 0.5)

# Plot combined coeffs
if 1:
    wave = np.zeros(4*sine_len, dtype=np.int)
    wave[0:sine_len] = LUT;
    wave[sine_len:sine_len*2] = LUT[np.arange(sine_len-1,-1,-1)];
    wave[sine_len*2:sine_len*3] = -LUT;
    wave[sine_len*3:sine_len*4] = -LUT[np.arange(sine_len-1,-1,-1)];
    plt.figure()
    plt.plot(wave)
    plt.grid()
    plt.xlabel("index")
    plt.ylabel("value")
    plt.title("Sine Table (close to continue)")
    plt.show()

# Dump Coefficients to FPGA mem file
if 0:
    # Xilinx mem init file
    write_meminit("sine_table_1k.v", LUT)
else:
    # Verilog readmemh file
    write_memh("sine_table_1k.memh", LUT)


"""
Bayesian updating of beliefs about the bias of a coin. The prior and posterior
distributions indicate probability masses at discrete candidate values of theta.
"""
import numpy as np
import matplotlib.pyplot as plt
plt.style.use('seaborn-darkgrid')

# theta is the vector of candidate values for the parameter theta.
# n_theta_vals is the number of candidate theta values.
# To produce the examples in the book, set n_theta_vals to either 3 or 63.
n_theta_vals = 3.
# Now make the vector of theta values:
theta = np.linspace(1/(n_theta_vals +1), n_theta_vals /(n_theta_vals +1), n_theta_vals )

# p_theta is the vector of prior probabilities on the theta values.
p_theta = np.minimum(theta, 1-theta)  # Makes a triangular belief distribution.
p_theta = p_theta / np.sum(p_theta)     # Makes sure that beliefs sum to 1.

# Specify the data. To produce the examples in the book, use either
# data = np.repeat([1,0], [3, 9]) or data = np.repeat([1,0], [1, 11])
data = np.repeat([1, 0], [3, 9])
n_heads = np.sum(data)
n_tails = len(data) - n_heads

# Compute the likelihood of the data for each value of theta:
p_data_given_theta = theta**n_heads * (1-theta)**n_tails

# Compute the posterior:
p_data = np.sum(p_data_given_theta * p_theta)
p_theta_given_data = p_data_given_theta * p_theta / p_data   # This is Bayes' rule!

# Plot the results.
plt.figure(figsize=(12, 11))
plt.subplots_adjust(hspace=0.7)

# Plot the prior:
plt.subplot(3, 1, 1)
plt.stem(theta, p_theta, markerfmt=' ')
plt.xlim(0, 1)
plt.xlabel('$\\theta$')
plt.ylabel('$P(\\theta)$')
plt.title('Prior')
# Plot the likelihood:
plt.subplot(3, 1, 2)
plt.stem(theta, p_data_given_theta, markerfmt=' ')
plt.xlim(0, 1)
plt.xlabel('$\\theta$')
plt.ylabel('$P(D|\\theta)$')
plt.title('Likelihood')
plt.text(0.6, np.max(p_data_given_theta)/2, 'D = %sH,%sT' % (n_heads, n_tails))
# Plot the posterior:
plt.subplot(3, 1, 3)
plt.stem(theta, p_theta_given_data, markerfmt=' ')
plt.xlim(0, 1)
plt.xlabel('$\\theta$')
plt.ylabel('$P(\\theta|D)$')
plt.title('Posterior')
plt.text(0.6, np.max(p_theta_given_data)/2, 'P(D) = %g' % p_data)

plt.savefig('Figure_4.1.png')


import numpy as np

__all__ = ['euclidean']

def euclidean(u, v):
    """Compute the euclidean distance between two numpy arrays.

    Args:
        u (numpy.ndarray): Input array.
        v (numpy.ndarray): Input array.

    Returns:
        float: Euclidean distance between u and v.

    """
    return np.sqrt(np.sum(np.square(u - v), axis=-1))


import math
import numpy as np
from scipy import stats

from matilda.quantitative_analysis.stochastic_modeling.base_models import ContinuousStochasticProcess

class BrownianMotionModel(ContinuousStochasticProcess):
    """
        Generate an instance of Brownian motion (i.e. the Wiener process):

            X(t) = X(0) + N(0, delta**2 * t; 0, t)

        where N(a,b; t0, t1) is a normally distributed random variable with mean a and
        variance b.  The parameters t0 and t1 make explicit the statistical
        independence of N on different time intervals; that is, if [t0, t1) and
        [t2, t3) are disjoint intervals, then N(a, b; t0, t1) and N(a, b; t2, t3)
        are independent.

        Written as an iteration scheme,

            X(t + dt) = X(t) + N(0, delta**2 * dt; t, t+dt)

        If `x0` is an array (or array-like), each value in `x0` is treated as
        an initial condition, and the value returned is a numpy array with one
        more dimension than `x0`.

        Arguments
        -----------------------------------------------------------------------------
        delta : float
            delta determines the "speed" of the Brownian motion.  The random variable
            of the position at time t, X(t), has a normal distribution whose mean is
            the position at time t=0 and whose variance is delta**2*t.

        Returns
        ------------------------------------------------------------------------------
        A numpy array of floats with shape `x0.shape + (n,)`.

        Note that the initial value `x0` is not included in the returned array.
        """
    def __init__(self, x0, delta=2):
        super().__init__(x0)
        self.delta = delta

    def sample_path_simulation(self):
        x0 = np.asarray(self.x0)
        # For each element of x0, generate a sample of n numbers from a normal distribution.
        r = stats.norm.rvs(size=x0.shape + (self.Nsim,), scale=self.delta * math.sqrt(self.dt))
        out = np.empty(r.shape)
        # This computes the Brownian motion by forming the cumulative sum of the random samples.
        np.cumsum(r, axis=-1, out=out)
        out += np.expand_dims(x0, axis=-1)  # Add the initial condition.

        return out


#!/usr/bin/env python3

'''
The Bifid Cipher uses a Polybius Square to encipher a message in a way that
makes it fairly difficult to decipher without knowing the secret.

https://www.braingle.com/brainteasers/codes/bifid.php
'''

import numpy as np

class BifidCipher():

    def __init__(self):
        SQUARE = [['a','b','c','d','e'],
                  ['f','g','h','i','k'],
                  ['l','m','n','o','p'],
                  ['q','r','s','t','u'],
                  ['v','w','x','y','z']]
        self.SQUARE = np.array(SQUARE)

    def letter_to_numbers(self, letter: str) -> np.ndarray:
        """
        Return the pair of numbers that represents the given letter in the
        polybius square
        """
        index1, index2 = np.where(self.SQUARE == letter)
        indexes = np.concatenate([index1 + 1, index2 + 1])
        return indexes

    def numbers_to_letter(self, index1: int, index2: int) -> str:
        """
        Return the letter corresponding to the position [index1, index2] in
        the polybius square
        """
        letter = self.SQUARE[index1 - 1, index2 - 1]
        return letter

    def encode(self, message: str) -> str:
        """
        Return the encoded version of message according to the polybius cipher
        """
        message = message.lower()
        message = message.replace(' ', '')
        message = message.replace("j", "i")

        first_step = np.empty((2, len(message)))
        for letter_index in range(len(message)):
            numbers = self.letter_to_numbers(message[letter_index])

            first_step[0, letter_index] = numbers[0]
            first_step[1, letter_index] = numbers[1]

        second_step = first_step.reshape(2 * len(message))
        encoded_message = ''
        for numbersIndex in range(len(message)):
            index1 = int(second_step[numbersIndex * 2])
            index2 = int(second_step[(numbersIndex * 2) + 1])
            letter = self.numbers_to_letter(index1, index2)
            encoded_message = encoded_message + letter

        return encoded_message

    def decode(self, message: str) -> str:
        """
        Return the decoded version of message according to the polybius cipher
        """
        message = message.lower()
        message.replace(' ', '')
        first_step = np.empty(2 * len(message))
        for letter_index in range(len(message)):
            numbers = self.letter_to_numbers(message[letter_index])
            first_step[letter_index * 2] = numbers[0]
            first_step[letter_index * 2 + 1] = numbers[1]

        second_step = first_step.reshape((2, len(message)))
        decoded_message = ''
        for numbersIndex in range(len(message)):
            index1 = int(second_step[0, numbersIndex])
            index2 = int(second_step[1, numbersIndex])
            letter = self.numbers_to_letter(index1, index2)
            decoded_message = decoded_message + letter

        return decoded_message


import numpy as np

def softmax_with_overflow(x):
    numerators = np.exp(x)
    denominator = np.sum(numerators)
    return numerators / denominator

def softmax(x):
    """return softmax for the NumPy array: x."""
    if x.ndim == 2: # mini-batch version
        x = x.T
        x = x - np.max(x, axis=0)
        y = np.exp(x) / np.sum(np.exp(x), axis=0)
        return y.T
    # constant for removing overflow
    c = np.max(x)
    numerators = np.exp(x - c)
    denominator = np.sum(numerators)
    return numerators / denominator

if __name__ == '__main__':
    x1 = np.array([0.1, 0.5, 1.0])
    x2 = np.array([1000, 2000, 3000]) # this input may cause a overflow

    print('[softmax_with_overflow]')
    print('{0} => {1}'.format(x1, softmax_with_overflow(x1)))
    print('{0} => {1}'.format(x2, softmax_with_overflow(x2)))

    print('\n[softmax]')
    print('{0} => {1}'.format(x1, softmax(x1)))
    print('{0} => {1}'.format(x2, softmax(x2)))