if orthogonal:
            orthogParm_hess = np.zeros([13, 13])
            orthogParm_hess[np.ix_(np.arange(5, 9), np.arange(9, 13))] = np.eye(4)
            orthogParm_hess[np.ix_(np.arange(9, 13), np.arange(5, 9))] = np.eye(4)
            return np.einsum('ijk, i', D2g, h[:-1]) + h[-1] * orthogParm_hess
        else:
            return np.einsum('ijk, i', D2g, h)

    return saddle_node_problem, saddle_node_jac, saddle_node_hess


decay = np.array([np.nan, np.nan], dtype=float)  # gamma
p1 = np.array([np.nan, np.nan, np.nan], dtype=float)  # (ell_1, delta_1, theta_1)
p2 = np.array([np.nan, np.nan, np.nan], dtype=float)  # (ell_2, delta_2, theta_2)
f = ToggleSwitch(decay, [p1, p2])
SN = SaddleNode(f)

# set up the minimization problem
loss, lossJacobian, lossHessian = minimize_hill(SN, punish_degenerate=True)  # minimization problem
xBounds = [[0, 10], [0, 10]]  # phase space bounds for saddle node equilibria points
vBounds = [[-5, 5], [-5, 5]]  # tangent vector coordinate bounds
hillBounds = [[1, 10]]  # hill coefficient bounds
parameterBounds = [[0, 10] for i in range(8)]  # bounds on remaining variable parameters
bounds = Bounds(*list(zip(*(xBounds + vBounds + hillBounds + parameterBounds))))
constraintFunction, constraintJacobian, constraintHessian = saddle_node_constraint(SN, orthogonal=False)
saddleNodeConstraint = NonlinearConstraint(constraintFunction, 0, 0, jac=constraintJacobian, hess=constraintHessian)
# saddleNodeConstraint = NonlinearConstraint(constraintFunction, 0, 0, jac=constraintJacobian, hess=BFGS())


def find_min_hill(hill, parameter):
nCoordinate = 2
gamma = np.array([1, 1], dtype=float)
hill = 4.1

componentParmValues = [
    np.array([1, 5, 3], dtype=float),
    np.array([1, 6, 3], dtype=float)
]
parameter1 = [np.copy(cPValue) for cPValue in componentParmValues]
compVars = [[j for j in range(3)]
            for i in range(nCoordinate)]  # set all parameters as variable
for i in range(nCoordinate):
    parameter1[i][compVars[i]] = np.nan

gammaVar = np.array([np.nan, np.nan])  # set both decay rates as variables
f = ToggleSwitch(gammaVar, parameter1)
f1 = f.coordinates[0]
f2 = f.coordinates[1]
H1 = f1.components[0]
H2 = f2.components[0]

# set some data to check evaluations with
x = np.array([4, 3], dtype=float)
x1 = x[0]
x2 = x[1]
p = ezcat(*[ezcat(*tup) for tup in zip(gamma, componentParmValues)
            ])  # this only works when all parameters are variable
f1p = ezcat(p[:4], hill)  # parameters for f1 and f2 function calls
f2p = ezcat(p[4:], hill)
H1p = f1p[1:]  # parameters for H1 and H2 function calls
H2p = f2p[1:]
import numpy as np
import matplotlib.pyplot as plt
from hill_model import *
from saddle_node import SaddleNode
from models import ToggleSwitch

np.set_printoptions(precision=2, floatmode='maxprec_equal')

# set some parameters to test using MATLAB toggle switch for ground truth
decay = np.array([np.nan, np.nan], dtype=float)  # gamma
p1 = np.array([np.nan, np.nan, np.nan],
              dtype=float)  # (ell_1, delta_1, theta_1)
p2 = np.array([np.nan, np.nan, np.nan],
              dtype=float)  # (ell_2, delta_2, theta_2)

f = ToggleSwitch(decay, [p1, p2])
f1 = f.coordinates[0]
f2 = f.coordinates[1]
H1 = f1.components[0]
H2 = f2.components[0]

p0 = np.array(
    [1, 1, 5, 3, 1, 1, 6, 3], dtype=float
)  # (gamma_1, ell_1, delta_1, theta_1, gamma_2, ell_2, delta_2, theta_2)
SN = SaddleNode(f)

# ==== find saddle node for a parameter choice
rho = 4.1
p = np.array([1, 1, 5, 3, 1, 1, 6, 3], dtype=float)

# x0Sol, v0Sol, rhoSol = [u0Sol.x[idx] for idx in [[0, 1], [2, 3], [4]]]
    else:
        return 0
    print(distance_loc)
    return distance_loc


np.set_printoptions(precision=5, floatmode='maxprec_equal')

# set some parameters to test using MATLAB toggle switch for ground truth
decay = np.array([np.nan, np.nan], dtype=float)  # gamma
p1 = np.array([np.nan, np.nan, np.nan],
              dtype=float)  # (ell_1, delta_1, theta_1)
p2 = np.array([np.nan, np.nan, np.nan],
              dtype=float)  # (ell_2, delta_2, theta_2)

f = ToggleSwitch(decay, [p1, p2])

SN = SaddleNode(f)

# ==== find saddle node for a parameter choice
hill = 4.1
p = np.array([1, 1, 5, 3, 1, 1, 6, 3], dtype=float)


def distance_func(p_loc, SN_loc=SN, rho_loc=hill):
    dist = hysteresis(p_loc, SN_loc, rho_loc)
    return -dist


distance = distance_func(p)
print('Distance = ', distance)
"""

import numpy as np
import matplotlib.pyplot as plt
import time
from hill_model import *
from saddle_node import SaddleNode
from models import ToggleSwitch

# set some parameters to test using MATLAB toggle switch for ground truth
# decay = np.array([np.nan, np.nan], dtype=float)  # gamma
decay = np.array([1, 1])
p1 = np.array([np.nan, np.nan, np.nan], dtype=float)  # (e# ll_1, delta_1, theta_1)
p2 = np.array([np.nan, np.nan, np.nan], dtype=float)  # (ell_2, delta_2, theta_2)

f = ToggleSwitch(decay, [p1, p2])
H1 = f.coordinates[0].components[0]
H2 = f.coordinates[1].components[0]
# SN = SaddleNode(f)

# p = np.random.random_sample([1, 8])
p = np.arange(1, 9)
p1 = p[0:4]
p2 = p[4:]
x1Bounds = H1.image(p1)
x2Bounds = H2.image(p2)
x1New = np.array([H1(x1, p1) for x1 in x1Bounds])
x2New = np.array([H2(x2, p2) for x2 in x2Bounds])


def update_bounds(p, x1Bounds, x2Bounds):