# %%
# Simple Pills
# ============
# This example demonstrates the simulation of capsule-shaped particles (pills)
# within a cylindrical container. The pills are randomly oriented and packed
# inside the drum, which is later removed to observe the settling behavior of
# the pills under gravity. The simulation highlights the complex interactions
# and dynamics of non-spherical granular materials.
# %%
# Keywords
# --------
# DEM, 3D, Pills, Non-spherical Particles
import numpy as np
import os, shutil, sys
from migflow import scontact, time_integration, gmsh_io
from migflow.quaternion import (
    Quaternion,
    random_rotation,
    to_quaternion,
    quaternion_between_vectors,
)
from cylinder_mod import generate_geometry

# %%
# Output Directory
# ----------------
# Prepare the directory that will store the transient simulation results.
outputdir = "output_3d_simple_pills" if len(sys.argv) < 2 else sys.argv[1]
shutil.rmtree(outputdir, True)


# %%
# Pill Geometry Utilities
# -----------------------
# Helper routines to assemble capsule-shaped bodies and distribute them in the drum.
def add_pill_to_problem(
    _p: scontact.ParticleProblem,
    R: float,
    AR: float,
    density: float,
    position: np.ndarray,
    direction: np.ndarray,
):
    """Create a capsule made of a cylinder and two hemispherical caps, then attach it to the problem."""

    # Pills default generation axis
    default_axis = np.array([0, 0, 1])

    # cylinder
    cyl_H = R * (AR - 2)
    cyl_mass = cyl_H * np.pi * R**2 * density
    cyl_inertia = (
        0.5
        * cyl_mass
        * R**2
        * np.array([0.5 + (cyl_H / R) ** 2 / 6, 0.5 + (cyl_H / R) ** 2 / 6, 1])
        * np.eye(3)
    )

    # semi spherical cap
    rel_pos = np.array([[0, 0, cyl_H / 2], [0, 0, -cyl_H / 2]])
    cap_mass = 2 / 3 * np.pi * R**3 * density
    cap_shift = cyl_H / 2 + 3 / 8 * R
    cap_inertia = cap_mass * R**2 * np.array([83 / 320, 83 / 320, 2 / 5]) * np.eye(3)
    cap_inertia_total = cap_inertia + cap_mass * (
        cap_shift**2 * np.eye(3)
        - np.outer(np.array([0, 0, cap_shift]), np.array([0, 0, cap_shift]))
    )
    cap_inertia_total += cap_inertia + cap_mass * (
        cap_shift**2 * np.eye(3)
        - np.outer(np.array([0, 0, -cap_shift]), np.array([0, 0, -cap_shift]))
    )

    inertia = cyl_inertia + cap_inertia_total
    mass = cyl_mass + 2 * cap_mass

    # Pill orientation
    qrot = quaternion_between_vectors(default_axis, direction)
    rot_pos = np.vstack([qrot.rotate_3Dvec(rel_pos[0]), qrot.rotate_3Dvec(rel_pos[1])])
    rotmat = qrot.to_rot_mat()
    rotmat[np.abs(rotmat) < 1e-14] = 0
    inertia_rot = rotmat @ inertia @ rotmat.T
    eigenvalues, eigenvectors = np.linalg.eigh(inertia_rot)
    if np.linalg.det(eigenvectors) < 0:
        eigenvectors[:, 0] = -eigenvectors[:, 0]
    qtheta = Quaternion.from_basis(eigenvectors)
    norm_theta = np.linalg.norm(qtheta.to_array())
    if not np.allclose(norm_theta, 1.0):
        print("Quaternion norm is too small", norm_theta)

    # Body initiation
    body = _p.add_body(position, 1 / mass, 0)
    p.body_invert_inertia()[body] = 1 / eigenvalues
    # Adding spheres and cylinder to body
    _p.add_segment_to_body(
        rot_pos[0], rot_pos[1], body, tag="bnd", material="default", radius=R
    )
    _p.add_particle_to_body(rot_pos[0], R, body, material="default")
    _p.add_particle_to_body(rot_pos[1], R, body, material="default")
    p.body_theta()[body] = to_quaternion(direction)
    return body


def gen_mesh_geometry(cyl_radius, cyl_height):
    """Build or reuse a Gmsh mesh describing the confining cylinder."""
    print(">> Trying generating mesh...")
    meshName = "mesh.msh"
    # meshPath = os.path.join(os.getcwd(), meshName)
    meshPath = meshName
    if os.path.exists(meshPath):
        print(f" -> Mesh already exists at {meshPath} \(ÔÔ)/")
        return meshPath
    else:
        meshPath = generate_geometry(cyl_radius, cyl_height, meshPath)
        print(f" -> Mesh created at {meshPath} \(ÔÔ)/")
    return meshPath


