###
# cLevel_Set.py (C) of thesis research "Modeling and Simulating Three Phases of Steel: Austenite, Feritte and Cementite" 2018 by Thijs Verbeek
# is licensed under a Creative Commons Attribution-NonCommercial-ShareAlike 4.0 International License.
#
# You should have received a copy of the license along with this
# work. If not, see <http://creativecommons.org/licenses/by-nc-sa/4.0/>.
###

import numpy as np
import scipy.optimize as optimize
from scipy.sparse import coo_matrix
from scipy.sparse import csr_matrix
from scipy.sparse import diags
from scipy.sparse import find
import scipy.sparse.linalg as spla
import scipy.linalg as la

from scipy.sparse import vstack, hstack, bmat

import matplotlib.pyplot as plt
import matplotlib.colors as colors

from Grid import cGrid2D
from set_layout_plot import plot_settings


def setcsrrow2id(amat, rowind):    
    indptr = amat.indptr
    values = amat.data
    indxs = amat.indices

    # get the range of the data that is changed
    rowpa = indptr[rowind]
    rowpb = indptr[rowind+1]

    # new value and its new rowindex
    values[rowpa] = 1.0
    indxs[rowpa] = rowind

    # number of new zero values
    diffvals = rowpb - rowpa - 1

    # filter the data and indices and adjust the range
    values = np.r_[values[:rowpa+1], values[rowpb:]]
    indxs = np.r_[indxs[:rowpa+1], indxs[rowpb:]]
    indptr = np.r_[indptr[:rowind+1], indptr[rowind+1:]-diffvals]

    # hard set the new sparse data
    amat.indptr = indptr
    amat.data = values
    amat.indices = indxs
    

class MidpointNormalize(colors.Normalize):
    	"""
    	Normalise the colorbar so that diverging bars work there way either side from a prescribed midpoint value)

    	e.g. im=ax1.imshow(array, norm=MidpointNormalize(midpoint=0.,vmin=-100, vmax=100))
    	"""
    	def __init__(self, vmin=None, vmax=None, midpoint=None, clip=False):
    		self.midpoint = midpoint
    		colors.Normalize.__init__(self, vmin, vmax, clip)

    	def __call__(self, value, clip=None):
    		# I'm ignoring masked values and all kinds of edge cases to make a
    		# simple example...
    		x, y = [self.vmin, self.midpoint, self.vmax], [0, 0.5, 1]
    		return np.ma.masked_array(np.interp(value, x, y), np.isnan(value))
        

def shiftedColorMap(cmap, start=0, midpoint=0.5, stop=1.0, name='shiftedcmap'):
    '''
    Function to offset the "center" of a colormap. Useful for
    data with a negative min and positive max and you want the
    middle of the colormap's dynamic range to be at zero

    Input
    -----
      cmap : The matplotlib colormap to be altered
      start : Offset from lowest point in the colormap's range.
          Defaults to 0.0 (no lower ofset). Should be between
          0.0 and `midpoint`.
      midpoint : The new center of the colormap. Defaults to
          0.5 (no shift). Should be between 0.0 and 1.0. In
          general, this should be  1 - vmax/(vmax + abs(vmin))
          For example if your data range from -15.0 to +5.0 and
          you want the center of the colormap at 0.0, `midpoint`
          should be set to  1 - 5/(5 + 15)) or 0.75
      stop : Offset from highets point in the colormap's range.
          Defaults to 1.0 (no upper ofset). Should be between
          `midpoint` and 1.0.
    '''
    cdict = {
        'red': [],
        'green': [],
        'blue': [],
        'alpha': []
    }

    # regular index to compute the colors
    reg_index = np.linspace(start, stop, 257)

    # shifted index to match the data
    shift_index = np.hstack([
        np.linspace(0.0, midpoint, 128, endpoint=False),
        np.linspace(midpoint, 1.0, 129, endpoint=True)
    ])

    for ri, si in zip(reg_index, shift_index):
        r, g, b, a = cmap(ri)

        cdict['red'].append((si, r, r))
        cdict['green'].append((si, g, g))
        cdict['blue'].append((si, b, b))
        cdict['alpha'].append((si, a, a))

    newcmap = colors.LinearSegmentedColormap(name, cdict)
    plt.register_cmap(cmap=newcmap)

    return newcmap


class cLevel_Set:
    def __init__(self, n, dimension, points, update_count, interface, name, a = None, b = None, which_phi = None):
        self.d = dimension
        self.points = points
        self.Initialize(points, interface, a, b, which_phi)
        self.phi_background = self.phi.copy()
        self.name = name

    def Initialize(self, points, interface, a, b, which_phi):
        if self.d == 1:
            i = 0
            self.phi = np.zeros(self.n)
            for p in points:
                self.phi[i] = p - interface
                i=i+1
        elif self.d == 2:
            #Analytical solution
            i=0
            self.n = len(points)
            self.phi = np.zeros(self.n)
            if which_phi == None:
                for p in points:
                    self.phi[i] = (p[0] - interface[1])
                    i=i+1
            elif which_phi == 1: #ga
#                    #Straight line
#                    for p in points:
#                        self.phi[i] = (p[0] - interface[1])
#                        i=i+1
#                    #Line
#                    for p in points:
#                        dydx = 1.05
#                        p2 = np.array([interface[1], 0])
#                        p1 = np.array([interface[1]+5, dydx*5])
#                        if p[1] < dydx*(p[0]-interface[1]):
#                            self.phi[i] = la.norm(np.cross(p2-p1, p1-p))/la.norm(p2-p1)
#                        else:
#                            self.phi[i] = -la.norm(np.cross(p2-p1, p1-p))/la.norm(p2-p1)
#                        i=i+1
                #Circle
#                for p in points:
#                    r = 1
#                    self.phi[i] = a - la.norm(p - interface)
#                    i=i+1
                #ellips,
                for p in points:
                    xshift = interface[0]
                    yshift = interface[1]
                    if p[0] < interface[0]:
                        x = 2*interface[0] - p[0]
                    else:
                        x = p[0]
                    if p[1] < interface[1]:
                        y = 2*interface[1] - p[1]
                    else:
                        y = p[1]

                    if b < a:
                        if ((p[0]-interface[0])/a)**2 + ((p[1]-interface[1])/b)**2 < 1:
                            self.phi[i] = cLevel_Set.DistancePointEllipse(a, b, x - xshift, y - yshift)
                        elif ((p[0]-interface[0])/a)**2 + ((p[1]-interface[1])/b)**2 > 1:
                            self.phi[i] = -cLevel_Set.DistancePointEllipse(a, b, x - xshift, y - yshift)
                        else:
                            self.phi[i] = 0
                    else:
                        if ((p[0]-interface[0])/a)**2 + ((p[1]-interface[1])/b)**2 < 1:
                            self.phi[i] = cLevel_Set.DistancePointEllipse(b, a, y - yshift, x - xshift)
                        elif ((p[0]-interface[0])/a)**2 + ((p[1]-interface[1])/b)**2 > 1:
                            self.phi[i] = -cLevel_Set.DistancePointEllipse(b, a, y - yshift, x - xshift)
                        else:
                            self.phi[i] = 0
                    i=i+1
            elif which_phi == 2: #at
#                    #Straight line
#                    for p in points:
#                        self.phi[i] = (p[0] - interface[1])
#                        i=i+1
#                    #Line
#                    for p in points:
#                        dydx = 1
#                        p2 = np.array([interface[1], 0])
#                        p1 = np.array([interface[1]+5, dydx*5])
#                        if p[1] < dydx*(p[0]-interface[1]):
#                            self.phi[i] = la.norm(np.cross(p2-p1, p1-p))/la.norm(p2-p1)
#                        else:
#                            self.phi[i] = -la.norm(np.cross(p2-p1, p1-p))/la.norm(p2-p1)
#                        i=i+1
                #Ellips
                for p in points:
                    xshift = interface[0]
                    yshift = interface[1]
                    if p[0] < interface[0]:
                        x = 2*interface[0] - p[0]
                    else:
                        x = p[0]
                    if p[1] < interface[1]:
                        y = 2*interface[1] - p[1]
                    else:
                        y = p[1]

                    if b < a:
                        if ((p[0]-interface[0])/a)**2 + ((p[1]-interface[1])/b)**2 < 1:
                            self.phi[i] = cLevel_Set.DistancePointEllipse(a, b, x - xshift, y - yshift)
                        elif ((p[0]-interface[0])/a)**2 + ((p[1]-interface[1])/b)**2 > 1:
                            self.phi[i] = -cLevel_Set.DistancePointEllipse(a, b, x - xshift, y - yshift)
                        else:
                            self.phi[i] = 0
                    else:
                        if ((p[0]-interface[0])/a)**2 + ((p[1]-interface[1])/b)**2 < 1:
                            self.phi[i] = cLevel_Set.DistancePointEllipse(b, a, y - yshift, x - xshift)
                        elif ((p[0]-interface[0])/a)**2 + ((p[1]-interface[1])/b)**2 > 1:
                            self.phi[i] = -cLevel_Set.DistancePointEllipse(b, a, y - yshift, x - xshift)
                        else:
                            self.phi[i] = 0
                    i=i+1
            #Rectangular domain without upper and lower zero line for internal points for double particle domain
            elif which_phi == 13:
                interface_other = interface.copy()
                interface_other[1] = interface[1] - 2 * (interface[1] - 10)
                # 1  5+  3
                # 4  5+ 6
                # 7  5+  9
                i = 0
                for p in points:
                    if p[0] < interface[0] - a: # 1 4 7
                        if p[1] > interface[1]: #1
                            self.phi[i] = - la.norm(p - np.hstack((interface[0] - a, interface[1])), 2)
                        elif p[1] < interface[1] and p[1] > interface_other[1]: #4
                            self.phi[i] = - np.abs(p[0] - (interface[0] - a))
                        elif p[1] < interface_other[1]: #7
                            self.phi[i] = - la.norm(p - np.hstack((interface_other[0] - a, interface_other[1])), 2)
                    elif p[0] > interface[0] - a and p[0] < interface[0] + a: #2 5+ 8
#                        if p[1] > interface[1]: #2
#                            self.phi[i] = - np.abs(p[1] - interface[1])
#                        elif p[1] <= interface[1] and p[1] >= interface_other[1]: #5+
                        x = np.ones((2,2)) * p
                        x[0,0] = interface[0] - a
                        x[1,0] = interface[0] + a
                        self.phi[i] = np.min(la.norm(x-p, 2, axis = 1))
#                        elif p[1] < interface_other[1]: #8
#                            self.phi[i] = - np.abs(p[1] - interface_other[1])
                    elif p[0] > interface[0] + a: # 3 6 9
                        if p[1] > interface[1]: #1
                            self.phi[i] = - la.norm(p - np.hstack((interface[0] + a, interface[1])), 2)
                        elif p[1] < interface[1] and p[1] > interface_other[1]: #4
                            self.phi[i] = - np.abs(p[0] - (interface[0] + a))
                        elif p[1] < interface_other[1]: #7
                            self.phi[i] = - la.norm(p - np.hstack((interface_other[0] + a, interface_other[1])), 2)
                    i = i+1


    def update(self, grid, dt, vn_ex, which_phi):
        phi_RK3 = self.phi_background.copy()
        if which_phi == 1:
            for tri_i in grid.background_mesh.point_in_tri[grid.background_mesh.i_ghost_fixed].nonzero()[1]:
                trii = grid.background_mesh.triangles_points_index[tri_i]
                grid.background_mesh.triangles[tri_i]     = grid.background_mesh.points[trii]
                grid.background_mesh.area_triangle[tri_i] = cGrid2D.cGrid.area(grid.background_mesh.triangles[tri_i])


        I = np.zeros(9*grid.background_mesh.n_tri, dtype = np.int)
        J = np.zeros(9*grid.background_mesh.n_tri, dtype = np.int)
        M_V = np.zeros(9*grid.background_mesh.n_tri)
#        ### Diff_V is used for SUPG method
#        Diff_V = np.zeros(9*grid.background_mesh.n_tri)
        Conv_V = np.zeros(9*grid.background_mesh.n_tri)
        l = 1
        for i in range(0,grid.background_mesh.n_tri):
            trii  =   grid.background_mesh.triangles_points_index[i]
            tri   =   grid.background_mesh.triangles[i]
            Delta = 2*grid.background_mesh.area_triangle[i]

            beta = np.zeros(3)
            beta[0] = (tri[1,1] - tri[2,1])/Delta
            beta[1] = (tri[2,1] - tri[0,1])/Delta
            beta[2] = (tri[0,1] - tri[1,1])/Delta
            gamma = np.zeros(3)
            gamma[0] = (tri[2,0] - tri[1,0])/Delta
            gamma[1] = (tri[0,0] - tri[2,0])/Delta
            gamma[2] = (tri[1,0] - tri[0,0])/Delta

#            v_ex_trii = vn_ex[trii]

            I[9*(l-1):9*(l-1)+9] = np.hstack((np.ones(3, dtype = np.int) * trii[0], np.ones(3, dtype = np.int) * trii[1], np.ones(3, dtype = np.int) * trii[2]))
            J[9*(l-1):9*(l-1)+9] = np.hstack((trii, trii, trii))

