text:
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Created on Wed Sep 19 15:02:31 2018

original author: @ggleizer
modified by @asamant to include only relevant parts.

"""
import numpy as np
from numpy import random
import control as ct
import warnings
import scipy.linalg as la
from scipy import integrate
from traffic_models.etcutil import normalize_state_to_lyapunov
import logging
import cvxpy as cvx

ct.use_numpy_matrix(flag=False)

_MAX_TRIES_TIGHT_LYAPUNOV = 10
_ASSERT_SMALL_NUMBER = 1e-6
_LMIS_SMALL_IDENTITY_FACTOR = 1e-6

''' linearetc.py '''
''' Main building blocks for building your ETC implementation'''

'''
    AUXILIARY
'''

def is_positive_definite(A):
    return (la.eig(A)[0] > 0).all()

def is_positive_semidefinite(A):
    return (la.eig(A)[0] >= 0).all()

'''
    SYSTEM DEFINITION
'''

class Plant:
    nx = 0
    ny = 0
    nu = 0
    nw = 0

class LinearPlant(Plant):
    A = np.array([])
    B = np.array([])
    C = np.array([])
    D = np.array([])
    E = np.array([])

    def __init__(self, A, B, C, D=None, E=None):
        # Retrieve A
        A = np.array(A)
        if len(A.shape) == 0:
            A = np.array([[A]])
        nx1, nx2 = A.shape
        assert nx1 == nx2, 'Matrix A needs to be square'
        self.nx = nx1
        self.A = A

        # Retrieve B
        try:
            nx, nu = B.shape
            self.B = B
        except ValueError:  # B is a vector
            nx = B.shape[0]
            nu = 1
            self.B = B.reshape(nx, nu)
        assert self.nx == nx, 'B should have the same number of rows as A'
        self.nu = nu

        # Retrieve C
        try:
            ny, nx = C.shape
            self.C = C
        except ValueError:
            nx = C.shape[0]
            ny = 1
            self.C = C.reshape(ny, nx)
        assert self.nx == nx, 'C should have the same number of columns as A'
        self.ny = ny

        # Retreive D
        if D is None:
            self.D = np.zeros((ny, nu))
        else:
            D = np.array(D)
            assert len(D.shape) in [0, 2], \
                'D cannot be a one-dimensional array'
            try:
                ny, nu = D.shape
                self.D = D
            except ValueError:
                ny = 1
                nu = 1
                self.D = np.array([[D]])
            assert self.ny == ny, 'D must have the same number of rows as C'
            assert self.nu == nu, 'D must have the same number of columns as B'

        # Retreive E
        if E is None:
            self.E = np.zeros((nx, 0))
            self.nw = 0
        else:
            try:
                nx, nw = E.shape
                self.E = E
            except ValueError:  # B is a vector
                nx = E.shape[0]
                nw = 1
                self.E = E.reshape(nx, nw)
            assert self.nx == nx, 'E should have the same number of rows as A'
            self.nw = nw

    @property
    def states_as_output(self):
        return (self.ny == self.nx) and (self.C == np.eye(self.nx)).all()

    def measurement(self, x, noise=0):
        return self.C @ x + noise

class Controller:
    nx = 0
    ny = 0
    nu = 0
    is_dynamic = False
    h = None

    @property
    def is_discrete_time(self):
        return not(self.h is None)

    def output(self, y, xc=None):
        return np.zeros(self.nu)

class LinearController(Controller):
    A = np.array([])
    B = np.array([])
    C = np.array([])
    D = np.array([])

    def __init__(self, *args):
        if len(args) <= 2:  # Static controller
            self.is_dynamic = False

            K = args[0]
            try:
                h = args[1]
            except IndexError:
                h = None

            K = np.array(K)
            if len(K.shape) == 0:
                K = np.array([[K]])
            assert len(K.shape) == 2, \
                'K needs to be a scalar or a 2-dimensional array'
            nu, ny = K.shape

            self.nx = 0
            self.nu = nu
            self.ny = ny
            self.A = np.zeros((0, 0))
            self.B = np.zeros((0, ny))
            self.C = np.zeros((nu, 0))
            self.D = K
            self.h = h

        elif len(args) in (4, 5):  # Dynamic controller
            try:
                A, B, C, D, h = args
            except ValueError:
                A, B, C, D = args
                h = None

            dummy_plant = LinearPlant(A, B, C, D)
            self.A = dummy_plant.A
            self.B = dummy_plant.B
            self.C = dummy_plant.C
            self.D = dummy_plant.D
            self.nx = dummy_plant.nx
            self.nu = dummy_plant.nu
            self.ny = dummy_plant.ny
            self.h = h
            self.is_dynamic = True

    @property
    def K(self):
        if not self.is_dynamic:
            return self.D
        else:
            raise AttributeError('Controller is dynamic, no gain available')

    def output(self, y, xc=None):
        if self.is_dynamic:
            return self.C @ xc + self.D @ y
        else:
            return self.D @ y

'''
    ETC ERRORS
'''

class ETCWarning(Warning):
    pass

class ETCError(Exception):
    pass

'''
    BASE CLASS (SAMPLING STRATEGIES)
'''

class SampleAndHoldController:
    plant: Plant
    controller: Controller
    is_discrete_time = False

    def __init__(self, plant: Plant, controller: Controller):
        assert plant.ny == controller.ny and plant.nu == controller.nu, \
            'Controller and plant have inconsistent dimensions'
        self.plant = plant
        self.controller = controller
        self.is_state_feedback = plant.states_as_output and \
            not controller.is_dynamic

class LinearSampleAndHoldController(SampleAndHoldController):
    P: np.array  # Lyapunov matrix
    Qlyap:  np.array  # Continuous-time Lyapunov decay matrix

    def __init__(self, plant, controller, P=None, Q=None):
        super().__init__(plant, controller)

        nx = plant.nx
        Acl = plant.A + plant.B @ controller.K  # Closed loop matrix

        # Get/make Lyapunov info
        if P is None:
            if Q is None:
                Q = np.eye(nx)
            else:
                assert (Q == Q.T).all() and is_positive_definite(Q), \
                    'Q must be symmetric positive definite'
            P = ct.lyap(Acl.T, Q)
            if not is_positive_definite(P):
                msg = 'Closed loop system is not stable in continuous-time'
                warnings.warn(ETCWarning(msg))
        else:  # P is given
            assert (P == P.T).all() and is_positive_definite(P), \
                'Provided P matrix is not symmetric positive definite'
            if Q is None:
                Q = - Acl.T @ P - P @ Acl
                if not is_positive_definite(Q):
                    msg = 'Provided P matrix is not a continuous-time' \
                        ' Lyapunov matrix'
            else:
                assert la.norm(Acl.T@P + P@Acl + Q) <= _ASSERT_SMALL_NUMBER, \
                    'Provided P and Q do not satisfy the continuous-time' \
                    ' Lyapunov equation'

        self.P = P
        self.Qlyap = Q

    def fp(self, t, x, uhat, disturbance=None):
        if disturbance is not None:
            return self.plant.A @ x + self.plant.B @ uhat + \
                self.plant.E @ disturbance(t)
        else:
            return self.plant.A @ x + self.plant.B @ uhat

    def fc(self, t, x, yhat):
        return self.controller.A @ x + self.controller.B @ yhat

    def evaluate_run(self, sim_out, target_level=0.01):
        """Generate metrics of a simulation.

        Compute metrics of a simulation with respect to the current
        controller, such as time to reach the target Lyapunov level set
        and the number of samples to do so.

        Parameters
        ----------
        sim_out : dict
            As returned by a ETC/STC simulation

        Returns
        -------
        time_to_reach : float
            Time it took to reach level zero
        sample_count : int
            Number of samples before reaching level zero
        """
        n = len(sim_out['t'])
        for i in range(n):
            x = sim_out['xphat'][:, i]
            if x @ self.P @ x <= target_level:
                break
        else:
            raise ETCError('State never reached target Lyapunov value:',
                           f' Target is {target_level}, last value was'
                           f' {x @ self.P @ x}.')

        return sim_out['t'][i], sum(sim_out['sample'][:i+1])

    def evaluate(self, N=1000, T=2, seed=None, initial_level=10,
                 target_level=0.01, **kwargs):
        """Evaluate the controller's performance.

        Compute metrics for N randomly generated initial conditions.
        The initial conditions satisfy x(0)'Px(0) = V_max.

        Parameters
        ----------
        N : int, optional
            Number of simulations. The default is 1000.
        T : float, optional
            Max time of each simulation. The default is 2.
        seed : int, optional
            Seed for random number generation. The default is None,
            thus rng is not called.
        initial_level : float, optional.
            The desired initial value of the Lyapunov function. The
            default is 10.
        target_level : float, optional.
            The target value of the Lyapunov function. The default is
            0.01
        **kwargs : optional.
            Additional parameters for the simulation

        Returns
        -------
        time_to_reach_array : np.array(float)
            Time it took to reach level zero for each simulation.
        sample_count_array : np.array(int)
            Number of samples before reaching level zero for each
            simulation.
        """
        nx = self.plant.nx
        if seed is not None:
            random.seed(seed)

        x0_array = random.random_sample((N, nx))

        # Preallocate the output variables
        time_to_reach_array = np.zeros(N)
        sample_count_array = np.zeros(N)

        # Time array
        t = np.arange(0, T, self.h)

        # Main loop
        for i in range(N):
            x0 = x0_array[i, :]
            x0 = normalize_state_to_lyapunov(x0, self.P, initial_level)
            xc0 = np.zeros((self.controller.nx,))
            # print(x0 @ self.P @ x0)
            sim_out = simulate_sample_and_hold_control(self, t, x0, xc0,
                                                       **kwargs)
            try:
                time_to_reach, sample_count = \
                    self.evaluate_run(sim_out, target_level=target_level)
            except ETCError as e:
                print(x0)
                raise e
            time_to_reach_array[i] = time_to_reach
            sample_count_array[i] = sample_count

        return time_to_reach_array, sample_count_array

'''
    PERIODIC CONTROL CLASSES
'''

class PeriodicController(SampleAndHoldController):
    """Periodic Controller."""
    is_discrete_time = True

    def __init__(self, plant: Plant, controller: Controller, h=None):
        if not controller.is_discrete_time:
            if controller.is_dynamic:
                raise ETCError('Dynamic controller needs to be discrete-time')
            if h is None:
                raise ETCError('h must be provided if controller is not'
                               ' discrete-time.')
        super().__init__(plant, controller)

        if h is None:
            self.h = controller.h
        else:
            self.h = h
            self.controller.h = h

    def trigger(*args, **kwargs):
        return True

class PeriodicLinearController(PeriodicController,
                               LinearSampleAndHoldController):
    """Periodic Linear Controller of Linear Plant."""

    def __init__(self, plant: Plant, controller: Controller, h=None,
                 P=None, Q=None):
        PeriodicController.__init__(self, plant, controller, h)
        LinearSampleAndHoldController.__init__(self, plant, controller, P, Q)

        # Get discrete-time matrices
        nx = plant.nx
        nu = plant.nu
        s = la.expm(np.block([[plant.A, plant.B],
                              [np.zeros((nu, nx + nu))]])*h)
        self.Ad = s[0:nx, 0:nx]
        self.Bd = s[0:nx, nx:nx+nu]
        self.Phi = self.Ad + self.Bd @ controller.K

'''
    UTILITY PERIODIC FUNCTIONS
'''

def most_economic_periodic(plant: LinearPlant,
                           controller: LinearController,
                           h_step: float, h_max: float):
    h_best = 0
    decay = 1
    for h in np.arange(h_step, h_max, h_step):
        p = PeriodicLinearController(plant, controller, h)
        a = np.max(np.abs(la.eig(p.Phi)[0]))
        if a < decay:
            decay = a
            h_best = h
    if decay >= 1:
        raise ETCError('No stable periodic implementation for given'
                       ' parameters. Try reducing h_step and h_max.')
    return h_best, np.log(decay)/h_best

def tight_lyapunov(pc: PeriodicLinearController):
    """Compute a tight Lyapunov matrix for a periodic controller.

    Parameters
    ----------
    pc : PeriodicLinearController
        The periodic linear controller

    Raises
    ------
    ETCError
        If it fails to find a tight one.

    Returns
    -------
    Pl : np.array
        The output Lyapunov matrix
    lbd_max : TYPE
        The maximum decay associated with it.
    works_for_ct : TYPE
        True if it this Lyapunov matrix also satisfies the Lyapunov
        equation for the continuous-time implementation.

    """
    _MULTIPLIER = 1.0001

    nx = pc.plant.nx
    factor = 1

    Adcl = pc.Phi
    Acl = pc.plant.A + pc.plant.B @ pc.controller.K
    rho_max = np.abs(np.max(la.eig(Adcl)[0]))**2
    Pvar = cvx.Variable(shape=(nx, nx), PSD=True)
    objective = 0  # cvx.norm(Pvar)
    constraints = [Pvar >> np.eye(nx),
                   Adcl.T @ Pvar @ Adcl << factor * rho_max * Pvar,
                   Acl.T @ Pvar + Pvar @ Acl << 0]  # Works in c.t.
    solving = True
    works_for_ct = True
    attempts = 0
    while solving and attempts <= _MAX_TRIES_TIGHT_LYAPUNOV:
        prob = cvx.Problem(cvx.Minimize(objective), constraints)
        prob.solve(eps=1e-6, max_iters=10000)
        if 'inaccurate' in prob.status:
            attempts += 1
        if prob.status == 'infeasible':
            if len(constraints) <= 2:
                attempts += 1
                factor *= _MULTIPLIER
                constraints[1] = Adcl.T @ Pvar @ Adcl \
                    << factor * rho_max * Pvar
            constraints = constraints[:2]
            works_for_ct = False
        else:
            solving = False
    if Pvar.value is None:
        raise ETCError('Tight Lyapunov failed,'
                       ' problem may be ill-conditioned.')

    Pl = Pvar.value
    Pl = (Pl + Pl.T)/2
    Ql = -(Acl.T @ Pl + Pl @ Acl)

    lbd_max = min(np.real(la.eig(Ql, Pl)[0]))
    lbd_max_dt = -np.log(rho_max)/pc.h
    lbd_max = max(lbd_max, lbd_max_dt)

    return Pl, lbd_max, works_for_ct

'''
    ETC CLASSES
'''

class ETC(SampleAndHoldController):
    threshold = 0
    is_dynamic = False
    must_trigger_at_first_crossing = True  # By default, most of them do

    def trigger(self, dt=None, x=None, xhat=None, y=None, yhat=None, u=None,
                uhat=None, t=None, *args):
        return False

class LinearETC(ETC, LinearSampleAndHoldController):
    pass

class LinearQuadraticETC(LinearETC):
    Qbar = np.array([])
    Qbar1 = np.array([])
    Qbar2 = np.array([])
    Qbar3 = np.array([])

class LinearPETC(LinearETC):
    kmin: int
    kmax: int
    h: float
    is_discrete_time = True
    triggering_function_uses_output = False
    triggering_is_time_varying = False

    def __init__(self, plant: LinearPlant,
                 controller: LinearController,
                 h=None, P=None, Q=None, kmin=1, kmax=50):
        super().__init__(plant, controller, P, Q)

        assert controller.is_discrete_time, \
            'Controller needs to be discrete-time'

        self.kmin = kmin
        if h is None:
            h = controller.h
        self.h = h
        if kmax is not None:
            self.kmax = kmax
        else:
            self.kmax = 50

        '''If provided P matrix is not a valid continuous-time Lyapunov
        matrix, for PETC it also suffices that it is a valid discrete-time
        Lyapunov matrix. The same goes even if the continuous-time controller
        is unstable. (TBP/TBC)
        '''
        nx = plant.nx
        p = PeriodicLinearController(plant, controller, h=h*kmin, P=P)
        controller.h = h  # Revert back because the statement above changed it
        Phi = p.Phi

        # Get/make Lyapunov info
        P = self.P
        if not is_positive_definite(P):
            if Q is None:
                Q = np.eye(nx)
            else:
                assert (Q == Q.T).all() and is_positive_definite(Q), \
                    'Q must be symmetric positive definite'
            P = ct.dlyap(Phi.T, Q)
            assert (P == P.T).all() and is_positive_definite(P), \
                'Closed-loop system is not stable'
        else:  # P is good in continuous time
            assert (P == P.T).all() and is_positive_definite(P), \
                'Provided P matrix is not symmetric positive definite'
            if Q is None:
                Q = -(Phi.T @ P @ Phi - P)
                assert is_positive_definite(Q), \
                    'Provided P matrix is not a Lyapunov matrix'
            else:
                assert (Phi.T @ P @ Phi - P == -Q).all(), \
                    'Provided P and Q do not satisfy the discrete-time' \
                    ' Lyapunov equation'
        self.P = P
        self.Qd = Q  # Qd for the discrete-time
        self.Phi = Phi
        self.Ad = p.Ad
        self.Bd = p.Bd

class LinearQuadraticPETC(LinearQuadraticETC, LinearPETC):
    def check_stability_impulsive(self, P = None, rho = None):
        """Check GES using the impulsive method of [1]

        Check global exponential stability of the closed-loop PETC
        by using Corollary III.3 in [1], which is based on an impulsive-
        systems modeling method. Using their remarks, it is the pair
        of LMIS, for some mu_i >= 0,

        exp(-2*rho*h) P + (-1)^i mu_i Q + A_i' Q A_i >= 0, i in {1,2}   (1)

        where

        A_1 = [A + BK,  0; I,  0],
        A_2 = [A,  BK; 0,  I].

        Note that, since lambda = exp(-2*rho*h) > 0, we can divide (1) by
            lambda, rename mu_i/lambda as nu_i and 1/lambda > 0 as alpha:

        P + (-1)^i nu_i Q + alpha A_i' Q A_i >= 0, i in {1,2}.   (2)

        This is linear on P, mu, and alpha.

        If P is given but rho is not, it minimizes rho to satisfy (1).
        If rho is given but P is not, it tries to find P that verifies (1).
        If both are given, it simply checks feasibility of (1), and, if it
            is not, the pair (None, None) is returned.
        If neither is given, it tries to find both rho and P simultaneously
            by solving (2).

        Parameters
        ----------
        P : np.array, optional
            The Lyapunov matrix for V(x) = x'Px. The default is None.
        rho : float, optional
            The desired GES decay rate. The default is None.

        Returns
        -------
        float
            The GES decay rate. None if none is found.
        np.array
            The Lyapunov matrix. None if none is found.

        [1] Heemels, W.P.M.H., Donkers, M.C.F., and Teel, A.R.
            (2013). Periodic event-triggered control for linear systems.
            IEEE Transactions on Automatic Control, 58(4),
            847--861.
        """

        if not self.is_state_feedback:
            raise ETCError('Output feedback not yet implemented.')

        raise ETCError('Future implementation.')

        n = self.plant.nx
        A1 = np.block([
            [self.Ad + self.Bd @ self.controller.K, self.zeros((n,n))],
            [np.eye(n),                             np.zeros((n,n))]
            ])
        A2 = np.block([
            [self.Ad,         self.Bd @ self.controller.K],
            [np.zeros((n,n)), np.eye(n)]
            ])
        Q = self.Qbar

        if P is None and rho is None:
            P = cvx.Variable((n, n), PSD=True)
            nu1 = cvx.Variable(pos=True)
            nu2 = cvx.Variable(pos=True)
            alpha = cvx.Variable(pos=True)
            constraints = [P - nu1*Q >> 0]
            prob = cvx.Problem(cvx.Minimize(0), constraints)
            prob.solve(eps=1e-6, max_iters=10000)
        elif P is None:
            return 0.1
        elif rho is None:
            return np.eye(self.nx)
        else:
            return

    def check_stability_pwa(self, eps=1e-3):
        """Check GES using the piecewise affine method of [1]

        Check global exponential stability of the closed-loop PETC
        by using Theorem III.4 in [1], which is based on a piecewise-
        affine model. It is the group of LMIs, for some P > 0,
        alpha_ij >= 0, beta_ij >= 0, kappa_i >= 0, i,j in {1,2},

        $$ e^{(-2*\rho*h)}P_i - A_i^\top P_jA_i + (-1)^i alpha_ij Q  $$
            + (-1)^j beta_ij A_i'QA_i >= 0,     i,j in {1,2}    $$     (1)
        P_i + (-1)^ikappa_iQ > 0,    i in {1,2}                      (2)

        where

        A_1 = [A + BK,  0; I,  0],
        A_2 = [A,  BK; 0,  I].

        This method performs a bisection algorithm on lambda
        := exp(-2*rho*h) to find the smallest value that satisfies
        (1) and (2).

        Parameters
        ----------
        eps : float, optional
            The precision for lambda in the bisection algorithm. The
            default is 1e-3.

        Returns
        -------
        float
            The GES decay rate, or 1 if it is not GES.
        (np.array, np.array)
            The Lyapunov matrices. None if system is not GES

        [1] Heemels, W.P.M.H., Donkers, M.C.F., and Teel, A.R.
            (2013). Periodic event-triggered control for linear systems.
            IEEE Transactions on Automatic Control, 58(4),
            847--861.
        """

        if not self.is_state_feedback:
            raise ETCError('Output feedback not yet implemented.')

        n = self.plant.nx
        A = {}
        A[1] = np.block([
            [self.Ad + self.Bd, np.zeros((n,n))],
            [np.eye(n),         np.zeros((n,n))]
            ])
        A[2] = np.block([
            [self.Ad,         self.Bd],
            [np.zeros((n,n)), np.eye(n)]
            ])
        Q = self.Qbar

        # CVX variables
        alpha = {(i,j): cvx.Variable(pos=True) for i in range(1,3)
                 for j in range(1,3)}
        beta = {(i,j): cvx.Variable(pos=True) for i in range(1,3)
                for j in range(1,3)}
        kappa = {i: cvx.Variable(pos=True) for i in range(1,3)}
        P = {i: cvx.Variable((2*n, 2*n), PSD=True) for i in range(1,3)}

        # CVX constraints : make a function of the externally defined lbd
        def make_constraints(lbd):
            con = []
            for i in range(1,3):
                for j in range(1,3):
                    con.append(lbd*P[i] - A[i].T @ P[j] @ A[i]
                               + ((-1)**i)*alpha[(i,j)]*Q
                               + ((-1)**j)*beta[(i,j)]*(A[i].T @ Q @ A[i])
                               >> 0)  # Eq. (1))
                con.append(P[i] + (-1)**i * kappa[i]* Q   # Eq. (2)
                           >> _LMIS_SMALL_IDENTITY_FACTOR*np.eye(2*n))
            return con

        # Start bisection algorithm: get extreme points
        a = 0
        b = 1

        # For b = 1, if GES then it must be feasible
        con = make_constraints(b)
        prob = cvx.Problem(cvx.Minimize(0), con)
        prob.solve()
        if 'infeasible' in prob.status:
            return 1, None
        Pout = (p.value for p in P)

        # For a = 0, if it is feasible then this is a deadbeat controller.
        # Can't be better then this
        con = make_constraints(a)
        prob = cvx.Problem(cvx.Minimize(0), con)
        prob.solve()
        if 'optimal' in prob.status:
            return 0, (p.value for p in P)

        # Now we should have b = 1 feasible and a = 0 infeasible. Start
        # bisection algorithm
        while b-a > eps:
            c = (a+b)/2
            con = make_constraints(c)
            prob = cvx.Problem(cvx.Minimize(0), con)
            prob.solve()
            if 'optimal' in prob.status:
                b = c
                Pout = (p.value for p in P)  # Store output P matrices
            elif 'infeasible' in prob.status:
                a = c
            else:
                warnings.warn(f'{prob.status}: TOL is {b-a}')
                break

        return -np.log(b)/2/self.h, Pout

    def check_stability_kmax(self, P):
        """Check stability based on simple S-procedure rule"""

        if not self.is_state_feedback:
            raise ETCError('Output feedback not yet implemented.')

        if self.kmax is None:
            raise ETCError('This method needs kmax')

        kmax = self.kmax
        Ad = self.Ad
        Bd = self.Bd
        K = self.controller.K
        n = self.plant.nx
        Q = self.Qbar

        mu = cvx.Variable((kmax,kmax),pos=True)  # S-procedure variable
        a = cvx.Variable(pos=True)  # Sample-wise contraction rate of V

        # Transition matrix at k=1
        M = Ad + Bd

        Nlist = []  # Store N matrices from 1 to k
        con = []  # constraint list
        # with the case oh k=1

        for k in range(kmax):
            MI = np.block([[M],[np.eye(n)]])
            N = MI.T @ Q @ MI
            con.append(-mu[k,k] * N.copy()
                       + sum(mu[i,k] * n.copy() for i,n in enumerate(Nlist))
                       - M.T.copy() @ P @ M.copy()
                       + a * P >> 0)
            # Iterate for next M
            Nlist.append(N.copy())
            M = Ad @ M + Bd

        prob = cvx.Problem(cvx.Minimize(a), con)
        prob.solve()
        return a.value

class DirectLyapunovPETC(LinearQuadraticPETC):
    """ Check Lyapunov direcly.

    The triggering condition is based on the continuous-time Lyapunov
    condition
             d(x'Px)/dt <= -rho*(x'Qx),     (1)
    where P and Q are the Lyapunov
    matrices of the continuous-time closed-loop system and 0 <= rho < 1
    is the margin parameter. The triggering can be either predictive or
    not. When not, it simply implements the condition (1). When
    predictive, it computes x(k+1) = Ad @ x(k) + Bd @ K @ xhat(k) as the
    estimate of the next checking time state value. Them it uses x(k+1)
    in (1) to determine the triggering condition."""

    def __init__(self, plant, controller, P=None, Q=None, rho=0.5,
                 threshold=0, h=None, kmin=1, kmax=None,
                 predictive=False):
        super().__init__(plant, controller, P=P, kmin=kmin, kmax=kmax)
        self.A = plant.A
        self.B = plant.B
        self.K = controller.K
        # self.P = P
        self.Q = Q
        if Q is None:
            self.Q = self.Qlyap
        self.rho = rho
        self.predictive = predictive
        if predictive:
            nx = plant.nx
            Abar = la.expm(np.block([[self.A, self.B @ self.K],
                              [np.zeros((nx, 2*nx))]]) * self.h)
            self.Abar = Abar
            self.Ad = Abar[:nx, :nx]
            self.Bd = Abar[:nx, nx:]
        self._generate_Qbar()

    def _generate_Qbar(self):
        nx = self.plant.nx
        Qbar1 = self.A.T @ self.P + self.P @ self.A + self.rho * self.Q
        Qbar2 = self.P @ self.B @ self.K
        Qbar3 = np.zeros((nx, nx))
        self.Qbar1 = Qbar1
        self.Qbar2 = Qbar2
        self.Qbar3 = Qbar3
        self.Qbar = np.block([[Qbar1, Qbar2], [Qbar2.T, Qbar3]])
        if self.predictive:
            self.Qbar = self.Abar.T @ self.Qbar @ self.Abar
            self.Qbar1 = self.Qbar[:nx,:nx]
            self.Qbar2 = self.Qbar[:nx,nx:]
            self.Qbar3 = self.Qbar[nx:,nx:]

    def trigger(self, dt, x, xhat, y=None, yhat=None,
                u=None, uhat=None, t=None, *args):
        if self.predictive:
            x = self.Ad @ x + self.Bd @ xhat
        z = self.A @ x + self.B @ self.K @ xhat
        return 2 * (z @ self.P @ x) + self.rho * (x @ self.Q @ x) > 0 \
            or dt >= (self.kmax - 1/2) * self.h

max_stars_repo_name: asamant/masters-thesis-sched-strat

max_issues_repo_name: asamant/masters-thesis-sched-strat

max_forks_repo_name: asamant/masters-thesis-sched-strat

reason: import numpy,from numpy,import scipy,from scipy,import cvxpy


text:
# Copyright (c) 2020 PaddlePaddle Authors. All Rights Reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
import numpy as np
import paddle

def shuffle_dim(x, axis, perm=None):
    """Permute input tensor along aixs given the permutation or randomly.

    Parameters
    ----------
    x : Tensor
        The input tensor.
    axis : int
        The axis to shuffle.
    perm : List[int], ndarray, optional
        The order to reorder the tensor along the ``axis``-th dimension.

        It is a permutation of ``[0, d)``, where d is the size of the
        ``axis``-th dimension of the input tensor. If not provided,
        a random permutation is used. Defaults to None.

    Returns
    ---------
    Tensor
        The shuffled tensor, which has the same shape as x does.
    """
    size = x.shape[axis]
    if perm is not None and len(perm) != size:
        raise ValueError("length of permutation should equals the input "
                         "tensor's axis-th dimension's size")
    if perm is not None:
        perm = np.array(perm)
    else:
        perm = np.random.permutation(size)

    perm = paddle.to_tensor(perm)
    out = paddle.gather(x, perm, axis)
    return out

ext: py

max_stars_repo_name: JiehangXie/PaddleSpeech

max_issues_repo_name: JiehangXie/PaddleSpeech

max_forks_repo_name: JiehangXie/PaddleSpeech

reason: import numpy


text:
r"""
Definition
----------
This model calculates the structure factor of a polyelectrolyte solution with
the RPA expression derived by Borue and Erukhimovich\ [#Borue]_.  Note however
that the fitting procedure here does not follow the notation in that reference
as 's' and 't' are **not** decoupled. Instead the scattering intensity $I(q)$
is calculated as

.. math::

    I(q) = K\frac{q^2+k^2}{4\pi L_b\alpha ^2}
    \frac{1}{1+r_{0}^2(q^2+k^2)(q^2-12hC_a/b^2)} + background

    k^2 = 4\pi L_b(2C_s + \alpha C_a)

    r_{0}^2 = \frac{1}{\alpha \sqrt{C_a} \left( b/\sqrt{48\pi L_b}\right)}

where

$K$ is the contrast factor for the polymer which is defined differently than in
other models and is given in barns where $1 barn = 10^{-24} cm^2$.  $K$ is
defined as:

.. math::

    K = a^2

    a = b_p - (v_p/v_s) b_s

where $b_p$ and $b_s$ are sum of the scattering lengths of the atoms
constituting the monomer of the polymer and the sum of the scattering lengths
of the atoms constituting the solvent molecules respectively, and $v_p$ and
$v_s$ are the partial molar volume of the polymer and the solvent respectively

$L_b$ is the Bjerrum length(|Ang|) - **Note:** This parameter needs to be
kept constant for a given solvent and temperature!

$h$ is the virial parameter (|Ang^3|/mol) - **Note:** See [#Borue]_ for the
correct interpretation of this parameter.  It incorporates second and third
virial coefficients and can be Negative.

$b$ is the monomer length(|Ang|), $C_s$ is the concentration of monovalent
salt(mol/L), $\alpha$ is the ionization degree (ionization degree : ratio of
charged monomers  to total number of monomers), $C_a$ is the polymer molar
concentration(mol/L), and $background$ is the incoherent background.

For 2D data the scattering intensity is calculated in the same way as 1D,
where the $\vec q$ vector is defined as

.. math::

    q = \sqrt{q_x^2 + q_y^2}

References
----------

.. [#Borue] V Y Borue, I Y Erukhimovich, *Macromolecules*, 21 (1988) 3240
.. [#] J F Joanny, L Leibler, *Journal de Physique*, 51 (1990) 545
.. [#] A Moussaid, F Schosseler, J P Munch, S Candau, *J. Journal de Physique
   II France*, 3 (1993) 573
.. [#] E Raphael, J F Joanny, *Europhysics Letters*, 11 (1990) 179

Authorship and Verification
----------------------------

* **Author:** NIST IGOR/DANSE **Date:** pre 2010
* **Last Modified by:** Paul Kienzle **Date:** July 24, 2016
* **Last Reviewed by:** Paul Butler and Richard Heenan **Date:**
  October 07, 2016
"""

from numpy import inf, pi, sqrt

name = "be_polyelectrolyte"
title = "Polyelectrolyte with the RPA expression derived by Borue and Erukhimovich"
description = """
            Evaluate
            F(x) = K 1/(4 pi Lb (alpha)^(2)) (q^(2)+k2)/(1+(r02)^(2))
                 (q^(2)+k2) (q^(2)-(12 h C/b^(2)))

            has 3 internal parameters :
                   The inverse Debye Length: K2 = 4 pi Lb (2 Cs+alpha C)
                   r02 =1/alpha/Ca^(0.5) (B/(48 pi Lb)^(0.5))
                   Ca = 6.022136e-4 C
            """
category = "shape-independent"

# pylint: disable=bad-whitespace, line-too-long
#   ["name", "units", default, [lower, upper], "type", "description"],
parameters = [
    ["contrast_factor",       "barns",   10.0,  [-inf, inf], "", "Contrast factor of the polymer"],
    ["bjerrum_length",        "Ang",      7.1,  [0, inf],    "", "Bjerrum length"],
    ["virial_param",          "Ang^3/mol", 12.0,  [-inf, inf], "", "Virial parameter"],
    ["monomer_length",        "Ang",     10.0,  [0, inf],    "", "Monomer length"],
    ["salt_concentration",    "mol/L",    0.0,  [-inf, inf], "", "Concentration of monovalent salt"],
    ["ionization_degree",     "",         0.05, [0, inf],    "", "Degree of ionization"],
    ["polymer_concentration", "mol/L",    0.7,  [0, inf],    "", "Polymer molar concentration"],
    ]
# pylint: enable=bad-whitespace, line-too-long

def Iq(q,
       contrast_factor=10.0,
       bjerrum_length=7.1,
       virial_param=12.0,
       monomer_length=10.0,
       salt_concentration=0.0,
       ionization_degree=0.05,
       polymer_concentration=0.7):
    """
    :param q:                     Input q-value
    :param contrast_factor:       Contrast factor of the polymer
    :param bjerrum_length:        Bjerrum length
    :param virial_param:          Virial parameter
    :param monomer_length:        Monomer length
    :param salt_concentration:    Concentration of monovalent salt
    :param ionization_degree:     Degree of ionization
    :param polymer_concentration: Polymer molar concentration
    :return:                      1-D intensity
    """

    concentration = polymer_concentration * 6.022136e-4

    k_square = 4.0 * pi * bjerrum_length * (2*salt_concentration +
                                            ionization_degree * concentration)

    r0_square = 1.0/ionization_degree/sqrt(concentration) * \
                (monomer_length/sqrt((48.0*pi*bjerrum_length)))

    term1 = contrast_factor/(4.0 * pi * bjerrum_length *
                             ionization_degree**2) * (q**2 + k_square)

    term2 = 1.0 + r0_square**2 * (q**2 + k_square) * \
        (q**2 - (12.0 * virial_param * concentration/(monomer_length**2)))

    return term1/term2

Iq.vectorized = True  # Iq accepts an array of q values

demo = dict(scale=1, background=0.1,
            contrast_factor=10.0,
            bjerrum_length=7.1,
            virial_param=12.0,
            monomer_length=10.0,
            salt_concentration=0.0,
            ionization_degree=0.05,
            polymer_concentration=0.7)

tests = [

    # Accuracy tests based on content in test/utest_other_models.py
    [{'contrast_factor':       10.0,
      'bjerrum_length':         7.1,
      'virial_param':          12.0,
      'monomer_length':        10.0,
      'salt_concentration':     0.0,
      'ionization_degree':      0.05,
      'polymer_concentration':  0.7,
      'background':             0.001,
     }, 0.001, 0.0948379],

    # Additional tests with larger range of parameters
    [{'contrast_factor':       10.0,
      'bjerrum_length':       100.0,
      'virial_param':           3.0,
      'monomer_length':         1.0,
      'salt_concentration':    10.0,
      'ionization_degree':      2.0,
      'polymer_concentration': 10.0,
      'background':             0.0,
     }, 0.1, -3.75693800588],

    [{'contrast_factor':       10.0,
      'bjerrum_length':       100.0,
      'virial_param':           3.0,
      'monomer_length':         1.0,
      'salt_concentration':    10.0,
      'ionization_degree':      2.0,
      'polymer_concentration': 10.0,
      'background':           100.0
     }, 5.0, 100.029142149],

    [{'contrast_factor':     100.0,
      'bjerrum_length':       10.0,
      'virial_param':        180.0,
      'monomer_length':        1.0,
      'salt_concentration':    0.1,
      'ionization_degree':     0.5,
      'polymer_concentration': 0.1,
      'background':             0.0,
     }, 200., 1.80664667511e-06],
    ]

max_stars_repo_name: jmborr/sasmodels

max_issues_repo_name: jmborr/sasmodels

max_forks_repo_name: jmborr/sasmodels

reason: from numpy


text:
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Plot ACFs of all variables
Created on Fri Jun 25 14:51:18 2021

@author: lizz
"""

import statsmodels.api as sm
from statsmodels.graphics.tsaplots import plot_acf

a_vel_raw = sm.tsa.stattools.acf(preds[0]['full'])[1]
a_vel = sm.tsa.stattools.acf(np.diff(preds[0]['full']))[1]
a_vel_lt = sm.tsa.stattools.acf(preds[0]['secular']+preds[0]['transient'])[1]
b_runoff_raw = sm.tsa.stattools.acf(runoff)[1]
b_runoff = sm.tsa.stattools.acf(np.diff(runoff))[1]
b_runoff_lt = sm.tsa.stattools.acf(rf_lowfreq(t_grid))[1]
F_runoff_raw = np.sqrt((1+(a_vel_raw*b_runoff_raw))/(1-(a_vel_raw*b_runoff_raw)))
F_runoff = np.sqrt((1+(a_vel*b_runoff))/(1-(a_vel*b_runoff)))
F_runoff_lt = np.sqrt((1+(a_vel_lt*b_runoff_lt))/(1-(a_vel_lt*b_runoff_lt)))

## modify CCSL for terminus
b_terminus_raw = sm.tsa.stattools.acf(td)[1]
b_terminus = sm.tsa.stattools.acf(np.diff(td))[1]
b_terminus_lt = sm.tsa.stattools.acf(tf_lowfreq(t_grid))[1]
F_terminus_raw = np.sqrt((1+(a_vel_raw*b_terminus_raw))/(1-(a_vel_raw*b_terminus_raw)))
F_terminus = np.sqrt((1+(a_vel*b_terminus))/(1-(a_vel*b_terminus)))
F_terminus_lt = np.sqrt((1+(a_vel_lt*b_terminus_lt))/(1-(a_vel_lt*b_terminus_lt)))

## modify CCSL for smb
b_smb_raw = sm.tsa.stattools.acf(smb)[1]
b_smb = sm.tsa.stattools.acf(np.diff(smb))[1]
b_smb_lt = sm.tsa.stattools.acf(smb_lowfreq(t_grid))[1]
F_smb_raw = np.sqrt((1+(a_vel_raw*b_smb_raw))/(1-(a_vel_raw*b_smb_raw)))
F_smb = np.sqrt((1+(a_vel*b_smb))/(1-(a_vel*b_smb)))
F_smb_lt = np.sqrt((1+(a_vel_lt*b_smb_lt))/(1-(a_vel_lt*b_smb_lt)))

## Plot ACFs
fig, axs = plt.subplots(4,3, sharey=True, sharex=True)
vars_toplot = (preds[0]['full'], np.diff(preds[0]['full']), preds[0]['full']-preds[0]['seasonal'],
               smb, np.diff(smb), smb_lowfreq(t_grid),
               runoff, np.diff(runoff), rf_lowfreq(t_grid),
               td, np.diff(td), tf_lowfreq(t_grid))
names = ('', '', 'Velocity',
         '', '', 'SMB',
         '', '', 'Runoff',
         '', '', 'Terminus')
factors = (F_smb_raw, F_smb, F_smb_lt,
           F_runoff_raw, F_runoff, F_runoff_lt,
           F_terminus_raw, F_terminus, F_terminus_lt)
for var,ax, n in zip(vars_toplot, axs.ravel(), names):
    plot_acf(var, ax, lags=30)
    ax.text(1.1, 0.5, str(n), transform=ax.transAxes)
    ax.set(title='')
for ax in axs[:,0]:
    ax.set(ylabel='Acorr')
for i, ax in enumerate(axs[1::, :].ravel()):
    ax.text(0.73, 0.85, 'F={:5.2f}'.format(factors[i]), transform=ax.transAxes)
for ax in axs[3,:]:
    ax.set(xlabel='Lags')
axs[0,0].set(title='Full')
axs[0,1].set(title='Single-diff')
axs[0,2].set(title='Long-term')
plt.tight_layout()
plt.show()

max_stars_repo_name: ehultee/helheim-fiesta

max_issues_repo_name: ehultee/helheim-fiesta

max_forks_repo_name: ehultee/helheim-fiesta

reason: import statsmodels,from statsmodels


text:
# Import Modules
from keras.models import Sequential
from keras.layers import MaxPooling2D
from keras.layers import Conv2D
from keras.layers import Activation, Dropout, Flatten, Dense
import tensorflow as tf
import numpy as np
import warnings
import cv2
import os

# Ignore warnings
old_v = tf.compat.v1.logging.get_verbosity()
tf.compat.v1.logging.set_verbosity(tf.compat.v1.logging.ERROR)
os.environ['TF_CPP_MIN_LOG_LEVEL'] = '2'
warnings.filterwarnings(action='ignore')

# Set Image Directory
IMG_DIR = "testImage/test2.jpg"

# Define Image Size
IMAGE_W = 128
IMAGE_H = 64

# Read and Resize Image
def resize_image(image_path):
    temp = cv2.imread(image_path)
    temp = cv2.cvtColor(temp, cv2.COLOR_BGR2RGB)
    temp = cv2.resize(temp, (IMAGE_W, IMAGE_H), interpolation=cv2.INTER_AREA)
    return temp

# Set Categories
categories = ["원피스", "블라우스", "코트", "롱자켓", "패딩", "티셔츠", "맨투맨", "니트", "자켓", "가디건",
              "점퍼", "뷔스티", "스웨터", "남방", "스커트", "슬랙스", "린넨팬츠", "데님팬츠"]
num_cat = len(categories)

# Set Image Size
image_w = 64
image_h = 128

# Set train, test dataSet
train_X, test_X, train_Y, test_Y = np.load("./detailer_data.npy", allow_pickle=True)
train_X = train_X.astype("float") / 256
test_X = test_X.astype("float") / 256

# Construct CNN Model
model = Sequential()
model.add(Conv2D(32, (3, 3), input_shape=train_X.shape[1:], padding='same'))
model.add(Activation('relu'))
model.add(MaxPooling2D(pool_size=(2, 2)))
model.add(Dropout(0.25))
model.add(Conv2D(64, (3, 3), padding='same'))
model.add(Activation('relu'))
model.add(Conv2D(64, (3, 3)))
model.add(MaxPooling2D(pool_size=(2, 2)))
model.add(Dropout(0.25))
model.add(Flatten())
model.add(Dense(512))
model.add(Activation('relu'))
model.add(Dropout(0.5))
model.add(Dense(num_cat))
model.add(Activation('softmax'))
model.compile(loss='binary_crossentropy', optimizer='rmsprop', metrics=['accuracy'])

# Load Weight
hdf5_file = "./detailer_model.hdf5"
model.load_weights(hdf5_file)

# Prediction of Look
pred_category = []

# Image resize
img = resize_image(IMG_DIR)
data = np.asarray(img)
X = np.array(data)
X = X.astype("float") / 256
X = X.reshape(-1, 128, 64, 3)

# Prediction
pred = model.predict(X)
result = [np.argmax(value) for value in pred]  # highest predicted category
pred_category.append(str(categories[int(result[0])]))
print(categories[int(result[0])])

max_stars_repo_name: ieesejin/graduation-project

max_issues_repo_name: ieesejin/graduation-project

max_forks_repo_name: ieesejin/graduation-project


text:
import numpy as np
from Model import Model

class FactorizationMachine(Model):

    def __init__(self, num_features, num_factors):

        self._num_features = num_features
        self._num_factors = num_factors # a.k.a. k
        self.__b = 0    # bias
        self.__w = np.random.randn(self._num_features)    # w vector
        self.__v = np.random.randn(self._num_features, self._num_factors)    # second order factor matrix

    def getPrediction(self, x):

        v2 = np.power(self.__v, 2)
        x2 = x.power(2)
        factor_member = np.sum(np.power(x.dot(self.__v), 2) - x2.dot(v2), axis=1) / 2

        prediction = self.__b + x.dot(self.__w) + factor_member
        return prediction

    def updateParameters(self, x_batch, z_batch, batch_size, lr=0.01):  # dL / dw ?

        pred_batch = self.getPrediction(x_batch)    # aka d
        diff_part = -2 * (z_batch - pred_batch) / batch_size

        db = np.sum(diff_part)

        xt = x_batch.transpose()
        dw = xt.dot(diff_part)

        m = x_batch.dot(self.__v) # sum v_{jf} * x_j
        x2t = xt.power(2)
        diff_vx2 = np.multiply(self.__v, x2t.dot(diff_part).reshape((self._num_features, 1)))

        #x_batch_dense = np.array(x_batch.todense())
        #diff_xvx = np.einsum('b,bi,bf->if', diff_part, x_batch_dense, m)

        dm = np.multiply(diff_part.reshape(-1, 1), m)
        diff_xvx = xt.dot(dm)

        #batch_range = range(batch_size)
        #diff_xvx = np.zeros((self._num_features, self._num_factors), dtype=np.float)
        #for t in batch_range:
        #    diff_xvx += diff_part[t] * x_batch_dense[t, :].reshape((self._num_features, 1)) .dot(m[t, :].reshape((1, self._num_factors)))

        dv = diff_xvx - diff_vx2

        self.__w -= lr * dw
        self.__b -= lr * db
        self.__v -= lr * dv

    def resetWeights(self):

        self.__b = 0.0
        self.__w = np.zeros(self._num_features, dtype=np.float)
        self.__v = np.zeros((self._num_features, self._num_factors), dtype=np.float)

    def getWeights(self):

        return self.__b, self.__w, self.__v


text:
##Lire un dataset avec Numpy

import numpy as np

world_alcohol = np.genfromtxt('world_alcohol.csv', delimiter = ',')

print(type(world_alcohol))


text:
import math
import random
from random import choice
from arler.Utilities.operate import findRoot

import networkx as nx
import numpy as np

class Agenda:
    def __init__(self, priorities):
        self.__priorities__ = priorities
        self.root = findRoot(self.__priorities__)

    @property
    def size(self):
        return len(self.__priorities__.nodes())

    def info(self, task):
        return self.__priorities__.nodes()[task]

    def subtasks(self, task):
        return list(self.__priorities__.successors(task))

    def addTo(self, parent, *args):
        edges = [(parent, child) for child in args]
        self.__priorities__.add_edges_from(edges)

    def next(self, task, distribution, explorationRate):
        if random.uniform(0, 1) < explorationRate:
            return choice(self.subtasks(task))
        else:
            assert len(distribution) == len(self.subtasks(task))
            return self.subtasks(task)[np.argmax(distribution)]

max_stars_repo_name: sdll/arler

max_issues_repo_name: sdll/arler

max_forks_repo_name: sdll/arler

reason: import numpy,import networkx


text:
#!/usr/bin/env python3

# Copyright (c) Facebook, Inc. and its affiliates.
# This source code is licensed under the MIT license found in the
# LICENSE file in the root directory of this source tree.

from typing import Dict, List, Optional

import numpy as np
import math
import magnum as mn

from collections import defaultdict

from habitat_sim.nav import NavMeshSettings
from habitat_sim.utils.common import quat_from_coeffs, quat_to_magnum, quat_to_coeffs, quat_from_magnum
from habitat_sim.physics import MotionType

from habitat.core.registry import registry
from habitat.core.simulator import (
    Config,
    Observations
)
from habitat.sims.habitat_simulator.habitat_simulator import HabitatSim
from habitat.tasks.pickplace.pickplace import ObjectStateSpec

@registry.register_simulator(name="PickPlaceSim-v0")
class PickPlaceSim(HabitatSim):
    r"""Simulator wrapper over habitat-sim with
    object rearrangement functionalities.
    """

    def __init__(self, config: Config) -> None:
        self.did_reset = False
        super().__init__(config=config)
        self.nearest_object_id = -1
        self.gripped_object_id = -1
        self.gripped_object_transformation = None
        self.max_distance = 2.0
        self.agent_object_handle = "cylinderSolid_rings_1_segments_12_halfLen_1_useTexCoords_false_useTangents_false_capEnds_true"

        agent_id = self.habitat_config.DEFAULT_AGENT_ID
        agent_config = self._get_agent_config(agent_id)

        self.navmesh_settings = NavMeshSettings()
        self.navmesh_settings.set_defaults()
        self.navmesh_settings.agent_radius = agent_config.RADIUS
        self.navmesh_settings.agent_height = agent_config.HEIGHT
        self.default_agent_id = agent_id
        self.ISLAND_RADIUS_LIMIT = 2.0

    def reconfigure(self, config: Config) -> None:
        self.remove_existing_objects()
        super().reconfigure(config)
        self._initialize_objects(config)

    def reset(self):
        sim_obs = super().reset()
        if self._update_agents_state():
            sim_obs = self.get_sensor_observations(agent_ids=self.default_agent_id)

        self._prev_sim_obs = sim_obs
        self._prev_sim_obs["gripped_object_id"] = -1
        self._prev_sim_obs["object_under_cross_hair"] = -1
        self._prev_step_data = defaultdict(int)
        self.did_reset = True
        self.gripped_object_id = -1
        self.nearest_object_id = -1
        return self._sensor_suite.get_observations(sim_obs)

    def remove_existing_objects(self):
        # first remove all existing objects
        existing_object_ids = self.get_existing_object_ids()

        for obj_id in existing_object_ids:
            self.remove_object(obj_id)
            object_ = self.get_object_from_scene(obj_id)
            if object_ is not None:
                self.remove_contact_test_object(object_.object_handle)
        self.remove_contact_test_object(self.agent_object_handle)
        self.clear_recycled_object_ids()
        self.clear_scene_objects()

    def _initialize_objects(self, sim_config):
        # Add contact test object for agent
        self.add_contact_test_object(self.agent_object_handle)
        objects = sim_config.objects
        obj_attr_mgr = self.get_object_template_manager()

        self.obj_id_to_semantic_obj_id_map = defaultdict(int)

        if objects is not None:
            # Sort objects by object id
            object_map = {}
            for object_ in objects:
                object_map[object_.object_id] = object_

            for key in sorted(object_map.keys()):
                object_ = object_map[key]
                object_handle = object_.object_template.split('/')[-1]
                object_template = "data/test_assets/objects/{}".format(object_handle)
                object_pos = object_.position
                rotation = quat_from_coeffs(object_.rotation)
                object_rotation = quat_to_magnum(rotation)

                object_template_id = obj_attr_mgr.load_object_configs(
                    object_template
                )[0]
                object_attr = obj_attr_mgr.get_template_by_ID(object_template_id)
                obj_attr_mgr.register_template(object_attr)

                object_id = self.add_object_by_handle(object_attr.handle)
                self.add_contact_test_object(object_attr.handle)

                self.set_translation(object_pos, object_id)
                self.set_rotation(object_rotation, object_id)

                object_.object_handle = "data/test_assets/objects/{}".format(object_handle)
                self.add_object_in_scene(object_id, object_)

                self.set_object_motion_type(MotionType.DYNAMIC, object_id)
                self.obj_id_to_semantic_obj_id_map[object_id] = object_.semantic_object_id
                self.set_object_semantic_id(object_.semantic_object_id, object_id)

    def get_resolution(self):
        resolution = self._default_agent.agent_config.sensor_specifications[
            0
        ].resolution
        return mn.Vector2(list(map(int, resolution)))

    def clear_scene_objects(self):
        self._scene_objects = []

    def add_object_in_scene(self, objectId, data):
        data.object_id = objectId
        self._scene_objects.append(data)

    def update_object_in_scene(self, prevObjectId, newObjectId):
        for index in range(len(self._scene_objects)):
            if self._scene_objects[index].object_id == prevObjectId:
                self._scene_objects[index].object_id = newObjectId

    def get_object_from_scene(self, objectId):
        for index in range(len(self._scene_objects)):
            if self._scene_objects[index].object_id == objectId:
                return self._scene_objects[index]
        return None

    def check_object_exists_in_scene(self, object_id):
        exists = object_id in self.get_existing_object_ids()
        return exists

    def is_collision(self, handle, translation, is_navigation_test = False):
        collision_filter = 1
        collision_mask = -1
        if is_navigation_test:
            collision_mask = 1
        return self.pre_add_contact_test(
            handle, translation, is_navigation_test, collision_filter, collision_mask
        )

    def is_agent_colliding(self, action, agentTransform):
        stepSize = 0.15
        if action == "move_forward":
            position = agentTransform.backward * (-1 * stepSize)
            newPosition = agentTransform.translation + position
            filteredPoint = self.pathfinder.try_step(
                agentTransform.translation,
                newPosition
            )
            filterDiff = filteredPoint - newPosition
            # adding buffer of 0.05 y to avoid collision with navmesh
            offsets = [0, 0.05, -0.05, 0.1, -0.1, -0.2]
            is_colliding = False
            for offset in offsets:
                finalPosition = newPosition + filterDiff + mn.Vector3(0.0, offset, 0.0)
                collision = self.is_collision(self.agent_object_handle, finalPosition, True)
                if collision:
                    is_colliding = True
            return {
                "collision": is_colliding,
            }
        elif action == "move_backward":
            position = agentTransform.backward * stepSize
            newPosition = agentTransform.translation + position
            filteredPoint = self.pathfinder.try_step(
                agentTransform.translation,
                newPosition
            )
            filterDiff = filteredPoint - newPosition
            # adding buffer of 0.05 y to avoid collision with navmesh
            offsets = [0, 0.05, -0.05, 0.1, -0.1, -0.2]
            is_colliding = False
            for offset in offsets:
                finalPosition = newPosition + filterDiff + mn.Vector3(0.0, offset, 0.0)
                collision = self.is_collision(self.agent_object_handle, finalPosition, True)
                if collision:
                    is_colliding = True
            return {
                "collision": is_colliding,
            }
        return {
            "collision": False
        }

    def draw_bb_around_nearest_object(self, object_id):
        if object_id == -1:
            if self.nearest_object_id != -1 and self.gripped_object_id != self.nearest_object_id:
                self.set_object_bb_draw(False, self.nearest_object_id)
                self.nearest_object_id = object_id
        else:
            if self.nearest_object_id != -1 and self.gripped_object_id != self.nearest_object_id:
                self.set_object_bb_draw(False, self.nearest_object_id)
                self.nearest_object_id = -1
            object_ = self.get_object_from_scene(object_id)
            if object_.is_receptacle == True:
                return
            if self.nearest_object_id != object_id:
                self.nearest_object_id = object_id
                self.set_object_bb_draw(True, self.nearest_object_id, 0)

    def step(self, action: int):
        dt = 1.0 / 20.0
        self._num_total_frames += 1
        collided = False

        agent_config = self._default_agent.agent_config
        action_spec = agent_config.action_space[action]

        crosshair_pos = np.array(self.get_resolution()) // 2
        crosshair_pos = list(map(int, crosshair_pos))

        ray = self.unproject(crosshair_pos)
        cross_hair_point = ray.direction
        ref_point = self._default_agent.body.object.absolute_translation

        nearest_object_id = self.find_nearest_object_under_crosshair(
            cross_hair_point, ref_point, self.get_resolution(), self.max_distance
        )
        object_under_cross_hair = nearest_object_id
        self._prev_step_data["nearest_object_id"] = nearest_object_id
        self._prev_step_data["gripped_object_id"] = self.gripped_object_id

        if action_spec.name == "grab_or_release_object_under_crosshair":
            self._prev_step_data["action"] = action_spec.name
            self._prev_step_data["is_grab_action"] = self.gripped_object_id == -1
            self._prev_step_data["is_release_action"] = self.gripped_object_id != -1
            # already gripped an object
            if self.gripped_object_id != -1:
                ref_transform = self._default_agent.body.object.transformation
                ray_hit_info = self.find_floor_position_under_crosshair(
                    cross_hair_point, ref_transform,
                    self.get_resolution(), action_spec.actuation.amount
                )

                floor_position = ray_hit_info.point
                if floor_position is None:
                    return True

                new_object_position = mn.Vector3(
                    floor_position.x, floor_position.y, floor_position.z
                )
                scene_object = self.get_object_from_scene(self.gripped_object_id)

                # find no collision point
                count = 0
                contact = self.is_collision(scene_object.object_handle, new_object_position)
                while contact and count < 4:
                    new_object_position = mn.Vector3(
                        new_object_position.x,
                        new_object_position.y + 0.25,
                        new_object_position.z,
                    )
                    contact = self.is_collision(scene_object.object_handle, new_object_position)
                    count += 1

                snapped_point = self.pathfinder.snap_point(new_object_position)
                snapped_point = np.array(new_object_position)
                is_point_on_separate_island = self.island_radius(snapped_point) < self.ISLAND_RADIUS_LIMIT

                if not (contact or is_point_on_separate_island):
                    new_object_id = self.add_object_by_handle(
                        scene_object.object_handle
                    )
                    self.set_translation(new_object_position, new_object_id)
                    self.set_object_semantic_id(scene_object.semantic_object_id, new_object_id)

                    self.update_object_in_scene(self.gripped_object_id, new_object_id)
                    self._prev_step_data["new_object_translation"] = np.array(new_object_position).tolist()
                    self._prev_step_data["new_object_id"] = new_object_id
                    self._prev_step_data["object_handle"] = scene_object.object_handle
                    self._prev_step_data["gripped_object_id"] = self.gripped_object_id
                    self.gripped_object_id = -1
            elif nearest_object_id != -1:
                self.gripped_object_transformation = self.get_transformation(
                    nearest_object_id
                )
                object_ = self.get_object_from_scene(nearest_object_id)
                if object_.is_receptacle != True:
                    self.remove_object(nearest_object_id)
                    self.gripped_object_id = nearest_object_id
                self._prev_step_data["gripped_object_id"] = self.gripped_object_id
        elif action_spec.name == "no_op":
            pass
        else:
            agent_transform = self._default_agent.body.object.transformation
            data = self.is_agent_colliding(action_spec.name, agent_transform)
            if not data["collision"]:
                self._default_agent.act(action)
                collided = data["collision"]
                self._last_state = self._default_agent.get_state()

        # step world physics
        super().step_world(dt)

        # self.draw_bb_around_nearest_object(object_under_cross_hair)

        # obtain observations
        self._prev_sim_obs = self.get_sensor_observations(agent_ids=self.default_agent_id, draw_crosshair=False)
        self._prev_sim_obs["collided"] = collided
        self._prev_sim_obs["gripped_object_id"] = self.gripped_object_id
        self._prev_sim_obs["object_under_cross_hair"] = object_under_cross_hair
        self._prev_step_data["collided"] = collided

        observations = self._sensor_suite.get_observations(self._prev_sim_obs)
        return observations

    def is_object_out_of_bounds(self):
        object_ids = self.get_existing_object_ids()
        for object_id in object_ids:
            position = self.get_translation(object_id)
            snapped_position = self.pathfinder.snap_point(position)

            if np.isnan(snapped_position.x) or math.isnan(snapped_position.x):
                return True
        return False

    def restore_object_states(self, object_states: Dict = {}):
        object_ids = []
        for object_state in object_states:
            object_id = object_state.object_id
            translation = object_state.translation
            object_rotation = object_state.rotation

            object_translation = mn.Vector3(translation)
            if isinstance(object_rotation, list):
                object_rotation = quat_from_coeffs(object_rotation)

            object_rotation = quat_to_magnum(object_rotation)
            self.set_translation(object_translation, object_id)
            self.set_rotation(object_rotation, object_id)
            object_ids.append(object_id)
        return object_ids

    def get_current_object_states(self):
        existing_object_ids = self.get_existing_object_ids()
        object_states = []
        for object_id in existing_object_ids:
            translation = self.get_translation(object_id)
            rotation = self.get_rotation(object_id)
            rotation = quat_from_magnum(rotation)
            scene_object = self.get_object_from_scene(object_id)

            object_state = ObjectStateSpec(
                object_id=object_id,
                translation=np.array(translation).tolist(),
                rotation=quat_to_coeffs(rotation).tolist(),
                object_handle=scene_object.object_handle,
            )
            object_states.append(object_state)
        return object_states

    def get_agent_pose(self):
        agent_translation = self._default_agent.body.object.translation
        agent_rotation = self._default_agent.body.object.rotation
        sensor_data = {}
        for sensor_key, v in self._default_agent._sensors.items():
            rotation = quat_from_magnum(v.object.rotation)
            rotation = quat_to_coeffs(rotation).tolist()
            translation = v.object.translation
            sensor_data[sensor_key] = {
                "rotation": rotation,
                "translation": np.array(translation).tolist()
            }

        return {
            "position": np.array(agent_translation).tolist(),
            "rotation": quat_to_coeffs(quat_from_magnum(agent_rotation)).tolist(),
            "sensor_data": sensor_data
        }

    def restore_sensor_states(self, sensor_data: Dict = {}):
        for sensor_key, v in self._default_agent._sensors.items():
            rotation = None
            if sensor_key in sensor_data.keys():
                rotation = sensor_data[sensor_key]["rotation"]
            else:
                rotation = sensor_data["rgb"]["rotation"]
            rotation = quat_from_coeffs(rotation)
            agent_rotation = quat_to_magnum(rotation)
            v.object.rotation = agent_rotation

    def get_sensor_states(self):
        sensor_states = {}
        for sensor_key, v in self._default_agent._sensors.items():
            rotation = v.object.rotation
            rotation = quat_from_magnum(rotation)
            rotation = quat_to_coeffs(rotation)
            sensor_states[sensor_key] = {
                "rotation": rotation
            }
        return sensor_states

    def add_objects_by_handle(self, objects):
        for object_ in objects:
            object_handle = object_.object_handle
            object_id = self.add_object_by_handle(object_handle)
            self.set_object_semantic_id(object_.semantic_object_id, object_id)

    def step_from_replay(self, action: int, replay_data: Dict = {}):
        dt = 1.0 / 20.0
        self._num_total_frames += 1
        collided = False

        agent_config = self._default_agent.agent_config
        action_spec = agent_config.action_space[action]

        if action_spec.name == "grab_or_release_object_under_crosshair":
            action_data = replay_data.action_data
            if action_data is not None and action_data.released_object_id != -1:
                if replay_data.is_release_action:
                    # Fetch object handle and drop point from replay
                    new_object_position = mn.Vector3(action_data.released_object_position)
                    scene_object = self.get_object_from_scene(action_data.released_object_id)
                    new_object_id = self.add_object_by_handle(
                        scene_object.object_handle
                    )
                    self.set_translation(new_object_position, new_object_id)
                    self.set_object_semantic_id(scene_object.semantic_object_id, new_object_id)

                    self.update_object_in_scene(new_object_id, action_data.released_object_id)
                    self.gripped_object_id = replay_data.gripped_object_id
                elif replay_data.is_grab_action:
                    self.gripped_object_transformation = self.get_transformation(
                        action_data.grab_object_id
                    )
                    self.remove_object(action_data.grab_object_id)
                    self.gripped_object_id = replay_data.gripped_object_id
        elif action_spec.name == "no_op":
            self.restore_object_states(replay_data.object_states)
        else:
            if replay_data.agent_state is not None:
                if action_spec.name == "look_up" or action_spec.name == "look_down":
                    sensor_data = replay_data.agent_state.sensor_data
                    self.restore_sensor_states(sensor_data)
                else:
                    position = np.array(replay_data.agent_state.position)
                    rotation = np.array(replay_data.agent_state.rotation)
                    success = self.set_agent_state(
                        position, rotation, reset_sensors=False
                    )
                collided = replay_data.collision
                self._last_state = self._default_agent.get_state()
            else:
                collided = replay_data.collision
                if not collided:
                    self._default_agent.act(action)

        # self.draw_bb_around_nearest_object(replay_data.object_under_cross_hair)

        # obtain observations
        self._prev_sim_obs = self.get_sensor_observations(agent_ids=self.default_agent_id, draw_crosshair=False)
        self._prev_sim_obs["collided"] = collided
        self._prev_sim_obs["gripped_object_id"] = self.gripped_object_id

        observations = self._sensor_suite.get_observations(self._prev_sim_obs)
        return observations

    def get_observations_at(
        self,
        position: Optional[List[float]] = None,
        rotation: Optional[List[float]] = None,
        sensor_states: Optional[List[Dict]] = None,
        object_states: Optional[List[Dict]] = None,
        keep_agent_at_new_pose: bool = False,
    ) -> Optional[Observations]:
        current_state = self.get_agent_state()
        current_object_states = self.get_current_object_states()
        current_sensor_states = self.get_sensor_states()
        if position is None or rotation is None:
            success = True
        else:
            success = self.set_agent_state(
                np.array(position), np.array(rotation), reset_sensors=False
            )
        if sensor_states is not None:
            self.restore_sensor_states(sensor_states)

        object_to_re_add = []
        if object_states is not None:
            current_state_objects = self.restore_object_states(object_states)

            for object_id in self.get_existing_object_ids():
                if object_id not in current_state_objects:
                    self.remove_object(object_id)
                    object_to_re_add.append(self.get_object_from_scene(object_id))

        if success:
            sim_obs = self.get_sensor_observations(agent_ids=self.default_agent_id)

            self._prev_sim_obs = sim_obs

            observations = self._sensor_suite.get_observations(sim_obs)
            if not keep_agent_at_new_pose:
                self.set_agent_state(
                    current_state.position,
                    current_state.rotation,
                    reset_sensors=False,
                )
                self.add_objects_by_handle(object_to_re_add)
                self.restore_object_states(current_object_states)
                self.restore_sensor_states(current_sensor_states)

            return observations
        else:
            return None


text:
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Created on Fri Sep 27 15:56:39 2019

@author: mathewsa

This is an experimental script for testing different sampling distributions.
It permits applying a truncated Gaussian and/or arbitrary sampling choices as
defined by the user (e.g. a linear function is applied below) in addition to
standard normal distributions. This script is to be run only after first running
and saving the GP after it has been trained upon the experimental data. This
script displays the electron density and temperature (and their corresponding
prediction intervals) with single sample realizations plotted in blue.

Note: in addition to modifying sampling distributions, optimization with
constraints during training of the overall GP can be implemented by following
the steps outlined in gptools based upon the class 'Constraint' imposing a
residual that additionally penalizes the loss function if the sought arbitrary
constraint is not satisfied (e.g. mean or first derivatives must be greater than 0):
https://github.com/markchil/gptools/blob/master/gptools/gaussian_process.py
"""

import sys
sys.path.append('C:/Users/mathewsa/') #provides path to gp_extras
import pickle
import numpy as np
from matplotlib import pyplot as plt
from sklearn.gaussian_process import GaussianProcessRegressor
from sklearn.gaussian_process.kernels import RBF, Matern, ConstantKernel as C
from gp_extras.kernels import HeteroscedasticKernel, LocalLengthScalesKernel
from scipy.optimize import differential_evolution
from scipy.linalg import cholesky, cho_solve
from scipy import stats
import gp_extras
from numpy.linalg import inv
plt.rcParams["font.family"] = "Times New Roman"
plt.rcParams['font.size'] = 18

time = 1.2 #in seconds, for single plot
psi_min = 0.85 #lower limit you want for plotting x-axis
psi_max = 1.05 #upper limit you want for plotting y-axis
dpsi = 0.01 #normalized poloidal flux coordinate spacing you want
lower, upper = 0.0, 1.0 #lower and upper bounds for the truncated Gaussian
mu, sigma = 0.0, 1.0 #mean and standard deviation for the truncated Gaussian
n_sampling = 1000 #increase for cleaner statistics
file_path = '.../trainedGPs/saved_GP_1091016033/'

# --------------------------------------------------------------
#                       End of user inputs
# --------------------------------------------------------------

X_n = np.load(str(file_path)+'X_n.npy')
y_n_TS = np.load(str(file_path)+'y_n_TS.npy')
y_n_TS_err = np.load(str(file_path)+'y_n_TS_err.npy')
X_T = np.load(str(file_path)+'X_T.npy')
y_T_TS = np.load(str(file_path)+'y_T_TS.npy')
y_T_TS_err = np.load(str(file_path)+'y_T_TS_err.npy')
n_max_iter = np.load(str(file_path)+'n_max_iter.npy')

def de_optimizer(obj_func, initial_theta, bounds):
    res = differential_evolution(lambda x: obj_func(x, eval_gradient=False),
                                 bounds, maxiter=n_max_iter, disp=False, polish=True)
    return res.x, obj_func(res.x, eval_gradient=False)

gp = pickle.load(open(str(file_path)+"gp.dump","rb"))
gp_T = pickle.load(open(str(file_path)+"gp_T.dump","rb"))

inputs_x_array = np.arange(psi_min,psi_max + dpsi,dpsi)
a = np.ones((len(inputs_x_array),2))
a[:,0] = inputs_x_array
a[:,1] = a[:,1]*time
inputs_x_array = inputs_x_array_n = inputs_x_array_T = a

lls_len_scale = gp.kernel_.k1.k2.theta_gp* 10**gp.kernel_.k1.k2.gp_l.predict(inputs_x_array)
m_lls_n = stats.mode(lls_len_scale)
lls_len_scale_T = gp_T.kernel_.k1.k2.theta_gp* 10**gp_T.kernel_.k1.k2.gp_l.predict(inputs_x_array)
m_lls_T = stats.mode(lls_len_scale_T)

err_T = np.abs(lls_len_scale_T - m_lls_T[0][0]) #proxy for error
err_n = np.abs(lls_len_scale - m_lls_n[0][0]) #proxy for error

check_n = len(np.where(err_n != 0)[0])
check_T = len(np.where(err_T != 0)[0])

X_train = X_n
X_test = inputs_x_array
mean_y_arr = gp.predict(X_test, return_cov=False)
mean_y_arr = mean_y_arr[:,0]

X_train_T = X_T
X_test_T = inputs_x_array
mean_y_arr_T = gp_T.predict(X_test_T, return_cov=False)
mean_y_arr_T = mean_y_arr_T[:,0]

n_samples = gp.sample_y(inputs_x_array,n_sampling)
T_samples = gp_T.sample_y(inputs_x_array,n_sampling)

inputs_y = inputs_x_array
i_index = 0
mean_n = []
sigma_n = []
mean_T = []
sigma_T = []
while i_index < len(inputs_y):
    mean_n.append(np.mean(n_samples[i_index]))
    sigma_n.append(np.std(n_samples[i_index]))
    mean_T.append(np.mean(T_samples[i_index]))
    sigma_T.append(np.std(T_samples[i_index]))
    i_index = i_index + 1

mean_n = np.array(mean_n)
sigma_n = np.array(sigma_n)
mean_T = np.array(mean_T)
sigma_T = np.array(sigma_T)

mean_n_true = mean_n
sigma_n_true = sigma_n
mean_T_true = mean_T
sigma_T_true = sigma_T

#non-Heteroscedastic sampling
X_train = X_n
K_trans1 = gp.kernel_(X_test, X_train)
K = gp.kernel_(X_train)
#inv_K = inv(gp.kernel_(X_train,X_train) + np.eye(len(X_train))*(y_n_TS_err)**2.)

K_trans1_T = gp_T.kernel_(X_test_T, X_train_T)
K_T = gp_T.kernel_(X_train_T)
#inv_K_T = inv(gp_T.kernel_(X_train_T,X_train_T) + np.eye(len(X_train_T))*(y_T_TS_err)**2.)

K_trans1_T = gp_T.kernel_(X_test_T, X_train_T)
K_T = gp_T.kernel_(X_train_T)
#inv_K_T = inv(gp_T.kernel_(X_train_T,X_train_T) + np.eye(len(X_train_T))*(y_T_TS_err)**2.)

from numpy import linalg as la

def nearestPD(A):
    """Find the nearest positive-definite matrix to input
    A Python/Numpy port of John D'Errico's `nearestSPD` MATLAB code [1], which
    credits [2].
    [1] https://www.mathworks.com/matlabcentral/fileexchange/42885-nearestspd
    [2] N.J. Higham, "Computing a nearest symmetric positive semidefinite
    matrix" (1988): https://doi.org/10.1016/0024-3795(88)90223-6
    """

    B = (A + A.T) / 2
    _, s, V = la.svd(B)

    H = np.dot(V.T, np.dot(np.diag(s), V))

    A2 = (B + H) / 2

    A3 = (A2 + A2.T) / 2

    if isPD(A3):
        return A3

    spacing = np.spacing(la.norm(A))
    # The above is different from [1]. It appears that MATLAB's `chol` Cholesky
    # decomposition will accept matrices with exactly 0-eigenvalue, whereas
    # Numpy's will not. So where [1] uses `eps(mineig)` (where `eps` is Matlab
    # for `np.spacing`), we use the above definition. CAVEAT: our `spacing`
    # will be much larger than [1]'s `eps(mineig)`, since `mineig` is usually on
    # the order of 1e-16, and `eps(1e-16)` is on the order of 1e-34, whereas
    # `spacing` will, for Gaussian random matrixes of small dimension, be on
    # othe order of 1e-16. In practice, both ways converge, as the unit test
    # below suggests.
    I = np.eye(A.shape[0])
    k = 1
    while not isPD(A3):
        mineig = np.min(np.real(la.eigvals(A3)))
        A3 += I * (-mineig * k**2 + spacing)
        k += 1

    return A3

def isPD(B):
    """Returns true when input is positive-definite, via Cholesky"""
    try:
        A = la.cholesky(B)
        return True
    except la.LinAlgError:
        return False

K = gp.kernel_(X_train,X_train)
K = nearestPD(K)
L_ = cholesky(K, lower=True)
#L_1 = np.linalg.cholesky(K)
v1 = cho_solve((L_, True), K_trans1.T)  # Line 5
inv_K = inv(gp.kernel_(X_train,X_train) + np.eye(len(X_train))*(y_n_TS_err)**2.)
y_cov0 = gp.kernel_(X_test) - K_trans1.dot(v1) # this is code from gp_samples
y_cov1 = gp.kernel_(X_test,X_test) - K_trans1.dot(v1) # this is best code and fix to from gp_samples
y_cov2 = gp.kernel_(X_test,X_test) - np.dot(np.dot(gp.kernel_(X_test,X_train), inv_K),(gp.kernel_(X_test,X_train)).T) # this is code I created, where there seems to be small deviation from RHS terms (i,e, second terms in equation should be equivalent)

j_K = nearestPD(y_cov2)
y_cov_L = cholesky(j_K, lower=True)
f_post = mean_n_true + np.dot(y_cov_L, np.random.normal(size=(len(X_test),1)))[:,0]
plt.figure()
plt.plot(X_test[:,0],f_post)
plt.plot(np.array(inputs_y)[:,0],mean_n_true,'r-')
plt.fill(np.concatenate([np.array(inputs_y)[:,0],np.array(inputs_y)[:,0][::-1]]),
     np.concatenate([mean_n_true - 1.96*sigma_n_true,
                    (mean_n_true + 1.96*sigma_n_true)[::-1]]),
fc='r',ec='None',label='95% prediction interval',alpha=0.1)
plt.title('Normal GP sampling')
plt.xlabel(r"$\psi$")
plt.ylabel("n"+r"$_e \ (10^{20} \ $"+"m"+r"$^{-3})$",color='r')
plt.show()
f_post = mean_n_true + np.dot(y_cov_L, -np.linspace(0.,1.,len(X_test)))
plt.figure()
plt.plot(X_test[:,0],f_post)
plt.plot(np.array(inputs_y)[:,0],mean_n_true,'r-')
plt.fill(np.concatenate([np.array(inputs_y)[:,0],np.array(inputs_y)[:,0][::-1]]),
     np.concatenate([mean_n_true - 1.96*sigma_n_true,
                    (mean_n_true + 1.96*sigma_n_true)[::-1]]),
fc='r',ec='None',label='95% prediction interval',alpha=0.1)
plt.title('Arbitrary sampling using a decreasing linear function')
plt.xlabel(r"$\psi$")
plt.ylabel("n"+r"$_e \ (10^{20} \ $"+"m"+r"$^{-3})$",color='r')
plt.show()

K_T = gp_T.kernel_(X_train_T,X_train_T)
K_T = nearestPD(K_T)
L__T = cholesky(K_T, lower=True)
v1_T = cho_solve((L__T, True), K_trans1_T.T)  # Line 5
inv_K_T = inv(gp_T.kernel_(X_train_T,X_train_T) + np.eye(len(X_train_T))*(y_T_TS_err)**2.)
y_cov0_T = gp_T.kernel_(X_test) - K_trans1_T.dot(v1_T) # this is code from gp_samples
y_cov1_T = gp_T.kernel_(X_test,X_test) - K_trans1_T.dot(v1_T) # this is best code and fix to from gp_samples
y_cov2_T = gp_T.kernel_(X_test,X_test) - np.dot(np.dot(gp_T.kernel_(X_test,X_train_T), inv_K_T),(gp_T.kernel_(X_test,X_train_T)).T) # this is code I created, where there seems to be small deviation from RHS terms (i,e, second terms in equation should be equivalent)

j_K_T = nearestPD(y_cov2_T)
y_cov_L_T = cholesky(j_K_T, lower=True)
f_post_T = mean_T_true + np.dot(y_cov_L_T, np.random.normal(size=(len(X_test),1)))[:,0]
plt.figure()
plt.plot(X_test[:,0],f_post_T)
plt.plot(np.array(inputs_y)[:,0],mean_T_true,'g-')
plt.fill(np.concatenate([np.array(inputs_y)[:,0],np.array(inputs_y)[:,0][::-1]]),
     np.concatenate([mean_T_true - 1.96*sigma_T_true,
                    (mean_T_true + 1.96*sigma_T_true)[::-1]]),
fc='g',ec='None',label='95% prediction interval',alpha=0.1)
plt.xlabel(r"$\psi$")
plt.ylabel("T"+r"$_e$"+" (keV)",color='g')
plt.title('Normal GP sampling')
plt.show()

f_post_T = mean_T_true + np.dot(y_cov_L_T, -np.linspace(0.,0.5,len(X_test)))
plt.figure()
plt.plot(X_test[:,0],f_post_T)
plt.plot(np.array(inputs_y)[:,0],mean_T_true,'g-')
plt.fill(np.concatenate([np.array(inputs_y)[:,0],np.array(inputs_y)[:,0][::-1]]),
     np.concatenate([mean_T_true - 1.96*sigma_T_true,
                    (mean_T_true + 1.96*sigma_T_true)[::-1]]),
fc='g',ec='None',label='95% prediction interval',alpha=0.1)
plt.xlabel(r"$\psi$")
plt.ylabel("T"+r"$_e$"+" (keV)",color='g')
plt.title('Arbitrary sampling using a decreasing linear function')
plt.show()

X = stats.truncnorm(
    (lower - mu) / sigma, (upper - mu) / sigma, loc=mu, scale=sigma)
N = stats.norm(loc=mu, scale=sigma)

fig, ax = plt.subplots(2, sharex=True)
ax[0].set_title('Standard Gaussian applied for sampling')
ax[0].hist(X.rvs(10000), 50, normed=True)
ax[1].set_title('Truncated Gaussian applied for sampling')
ax[1].hist(N.rvs(10000), 50, normed=True)
plt.show()

f_post_T = mean_T_true + np.dot(y_cov_L_T, X.rvs(len(X_test)))
plt.figure()
plt.plot(X_test[:,0],f_post_T)
plt.plot(np.array(inputs_y)[:,0],mean_T_true,'g-')
plt.fill(np.concatenate([np.array(inputs_y)[:,0],np.array(inputs_y)[:,0][::-1]]),
     np.concatenate([mean_T_true - 1.96*sigma_T_true,
                    (mean_T_true + 1.96*sigma_T_true)[::-1]]),
fc='g',ec='None',label='95% prediction interval',alpha=0.1)
plt.xlabel(r"$\psi$")
plt.ylabel("T"+r"$_e$"+" (keV)",color='g')
plt.title('GP sampling using a truncated Gaussian')
plt.show()

reason: import numpy,from numpy,from scipy


text:
import numpy as np
from random import sample, shuffle, randint

'''
 create batches

'''
def create_batches(data_):
    qr = list(zip(data_['q'], data_['r'], data_['respect']))
    batches = {}
    for qi,ri,respecti in qr:
        lqi, lri = len(qi), len(ri)
        if (lqi,lri) in batches:
            batchi = batches[(lqi,lri)]
        else:
            batchi = []
        batchi += [(qi, ri, respecti)]
        batches[(lqi,lri)] = batchi
    return [ batches[k] for k in batches ]

'''
 split data into train (80%), test (20%)

'''
def split_dataset(batches, ratio = [0.8, 0.2] ):

    nbatches = len(batches)
    num_train = int(ratio[0]*nbatches)

    # shuffle batches
    shuffle(batches) # why do i even bother to write comments!

    trainset = batches[:num_train]
    testset  = batches[num_train:]

    return trainset, testset

'''
 generate batches, by random sampling a bunch of items
    yield (x_gen, y_gen)

'''
def rand_batch_gen(dataset):
    while True: # use try catch here; just repeat idx=.. and batch=...
        #  why do you us try-catch? clip the idx value based on the data at hand. can't we do that?
        #   i did that.. but not working; weird exception - out of bounds; you try fixing it then!
        # shall I run this once?sure
        idx = randint(0, len(dataset)-1) # choose a random batch id
        batch = dataset[idx] # fetch the batch
        bx = [bi[0] for bi in batch]
        by = [bi[1] for bi in batch]
        br = [bi[2] for bi in batch]
        yield ( np.array(bx, dtype=np.int32).reshape([len(bx), len(bx[0])]),
                np.array(by, dtype=np.int32).reshape([len(by), len(by[0])]),
                np.array(br, dtype=np.int32) )

'''
 a generic decode function
    inputs : sequence, lookup

'''
def decode(sequence, lookup, separator=''): # 0 used for padding, is ignored
    return separator.join([ lookup[element] for element in sequence if element ])


text:
import numpy as np

import torch

from common.metrics import hit, mrr, ndcg
from .AmazonDataset import AmazonDataset

def evaluate_neg(model, test_dataset: AmazonDataset, test_loader, candidates, top_k):
    with torch.no_grad():
        Mrr, Hr, Ndcg = [], [], []
        loss = 0  # No effect, ignore this line
        for _, (user, item, query) in enumerate(test_loader):
            # ---------Test---------
            assert len(user) == 1 and len(item) == 1 and len(query) == 1
            user = user.cuda()
            query = query.cuda()
            all_items_ids = torch.cat([item.cuda(), torch.tensor(candidates[item.item()]).cuda()], dim=0)
            all_items_embed, all_items_bias = model(None, all_items_ids, None, mode='output_embedding')
            # ---------rank all---------
            pred = model(user, None, query, 'test')
            scores = torch.sum(pred.repeat(len(all_items_ids), 1) * all_items_embed, dim=1) + all_items_bias
            _, ranking_list = scores.topk(top_k, dim=-1, largest=True)
            ranking_list = ranking_list.tolist()
            Mrr.append(mrr(0, ranking_list))
            Hr.append(hit(0, ranking_list))
            Ndcg.append(ndcg(0, ranking_list))
    return np.mean(Mrr), np.mean(Hr), np.mean(Ndcg)

def evaluate(model, test_dataset: AmazonDataset, test_loader, top_k):
    with torch.no_grad():
        Mrr, Hr, Ndcg = [], [], []
        loss = 0  # No effect, ignore this line
        all_items_ids = torch.tensor(list(range(len(test_dataset.users),
                                                len(test_dataset.users) + len(test_dataset.item_map))),
                                     dtype=torch.long).cuda()
        all_items_embed, all_items_bias = model(None, all_items_ids, None, mode='output_embedding')

        for _, (user, item, query) in enumerate(test_loader):
            # ---------Test---------
            assert len(user) == 1 and len(item) == 1 and len(query) == 1
            user = user.cuda()
            query = query.cuda()
            # ---------rank all---------
            pred = model(user, None, query, 'test')

            # scores = torch.sum(pred.repeat(len(all_items_ids), 1) * all_items_embed, dim=1)
            scores = torch.sum(pred.repeat(len(all_items_ids), 1) * all_items_embed, dim=1) + all_items_bias
            # scores = (pred @ all_items_embed.t()).squeeze(dim=0) + all_items_bias
            _, ranking_list = scores.topk(top_k, dim=-1, largest=True)
            ranking_list = ranking_list.tolist()
            Mrr.append(mrr(item-len(test_dataset.users), ranking_list))
            Hr.append(hit(item-len(test_dataset.users), ranking_list))
            Ndcg.append(ndcg(item-len(test_dataset.users), ranking_list))

        # pred, pos = model(user, item, query, 'test')
        # pred = pred.squeeze(dim=1)
        # negs = test_dataset.neg_candidates(item)
        # candidates = [pos]
        # candidates += [model(user, negs, query, 'output_embedding')]
        # candidates = torch.cat(candidates, dim=0).squeeze(dim=1)
        #
        # # similarity function
        # scores = torch.sum(pred.repeat(100, 1) * candidates, dim=1)
        #
        # _, ranking_list = scores.sort(dim=-1, descending=False)
        # ranking_list = ranking_list.tolist()
        # top_idx = []
        # while len(top_idx) < top_k:
        #     candidate_item = ranking_list.pop()
        #     top_idx.append(candidate_item)
        #
        # Mrr.append(mrr(0, top_idx))
        # Hr.append(hit(0, top_idx))
        # Ndcg.append(ndcg(0, top_idx))
    return np.mean(Mrr), np.mean(Hr), np.mean(Ndcg)

max_stars_repo_name: PTYin/product-search

max_issues_repo_name: PTYin/product-search

max_forks_repo_name: PTYin/product-search


text:
"""
Last observed value
"""

import jax
import jax.numpy as np
import jax.random as rand
import tigerforecast
from tigerforecast.methods import Method
from tigerforecast.utils import generate_key
import scipy.linalg as la
import numpy as onp

class WaveFiltering(Method):
    """
    Description: Predicts the last value in the time series, i.e. x(t) = x(t-1)
    """

    compatibles = set(['TimeSeries'])

    def __init__(self):
        self.initialized = False
        self.uses_regressors = False

    # return top k eigen pairs in descending order
    def eigen_pairs(self, T, k):
        v = onp.fromfunction(lambda i: 1.0 / ((i+2)**3 - (i+2)), (2 * T - 1,))
        Z = 2 * la.hankel(v[:T], v[T-1:])
        eigen_values, eigen_vectors = np.linalg.eigh(Z)
        return np.flip(eigen_values[-k:], axis=0), np.flip(eigen_vectors[:,-k:], axis=1)
        # return eigen_values[-k:], eigen_vectors[:,-k:]

    def initialize(self, n, m, k, T, eta, R_M):
        """
        Description: Initialize the (non-existent) hidden dynamics of the method
        Args:
            None
        Returns:
            None
        """
        self.initialized = True
        self.n, self.m, self.k, self.T = n, m, k, T
        self.eta, self.R_M = eta, R_M
        self.k_prime = n * k + 2 * n + m
        self.M = rand.uniform(generate_key(), shape=(self.m, self.k_prime))
        if (4 * k > T):
            raise Exception("Method parameter k must be less than T/4")
        self.X = np.zeros((n,T))
        self.Y = np.zeros((m,T))
        self.X_sim = None
        self.t = 0
        self.y_hat = None
        self.k_values, self.k_vectors = self.eigen_pairs(T, k)
        self.eigen_diag = np.diag(self.k_values**0.25)

        @jax.jit
        def _update_x(X, x):
            new_x = np.roll(X, 1, axis=1)
            new_x = jax.ops.index_update(new_x, jax.ops.index[:,0], x)
            return new_x
        self._update_x = _update_x
        self.params = {}

    def predict(self, x):
        """
        Description: Takes input observation and returns next prediction value
        Args:
            x (float/numpy.ndarray): value at current time-step
        Returns:
            Predicted value for the next time-step
        """
        '''
        if self.X.size == 0:
            # self.X = np.asarray([x]).T
            self.X = x.reshape(-1,1)
        else:
            # self.X = np.hstack((self.X, np.asarray([x]).T))
            self.X = np.hstack((self.X, x.reshape(-1,1)))
        '''

        # print("-----------------------------")
        # print("x:")
        # print(x)
        # print("type(x) : " + str(type(x)))
        # print("self.X")
        # print(self.X)
        # print("self.X.shape: " + str(self.X.shape))
        self.X = self._update_x(self.X, x)
        X_sim_pre = self.X.dot(self.k_vectors).dot(self.eigen_diag)

        '''
        if (self.t == 0): # t = 0 results in an excessively complicated corner case otherwise
            self.X_sim = np.append(np.zeros(self.n * self.k + self.n), np.append(self.X[:,0], np.zeros(self.m)))
        else:
            eigen_diag = np.diag(self.k_values**0.25)
            if (self.t <= self.T):
                X_sim_pre = self.X[:,0:self.t-1].dot(np.flipud(self.k_vectors[0:self.t-1,:])).dot(eigen_diag)
            else:
                X_sim_pre = self.X[:,self.t-self.T-1:self.t-1].dot(np.flipud(self.k_vectors)).dot(eigen_diag)
        '''

            # x_y_cols = np.append(np.append(self.X[:,self.t-1], self.X[:,self.t]), self.Y[:,self.t-1])
        x_y_cols = np.append(np.append(self.X[:,1], self.X[:,0]), self.Y[:,1])
        '''print("x_y_cols.shape : " + str(x_y_cols.shape))
        print("self.X[:,1].shape : " + str(self.X[:,1].shape))
        print(self.X[:,1])
        print("self.X[:,0].shape : " + str(self.X[:,0].shape))
        print(self.X[:,0])
        print("self.Y[:,1].shape : " + str(self.Y[:,1].shape))
        print("X_sim_pre.shape : " + str(X_sim_pre.shape))'''
        self.X_sim = np.append(X_sim_pre.T.flatten(), x_y_cols)
        # print("self.X_sim.shape : " + str(self.X_sim.shape))
        self.y_hat = self.M.dot(self.X_sim)
        return self.y_hat

    def forecast(self, x, timeline = 1):
        """
        Description: Forecast values 'timeline' timesteps in the future
        Args:
            x (int/numpy.ndarray):  Value at current time-step
            timeline (int): timeline for forecast
        Returns:
            Forecasted values 'timeline' timesteps in the future
        """
        return np.ones(timeline) * x

    def update(self, y):
        """
        Description: Takes update rule and adjusts internal parameters
        Args:
            y (float/np.ndarray): true value
        Returns:
            None
        """
        '''
        if self.Y.size == 0:
            self.Y = np.asarray([y]).T
        else:
            # self.Y = np.append(self.Y, np.asarray([y]).T)
            self.Y = np.hstack((self.Y, np.asarray([y]).T))
        '''
        self.Y = self._update_x(self.Y, y)

        # y_delta = np.asarray([self.y_hat]).T - np.asarray([y]).T
        y_delta = self.y_hat.reshape(-1,1) - y.reshape(-1,1)
        self.M = self.M - 2 * self.eta * np.outer(y_delta, self.X_sim) # changed from +2 to -2
        if (np.linalg.norm(self.M) > self.R_M):
            self.M = self.M * (self.R_M / np.linalg.norm(self.M))
        self.t += 1
        return

    def __str__(self):
        return "<WaveFiltering Method>"

max_stars_repo_name: danielsuo/TigerForecast

max_issues_repo_name: danielsuo/TigerForecast

max_forks_repo_name: danielsuo/TigerForecast

reason: import numpy,import scipy,import jax


text:
import os

import numpy as np

import qml
from qml.representations import get_slatm_mbtypes

def test_slatm_global_representation():

    files = ["qm7/0001.xyz",
             "qm7/0002.xyz",
             "qm7/0003.xyz",
             "qm7/0004.xyz",
             "qm7/0005.xyz",
             "qm7/0006.xyz",
             "qm7/0007.xyz",
             "qm7/0008.xyz",
             "qm7/0009.xyz",
             "qm7/0010.xyz"]

    path = os.path.dirname(os.path.realpath(__file__))

    mols = []
    for xyz_file in files:

        mol = qml.Compound(xyz=path + "/" + xyz_file)
        mols.append(mol)

    mbtypes = get_slatm_mbtypes(np.array([mol.nuclear_charges for mol in mols]))

    for i, mol in enumerate(mols):

        mol.generate_slatm(mbtypes, local=False)

    X_qml = np.array([mol.representation for mol in mols])
    X_ref = np.loadtxt(path + "/data/slatm_global_representation.txt")

    assert np.allclose(X_qml, X_ref), "Error in SLATM generation"

def test_slatm_local_representation():

    files = ["qm7/0001.xyz",
             "qm7/0002.xyz",
             "qm7/0003.xyz",
             "qm7/0004.xyz",
             "qm7/0005.xyz",
             "qm7/0006.xyz",
             "qm7/0007.xyz",
             "qm7/0008.xyz",
             "qm7/0009.xyz",
             "qm7/0010.xyz"]

    path = os.path.dirname(os.path.realpath(__file__))

    mols = []
    for xyz_file in files:

        mol = qml.Compound(xyz=path + "/" + xyz_file)
        mols.append(mol)

    mbtypes = get_slatm_mbtypes(np.array([mol.nuclear_charges for mol in mols]))

    for i, mol in enumerate(mols):
        mol.generate_slatm(mbtypes, local=True)

    X_qml = []
    for i, mol in enumerate(mols):
        for rep in mol.representation:
            X_qml.append(rep)

    X_qml = np.asarray(X_qml)
    X_ref = np.loadtxt(path + "/data/slatm_local_representation.txt")

    assert np.allclose(X_qml, X_ref), "Error in SLATM generation"

if __name__ == "__main__":

    test_slatm_global_representation()
    test_slatm_local_representation()

max_stars_repo_name: mikstr/qml

max_issues_repo_name: FarnazH/qml

max_forks_repo_name: FarnazH/qml