import taichi as ti
from taichi.linalg import MatrixFreeCG, LinearOperator
ti.init(arch=ti.cpu)
q = 0.5
n_particles = int(8192 * q**2)
n_grid = int(128 * q)
dx, inv_dx = 1 / n_grid, float(n_grid)
dt = 5e-5

p_rho = 1
p_vol = (dx * 0.5) ** 2
p_mass = p_vol * p_rho
gravity = 98
bound = 3
M = 10
G = 1e-3
E, nu = 9e3, 0.3  # Young's modulus and Poisson's ratio
mu, la = E / (2 * (1 + nu)), E * nu / ((1 + nu) * (1 - 2 * nu))
kappa = .666666 * mu + la
x = ti.Vector.field(2, float, n_particles)
psi_max = ti.field(float, n_particles)
c = ti.field(float, n_particles)

color = ti.Vector.field(3, float, n_particles)
v = ti.Vector.field(2, float, n_particles)
C = ti.Matrix.field(2, 2, float, n_particles)
F = ti.Matrix.field(2, 2, dtype=float, shape=n_particles)  # deformation gradient

grid_v = ti.Vector.field(2, float, (n_grid, n_grid))
grid_m = ti.field(float, (n_grid, n_grid))
rhs  = ti.field(float, (n_grid, n_grid))
b =   ti.field(float, (n_grid, n_grid))
grid_c = ti.field(float, (n_grid, n_grid))
gradc_scp = ti.Vector.field(2, float, n_particles)
grad = ti.field(float, (n_grid, n_grid))
diag =  ti.field(float, (n_grid, n_grid))
@ti.data_oriented
class ConjugateGradientSolver2D:
    def __init__(self, n, m):
        self.n = n
        self.m = m
        self.x = ti.field(dtype=ti.f32, shape=(n, m))   # Solution
        self.b = ti.field(dtype=ti.f32, shape=(n, m))   # Right-hand side
        self.r = ti.field(dtype=ti.f32, shape=(n, m))  # Residual
        self.p = ti.field(dtype=ti.f32, shape=(n, m))  # Search direction
        self.z = ti.field(dtype=ti.f32, shape=(n, m))  # Preconditioned residual
        self.q = ti.field(dtype=ti.f32, shape=(n, m))  # A * p
        self.diagonal = ti.field(dtype=ti.f32, shape=(n, m))   # Diagonal for Jacobi preconditioner
        self.user_matvec = None  # User-provided matrix-vector multiplication function

    def matvec(self, p, q):
        """Matrix-free multiplication: q = A * p (user-provided)"""
        self.user_matvec(p, q)

    @ti.kernel
    def apply_jacobi_preconditioner(self, r: ti.template(), z: ti.template()):
        """Apply Jacobi preconditioner: z = M^-1 * r in 2D"""
        for i, j in ti.ndrange(self.n, self.m):
            if self.diagonal[i, j] != 0:
                z[i, j] = r[i, j] / self.diagonal[i, j]

    @ti.kernel
    def dot(self, v1: ti.template(), v2: ti.template()) -> ti.f32:
        """Dot product of two vectors in 2D"""
        sum = 0.0
        for i, j in ti.ndrange(self.n, self.m):
            sum += v1[i, j] * v2[i, j]
        return sum

    @ti.kernel
    def vec_add(self, v1: ti.template(), v2: ti.template(), alpha: ti.f32):
        """Element-wise operation: v1 += alpha * v2 in 2D"""
        for i, j in ti.ndrange(self.n, self.m):
            v1[i, j] += alpha * v2[i, j]

    @ti.kernel
    def vec_sub(self, v1: ti.template(), v2: ti.template(), alpha: ti.f32):
        """Element-wise operation: v1 -= alpha * v2 in 2D"""
        for i, j in ti.ndrange(self.n, self.m):
            v1[i, j] -= alpha * v2[i, j]

    @ti.kernel
    def vec_mul(self, v1: ti.template(), alpha: ti.f32):
        """Element-wise operation: v1 *= alpha in 2D"""
        for i, j in ti.ndrange(self.n, self.m):
            v1[i, j] *= alpha

    @ti.kernel
    def vec_add_scaled(self, v1: ti.template(), v2: ti.template(), beta: ti.f32):
        """Element-wise operation: v1 = beta * v1 + v2 in 2D"""
        for i, j in ti.ndrange(self.n, self.m):
            v1[i, j] = beta * v1[i, j] + v2[i, j]

    def conjugate_gradient(self, tol=1e-6, max_iter=1000):
        """Conjugate Gradient method in 2D"""
        # Initial residual r = b - A * x
        self.matvec(self.x, self.q)
        self.r.copy_from(self.b)
        self.vec_sub(self.r, self.q, 1.0)

        # Preconditioner: z = M^-1 * r (Jacobi)
        self.apply_jacobi_preconditioner(self.r, self.z)

        # Initial direction p = z
        self.p.copy_from(self.z)

        rz_old = self.dot(self.r, self.z)

        for iter in range(max_iter):
            # q = A * p
            self.q.fill(0)
            self.matvec(self.p, self.q)

            # alpha = rz_old / (p^T * A * p)
            pq = self.dot(self.p, self.q)
            alpha = rz_old / pq

            # Update x = x + alpha * p
            self.vec_add(self.x, self.p, alpha)

            # Update residual r = r - alpha * q
            self.vec_sub(self.r, self.q, alpha)

            # Check convergence: ||r|| < tol
            rz_new = self.dot(self.r, self.r)
            print(rz_new ** 0.5)
            if rz_new < tol ** 2:
                print(f"Converged in {iter} iterations.")
                break

            # Preconditioner: z = M^-1 * r
            self.apply_jacobi_preconditioner(self.r, self.z)

            # Update rz_new = r^T * z
            rz_new = self.dot(self.r, self.z)

            # Beta = rz_new / rz_old
            beta = rz_new / rz_old

            # Update search direction: p = z + beta * p
            self.vec_mul(self.p, beta)
            self.vec_add(self.p, self.z, 1.0)

            rz_old = rz_new

        else:
            print(f"Did not converge in {max_iter} iterations.")