#            M_V[9*(l-1):9*(l-1)+9] = np.abs(Delta) / 24 * ( (np.ones((3,3)) + np.eye(3)) +
#                                                                 diff * diff_delta * ((v_ex_trii[:,0] * np.ones((3,3)) + np.sum(v_ex_trii[:,0]) * np.ones((3,3)) ) * beta.reshape(3,1) ) +
#                                                                 diff * diff_delta * ((v_ex_trii[:,1] * np.ones((3,3)) + np.sum(v_ex_trii[:,1]) * np.ones((3,3)) ) * gamma.reshape(3,1) ) ).reshape(9)

            M_V[9*(l-1):9*(l-1)+9] = np.abs(Delta) / 24 * ( (np.ones((3,3)) + np.eye(3))).reshape(9)

#            Conv_V[9*(l-1):9*(l-1)+9] = -np.abs(Delta) / 24 * ((v_ex_trii[:,0].reshape(3,1) * np.ones((3,3)) + np.sum(v_ex_trii[:,0]) * np.ones((3,3)) ) * beta +
#                                                                  (v_ex_trii[:,1].reshape(3,1) * np.ones((3,3)) + np.sum(v_ex_trii[:,1]) * np.ones((3,3)) ) * gamma).reshape(9)

            Conv_V[9*(l-1):9*(l-1)+9] = -np.abs(Delta) / 24 * np.array([2*(vn_ex[trii[0],0] * beta[0] + vn_ex[trii[0],1] * gamma[0]) + vn_ex[trii[1],0] * beta[0] + vn_ex[trii[1],1] * gamma[0] + vn_ex[trii[2],0] * beta[0] + vn_ex[trii[2],1] * gamma[0],
                                                                        2*(vn_ex[trii[0],0] * beta[1] + vn_ex[trii[0],1] * gamma[1]) + vn_ex[trii[1],0] * beta[1] + vn_ex[trii[1],1] * gamma[1] + vn_ex[trii[2],0] * beta[1] + vn_ex[trii[2],1] * gamma[1],
                                                                        2*(vn_ex[trii[0],0] * beta[2] + vn_ex[trii[0],1] * gamma[2]) + vn_ex[trii[1],0] * beta[2] + vn_ex[trii[1],1] * gamma[2] + vn_ex[trii[2],0] * beta[2] + vn_ex[trii[2],1] * gamma[2],
                                                                        2*(vn_ex[trii[1],0] * beta[0] + vn_ex[trii[1],1] * gamma[0]) + vn_ex[trii[0],0] * beta[0] + vn_ex[trii[0],1] * gamma[0] + vn_ex[trii[2],0] * beta[0] + vn_ex[trii[2],1] * gamma[0],
                                                                        2*(vn_ex[trii[1],0] * beta[1] + vn_ex[trii[1],1] * gamma[1]) + vn_ex[trii[0],0] * beta[1] + vn_ex[trii[0],1] * gamma[1] + vn_ex[trii[2],0] * beta[1] + vn_ex[trii[2],1] * gamma[1],
                                                                        2*(vn_ex[trii[1],0] * beta[2] + vn_ex[trii[1],1] * gamma[2]) + vn_ex[trii[0],0] * beta[2] + vn_ex[trii[0],1] * gamma[2] + vn_ex[trii[2],0] * beta[2] + vn_ex[trii[2],1] * gamma[2],
                                                                        2*(vn_ex[trii[2],0] * beta[0] + vn_ex[trii[2],1] * gamma[0]) + vn_ex[trii[0],0] * beta[0] + vn_ex[trii[0],1] * gamma[0] + vn_ex[trii[1],0] * beta[0] + vn_ex[trii[1],1] * gamma[0],
                                                                        2*(vn_ex[trii[2],0] * beta[1] + vn_ex[trii[2],1] * gamma[1]) + vn_ex[trii[0],0] * beta[1] + vn_ex[trii[0],1] * gamma[1] + vn_ex[trii[1],0] * beta[1] + vn_ex[trii[1],1] * gamma[1],
                                                                        2*(vn_ex[trii[2],0] * beta[2] + vn_ex[trii[2],1] * gamma[2]) + vn_ex[trii[0],0] * beta[2] + vn_ex[trii[0],1] * gamma[2] + vn_ex[trii[1],0] * beta[2] + vn_ex[trii[1],1] * gamma[2]])


#            Diff_V[9*(l-1):9*(l-1)+9] = np.abs(Delta) / 24 * diff_delta * (2*(np.sum(v_ex_trii[:,0])**2 - (v_ex_trii[0,0] * v_ex_trii[1,0] + v_ex_trii[0,0] * v_ex_trii[2,0] + v_ex_trii[1,0] * v_ex_trii[2,0]))* (beta.reshape(3,1) @ beta.reshape(1,3))  +
#                                                                              (2 * np.sum(v_ex_trii[:,0] * v_ex_trii[:,1]) + v_ex_trii[0,0] *(v_ex_trii[1,1] + v_ex_trii[2,1]) + v_ex_trii[0,1] *(v_ex_trii[1,0]) + v_ex_trii[2,0] + v_ex_trii[1,0] * v_ex_trii[2,1] + v_ex_trii[1,1] * v_ex_trii[2,0]) *
#                                                                              ((beta.reshape(3,1) @ gamma.reshape(1,3)) + (gamma.reshape(3,1) @ beta.reshape(1,3))) +
#                                                                               2*(np.sum(v_ex_trii[:,1])**2 - (v_ex_trii[0,1] * v_ex_trii[1,1] + v_ex_trii[0,1] * v_ex_trii[2,1] + v_ex_trii[1,1] * v_ex_trii[2,1])) * gamma.reshape(3,1) @ gamma.reshape(1,3) ).reshape(9)

            l = l+1
        M = coo_matrix((M_V,(I,J)), shape = (grid.background_mesh.n,grid.background_mesh.n))
        M = M.tocsr()
        M_L = diags(np.asarray(csr_matrix.sum(M, axis = 1)).ravel(), format = 'csr')
#        M = M.tocoo()

        Conv = coo_matrix((Conv_V,(I,J)), shape = (grid.background_mesh.n,grid.background_mesh.n))
        Conv = Conv.tocsr()

        ### RK2 TVD
#        phi1 = spla.spsolve(M_L, M_L @ phi_RK2 + dt * Conv @ phi_RK2)
#        phi_RK2 = spla.spsolve(M_L, M_L/2 @ (phi_RK2 + phi1) + dt / 2 * Conv @ phi1)

        ### RK3 TVD
        phi1 = spla.spsolve(M_L, M_L @ phi_RK3 + dt * Conv @ phi_RK3)
        phi2 = spla.spsolve(M_L, M_L @ (3 * phi_RK3 / 4 + phi1 / 4) + dt / 4 * Conv @ phi1)
        phi_RK3 = spla.spsolve(M_L, M_L @ (phi_RK3 / 3 + 2 * phi2 / 3) + 2 * dt / 3 * Conv @ phi2)

        self.phi_background = phi_RK3.copy()
        if which_phi == 1 and grid.background_mesh.i_ghost_fixed != -1:
            patch_x_i = np.setdiff1d(grid.background_mesh.triangles_points_index[grid.background_mesh.point_in_tri[grid.background_mesh.i_ghost_fixed].nonzero()[1]].flatten(), grid.background_mesh.i_ghost_fixed)
            new_fixed_point_sign = - np.sign(np.sum(self.phi_background[patch_x_i]))
            if np.abs(np.sum(np.sign(self.phi_background[patch_x_i]))) == len(patch_x_i):
                print('moved fixed point because no neighbouring points have different sign')
                found_point = False
                new_points = np.zeros(0, dtype = np.int)
                while not(found_point):
                    patch_x_i = np.setdiff1d(grid.background_mesh.triangles_points_index[grid.background_mesh.point_in_tri[patch_x_i].nonzero()[1]].flatten(), grid.background_mesh.i_ghost_fixed)
                    for x_i in patch_x_i:
                        patch0_x_i = np.unique(grid.background_mesh.triangles_points_index[grid.background_mesh.point_in_tri[x_i].nonzero()[1]].flatten())
                        if any(np.sign(self.phi_background[patch0_x_i]) == new_fixed_point_sign):
                            found_point = True
                            new_points = np.append(new_points, x_i)
                distance = la.norm(grid.background_mesh.points[new_points] - grid.background_mesh.points[grid.background_mesh.i_ghost_fixed], 2, axis = 1)
                argmin_distance = np.argmin(distance)
                self.phi_background[grid.background_mesh.i_ghost_fixed] = -new_fixed_point_sign * distance[argmin_distance]
                grid.background_mesh.i_ghost_fixed = new_points[argmin_distance]
            self.phi_background[grid.background_mesh.i_ghost_fixed] = 0