def gen_random_layer(pillR, cylR):
    """Generate a non-overlapping set of 2D positions inside the drum cross section."""
    d = pillR
    polypod_r = d / 2
    itmax = 2000
    it = 0
    positions = []
    while it < itmax:
        it += 1
        pos = np.array([np.random.uniform(-cylR, cylR), np.random.uniform(-cylR, cylR)])
        if np.linalg.norm(pos) < cylR - 1.05 * polypod_r:
            valid = True
            for p in positions:
                if np.linalg.norm(pos - p) < d:
                    valid = False
                    break
            if valid:
                positions.append(pos)
    positions = np.array(positions)
    return positions


def add_pill_layer(p, insert_height, pill_r, pill_ar, rho, cylr):
    """Insert a horizontal layer of pills with random orientations."""
    positions = gen_random_layer(pill_r * pill_ar, cylr)
    hh = np.ones_like(positions[:, 0]) * insert_height
    positions = np.hstack((positions, hh[:, None]))
    for pos in positions:
        bid = add_pill_to_problem(p, pill_r, pill_ar, rho, pos, random_rotation())
        # p.body_omega()[bid] = np.random.uniform(-1,1,3) * 2*np.pi
        p.body_velocity()[bid] = (
            np.array(
                [
                    np.random.uniform(-1, 1),
                    np.random.uniform(-1, 1),
                    np.random.uniform(-0.5, -1),
                ]
            )
            * pill_r
            * pill_ar
        )
    return positions.shape[0]


# %%
# Particle Problem
# ----------------
# We begin by defining the 3D particle problem and configuring the contact
# solver. The contact geometry is updated dynamically, and the prediction
# basis is activated to improve convergence of the contact iterations.
p = scontact.ParticleProblem(3)
p.set_fixed_contact_geometry(0)
p.set_predict_basis(1)


# %%
# Material Properties and Parameters
# ----------------------------------
# The particle density, radius, gravity vector and the friction coefficient are defined.
# A large friction coefficient is assigned between particles to ensure interlocking and prevent sliding.
rho = 1000
pill_r = 0.05
pill_ar = 4.0
pill_R = pill_r * pill_ar
g = np.array([0.0, 0.0, -9.81])

H = 1.0
R = 1.0

material_wall = "wall"
material_bottom = "bottom"
mu_pill_pill = 0.4
mu_pill_wall = 0.3
mu_pill_bottom = 0.5
p.set_friction_coefficient(mu_pill_pill, "default", "default")
p.set_friction_coefficient(mu_pill_wall, "default", material_wall)
p.set_friction_coefficient(mu_pill_bottom, "default", material_bottom)

# %%
# Pill Packing
# ------------
# Fill the container with randomly oriented pills and report the number of created bodies.
add_pill_layer(p, pill_r * pill_ar, pill_r, pill_ar, rho, 0.5 * R)
print(">> Done %d pills generation" % p.n_bodies())

# %%
# Mesh And Boundaries
# -------------------
# Generate the confining mesh, attach it as a rigid body, and mark the boundary patches.
mesh = generate_geometry(R, H)
gmsh_io.load_msh_boundaries(p, mesh, tags=["bottom_inner"], material=material_bottom)

# Write initial configuration
p.write_mig(outputdir, 0, write_contacts=True)

# %%
# Body Forces
# -----------
# Pre-compute gravitational force contributions for each rigid body.
fp = np.zeros((p.n_particles(), 3))
inv_mass = p.body_invert_mass().ravel()
mass = np.where(inv_mass > 0, 1.0 / inv_mass, 0.0)
bf = mass[:, None] * g
pills_weight = bf.copy()

# %%
# Time Integration
# Select constant time step, solver tolerance, and output frequency.
# Advance the system in time, periodically exporting states and removing the drum once settled.
tEnd = 5.0
dt = 1e-3
ctol = pill_r / 100
outf = int(0.01 / dt)
t = 0
i = 0

while t < tEnd:
    sub = time_integration._advance_particles(p, fp, dt, 1, ctol, fbody=bf)
    q = p.get_current_quality(ctol)

    fbottom = p.get_boundary_forces("bottom_inner")[2]
    qratio = abs(q) / ctol / sub
    if qratio < 1:
        print(
            f"Quality ratio : {abs(q)/ctol/sub:.6f}[-], NSUB {sub:d} Bottom reaction force: {fbottom:.6g} N, Pills weight: {np.abs(pills_weight[:,2].sum()):.6g} N"
        )
    else:
        print(f"Error, solver tolerance is not respected")

    t += dt
    i += 1

    if i % outf == 0:
        print(f"{i =:4d}, {t:.6g}/{tEnd:.6g}")
        p.write_mig(outputdir, t, write_contacts=True)

# %%
# Plot
# ----
# .. code-block:: shell
#
#  python3 -m migflow.plot.migplot output_3d_simple_pills --actors particles
# %%
# Artifacts
# ---------
# - output_3d_simple_pills/animation.mp4