@ti.func
def compute_stress(Fp,g=1.):
    B = Fp@Fp.transpose()
    devB = B - ti.Matrix.identity(float, 2)  * 1. / 2.*B.trace()
    J = Fp.determinant()
    tau_dev = mu * 1./J * devB
    prime = kappa/2. * (J-1./J)
    tau_vol = J*prime*ti.Matrix.identity(float, 2)
    out = g * (tau_dev + tau_vol) if J >= 1. else g * tau_dev + tau_vol
    return out
@ti.func
def get_psi_pos(Fp):
    J = Fp.determinant()
    JaF = 1/ti.sqrt(J)*Fp
    psi_dev = mu/2*((JaF.transpose()@JaF).trace()-2)
    psi_vol = kappa/2 * ((J**2-1)/2 - ti.log(J))
    out = psi_dev + psi_vol if J >= 1. else  psi_dev
    return out
@ti.kernel
def reset_grid():
    for i, j in grid_m:
        grid_v[i, j] = [0, 0]
        grid_m[i, j] = 0
        rhs[i,j] = 0
        b[i,j] = 0
        grid_c[i,j] = 0
        diag[i,j] = 0
        grad[i,j] = 0
@ti.kernel
def p2g():
    for p in x:
        Xp = x[p] / dx
        base = int(Xp - 0.5)
        #color[p][0] = 1-c[p]
        #color[p][1] = c[p]
        fx = Xp - base
        w = [0.5 * (1.5 - fx) ** 2, 0.75 - (fx - 1) ** 2, 0.5 * (fx - 0.5) ** 2]
        F[p] = (ti.Matrix.identity(float, 2) + dt * C[p]) @ F[p]
        U, sig, V = ti.svd(F[p])
        J = F[p].determinant()
        stress = compute_stress(F[p], c[p])
        stress = (-dt * p_vol * 4 * inv_dx * inv_dx) * stress
        affine = stress + p_mass * C[p]
        for i, j in ti.static(ti.ndrange(3, 3)):
            offset = ti.Vector([i, j])
            dpos = (offset - fx) * dx
            grid_idx = base + offset
            weight = w[i].x * w[j].y
            grid_v[grid_idx] += weight * (p_mass * v[p] + affine @ dpos)
            grid_m[grid_idx] += weight * p_mass
@ti.kernel
def grid_op():
    for i, j in grid_m:
        if grid_m[i, j] > 0:
            grid_v[i, j] /= grid_m[i, j]
        grid_v[i, j].y -= dt * gravity
        if i < bound and grid_v[i, j].x < 0:
            grid_v[i, j].x = 0
        if i > n_grid - bound and grid_v[i, j].x > 0:
            grid_v[i, j].x = 0
        if j < bound and grid_v[i, j].y < 0:
            grid_v[i, j].y = 0
        if j > n_grid - bound and grid_v[i, j].y > 0:
            grid_v[i, j].y = 0

@ti.kernel
def g2p():
    for p in x:
        Xp = x[p] / dx
        base = int(Xp - 0.5)
        fx = Xp - base
        w = [0.5 * (1.5 - fx) ** 2, 0.75 - (fx - 1) ** 2, 0.5 * (fx - 0.5) ** 2]
        new_v = ti.Vector.zero(float, 2)
        new_C = ti.Matrix.zero(float, 2, 2)
        for i, j in ti.static(ti.ndrange(3, 3)):
            offset = ti.Vector([i, j])
            dpos = (offset - fx) * dx
            grid_idx = base + offset
            grid_idx = ti.Vector([ti.math.clamp(grid_idx[0], 0, grid_v.shape[0] - 1),ti.math.clamp(grid_idx[1], 0, grid_v.shape[1] - 1)])
            weight = w[i].x * w[j].y
            g_v = grid_v[grid_idx]
            new_v += weight * g_v
            new_C += 4 * weight * g_v.outer_product(dpos) / dx**2
        v[p] = new_v
        x[p] += dt * v[p]
        C[p] = new_C
@ti.kernel
def init():
    for i in range(n_particles):
        x[i] = [ti.random() * 0.5 + 0.25, ti.random() * 0.7 + 0.2]
        v[i] = [0, -1]
        F[i] = ti.Matrix([[1, 0], [0, 1]])
        c[i] = 1
# Initialize the particles
init()

# Main GUI loop
gui = ti.ui.Window("MPM88", (1024, 1024))
canvas = gui.get_canvas()
solver = ConjugateGradientSolver2D(n_grid,n_grid)
while gui.running:
    canvas.set_background_color((1, 1, 1))
    for s in range(5):
        reset_grid()
        p2g()
        grid_op()
        g2p()
    canvas.circles(x, radius=0.005, per_vertex_color=color)
    #canvas.set_image(sol.to_numpy())
    gui.show()
Add phase field fracture to this, use method outlined in CD-MPM https://joshuahwolper.com/cdmpm