#   ### Reinitialize on enriched mesh
#    def reinitialize(self, grid, which_phi, Force_reinit = None, plot_grad = None, t_reinit = None):
#        diff = 1
#        if Force_reinit:
#            reinit_bound = 0
#        else:
#            reinit_bound = self.FPT * 50
#        if which_phi == 1: #ga
#            n = grid.background_mesh.n_ga
#            F = grid.background_mesh.Tau_all_ga
#            N_interface = grid.background_mesh.Tau_all_ga.shape[0]
#        elif which_phi == 2: #t
#            n = grid.background_mesh.n_t
#            F = grid.background_mesh.Tau_all_t
#            N_interface = grid.background_mesh.Tau_all_t.shape[0]
##        eps = 1e-3
##        signphi = self.phi_background/np.sqrt(self.phi_background**2 + eps**2)
#        old_phi = self.phi_background.copy()
#        sign_phi = np.sign(old_phi)
#        w = n.copy()
#
#        band = 2.5*grid.background_mesh.h_min
#
#        where_interface_points = np.unique(F.nonzero()[1])
#        find3 = find(grid.background_mesh.three_neighbours[where_interface_points])
#        where_band = np.unique(find3[1])
#        band_reinit = np.unique(find3[1][np.where(find3[2] <= 1)[0]])
#
#        grad_phi = np.zeros((grid.background_mesh.n,2))
#        for j in range(0,grid.background_mesh.n):
#            xp = grid.background_mesh.points[j]
#            other_ii = np.nonzero(grid.background_mesh.patch_matrix[j])[1]
#            other_i = grid.background_mesh.points[other_ii]
#            hp = grid.background_mesh.patch_matrix[j, j]
#            n_row = len(other_ii)
#
#            eta = (other_i - xp)/hp
#            M_eta = np.ones((n_row, 6))
#            phi_patch = np.zeros(n_row)
#            for i in range(0,n_row):
#                etaj = eta[i]
#                M_eta[i,1:6] = np.array([etaj[0], etaj[1], etaj[0]**2, etaj[0] * etaj[1], etaj[1]**2])
#                phi_patch[i] = old_phi[other_ii[i]]
#            B = la.lstsq(M_eta, phi_patch)[0][[1,2]]
#            grad_phi[j] = B/hp
#
#        normphi = la.norm(grad_phi,axis = 1)
#
#        eps = la.norm(normphi[band_reinit] - 1, 2)
#        eps_old = 1e6
#        reinit_done = False
#        if eps < reinit_bound:
#            print('Reinit not done, eps = %.3e ' %eps , 'FPT = %.3e' %self.FPT)
#            reinit_done = True
#
#        if plot_grad and reinit_done:
#            f5, (ax52, ax53) = plt.subplots(1,2, figsize=(9.5,9.5), sharex = True, sharey = True)
#            bwr_ga = plt.cm.bwr
#            elev_min = np.min(normphi[where_band]-1)
#            elev_max = np.max(normphi[where_band]-1)
#            if which_phi == 1:
#                [ax52.plot(grid.enriched_mesh.points[grid.interface_gammaalpha.edges_index[i,:],0],grid.enriched_mesh.points[grid.interface_gammaalpha.edges_index[i,:],1], linewidth = 2, color = 'darkblue') for i in range(0,len(grid.interface_gammaalpha.edges))]
#                [ax52.plot(grid.enriched_mesh.points[grid.interface_gammaalpha.ghost_edges_index[i,:],0],grid.enriched_mesh.points[grid.interface_gammaalpha.ghost_edges_index[i,:],1], linewidth = 2, color = 'lightblue') for i in range(0,len(grid.interface_gammaalpha.ghost_edges_index))]
#                ax52.set_title(r'$|||\nabla \phi^{\gamma \alpha}||_2-1|$ before reinit at t = %.3f' %t_reinit)
#            elif which_phi == 2:
#                [ax52.plot(grid.enriched_mesh.points[grid.interface_alphatheta.edges_index[i,:],0],grid.enriched_mesh.points[grid.interface_alphatheta.edges_index[i,:],1], linewidth = 2, color = 'red') for i in range(0,len(grid.interface_alphatheta.edges))]
#                [ax52.plot(grid.enriched_mesh.points[grid.interface_gammatheta.edges_index[i,:],0],grid.enriched_mesh.points[grid.interface_gammatheta.edges_index[i,:],1], linewidth = 2, color = 'green') for i in range(0,len(grid.interface_gammatheta.edges))]
#                ax52.set_title(r'$|||\nabla \phi^{\theta}||_2-1|$ before reinit at t = %.3f' %t_reinit)
#            surf52 = ax52.tripcolor(grid.background_mesh.points[:,0], grid.background_mesh.points[:,1], grid.background_mesh.triangles_points_index, normphi-1, cmap = bwr_ga, edgecolors= 'None', clim=(elev_min, elev_max), norm = MidpointNormalize(midpoint=0,vmin=elev_min, vmax=elev_max))
#            f5.colorbar(surf52, ax = ax52)
#            ax52.set_xlim([grid.x0, grid.xN])
#            ax52.set_ylim([grid.x0, grid.xN])
#
#
#        tau = 0
#        dtau = grid.enriched_mesh.h_min / 2
#        diff_delta = 1/(2*np.sqrt(1/dtau**2 + (abs_vn/grid.enriched_mesh.h_max)**2))
##        phi_RK2 = self.phi_background.copy()
#        phi_RK3 = old_phi.copy()
#        temp_phi = phi_RK3.copy()
#        kk = 0
#        if eps > reinit_bound:
#            while tau <= band: # and eps_old > eps:
#                eps_old = eps
#                band_done = tau
#                if tau == 0:
#                    print('Reinit done, eps, FPT ', 0, eps, self.FPT)
#                    I = -np.ones(9*grid.background_mesh.n_tri, dtype = np.int)
#                    J = -np.ones(9*grid.background_mesh.n_tri, dtype = np.int)
#                    M_V = np.zeros(9*grid.background_mesh.n_tri)
#                    RHS = np.zeros(grid.background_mesh.n)
#                else:
#                    print(kk, '%.3f' % tau, '%.3f' % eps)
#                print('max/min', '%.3e' % (np.max(normphi[band_reinit] - 1)), '%.3e' % (np.min(normphi[band_reinit] - 1)))
#                Conv_V = np.zeros(9*grid.background_mesh.n_tri)
#                Diff_V = np.zeros(9*grid.background_mesh.n_tri)
#                Diff_M_V = np.zeros(9*grid.background_mesh.n_tri)
#                Diff_RHS = np.zeros(grid.background_mesh.n)
#                l = 1
#                for i in range(0,grid.background_mesh.n_tri):
#                    trii  =   grid.background_mesh.triangles_points_index[i]
#                    tri   =   grid.background_mesh.triangles[i]
#                    Delta = 2*grid.background_mesh.area_triangle[i]
#
#                    beta = np.zeros(3)
#                    beta[0] = (tri[1,1] - tri[2,1])/Delta
#                    beta[1] = (tri[2,1] - tri[0,1])/Delta
#                    beta[2] = (tri[0,1] - tri[1,1])/Delta
#                    gamma = np.zeros(3)
#                    gamma[0] = (tri[2,0] - tri[1,0])/Delta
#                    gamma[1] = (tri[0,0] - tri[2,0])/Delta
#                    gamma[2] = (tri[1,0] - tri[0,0])/Delta
#
#                    sum_sign_phi = int(np.sum(sign_phi[trii]))
#
#                    #if sign of all corners is the same integrating is easy and can be done exact:
#                    if np.abs(sum_sign_phi) >= 2:
#                        if tau == 0:
#                            I[9*(l-1):9*(l-1)+9] = np.hstack((np.ones(3, dtype = np.int) * trii[0], np.ones(3, dtype = np.int) * trii[1], np.ones(3, dtype = np.int) * trii[2]))
#                            J[9*(l-1):9*(l-1)+9] = np.hstack((trii, trii, trii))
#
##                            M_V[9*(l-1):9*(l-1)+9] = np.abs(Delta) / 24 * (np.ones((3,3)) + np.eye(3)).reshape(9)
#                            M_V[9*(l-1):9*(l-1)+9] = np.abs(Delta) / 24 *  (np.ones((3,3)) + np.eye(3)).reshape(9)
#
#                            RHS[trii] = RHS[trii] + np.abs(Delta) / 6 * sign_phi[trii]
#                        Diff_M_V[9*(l-1):9*(l-1)+9] = - sum_sign_phi / sum_sign_phi * np.abs(Delta) / 24 * (diff * diff_delta * ((v_ex_trii[:,0] * np.ones((3,3)) + np.sum(v_ex_trii[:,0]) * np.ones((3,3)) ) * beta.reshape(3,1) ) +
#                                                                 diff * diff_delta * ((v_ex_trii[:,1] * np.ones((3,3)) + np.sum(v_ex_trii[:,1]) * np.ones((3,3)) ) * gamma.reshape(3,1) )).reshape(9)
#
#                        Conv_V[9*(l-1):9*(l-1)+9] = - sum_sign_phi / sum_sign_phi * np.abs(Delta) / 24 * np.array([(2*(w[trii[0],0] * beta[0] + w[trii[0],1] * gamma[0]) + w[trii[1],0] * beta[0] + w[trii[1],1] * gamma[0] + w[trii[2],0] * beta[0] + w[trii[2],1] * gamma[0]),
#                                                                                   (2*(w[trii[0],0] * beta[1] + w[trii[0],1] * gamma[1]) + w[trii[1],0] * beta[1] + w[trii[1],1] * gamma[1] + w[trii[2],0] * beta[1] + w[trii[2],1] * gamma[1]),
#                                                                                   (2*(w[trii[0],0] * beta[2] + w[trii[0],1] * gamma[2]) + w[trii[1],0] * beta[2] + w[trii[1],1] * gamma[2] + w[trii[2],0] * beta[2] + w[trii[2],1] * gamma[2]),
#                                                                                   (2*(w[trii[1],0] * beta[0] + w[trii[1],1] * gamma[0]) + w[trii[0],0] * beta[0] + w[trii[0],1] * gamma[0] + w[trii[2],0] * beta[0] + w[trii[2],1] * gamma[0]),
#                                                                                   (2*(w[trii[1],0] * beta[1] + w[trii[1],1] * gamma[1]) + w[trii[0],0] * beta[1] + w[trii[0],1] * gamma[1] + w[trii[2],0] * beta[1] + w[trii[2],1] * gamma[1]),
#                                                                                   (2*(w[trii[1],0] * beta[2] + w[trii[1],1] * gamma[2]) + w[trii[0],0] * beta[2] + w[trii[0],1] * gamma[2] + w[trii[2],0] * beta[2] + w[trii[2],1] * gamma[2]),
#                                                                                   (2*(w[trii[2],0] * beta[0] + w[trii[2],1] * gamma[0]) + w[trii[0],0] * beta[0] + w[trii[0],1] * gamma[0] + w[trii[1],0] * beta[0] + w[trii[1],1] * gamma[0]),
#                                                                                   (2*(w[trii[2],0] * beta[1] + w[trii[2],1] * gamma[1]) + w[trii[0],0] * beta[1] + w[trii[0],1] * gamma[1] + w[trii[1],0] * beta[1] + w[trii[1],1] * gamma[1]),
#                                                                                   (2*(w[trii[2],0] * beta[2] + w[trii[2],1] * gamma[2]) + w[trii[0],0] * beta[2] + w[trii[0],1] * gamma[2] + w[trii[1],0] * beta[2] + w[trii[1],1] * gamma[2])])
#
#                    #else we need to splits the triangle in the positive and negative part
#                    else:
#                        if np.abs(sum_sign_phi) == 1:
#                            where_one_minplus, = np.where(sign_phi[trii] == -sum_sign_phi)[0]
#                            index_cutoff = trii[where_one_minplus]
#                            cut_off = grid.background_mesh.points[index_cutoff]
#                            other_ii = np.setdiff1d(trii, index_cutoff)
#                            where_other_ii = -np.ones(2, dtype = np.int)
#                            where_other_ii[0], = np.where(trii == other_ii[0])
#                            where_other_ii[1], = np.where(trii == other_ii[1])
#                            other = grid.background_mesh.points[other_ii]
#
#
#                            taue = np.zeros(2)
#                            pe = np.zeros((2,2))
#                            wkc = np.zeros((2,2))
#                            for k in range(0,2):
#                                #make sure the cut off point is set as first in the edge, so 1-taue is its weight.
#                                edgeii = np.hstack((index_cutoff, other_ii[k]))
#                                taue[k] = -old_phi[edgeii[0]]/(old_phi[edgeii[1]] - old_phi[edgeii[0]])
#                                pe[k]  = (1-taue[k])*cut_off + taue[k]*other[k]
#                                wkc[k] = (1-taue[k])*w[index_cutoff] + taue[k]*w[other_ii[k]]
#
#
#                            tri0 = np.vstack((cut_off, pe[0], pe[1]))
#                            tri1 = np.vstack((other[0], pe[0], pe[1]))
#                            tri2 = np.vstack((other[0], other[1], pe[1]))
#                            Delta0 = 2*cGrid2D.cGrid.area(tri0)
#                            Delta1 = 2*cGrid2D.cGrid.area(tri1)
#                            Delta2 = 2*cGrid2D.cGrid.area(tri2)
#
#                            #sign phi(cutoff) = - phi(other_ii[0/1])
#                            Conv_V[9*(l-1):9*(l-1)+9] = - sign_phi[index_cutoff] * np.hstack((np.abs(Delta0) / 6 *(beta[where_one_minplus] * (w[index_cutoff,0] + (1-taue[0])*wkc[0,0] + (1-taue[1])*wkc[1,0]) + gamma[where_one_minplus] * (w[index_cutoff,1] + (1-taue[0])*wkc[0,1] + (1-taue[1])*wkc[1,1])) -
#                                                                                              (np.abs(Delta1)+np.abs(Delta2)) / 8 * (beta[where_one_minplus] * ((1-taue[0])  *wkc[0,0] + (1-taue[1])*wkc[1,0]) + gamma[where_one_minplus] * ((1-taue[0]) * wkc[0,1] + (1-taue[1])*wkc[1,1])),
#                                                                                              np.abs(Delta0) / 6 *(beta[where_other_ii[0]] * (w[index_cutoff,0] + (1-taue[0])*wkc[0,0] + (1-taue[1])*wkc[1,0]) + gamma[where_other_ii[0]] * (w[index_cutoff,1] + (1-taue[0])*wkc[0,1] + (1-taue[1])*wkc[1,1])) -
#                                                                                              (np.abs(Delta1)+np.abs(Delta2)) / 8 * (beta[where_other_ii[0]] * ((1-taue[0])  *wkc[0,0] + (1-taue[1])*wkc[1,0]) + gamma[where_other_ii[0]] * ((1-taue[0]) * wkc[0,1] + (1-taue[1])*wkc[1,1])),
#                                                                                              np.abs(Delta0) / 6 *(beta[where_other_ii[1]] * (w[index_cutoff,0] + (1-taue[0])*wkc[0,0] + (1-taue[1])*wkc[1,0]) + gamma[where_other_ii[1]] * (w[index_cutoff,1] + (1-taue[0])*wkc[0,1] + (1-taue[1])*wkc[1,1])) -
#                                                                                              (np.abs(Delta1)+np.abs(Delta2)) / 8 * (beta[where_other_ii[1]] * ((1-taue[0])  *wkc[0,0] + (1-taue[1])*wkc[1,0]) + gamma[where_other_ii[1]] * ((1-taue[0]) * wkc[0,1] + (1-taue[1])*wkc[1,1])),
#
#                                                                                              np.abs(Delta0) / 6 * taue[0] *        (beta[where_one_minplus] * wkc[0,0] + gamma[where_one_minplus] * wkc[0,1]) -
#                                                                                              (np.abs(Delta1)+np.abs(Delta2)) / 8 * (beta[where_one_minplus] * (w[other_ii[0],0] + taue[0] * wkc[0,0]) + gamma[where_one_minplus] * (w[other_ii[0],1] + taue[0] * wkc[0,1])),
#                                                                                              np.abs(Delta0) / 6 * taue[0] *        (beta[where_other_ii[0]] * wkc[0,0] + gamma[where_other_ii[0]] * wkc[0,1]) -
#                                                                                              (np.abs(Delta1)+np.abs(Delta2)) / 8 * (beta[where_other_ii[0]] * (w[other_ii[0],0] + taue[0] * wkc[0,0]) + gamma[where_other_ii[0]] * (w[other_ii[0],1] + taue[0] * wkc[0,1])),
#                                                                                              np.abs(Delta0) / 6 * taue[0] *        (beta[where_other_ii[1]] * wkc[0,0] + gamma[where_other_ii[1]] * wkc[0,1]) -
#                                                                                              (np.abs(Delta1)+np.abs(Delta2)) / 8 * (beta[where_other_ii[1]] * (w[other_ii[0],0] + taue[0] * wkc[0,0]) + gamma[where_other_ii[1]] * (w[other_ii[0],1] + taue[0] * wkc[0,1])),
#
#                                                                                              np.abs(Delta0) / 6 * taue[1] *        (beta[where_one_minplus] * wkc[1,0] + gamma[where_one_minplus] * wkc[1,1]) -
#                                                                                              (np.abs(Delta1)+np.abs(Delta2)) / 8 * (beta[where_one_minplus] * (w[other_ii[1],0] + taue[1] * wkc[1,0]) + gamma[where_one_minplus] * (w[other_ii[1],1] + taue[1] * wkc[1,1])),
#                                                                                              np.abs(Delta0) / 6 * taue[1] *        (beta[where_other_ii[0]] * wkc[1,0] + gamma[where_other_ii[0]] * wkc[1,1]) -
#                                                                                              (np.abs(Delta1)+np.abs(Delta2)) / 8 * (beta[where_other_ii[0]] * (w[other_ii[1],0] + taue[1] * wkc[1,0]) + gamma[where_other_ii[0]] * (w[other_ii[1],1] + taue[1] * wkc[1,1])),
#                                                                                              np.abs(Delta0) / 6 * taue[1] *        (beta[where_other_ii[1]] * wkc[1,0] + gamma[where_other_ii[1]] * wkc[1,1]) -
#                                                                                              (np.abs(Delta1)+np.abs(Delta2)) / 8 * (beta[where_other_ii[1]] * (w[other_ii[1],0] + taue[1] * wkc[1,0]) + gamma[where_other_ii[1]] * (w[other_ii[1],1] + taue[1] * wkc[1,1]))
#                                                                                              ))
#                            if tau == 0:
#                                I[9*(l-1):9*(l-1)+9] = np.hstack((np.ones(3, dtype = np.int) * index_cutoff, np.ones(3, dtype = np.int) * other_ii[0], np.ones(3, dtype = np.int) * other_ii[1]))
#                                J[9*(l-1):9*(l-1)+9] = np.hstack((index_cutoff, other_ii[0], other_ii[1],
#                                                                  index_cutoff, other_ii[0], other_ii[1],
#                                                                  index_cutoff, other_ii[0], other_ii[1]))
##                                M_V[9*(l-1):9*(l-1)+9] = np.abs(Delta) / 24 * (np.ones((3,3)) + np.eye(3)).reshape(9)
#                                M_V[9*(l-1):9*(l-1)+9] = np.abs(Delta) / 24 * (np.ones((3,3)) + np.eye(3)).reshape(9)
#                                RHS[index_cutoff] = RHS[index_cutoff] + sign_phi[index_cutoff] * (np.abs(Delta0) / 6 * (3 - taue[0] - taue[1]) - (np.abs(Delta1) + np.abs(Delta2)) / 8 * (2 - taue[0] - taue[1]) )
#                                RHS[other_ii[0]]  = RHS[other_ii[0]]  + sign_phi[index_cutoff] * (np.abs(Delta0) / 6 * taue[0] - (np.abs(Delta1) + np.abs(Delta2)) / 8 * (1 + taue[0]))
#                                RHS[other_ii[1]]  = RHS[other_ii[1]]  + sign_phi[index_cutoff] * (np.abs(Delta0) / 6 * taue[1] - (np.abs(Delta1) + np.abs(Delta2)) / 8 * (1 + taue[1]))
#                            if np.abs(np.abs(Delta) - np.abs(Delta0) - np.abs(Delta1) - np.abs(Delta2)) > 1e-11:
#                                print('Made the wrong triangles in reinitialize')
#                            if np.setdiff1d(trii, np.hstack([index_cutoff, other_ii])).shape[0] != 0:
#                                print('wrong indices?', trii, np.hstack([index_cutoff, other_ii]))
#                            if np.setdiff1d(np.arange(0,3), np.hstack([where_one_minplus, where_other_ii])).shape[0] != 0:
#                                print('wrong indices?', trii, np.hstack([index_cutoff, other_ii]), np.hstack([where_one_minplus, where_other_ii]))
#
#                        elif np.abs(sum_sign_phi)== 0:
#                            where_index0, = np.where(sign_phi[trii] == 0)[0]
#                            index0 = trii[where_index0]
#                            point0 = grid.background_mesh.points[index0]
#                            other_ii = np.setdiff1d(trii, index0)
#                            where_other_ii = -np.ones(2, dtype = np.int)
#                            where_other_ii[0], = np.where(trii == other_ii[0])
#                            where_other_ii[1], = np.where(trii == other_ii[1])
#                            other = grid.background_mesh.points[other_ii]
#
#                            taue = -old_phi[other_ii[0]]/(old_phi[other_ii[1]] - old_phi[other_ii[0]])
#                            pe   = (1-taue)*other[0] + taue*other[1]
#                            wkc  = (1-taue)*w[other_ii[0]] + taue*w[other_ii[1]]
#
#                            tri0 = np.vstack((point0, pe, other[0]))
#                            tri1 = np.vstack((point0, pe, other[1]))
#                            Delta0 = 2*cGrid2D.cGrid.area(tri0)
#                            Delta1 = 2*cGrid2D.cGrid.area(tri1)
#
#                            Conv_V[9*(l-1):9*(l-1)+9] = - np.hstack(((np.abs(Delta0) / 6 * sign_phi[other_ii[0]] + np.abs(Delta1) / 6 * sign_phi[other_ii[1]]) * (beta[where_index0] * w[index0,0] + gamma[where_index0] * w[index0,1]),
#                                                                     (np.abs(Delta0) / 6 * sign_phi[other_ii[0]] + np.abs(Delta1) / 6 * sign_phi[other_ii[1]]) * (beta[where_other_ii[0]] * w[other_ii[0],0] + gamma[where_other_ii[0]] * w[other_ii[0],1]),
#                                                                     (np.abs(Delta0) / 6 * sign_phi[other_ii[0]] + np.abs(Delta1) / 6 * sign_phi[other_ii[1]]) * (beta[where_other_ii[1]] * w[other_ii[1],0] + gamma[where_other_ii[1]] * w[other_ii[1],1]),
#
#                                                                     np.abs(Delta0) / 6 * sign_phi[other_ii[0]] * (beta[where_index0] * w[other_ii[0],0] + gamma[where_index0] * w[other_ii[0],1]) +
#                                                                     (np.abs(Delta0) * sign_phi[other_ii[0]] + np.abs(Delta1) * sign_phi[other_ii[1]]) / 6 * (1-taue)* (beta[where_index0] * wkc[0] + gamma[where_index0] * wkc[1]),
#                                                                     np.abs(Delta0) / 6 * sign_phi[other_ii[0]] * (beta[where_other_ii[0]] * w[other_ii[0],0] + gamma[where_other_ii[0]] * w[other_ii[0],1]) +
#                                                                     (np.abs(Delta0) * sign_phi[other_ii[0]] + np.abs(Delta1) * sign_phi[other_ii[1]]) / 6 * (1-taue) * (beta[where_other_ii[0]] * wkc[0] + gamma[where_other_ii[0]] * wkc[1]),
#                                                                     np.abs(Delta0) / 6 * sign_phi[other_ii[0]] * (beta[where_other_ii[1]] * w[other_ii[0],0] + gamma[where_other_ii[1]] * w[other_ii[0],1]) +
#                                                                     (np.abs(Delta0) * sign_phi[other_ii[0]] + np.abs(Delta1) * sign_phi[other_ii[1]]) / 6 * (1-taue) * (beta[where_other_ii[1]] * wkc[0] + gamma[where_other_ii[1]] * wkc[1]),
#
#                                                                     np.abs(Delta1) / 6 * sign_phi[other_ii[1]] * (beta[where_index0] * w[other_ii[1],0] + gamma[where_index0] * w[other_ii[1],0]) +
#                                                                     (np.abs(Delta0) * sign_phi[other_ii[0]] + np.abs(Delta1) * sign_phi[other_ii[1]]) / 6 * taue* (beta[where_index0] * wkc[0] + gamma[where_index0] * wkc[1]),
#                                                                     np.abs(Delta0) / 6 * sign_phi[other_ii[0]] * (beta[where_other_ii[0]] * w[other_ii[1],0] + gamma[where_other_ii[0]] * w[other_ii[1],1]) +
#                                                                     (np.abs(Delta0) * sign_phi[other_ii[0]] + np.abs(Delta1) * sign_phi[other_ii[1]]) / 6 * taue * (beta[where_other_ii[0]] * wkc[0] + gamma[where_other_ii[0]] * wkc[1]),
#                                                                     np.abs(Delta0) / 6 * sign_phi[other_ii[0]] * (beta[where_other_ii[1]] * w[other_ii[1],0] + gamma[where_other_ii[1]] * w[other_ii[1],1]) +
#                                                                     (np.abs(Delta0) * sign_phi[other_ii[0]] + np.abs(Delta1) * sign_phi[other_ii[1]]) / 6 * taue * (beta[where_other_ii[1]] * wkc[0] + gamma[where_other_ii[1]] * wkc[1])
#                                                                     ))
#
#
#                            if tau == 0:
#                                I[9*(l-1):9*(l-1)+9] = np.hstack((np.ones(3, dtype = np.int) * index0, np.ones(3, dtype = np.int) * other_ii[0], np.ones(3, dtype = np.int) * other_ii[1]))
#                                J[9*(l-1):9*(l-1)+9] = np.hstack((index0, other_ii[0], other_ii[1],
#                                                                  index0, other_ii[0], other_ii[1],
#                                                                  index0, other_ii[0], other_ii[1]))
##                                M_V[9*(l-1):9*(l-1)+9] = np.abs(Delta) / 24 * (np.ones((3,3)) + np.eye(3)).reshape(9)
#                                M_V[9*(l-1):9*(l-1)+9] = np.abs(Delta) / 24 * ( (np.ones((3,3)) + np.eye(3)) +
#                                                                 diff * diff_delta * ((v_ex_trii[:,0] * np.ones((3,3)) + np.sum(v_ex_trii[:,0]) * np.ones((3,3)) ) * beta.reshape(3,1) ) +
#                                                                 diff * diff_delta * ((v_ex_trii[:,1] * np.ones((3,3)) + np.sum(v_ex_trii[:,1]) * np.ones((3,3)) ) * gamma.reshape(3,1) ) ).reshape(9)
#                                RHS[index0]       = RHS[index0]      + (np.abs(Delta0) * sign_phi[other_ii[0]] + np.abs(Delta1) * sign_phi[other_ii[1]]) / 6
#                                RHS[other_ii[0]]  = RHS[other_ii[0]] + np.abs(Delta0) / 6 * sign_phi[other_ii[0]] * (1 + (1-taue)) + np.abs(Delta1) / 6 * sign_phi[other_ii[1]] * (1-taue)
#                                RHS[other_ii[1]]  = RHS[other_ii[1]] + np.abs(Delta1) / 6 * sign_phi[other_ii[1]] * (1 + taue)     + np.abs(Delta0) / 6 * sign_phi[other_ii[0]] * taue
#                        else:
#                            print('another variant? sign_phi[trii] =', sign_phi[trii])
#
#                    l = l+1
#                if tau == 0:
#                    M = coo_matrix((M_V,(I,J)), shape = (grid.background_mesh.n,grid.background_mesh.n))
#                    M = M.tocsr()
#                    M_L = diags(np.asarray(csr_matrix.sum(M, axis = 1)).ravel(), format = 'csr')
#                    M = M.tocoo()
#
#
#                Conv = coo_matrix((Conv_V,(I,J)), shape = (grid.background_mesh.n,grid.background_mesh.n))
#                Conv = Conv.tocsr()
#
##               #TVD RK2/3 scheme
##                System = bmat([[M_L, F.T],[F, None]], format = 'csr')
##
##    #            phi1 = spla.spsolve(System, np.hstack((M_L @ phi_RK3 + dtau * (Conv @ phi_RK3 + RHS), np.zeros(N_interface))) )[0:grid.N]
##    #            phi_RK2 = spla.spsolve(System, np.hstack((M_L/2 @ (phi_RK2 + phi1) + dtau / 2 * (Conv @ phi1 + RHS), np.zeros(N_interface))) )[0:grid.N]
##
##                phi1 = spla.spsolve(System, np.hstack((M_L @ phi_RK3 + dtau * (Conv @ phi_RK3 + RHS), np.zeros(N_interface))) )[0:grid.N]
##                phi2 = spla.spsolve(System, np.hstack((M_L @ (3 * phi_RK3 / 4 + phi1 / 4) + dtau / 4 * (Conv @ phi1 + RHS), np.zeros(N_interface))) )[0:grid.N]
##                phi_RK3 = spla.spsolve(System, np.hstack((M_L @ (phi_RK3 / 3 + 2 * phi2 / 3) + 2 * dtau / 3 * (Conv @ phi2 + RHS), np.zeros(N_interface))) )[0:grid.N]
##
##    #            phi1 = spla.spsolve(M_L, M_L @ phi_RK3 + dtau * (Conv @ phi_RK3 + RHS))
##    #            phi2 = spla.spsolve(M_L, M_L @ (3 * phi_RK3 / 4 + phi1 / 4) + dtau / 4 * (Conv @ phi1 + RHS))
##    #            phi_RK3 = spla.spsolve(M_L, M_L @ (phi_RK3 / 3 + 2 * phi2 / 3) + 2 * dtau / 3 * (Conv @ phi2 + RHS))
##
##                temp_phi = phi_RK3
#
#
#
#                n = np.zeros((grid.background_mesh.n,2))
#                grad_phi = np.zeros((grid.background_mesh.n,2))
#                for j in range(0,grid.background_mesh.n):
#                    xp = grid.background_mesh.points[j]
#                    other_ii = grid.background_mesh.patch_matrix[j].nonzero()[1]
#                    other_i = grid.background_mesh.points[other_ii]
#                    hp = grid.background_mesh.patch_matrix[j, j]
#                    n_row = len(other_ii)
#
#                    eta = (other_i - xp)/hp
#                    M_eta = np.ones((n_row, 6))
#                    phi_patch = np.zeros(n_row)
#                    for i in range(0,n_row):
#                        etaj = eta[i]
#                        M_eta[i,1:6] = np.array([etaj[0], etaj[1], etaj[0]**2, etaj[0] * etaj[1], etaj[1]**2])
#                        phi_patch[i] = temp_phi[other_ii[i]]
#                    B = la.lstsq(M_eta, phi_patch)[0][[1,2]]
#                    n[j] = B/la.norm(B)
#                    grad_phi[j] = B/hp
#                w = n.copy()
#        #        sign_phi_new = np.sign(temp_phi)
#                normphi_ga = la.norm(grad_phi,axis = 1)
#                eps = la.norm(normphi_ga[band_reinit] - 1, 2)
#
#                tau = tau + dtau
#                kk = kk+1
#
#        self.phi_background = np.abs(sign_phi - np.sign(temp_phi)) * old_phi / 2 + np.abs(sign_phi + np.sign(temp_phi)) * temp_phi / 2
##        self.phi = self.phi_background.copy() #done in do_time_step
#        if which_phi == 1 and grid.background_mesh.i_ghost_fixed != -1:
#            self.phi_background[grid.background_mesh.i_ghost_fixed] = 0
#
#        if np.max(np.abs(temp_phi - self.phi_background)) > 1e-10:
#            where_max = np.argmax(np.abs(temp_phi - self.phi_background))
#            print('had to set one or more phi values back to old phi, because sign changed during reinitialisation')
#            print('where ii', (np.sign(temp_phi) - np.sign(self.phi_background)).nonzero())
#            print('max, argmax ', np.max(np.abs(temp_phi - self.phi_background)), where_max)
#            print('phi temp, background ', temp_phi[where_max], self.phi_background[where_max])
#
#        if which_phi == 1: #ga
#            grid.background_mesh.n_ga = n
#            grid.n_ga = n
#        elif which_phi == 2: #t
#            grid.n_t = n
#            grid.background_mesh.n_t = n
#        if reinit_done:
#            print(kk, tau, eps)
#
#        if plot_grad and reinit_done:
#            elev_min = np.min(normphi[where_band]-1)
#            elev_max = np.max(normphi[where_band]-1)
#            if which_phi == 1:
#                [ax53.plot(grid.enriched_mesh.points[grid.interface_gammaalpha.edges_index[i,:],0],grid.enriched_mesh.points[grid.interface_gammaalpha.edges_index[i,:],1], linewidth = 2, color = 'darkblue') for i in range(0,len(grid.interface_gammaalpha.edges))]
#                [ax53.plot(grid.enriched_mesh.points[grid.interface_gammaalpha.ghost_edges_index[i,:],0],grid.enriched_mesh.points[grid.interface_gammaalpha.ghost_edges_index[i,:],1], linewidth = 2, color = 'lightblue') for i in range(0,len(grid.interface_gammaalpha.ghost_edges_index))]
#                ax53.set_title(r'$|||\nabla \phi^{\gamma \alpha}||_2-1|$ after reinit $\tau$ = %.3f' % tau)
#            elif which_phi == 2:
#                [ax53.plot(grid.enriched_mesh.points[grid.interface_alphatheta.edges_index[i,:],0],grid.enriched_mesh.points[grid.interface_alphatheta.edges_index[i,:],1], linewidth = 2, color = 'red') for i in range(0,len(grid.interface_alphatheta.edges))]
#                [ax53.plot(grid.enriched_mesh.points[grid.interface_gammatheta.edges_index[i,:],0],grid.enriched_mesh.points[grid.interface_gammatheta.edges_index[i,:],1], linewidth = 2, color = 'green') for i in range(0,len(grid.interface_gammatheta.edges))]
#                ax53.set_title(r'$|||\nabla \phi^{\theta}||_2-1|$ after reinit $\tau$ = %.3f' % tau)
#            surf53 = ax53.tripcolor(grid.background_mesh.points[:,0], grid.background_mesh.points[:,1], grid.background_mesh.triangles_points_index, normphi-1, cmap = bwr_ga, edgecolors= 'None', clim=(elev_min, elev_max), norm = MidpointNormalize(midpoint=0,vmin=elev_min, vmax=elev_max))
#            f5.colorbar(surf53, ax = ax53)
#            ax53.set_xlim([grid.x0, grid.xN])
#            ax53.set_ylim([grid.x0, grid.xN])


#   ### Reinitialize on background mesh
    def reinitialize(self, grid, which_phi, Force_reinit = False, plot_grad = False, t_reinit = -1):
        if Force_reinit:
            reinit_bound = 0
        else:
            reinit_bound = self.FPT * 25
        if which_phi == 1: #ga
            n = grid.background_mesh.n_ga
            F = grid.background_mesh.Tau_all_ga
            N_interface = grid.background_mesh.Tau_all_ga.shape[0]

            for tri_i in range(0, grid.background_mesh.n_tri):
                trii = grid.background_mesh.triangles_points_index[tri_i]
                grid.background_mesh.triangles[tri_i]     = grid.background_mesh.points[trii]
                grid.background_mesh.area_triangle[tri_i] = cGrid2D.cGrid.area(grid.background_mesh.triangles[tri_i])
        elif which_phi == 2: #t
            n = grid.background_mesh.n_t
            F = grid.background_mesh.Tau_all_t
            N_interface = grid.background_mesh.Tau_all_t.shape[0]
            
        old_phi = self.phi_background.copy()
        sign_phi = np.sign(old_phi)
        w = n.copy()

        band = 2.5*grid.background_mesh.h_min

        where_interface_points = np.unique(F.nonzero()[1])
        find3 = find(grid.background_mesh.three_neighbours[where_interface_points])
        where_band = np.unique(find3[1])
        band_reinit = np.unique(find3[1][np.where(find3[2] <= 1)[0]])

        grad_phi = np.zeros((grid.background_mesh.n,2))
        for j in range(0,grid.background_mesh.n):
            xp = grid.background_mesh.points[j]
            other_ii = np.nonzero(grid.background_mesh.patch_matrix[j])[1]
            other_i = grid.background_mesh.points[other_ii]
            hp = grid.background_mesh.patch_matrix[j, j]
            n_row = len(other_ii)

            eta = (other_i - xp)/hp
            M_eta = np.ones((n_row, 6))
            phi_patch = np.zeros(n_row)
            for i in range(0,n_row):
                etaj = eta[i]
                M_eta[i,1:6] = np.array([etaj[0], etaj[1], etaj[0]**2, etaj[0] * etaj[1], etaj[1]**2])
                phi_patch[i] = old_phi[other_ii[i]]
            B = la.lstsq(M_eta, phi_patch)[0][[1,2]]
            grad_phi[j] = B/hp

        normphi = la.norm(grad_phi,axis = 1)

        eps = la.norm(normphi[band_reinit] - 1, 2)
        eps_old = 1e6
        reinit_done = False
        if eps < reinit_bound:
            print('Reinit not done, eps = %.3e ' %eps , 'FPT = %.3e' %self.FPT)
        else:
            reinit_done = True


        if plot_grad and reinit_done:
            plot_settings()
            f5, (ax52, ax53) = plt.subplots(1,2, figsize=(6.202*2,4.6515), sharex = True, sharey = True)
            bwr_ga = plt.cm.bwr
            elev_min = np.min(normphi[where_band]-1)
            elev_max = np.max(normphi[where_band]-1)
            if which_phi == 1:
                [ax52.plot(grid.enriched_mesh.points[grid.interface_gammaalpha.edges_index[i,:],0],grid.enriched_mesh.points[grid.interface_gammaalpha.edges_index[i,:],1], linewidth = 2, color = 'darkblue') for i in range(0,len(grid.interface_gammaalpha.edges))]
                [ax52.plot(grid.enriched_mesh.points[grid.interface_gammaalpha.ghost_edges_index[i,:],0],grid.enriched_mesh.points[grid.interface_gammaalpha.ghost_edges_index[i,:],1], linewidth = 2, color = 'lightblue') for i in range(0,len(grid.interface_gammaalpha.ghost_edges_index))]
                ax52.set_title(r'$||\nabla \phi^{\gamma \alpha}||_2-1$ before reinit at t = %.4f' %t_reinit)
            elif which_phi == 2:
                [ax52.plot(grid.enriched_mesh.points[grid.interface_alphatheta.edges_index[i,:],0],grid.enriched_mesh.points[grid.interface_alphatheta.edges_index[i,:],1], linewidth = 2, color = 'red') for i in range(0,len(grid.interface_alphatheta.edges))]
                [ax52.plot(grid.enriched_mesh.points[grid.interface_gammatheta.edges_index[i,:],0],grid.enriched_mesh.points[grid.interface_gammatheta.edges_index[i,:],1], linewidth = 2, color = 'green') for i in range(0,len(grid.interface_gammatheta.edges))]
                ax52.set_title(r'$||\nabla \phi^{\theta}||_2-1$ before reinit at t = %.4f' %t_reinit)
            surf52 = ax52.tripcolor(grid.background_mesh.points[:,0], grid.background_mesh.points[:,1], grid.background_mesh.triangles_points_index, normphi-1, cmap = bwr_ga, edgecolors= 'None', clim=(elev_min, elev_max), norm = MidpointNormalize(midpoint=0,vmin=elev_min, vmax=elev_max))
            f5.colorbar(surf52, ax = ax52)
            ax52.set_xlim([grid.x0, grid.xN])
            ax52.set_ylim([grid.x0, grid.xN])


        tau = 0
        dtau = grid.enriched_mesh.h_min / 2
#        phi_RK2 = self.phi_background.copy()
        phi_RK3 = old_phi.copy()
        temp_phi = phi_RK3.copy()
        kk = 0
        if eps > reinit_bound:
            while tau <= band: # and eps_old > eps:
                eps_old = eps
                band_done = tau
                if tau == 0:
                    print('Reinit done, eps, FPT ', 0, eps, self.FPT)
                    I = -np.ones(9*grid.background_mesh.n_tri, dtype = np.int)
                    J = -np.ones(9*grid.background_mesh.n_tri, dtype = np.int)
                    M_V = np.zeros(9*grid.background_mesh.n_tri)
                    RHS = np.zeros(grid.background_mesh.n)
                else:
                    print(kk, '%.3f' % tau, '%.3f' % eps)
                print('max/min', '%.3e' % (np.max(normphi[band_reinit] - 1)), '%.3e' % (np.min(normphi[band_reinit] - 1)))
                Conv_V = np.zeros(9*grid.background_mesh.n_tri)
                l = 1
                #To do: updating/making F could be done in this for loop at tau = 0 in stead of before reinit in do_time_step2D. This avoids making Tau_all_ga/t every timestep before reinit.
                for i in range(0,grid.background_mesh.n_tri):
                    trii  =   grid.background_mesh.triangles_points_index[i]
                    tri   =   grid.background_mesh.points[trii]
                    Delta = 2*grid.background_mesh.area_triangle[i]

                    beta = np.zeros(3)
                    beta[0] = (tri[1,1] - tri[2,1])/Delta
                    beta[1] = (tri[2,1] - tri[0,1])/Delta
                    beta[2] = (tri[0,1] - tri[1,1])/Delta
                    gamma = np.zeros(3)
                    gamma[0] = (tri[2,0] - tri[1,0])/Delta
                    gamma[1] = (tri[0,0] - tri[2,0])/Delta
                    gamma[2] = (tri[1,0] - tri[0,0])/Delta

                    sum_sign_phi = int(np.sum(sign_phi[trii]))

                    #if sign of all corners is the same integrating is easy and can be done exact:
                    if np.abs(sum_sign_phi) >= 2:
                        if tau == 0:
                            I[9*(l-1):9*(l-1)+9] = np.hstack((np.ones(3, dtype = np.int) * trii[0], np.ones(3, dtype = np.int) * trii[1], np.ones(3, dtype = np.int) * trii[2]))
                            J[9*(l-1):9*(l-1)+9] = np.hstack((trii, trii, trii))

                            M_V[9*(l-1):9*(l-1)+9] = np.abs(Delta) / 24 * (np.ones((3,3)) + np.eye(3)).reshape(9)

                            RHS[trii] = RHS[trii] + np.abs(Delta) / 6 * sign_phi[trii]

                        if sum_sign_phi <= -2:
                            Conv_V[9*(l-1):9*(l-1)+9] = np.abs(Delta) / 24 * np.array([(2*(w[trii[0],0] * beta[0] + w[trii[0],1] * gamma[0]) + w[trii[1],0] * beta[0] + w[trii[1],1] * gamma[0] + w[trii[2],0] * beta[0] + w[trii[2],1] * gamma[0]),
                                                                                       (2*(w[trii[0],0] * beta[1] + w[trii[0],1] * gamma[1]) + w[trii[1],0] * beta[1] + w[trii[1],1] * gamma[1] + w[trii[2],0] * beta[1] + w[trii[2],1] * gamma[1]),
                                                                                       (2*(w[trii[0],0] * beta[2] + w[trii[0],1] * gamma[2]) + w[trii[1],0] * beta[2] + w[trii[1],1] * gamma[2] + w[trii[2],0] * beta[2] + w[trii[2],1] * gamma[2]),
                                                                                       (2*(w[trii[1],0] * beta[0] + w[trii[1],1] * gamma[0]) + w[trii[0],0] * beta[0] + w[trii[0],1] * gamma[0] + w[trii[2],0] * beta[0] + w[trii[2],1] * gamma[0]),
                                                                                       (2*(w[trii[1],0] * beta[1] + w[trii[1],1] * gamma[1]) + w[trii[0],0] * beta[1] + w[trii[0],1] * gamma[1] + w[trii[2],0] * beta[1] + w[trii[2],1] * gamma[1]),
                                                                                       (2*(w[trii[1],0] * beta[2] + w[trii[1],1] * gamma[2]) + w[trii[0],0] * beta[2] + w[trii[0],1] * gamma[2] + w[trii[2],0] * beta[2] + w[trii[2],1] * gamma[2]),
                                                                                       (2*(w[trii[2],0] * beta[0] + w[trii[2],1] * gamma[0]) + w[trii[0],0] * beta[0] + w[trii[0],1] * gamma[0] + w[trii[1],0] * beta[0] + w[trii[1],1] * gamma[0]),
                                                                                       (2*(w[trii[2],0] * beta[1] + w[trii[2],1] * gamma[1]) + w[trii[0],0] * beta[1] + w[trii[0],1] * gamma[1] + w[trii[1],0] * beta[1] + w[trii[1],1] * gamma[1]),
                                                                                       (2*(w[trii[2],0] * beta[2] + w[trii[2],1] * gamma[2]) + w[trii[0],0] * beta[2] + w[trii[0],1] * gamma[2] + w[trii[1],0] * beta[2] + w[trii[1],1] * gamma[2])])
        #                    Conv_V[9*(l-1):9*(l-1)+9] =  np.abs(Delta) / 6 * np.array([w[trii[0],0] * beta[0] + w[trii[0],1] * gamma[0], w[trii[0],0] * beta[1] + w[trii[0],1] * gamma[1], w[trii[0],0] * beta[2] + w[trii[0],1] * gamma[2],
        #                                                                               w[trii[1],0] * beta[0] + w[trii[1],1] * gamma[0], w[trii[1],0] * beta[1] + w[trii[1],1] * gamma[1], w[trii[1],0] * beta[2] + w[trii[1],1] * gamma[2],
        #                                                                               w[trii[2],0] * beta[0] + w[trii[2],1] * gamma[0], w[trii[2],0] * beta[1] + w[trii[2],1] * gamma[1], w[trii[2],0] * beta[2] + w[trii[2],1] * gamma[2] ])
                        elif sum_sign_phi >= 2:
                            Conv_V[9*(l-1):9*(l-1)+9] = -np.abs(Delta) / 24 * np.array([(2*(w[trii[0],0] * beta[0] + w[trii[0],1] * gamma[0]) + w[trii[1],0] * beta[0] + w[trii[1],1] * gamma[0] + w[trii[2],0] * beta[0] + w[trii[2],1] * gamma[0]),
                                                                                        (2*(w[trii[0],0] * beta[1] + w[trii[0],1] * gamma[1]) + w[trii[1],0] * beta[1] + w[trii[1],1] * gamma[1] + w[trii[2],0] * beta[1] + w[trii[2],1] * gamma[1]),
                                                                                        (2*(w[trii[0],0] * beta[2] + w[trii[0],1] * gamma[2]) + w[trii[1],0] * beta[2] + w[trii[1],1] * gamma[2] + w[trii[2],0] * beta[2] + w[trii[2],1] * gamma[2]),
                                                                                        (2*(w[trii[1],0] * beta[0] + w[trii[1],1] * gamma[0]) + w[trii[0],0] * beta[0] + w[trii[0],1] * gamma[0] + w[trii[2],0] * beta[0] + w[trii[2],1] * gamma[0]),
                                                                                        (2*(w[trii[1],0] * beta[1] + w[trii[1],1] * gamma[1]) + w[trii[0],0] * beta[1] + w[trii[0],1] * gamma[1] + w[trii[2],0] * beta[1] + w[trii[2],1] * gamma[1]),
                                                                                        (2*(w[trii[1],0] * beta[2] + w[trii[1],1] * gamma[2]) + w[trii[0],0] * beta[2] + w[trii[0],1] * gamma[2] + w[trii[2],0] * beta[2] + w[trii[2],1] * gamma[2]),
                                                                                        (2*(w[trii[2],0] * beta[0] + w[trii[2],1] * gamma[0]) + w[trii[0],0] * beta[0] + w[trii[0],1] * gamma[0] + w[trii[1],0] * beta[0] + w[trii[1],1] * gamma[0]),
                                                                                        (2*(w[trii[2],0] * beta[1] + w[trii[2],1] * gamma[1]) + w[trii[0],0] * beta[1] + w[trii[0],1] * gamma[1] + w[trii[1],0] * beta[1] + w[trii[1],1] * gamma[1]),
                                                                                        (2*(w[trii[2],0] * beta[2] + w[trii[2],1] * gamma[2]) + w[trii[0],0] * beta[2] + w[trii[0],1] * gamma[2] + w[trii[1],0] * beta[2] + w[trii[1],1] * gamma[2])])
        #                    Conv_V[9*(l-1):9*(l-1)+9] = - np.abs(Delta) / 6 * np.array([w[trii[0],0] * beta[0] + w[trii[0],1] * gamma[0], w[trii[0],0] * beta[1] + w[trii[0],1] * gamma[1], w[trii[0],0] * beta[2] + w[trii[0],1] * gamma[2],
        #                                                                                w[trii[1],0] * beta[0] + w[trii[1],1] * gamma[0], w[trii[1],0] * beta[1] + w[trii[1],1] * gamma[1], w[trii[1],0] * beta[2] + w[trii[1],1] * gamma[2],
        #                                                                                w[trii[2],0] * beta[0] + w[trii[2],1] * gamma[0], w[trii[2],0] * beta[1] + w[trii[2],1] * gamma[1], w[trii[2],0] * beta[2] + w[trii[2],1] * gamma[2] ])
                        else:
                            print('no sign? Conv', sign_phi[trii])
                    #else we need to splits the triangle in the positive and negative part
                    else:
                        if np.abs(sum_sign_phi) == 1:
                            where_one_minplus, = np.where(sign_phi[trii] == -sum_sign_phi)[0]
                            index_cutoff = trii[where_one_minplus]
                            cut_off = grid.background_mesh.points[index_cutoff]
                            other_ii = np.setdiff1d(trii, index_cutoff)
                            where_other_ii = -np.ones(2, dtype = np.int)
                            where_other_ii[0], = np.where(trii == other_ii[0])
                            where_other_ii[1], = np.where(trii == other_ii[1])
                            other = grid.background_mesh.points[other_ii]


                            taue = np.zeros(2)
                            pe = np.zeros((2,2))
                            wkc = np.zeros((2,2))
                            for k in range(0,2):
                                #make sure the cut off point is set as first in the edge, so 1-taue is its weight.
                                edgeii = np.hstack((index_cutoff, other_ii[k]))
                                taue[k] = -old_phi[edgeii[0]]/(old_phi[edgeii[1]] - old_phi[edgeii[0]])
                                pe[k]  = (1-taue[k])*cut_off + taue[k]*other[k]
                                wkc[k] = (1-taue[k])*w[index_cutoff] + taue[k]*w[other_ii[k]]


                            tri0 = np.vstack((cut_off, pe[0], pe[1]))
                            tri1 = np.vstack((other[0], pe[0], pe[1]))
                            tri2 = np.vstack((other[0], other[1], pe[1]))
                            Delta0 = 2*cGrid2D.cGrid.area(tri0)
                            Delta1 = 2*cGrid2D.cGrid.area(tri1)
                            Delta2 = 2*cGrid2D.cGrid.area(tri2)

                            #sign phi(cutoff) = - phi(other_ii[0/1])
                            Conv_V[9*(l-1):9*(l-1)+9] = - sign_phi[index_cutoff] * np.hstack((np.abs(Delta0) / 6 *(beta[where_one_minplus] * (w[index_cutoff,0] + (1-taue[0])*wkc[0,0] + (1-taue[1])*wkc[1,0]) + gamma[where_one_minplus] * (w[index_cutoff,1] + (1-taue[0])*wkc[0,1] + (1-taue[1])*wkc[1,1])) -
                                                                                              (np.abs(Delta1)+np.abs(Delta2)) / 8 * (beta[where_one_minplus] * ((1-taue[0])  *wkc[0,0] + (1-taue[1])*wkc[1,0]) + gamma[where_one_minplus] * ((1-taue[0]) * wkc[0,1] + (1-taue[1])*wkc[1,1])),
                                                                                              np.abs(Delta0) / 6 *(beta[where_other_ii[0]] * (w[index_cutoff,0] + (1-taue[0])*wkc[0,0] + (1-taue[1])*wkc[1,0]) + gamma[where_other_ii[0]] * (w[index_cutoff,1] + (1-taue[0])*wkc[0,1] + (1-taue[1])*wkc[1,1])) -
                                                                                              (np.abs(Delta1)+np.abs(Delta2)) / 8 * (beta[where_other_ii[0]] * ((1-taue[0])  *wkc[0,0] + (1-taue[1])*wkc[1,0]) + gamma[where_other_ii[0]] * ((1-taue[0]) * wkc[0,1] + (1-taue[1])*wkc[1,1])),
                                                                                              np.abs(Delta0) / 6 *(beta[where_other_ii[1]] * (w[index_cutoff,0] + (1-taue[0])*wkc[0,0] + (1-taue[1])*wkc[1,0]) + gamma[where_other_ii[1]] * (w[index_cutoff,1] + (1-taue[0])*wkc[0,1] + (1-taue[1])*wkc[1,1])) -
                                                                                              (np.abs(Delta1)+np.abs(Delta2)) / 8 * (beta[where_other_ii[1]] * ((1-taue[0])  *wkc[0,0] + (1-taue[1])*wkc[1,0]) + gamma[where_other_ii[1]] * ((1-taue[0]) * wkc[0,1] + (1-taue[1])*wkc[1,1])),

                                                                                              np.abs(Delta0) / 6 * taue[0] *        (beta[where_one_minplus] * wkc[0,0] + gamma[where_one_minplus] * wkc[0,1]) -
                                                                                              (np.abs(Delta1)+np.abs(Delta2)) / 8 * (beta[where_one_minplus] * (w[other_ii[0],0] + taue[0] * wkc[0,0]) + gamma[where_one_minplus] * (w[other_ii[0],1] + taue[0] * wkc[0,1])),
                                                                                              np.abs(Delta0) / 6 * taue[0] *        (beta[where_other_ii[0]] * wkc[0,0] + gamma[where_other_ii[0]] * wkc[0,1]) -
                                                                                              (np.abs(Delta1)+np.abs(Delta2)) / 8 * (beta[where_other_ii[0]] * (w[other_ii[0],0] + taue[0] * wkc[0,0]) + gamma[where_other_ii[0]] * (w[other_ii[0],1] + taue[0] * wkc[0,1])),
                                                                                              np.abs(Delta0) / 6 * taue[0] *        (beta[where_other_ii[1]] * wkc[0,0] + gamma[where_other_ii[1]] * wkc[0,1]) -
                                                                                              (np.abs(Delta1)+np.abs(Delta2)) / 8 * (beta[where_other_ii[1]] * (w[other_ii[0],0] + taue[0] * wkc[0,0]) + gamma[where_other_ii[1]] * (w[other_ii[0],1] + taue[0] * wkc[0,1])),

                                                                                              np.abs(Delta0) / 6 * taue[1] *        (beta[where_one_minplus] * wkc[1,0] + gamma[where_one_minplus] * wkc[1,1]) -
                                                                                              (np.abs(Delta1)+np.abs(Delta2)) / 8 * (beta[where_one_minplus] * (w[other_ii[1],0] + taue[1] * wkc[1,0]) + gamma[where_one_minplus] * (w[other_ii[1],1] + taue[1] * wkc[1,1])),
                                                                                              np.abs(Delta0) / 6 * taue[1] *        (beta[where_other_ii[0]] * wkc[1,0] + gamma[where_other_ii[0]] * wkc[1,1]) -
                                                                                              (np.abs(Delta1)+np.abs(Delta2)) / 8 * (beta[where_other_ii[0]] * (w[other_ii[1],0] + taue[1] * wkc[1,0]) + gamma[where_other_ii[0]] * (w[other_ii[1],1] + taue[1] * wkc[1,1])),
                                                                                              np.abs(Delta0) / 6 * taue[1] *        (beta[where_other_ii[1]] * wkc[1,0] + gamma[where_other_ii[1]] * wkc[1,1]) -
                                                                                              (np.abs(Delta1)+np.abs(Delta2)) / 8 * (beta[where_other_ii[1]] * (w[other_ii[1],0] + taue[1] * wkc[1,0]) + gamma[where_other_ii[1]] * (w[other_ii[1],1] + taue[1] * wkc[1,1]))
                                                                                              ))
                            if tau == 0:
                                I[9*(l-1):9*(l-1)+9] = np.hstack((np.ones(3, dtype = np.int) * index_cutoff, np.ones(3, dtype = np.int) * other_ii[0], np.ones(3, dtype = np.int) * other_ii[1]))
                                J[9*(l-1):9*(l-1)+9] = np.hstack((index_cutoff, other_ii[0], other_ii[1],
                                                                  index_cutoff, other_ii[0], other_ii[1],
                                                                  index_cutoff, other_ii[0], other_ii[1]))
                                M_V[9*(l-1):9*(l-1)+9] = np.abs(Delta) / 24 * (np.ones((3,3)) + np.eye(3)).reshape(9)

                                RHS[index_cutoff] = RHS[index_cutoff] + sign_phi[index_cutoff] * (np.abs(Delta0) / 6 * (3 - taue[0] - taue[1]) - (np.abs(Delta1) + np.abs(Delta2)) / 8 * (2 - taue[0] - taue[1]) )
                                RHS[other_ii[0]]  = RHS[other_ii[0]]  + sign_phi[index_cutoff] * (np.abs(Delta0) / 6 * taue[0] - (np.abs(Delta1) + np.abs(Delta2)) / 8 * (1 + taue[0]))
                                RHS[other_ii[1]]  = RHS[other_ii[1]]  + sign_phi[index_cutoff] * (np.abs(Delta0) / 6 * taue[1] - (np.abs(Delta1) + np.abs(Delta2)) / 8 * (1 + taue[1]))
                            if np.abs(np.abs(Delta) - np.abs(Delta0) - np.abs(Delta1) - np.abs(Delta2)) > 1e-11:
                                print('Made the wrong triangles in reinitialize', i)
                            if np.setdiff1d(trii, np.hstack([index_cutoff, other_ii])).shape[0] != 0:
                                print('wrong indices?', trii, np.hstack([index_cutoff, other_ii]))
                            if np.setdiff1d(np.arange(0,3), np.hstack([where_one_minplus, where_other_ii])).shape[0] != 0:
                                print('wrong indices?', trii, np.hstack([index_cutoff, other_ii]), np.hstack([where_one_minplus, where_other_ii]))

                        elif np.abs(sum_sign_phi)== 0:
                            where_index0, = np.where(sign_phi[trii] == 0)[0]
                            index0 = trii[where_index0]
                            point0 = grid.background_mesh.points[index0]
                            other_ii = np.setdiff1d(trii, index0)
                            where_other_ii = -np.ones(2, dtype = np.int)
                            where_other_ii[0], = np.where(trii == other_ii[0])
                            where_other_ii[1], = np.where(trii == other_ii[1])
                            other = grid.background_mesh.points[other_ii]

                            taue = -old_phi[other_ii[0]]/(old_phi[other_ii[1]] - old_phi[other_ii[0]])
                            pe   = (1-taue)*other[0] + taue*other[1]
                            wkc  = (1-taue)*w[other_ii[0]] + taue*w[other_ii[1]]

                            tri0 = np.vstack((point0, pe, other[0]))
                            tri1 = np.vstack((point0, pe, other[1]))
                            Delta0 = 2*cGrid2D.cGrid.area(tri0)
                            Delta1 = 2*cGrid2D.cGrid.area(tri1)

                            Conv_V[9*(l-1):9*(l-1)+9] = - np.hstack(((np.abs(Delta0) / 6 * sign_phi[other_ii[0]] + np.abs(Delta1) / 6 * sign_phi[other_ii[1]]) * (beta[where_index0] * w[index0,0] + gamma[where_index0] * w[index0,1]),
                                                                     (np.abs(Delta0) / 6 * sign_phi[other_ii[0]] + np.abs(Delta1) / 6 * sign_phi[other_ii[1]]) * (beta[where_other_ii[0]] * w[other_ii[0],0] + gamma[where_other_ii[0]] * w[other_ii[0],1]),
                                                                     (np.abs(Delta0) / 6 * sign_phi[other_ii[0]] + np.abs(Delta1) / 6 * sign_phi[other_ii[1]]) * (beta[where_other_ii[1]] * w[other_ii[1],0] + gamma[where_other_ii[1]] * w[other_ii[1],1]),

                                                                     np.abs(Delta0) / 6 * sign_phi[other_ii[0]] * (beta[where_index0] * w[other_ii[0],0] + gamma[where_index0] * w[other_ii[0],1]) +
                                                                     (np.abs(Delta0) * sign_phi[other_ii[0]] + np.abs(Delta1) * sign_phi[other_ii[1]]) / 6 * (1-taue)* (beta[where_index0] * wkc[0] + gamma[where_index0] * wkc[1]),
                                                                     np.abs(Delta0) / 6 * sign_phi[other_ii[0]] * (beta[where_other_ii[0]] * w[other_ii[0],0] + gamma[where_other_ii[0]] * w[other_ii[0],1]) +
                                                                     (np.abs(Delta0) * sign_phi[other_ii[0]] + np.abs(Delta1) * sign_phi[other_ii[1]]) / 6 * (1-taue) * (beta[where_other_ii[0]] * wkc[0] + gamma[where_other_ii[0]] * wkc[1]),
                                                                     np.abs(Delta0) / 6 * sign_phi[other_ii[0]] * (beta[where_other_ii[1]] * w[other_ii[0],0] + gamma[where_other_ii[1]] * w[other_ii[0],1]) +
                                                                     (np.abs(Delta0) * sign_phi[other_ii[0]] + np.abs(Delta1) * sign_phi[other_ii[1]]) / 6 * (1-taue) * (beta[where_other_ii[1]] * wkc[0] + gamma[where_other_ii[1]] * wkc[1]),

                                                                     np.abs(Delta1) / 6 * sign_phi[other_ii[1]] * (beta[where_index0] * w[other_ii[1],0] + gamma[where_index0] * w[other_ii[1],0]) +
                                                                     (np.abs(Delta0) * sign_phi[other_ii[0]] + np.abs(Delta1) * sign_phi[other_ii[1]]) / 6 * taue* (beta[where_index0] * wkc[0] + gamma[where_index0] * wkc[1]),
                                                                     np.abs(Delta0) / 6 * sign_phi[other_ii[0]] * (beta[where_other_ii[0]] * w[other_ii[1],0] + gamma[where_other_ii[0]] * w[other_ii[1],1]) +
                                                                     (np.abs(Delta0) * sign_phi[other_ii[0]] + np.abs(Delta1) * sign_phi[other_ii[1]]) / 6 * taue * (beta[where_other_ii[0]] * wkc[0] + gamma[where_other_ii[0]] * wkc[1]),
                                                                     np.abs(Delta0) / 6 * sign_phi[other_ii[0]] * (beta[where_other_ii[1]] * w[other_ii[1],0] + gamma[where_other_ii[1]] * w[other_ii[1],1]) +
                                                                     (np.abs(Delta0) * sign_phi[other_ii[0]] + np.abs(Delta1) * sign_phi[other_ii[1]]) / 6 * taue * (beta[where_other_ii[1]] * wkc[0] + gamma[where_other_ii[1]] * wkc[1])
                                                                     ))


                            if tau == 0:
                                I[9*(l-1):9*(l-1)+9] = np.hstack((np.ones(3, dtype = np.int) * index0, np.ones(3, dtype = np.int) * other_ii[0], np.ones(3, dtype = np.int) * other_ii[1]))
                                J[9*(l-1):9*(l-1)+9] = np.hstack((index0, other_ii[0], other_ii[1],
                                                                  index0, other_ii[0], other_ii[1],
                                                                  index0, other_ii[0], other_ii[1]))
                                M_V[9*(l-1):9*(l-1)+9] = np.abs(Delta) / 24 * (np.ones((3,3)) + np.eye(3)).reshape(9)

                                RHS[index0]       = RHS[index0]      + (np.abs(Delta0) * sign_phi[other_ii[0]] + np.abs(Delta1) * sign_phi[other_ii[1]]) / 6
                                RHS[other_ii[0]]  = RHS[other_ii[0]] + np.abs(Delta0) / 6 * sign_phi[other_ii[0]] * (1 + (1-taue)) + np.abs(Delta1) / 6 * sign_phi[other_ii[1]] * (1-taue)
                                RHS[other_ii[1]]  = RHS[other_ii[1]] + np.abs(Delta1) / 6 * sign_phi[other_ii[1]] * (1 + taue)     + np.abs(Delta0) / 6 * sign_phi[other_ii[0]] * taue
                        else:
                            print('another variant? sign_phi[trii] =', sign_phi[trii])

                    l = l+1
                if tau == 0:
                    M = coo_matrix((M_V,(I,J)), shape = (grid.background_mesh.n,grid.background_mesh.n))
                    M = M.tocsr()
                    M_L = diags(np.asarray(csr_matrix.sum(M, axis = 1)).ravel(), format = 'csr')
                    M = M.tocoo()


                Conv = coo_matrix((Conv_V,(I,J)), shape = (grid.background_mesh.n,grid.background_mesh.n))
                Conv = Conv.tocsr()

               #TVD RK2/3 scheme with or without least square approach to combine dirichlet condition
                System = bmat([[M_L, F.T],[F, None]], format = 'csr')
#
#    #            phi1 = spla.spsolve(System, np.hstack((M_L @ phi_RK3 + dtau * (Conv @ phi_RK3 + RHS), np.zeros(N_interface))) )[0:grid.N]
#    #            phi_RK2 = spla.spsolve(System, np.hstack((M_L/2 @ (phi_RK2 + phi1) + dtau / 2 * (Conv @ phi1 + RHS), np.zeros(N_interface))) )[0:grid.N]
#
                phi1 = spla.spsolve(System, np.hstack((M_L @ temp_phi + dtau * (Conv @ temp_phi + RHS), np.zeros(N_interface))) )[0:grid.N]
                phi2 = spla.spsolve(System, np.hstack((M_L @ (3 * temp_phi / 4 + phi1 / 4) + dtau / 4 * (Conv @ phi1 + RHS), np.zeros(N_interface))) )[0:grid.N]
                phi_RK3 = spla.spsolve(System, np.hstack((M_L @ (temp_phi / 3 + 2 * phi2 / 3) + 2 * dtau / 3 * (Conv @ phi2 + RHS), np.zeros(N_interface))) )[0:grid.N]
#
#    #            phi1 = spla.spsolve(M_L, M_L @ phi_RK3 + dtau * (Conv @ phi_RK3 + RHS))
#    #            phi2 = spla.spsolve(M_L, M_L @ (3 * phi_RK3 / 4 + phi1 / 4) + dtau / 4 * (Conv @ phi1 + RHS))
#    #            phi_RK3 = spla.spsolve(M_L, M_L @ (phi_RK3 / 3 + 2 * phi2 / 3) + 2 * dtau / 3 * (Conv @ phi2 + RHS))
#
                temp_phi = phi_RK3

#                #TVD RK2/3 scheme
#                System = vstack((M_L, F), format = 'csr')
#                phi1 = spla.lsqr(System, np.hstack((M_L @ temp_phi + dtau * (Conv @ temp_phi + RHS), np.zeros(N_interface))) )[0]
#                phi2 = spla.lsqr(System, np.hstack((M_L @ (3 * temp_phi / 4 + phi1 / 4) + dtau / 4 * (Conv @ phi1 + RHS), np.zeros(N_interface))) )[0]
#                phi_RK3 = spla.lsqr(System, np.hstack((M_L @ (temp_phi / 3 + 2 * phi2 / 3) + 2 * dtau / 3 * (Conv @ phi2 + RHS), np.zeros(N_interface))) )[0]
#
#                temp_phi = phi_RK3.copy()



                n = np.zeros((grid.background_mesh.n,2))
                grad_phi = np.zeros((grid.background_mesh.n,2))
                for j in range(0,grid.background_mesh.n):
                    xp = grid.background_mesh.points[j]
                    other_ii = grid.background_mesh.patch_matrix[j].nonzero()[1]
                    other_i = grid.background_mesh.points[other_ii]
                    hp = grid.background_mesh.patch_matrix[j, j]
                    n_row = len(other_ii)

                    eta = (other_i - xp)/hp
                    M_eta = np.ones((n_row, 6))
                    phi_patch = np.zeros(n_row)
                    for i in range(0,n_row):
                        etaj = eta[i]
                        M_eta[i,1:6] = np.array([etaj[0], etaj[1], etaj[0]**2, etaj[0] * etaj[1], etaj[1]**2])
                        phi_patch[i] = temp_phi[other_ii[i]]
                    B = la.lstsq(M_eta, phi_patch)[0][[1,2]]
                    n[j] = B/la.norm(B)
                    grad_phi[j] = B/hp
                w = n.copy()
        #        sign_phi_new = np.sign(temp_phi)
                normphi = la.norm(grad_phi,axis = 1)
                eps = la.norm(normphi[band_reinit] - 1, 2)

                tau = tau + dtau
                kk = kk+1

#        self.phi_background = np.abs(sign_phi - np.sign(temp_phi)) * old_phi / 2 + np.abs(sign_phi + np.sign(temp_phi)) * temp_phi / 2
#        self.phi = self.phi_background.copy() #done in do_time_step
        if which_phi == 1 and grid.background_mesh.i_ghost_fixed != -1:
#            self.phi_background[grid.background_mesh.i_ghost_fixed] = 0
            temp_phi[grid.background_mesh.i_ghost_fixed] = 0

        self.phi_background = temp_phi

        ### Sign has to remain the same in reinitialization. Because this method fails, this does not always happen. 
        set_sign =  np.abs(sign_phi - np.sign(temp_phi)) * old_phi / 2 + np.abs(sign_phi + np.sign(temp_phi)) * temp_phi / 2
        where_ii = []
        if np.max(np.abs(temp_phi - set_sign)) > 1e-10:
            where_max = np.argmax(np.abs(temp_phi - set_sign))
            print('should to set one or more phi values back to old phi, because sign changed during reinitialisation')
            where_ii = (np.sign(temp_phi) - np.sign(set_sign)).nonzero()
            print('where ii', where_ii)
            print('max, argmax ', np.max(np.abs(temp_phi - set_sign)), where_max)
            print('phi temp, background ', temp_phi[where_max], set_sign[where_max])
            n = np.zeros((grid.background_mesh.n,2))
            grad_phi = np.zeros((grid.background_mesh.n,2))
            for j in range(0,grid.background_mesh.n):
                xp = grid.background_mesh.points[j]
                other_ii = grid.background_mesh.patch_matrix[j].nonzero()[1]
                other_i = grid.background_mesh.points[other_ii]
                hp = grid.background_mesh.patch_matrix[j, j]
                n_row = len(other_ii)

                eta = (other_i - xp)/hp
                M_eta = np.ones((n_row, 6))
                phi_patch = np.zeros(n_row)
                for i in range(0,n_row):
                    etaj = eta[i]
                    M_eta[i,1:6] = np.array([etaj[0], etaj[1], etaj[0]**2, etaj[0] * etaj[1], etaj[1]**2])
                    phi_patch[i] = temp_phi[other_ii[i]]
                B = la.lstsq(M_eta, phi_patch)[0][[1,2]]
                n[j] = B/la.norm(B)
                grad_phi[j] = B/hp
            normphi = la.norm(grad_phi,axis = 1)

        if which_phi == 1: #ga
            grid.background_mesh.n_ga = n
#            grid.n_ga = n
        elif which_phi == 2: #t
            grid.background_mesh.n_t = n
#            grid.n_t = n
        if reinit_done:
            print(kk, tau, eps)

        if plot_grad and reinit_done:
#            elev_min = np.min(normphi[where_band]-1)
#            elev_max = np.max(normphi[where_band]-1)
            if which_phi == 1:
                [ax53.plot(grid.enriched_mesh.points[grid.interface_gammaalpha.edges_index[i,:],0],grid.enriched_mesh.points[grid.interface_gammaalpha.edges_index[i,:],1], linewidth = 2, color = 'darkblue') for i in range(0,len(grid.interface_gammaalpha.edges))]
                [ax53.plot(grid.enriched_mesh.points[grid.interface_gammaalpha.ghost_edges_index[i,:],0],grid.enriched_mesh.points[grid.interface_gammaalpha.ghost_edges_index[i,:],1], linewidth = 2, color = 'lightblue') for i in range(0,len(grid.interface_gammaalpha.ghost_edges_index))]
                ax53.plot(grid.background_mesh.points[where_ii,0], grid.background_mesh.points[where_ii,1], 'ok')
                ax53.set_title(r'$||\nabla \phi^{\gamma \alpha}||_2-1$ after reinit $\tau$ = %.3f' % tau)
            elif which_phi == 2:
                [ax53.plot(grid.enriched_mesh.points[grid.interface_alphatheta.edges_index[i,:],0],grid.enriched_mesh.points[grid.interface_alphatheta.edges_index[i,:],1], linewidth = 2, color = 'red') for i in range(0,len(grid.interface_alphatheta.edges))]
                [ax53.plot(grid.enriched_mesh.points[grid.interface_gammatheta.edges_index[i,:],0],grid.enriched_mesh.points[grid.interface_gammatheta.edges_index[i,:],1], linewidth = 2, color = 'green') for i in range(0,len(grid.interface_gammatheta.edges))]
                ax53.set_title(r'$||\nabla \phi^{\theta}||_2-1$ after reinit $\tau$ = %.3f' % tau)
            surf53 = ax53.tripcolor(grid.background_mesh.points[:,0], grid.background_mesh.points[:,1], grid.background_mesh.triangles_points_index, normphi-1, cmap = bwr_ga, edgecolors= 'None', clim=(elev_min, elev_max), norm = MidpointNormalize(midpoint=0,vmin=elev_min, vmax=elev_max))
            f5.colorbar(surf53, ax = ax53)
            ax53.set_xlim([grid.x0+6, grid.xN-6])
            ax53.set_ylim([grid.y0+6, grid.yN-6])
            plt.savefig('Results/Thesis_plots/Reinit_fixed%.f.jpg' %t_reinit, format='jpg',dpi=200)
            plt.close(f5)
#        if which_phi == 1:
        if reinit_done:
            return True
        else:
            return False


    ### Curvature on background mesh as advised in Dennis' phd work, commented part below is the other approach
    def curvature(self, grid, xii, which_phi):
        if self.d == 1:
            return 0
        elif self.d == 2:
            if which_phi == 1:
                n = grid.n_ga
            elif which_phi == 2:
                n = grid.n_t
            if xii > grid.N or grid.has_point_been_shifted[xii,1]%4 == 0:
                xp = grid.enriched_mesh.points[xii]
                find3 = find(grid.enriched_mesh.three_neighbours[xii])
                other_ii = np.unique(find3[1][np.where(find3[2] <= 2)[0]])
                other_i = grid.enriched_mesh.points[other_ii]
                hp = grid.enriched_mesh.patch_matrix[xii, xii]
                n_row = len(other_ii)

                eta = (other_i - xp)/hp
                M_eta = np.ones((n_row, 6))
                nx_patch = np.zeros(n_row)
                ny_patch = np.zeros(n_row)
                for i in range(0,n_row):
                    etaj = eta[i]
                    M_eta[i,1:6] = np.array([etaj[0], etaj[1], etaj[0]**2, etaj[0] * etaj[1], etaj[1]**2])
                    nx_patch[i] = n[other_ii[i],0]
                    ny_patch[i] = n[other_ii[i],1]
                #double gradient approx (grad . grad phi)
    #            solve = la.lstsq(M_eta, phi_patch)[0] #phi_patch = n_patch
    #            B = solve[[1,2]]
    #            C = np.zeros((2,2))
    #            C[0] = solve[3]
    #            C[1,0] = solve[4]
    #            C[0,1] = solve[4]
    #            C[1,1] = solve[5]
    #            return 2 * B.T @ (C - np.trace(C) * np.eye(2)) @ B / (hp * la.norm(B,2)**3)
                #single gradient approx (grad n)
            else:
                xp = grid.background_mesh.points[xii]
                find3 = find(grid.background_mesh.three_neighbours[xii])
                other_ii = np.unique(find3[1][np.where(find3[2] <= 2)[0]])
                other_i = grid.background_mesh.points[other_ii]
                hp = grid.background_mesh.patch_matrix[xii, xii]
                n_row = len(other_ii)

                eta = (other_i - xp)/hp
                M_eta = np.ones((n_row, 6))
                nx_patch = np.zeros(n_row)
                ny_patch = np.zeros(n_row)
                for i in range(0,n_row):
                    etaj = eta[i]
                    M_eta[i,1:6] = np.array([etaj[0], etaj[1], etaj[0]**2, etaj[0] * etaj[1], etaj[1]**2])
                    nx_patch[i] = n[other_ii[i],0]
                    ny_patch[i] = n[other_ii[i],1]
            return -(la.lstsq(M_eta, nx_patch)[0][1] + la.lstsq(M_eta, ny_patch)[0][2])/hp


    ### Curvature on enriched mesh as advised in Dennis' phd work, commented part below is the other approach
    def curvature_enr(self, grid, xii, which_phi):
        if self.d == 1:
            return 0
        elif self.d == 2:
            if which_phi == 1:
                n = grid.n_ga
            elif which_phi == 2:
                n = grid.n_t
            xp = grid.enriched_mesh.points[xii]
            find3 = find(grid.enriched_mesh.three_neighbours[xii])
            other_ii = np.unique(find3[1][np.where(find3[2] <= 2)[0]])
            other_i = grid.enriched_mesh.points[other_ii]
            hp = grid.enriched_mesh.patch_matrix[xii, xii]
            n_row = len(other_ii)

            eta = (other_i - xp)/hp
            M_eta = np.ones((n_row, 6))
            nx_patch = np.zeros(n_row)
            ny_patch = np.zeros(n_row)
            for i in range(0,n_row):
                etaj = eta[i]
                M_eta[i,1:6] = np.array([etaj[0], etaj[1], etaj[0]**2, etaj[0] * etaj[1], etaj[1]**2])
                nx_patch[i] = n[other_ii[i],0]
                ny_patch[i] = n[other_ii[i],1]
            #double gradient approx (grad . grad phi)
#            solve = la.lstsq(M_eta, phi_patch)[0] #phi_patch = n_patch, SET n = phi
#            B = solve[[1,2]]
#            C = np.zeros((2,2))
#            C[0] = solve[3]
#            C[1,0] = solve[4]
#            C[0,1] = solve[4]
#            C[1,1] = solve[5]
#            return 2 * B.T @ (C - np.trace(C) * np.eye(2)) @ B / (hp * la.norm(B,2)**3)
            #single gradient approx (grad n)
            return -(la.lstsq(M_eta, nx_patch)[0][1] + la.lstsq(M_eta, ny_patch)[0][2])/hp


    def interpolate(self, trii, tri, p):
        p0 = tri[0]
        p1 = tri[1]
        p2 = tri[2]
        Delta = ((p1[0] - p0[0])*(p2[1] - p0[1]) - (p1[1] - p0[1])*(p2[0] - p0[0]))

        b0 = (p1[1] - p2[1])/Delta
        b1 = (p2[1] - p0[1])/Delta
        b2 = (p0[1] - p1[1])/Delta

        g0 = (p2[0] - p1[0])/Delta
        g1 = (p0[0] - p2[0])/Delta
        g2 = (p1[0] - p0[0])/Delta

        a0 = 1 - b0*p0[0] - g0*p0[1]
        a1 = 1 - b1*p1[0] - g1*p1[1]
        a2 = 1 - b2*p2[0] - g2*p2[1]

        psi = np.zeros(3)
        psi[0] = a0 + b0*p[0] + g0*p[1]
        psi[1] = a1 + b1*p[0] + g1*p[1]
        psi[2] = a2 + b2*p[0] + g2*p[1]

        if any(psi > 1) or any(psi < 0):
            where_bad1, = np.where(psi > 1)
            where_bad0, = np.where(psi < 0)
            if any(psi[where_bad1]-1 > 1e-12) or any(np.abs(psi[where_bad0]) > 1e-12):
                print('interpolate phi outside of this triangle, ',trii , 'psi ', psi)

        return self.phi[trii] @ psi


    def FPT_value(self, grid):
        band = 5*grid.background_mesh.h_min
        band_reinit, = np.where(np.abs(self.phi_background) < band/2)
        grad_phi = np.zeros((grid.background_mesh.n,2))
        for j in range(0,grid.background_mesh.n):
            xp = grid.background_mesh.points[j]
            other_ii = np.nonzero(grid.background_mesh.patch_matrix[j])[1]
            other_i = grid.background_mesh.points[other_ii]
            hp = grid.background_mesh.patch_matrix[j, j]
            n_row = len(other_ii)

            eta = (other_i - xp)/hp
            M_eta = np.ones((n_row, 6))
            phi_patch = np.zeros(n_row)
            for i in range(0,n_row):
                etaj = eta[i]
                M_eta[i,1:6] = np.array([etaj[0], etaj[1], etaj[0]**2, etaj[0] * etaj[1], etaj[1]**2])
                phi_patch[i] = self.phi_background[other_ii[i]]
            B = la.lstsq(M_eta, phi_patch)[0][[1,2]]
            grad_phi[j] = B/hp

        self.FPT = la.norm(la.norm(grad_phi[band_reinit],2,axis = 1) - 1, 2)


    ### Function that approaches/calculates the distance from a point to an ellipse
    @staticmethod
    def DistancePointEllipse(e0, e1, y0, y1):
        distance = -1
        if (y1 > 0):
            if(y0 > 0):
               # Compute the unique roottbarof F(t) on (-e1e1 , + inf)
                def F(t):
                    return (e0*y0/(t+e0**2))**2 + (e1*y1/(t+e1**2))**2 - 1
                t0 = -e1**2 + e1*y1
                t1 = -e1**2 + np.sqrt(e0**2 * y0**2 + e1**2 * y1**2)
                if np.sum(np.sign([F(t0),F(t1)])) != 0:
                    print('not different signs F(t0), F(t1), t0, t1, e0, e1, y0, y1', F(t0), F(t1),t0, t1,  e0, e1, y0, y1)
                tbar = optimize.bisect(F, t0, t1)
                x0 = e0*e0*y0 / (tbar + e0*e0)
                x1 = e1*e1*y1 / (tbar + e1*e1)
                distance =  np.sqrt((x0 - y0)*(x0 - y0) + (x1 - y1)*(x1 - y1))
            else: # y0 == 0
                x0 = 0
                x1 = e1
                distance= np.abs(y1 - e1)
        else: # y1 == 0
            if y0 < (e0*e0 - e1*e1) / e0:
                x0 = e0*e0*y0 / (e0*e0 - e1*e1)
                x1 = e1*np.sqrt(1 - (x0/e0)*(x0/e0))
                distance = np.sqrt((x0 - y0)*(x0 - y0) + x1*x1)
            else:
                x0 = e0
                x1 = 0
                distance= np.abs(y0 - e0)
        return distance



