import taichi as ti

ti.init(arch=ti.cuda, default_fp=ti.f32)

"""
Characteristic Scales
"""
# Beams
dt  = 0.02 # timestep
C_d = 1.0 # characteristic distance
C_f = 1.0 # characteristic force
C_m = 1.0 # characteristic mass
C_v = 1.0 # characteristic vel

VEL_DAMPING = 0.999
GRAV_VEC = ti.Vector([0.0, -0.01, 0.0])

# Cubes
dt  = 0.005 # timestep
C_d = 1.0 # characteristic distance
C_f = 1.0 # characteristic force
C_m = 1.0 # characteristic mass
C_v = 1.0 # characteristic vel

VEL_DAMPING = 0.9998
GRAV_VEC = ti.Vector([0.0, -0.01, 0.0])

# FLUIDS 0.01, 0.9999
dt  = 0.02 # timestep
C_d = 1.0 # characteristic distance
C_f = 1.0 # characteristic force
C_m = 1.0 # characteristic mass
C_v = 1.0 # characteristic vel

VEL_DAMPING = 0.9995
GRAV_VEC = ti.Vector([0.0, -0.03, 0.0])

"""
Domain
"""

# Beams
domain = ti.Matrix([
    [0,150*C_d], 
    [0,150*C_d], 
    [0,150*C_d]
])

# Cubes
domain = ti.Matrix([
    [0,100*C_d], 
    [0,100*C_d], 
    [0,100*C_d]
])

# Fluids w Cubes
domain = ti.Matrix([
    [0, 50*C_d], 
    [0, 50*C_d], 
    [0, 50*C_d]
])

domain_ranges = ti.Vector([domain[0,1]-domain[0,0], domain[1,1]-domain[1,0], domain[2,1]-domain[2,0]])
domain_mins = ti.Vector([domain[0,0], domain[1,0], domain[2,0]])
domain_maxs = ti.Vector([domain[0,1], domain[1,1], domain[2,1]])
domain_centers = domain_ranges/2.0 + domain_mins
total_particle_dim = domain_ranges / C_d

def unnormalize_coords(x,y,z):
    vec = ti.Vector([x,y,z])
    vec = vec*domain_ranges + domain_mins
    return [vec.x, vec.y, vec.z]
"""
Config LUTs
"""

Axis_Plane = {
    "X-YZ": 0,
    "Y-XZ": 1,
    "Z-XY": 2
}

PackingTypes = {
    "SimpleCubic": {
        "spacing": [C_d, C_d, C_d]
    },
    "HexGrid": { 
        "spacing": [C_d, C_d/2 * ti.sqrt(3)]
    },
    "FCC": {
        "spacing": []
    },
    "BCC": {
        "spacing": []
    },
    "HCP": {
        "spacing": []
    },
    "Random": {
        "spacing": [C_d*1.3, C_d*1.3, C_d*1.3]
    }
}

for i, packtype in enumerate(PackingTypes.values()):
    packtype["idx"] = i

"""
Types
"""
N_LAW_BREAKPOINTS = 5
vec3i16 = ti.types.vector(3,dtype=ti.i16)
vec3i32 = ti.types.vector(3,dtype=ti.i32)
vec3f = ti.types.vector(3,dtype=float)
mat3f = ti.types.matrix(3,2,dtype=float)
force_law = ti.types.matrix(n=N_LAW_BREAKPOINTS, m=3, dtype=float)
force_distances = ti.types.vector(N_LAW_BREAKPOINTS - 1, dtype=float)


"""
Bins
"""
# TODO: support separate bin counts per axis
BINS_PER_AXIS = 100000
for i in range(3):
    bins_per_domain_axis = ti.ceil(domain_ranges[i] / (C_d*3))
    if bins_per_domain_axis < BINS_PER_AXIS:
        BINS_PER_AXIS = bins_per_domain_axis
bin_dim = domain_ranges / BINS_PER_AXIS

bin_counts = ti.field(dtype=int, shape=(BINS_PER_AXIS, BINS_PER_AXIS, BINS_PER_AXIS))
col_counts = ti.field(dtype=int, shape=(BINS_PER_AXIS, BINS_PER_AXIS))
plane_counts = ti.field(dtype=int, shape=(BINS_PER_AXIS))

bin_starts = ti.field(dtype=int, shape=(BINS_PER_AXIS, BINS_PER_AXIS, BINS_PER_AXIS))
col_starts = ti.field(dtype=int, shape=(BINS_PER_AXIS, BINS_PER_AXIS))
plane_starts = ti.field(dtype=int, shape=(BINS_PER_AXIS))

bin_ends = ti.field(dtype=int, shape=(BINS_PER_AXIS, BINS_PER_AXIS, BINS_PER_AXIS))
bin_cur = ti.field(dtype=int, shape=(BINS_PER_AXIS, BINS_PER_AXIS, BINS_PER_AXIS))

"""Scene Config"""
inter_particle_force_law = {
    "mass": 1.0 * C_m,
    "law": force_law([
        [0.9*C_d,  11.0*C_f,  0.0],
        [1.0*C_d,  1.0*C_f,  -11.0/0.11],
        [1.01*C_d, 0.0*C_f,  0.0],
        [1.3*C_d,  0.0*C_f,  0.0],
        [1.4*C_d,  0.0*C_f,  0.0],
    ]),
}

MATERIAL_CONFIG_BEAMS = [ 
    {
        "def": {
            "mass": 1.0 * C_m,
            "law": force_law( [       # Hermite Spline Definition
                [0.9*C_d,  2.5*C_f,  0.0],
                [1.0*C_d,  0.0*C_f,  -5/0.2],
                [1.1*C_d,  -2.5*C_f,  0.0],
                [1.3*C_d,  0.3*C_f,  0.0],
                [1.4*C_d,  0.0*C_f,  0.0],
            ] ),
            "self_weighted": 0,
            "is_static": 1,
            "color": ti.Vector([0.3,0.7,0.5]),
        },
        "objs": [
            { # Ground Plane Collider
                "origin": ti.Vector(unnormalize_coords(0.001, 0.001, 0.001)),
                "bbox":   ti.Vector(unnormalize_coords(1, 0, 1)),
                "normal": Axis_Plane["Y-XZ"],
                "grid_orientation": 0,
                "packing": PackingTypes["HexGrid"],
                "color": ti.Vector([1,1,1]),
            },
        ],
    },

    {
        "def":{
            "mass": 1.0 * C_m,
            "law": force_law( [       # Hermite Spline Definition
                [0.9*C_d,  0.25*C_f,  0.0],
                [1.0*C_d,  0.0*C_f,  -0.5/0.2],
                [1.1*C_d,  -0.25*C_f,  0.0],
                [1.3*C_d,  0.0*C_f,  0.0],
                [1.4*C_d,  0.0*C_f,  0.0],
            ] ),
            "self_weighted": 0,
            "is_static": 0,
            "color": ti.Vector([0.8,0.8,0.5]),
        },
        "objs": [
            { # HexGrid Cantilevering Beam (Planar)
                "origin": ti.Vector(unnormalize_coords(0.15, 0.3, 0.0)),
                "bbox":   ti.Vector(unnormalize_coords(0.0, 0.1, 1.0)),
                "normal": Axis_Plane["X-YZ"],
                "grid_orientation": 0,
                "packing": PackingTypes["HexGrid"],
                "fixed_nodes_bbox": ti.Vector([1,0,0]),
                "fixed_load": ti.Vector([0,-0.006,0]),
                "fixed_load_bbox": ti.Matrix.cols([
                    unnormalize_coords(0,0,0.95),
                    unnormalize_coords(1,1,1.0),
                ]),
                "color": ti.Vector([1,1,1]),
            },
            { # HexGrid Supported Beam (Planar)
                "origin": ti.Vector(unnormalize_coords(0.15, 0.6, 0.0)),
                "bbox":   ti.Vector(unnormalize_coords(0.0, 0.1, 1.0)),
                "normal": Axis_Plane["X-YZ"],
                "grid_orientation": 0,
                "packing": PackingTypes["HexGrid"],
                "fixed_nodes_bbox": ti.Vector([-1,0,0]),
                "fixed_load": ti.Vector([0,-0.01,0]),
                "fixed_load_bbox": ti.Matrix.cols([
                    unnormalize_coords(0,0,0.475),
                    unnormalize_coords(1,1,0.525),
                ]),
                "color": ti.Vector([1,1,1]),
            },
            { # HexGrid Supported Beam (Planar)
                "origin": ti.Vector(unnormalize_coords(0.15, 0.9, 0.0)),
                "bbox":   ti.Vector(unnormalize_coords(0.0, 0.1, 1.0)),
                "normal": Axis_Plane["X-YZ"],
                "grid_orientation": 0,
                "packing": PackingTypes["HexGrid"],
                "fixed_nodes_bbox": ti.Vector([-1,0,0]),
                "fixed_load": ti.Vector([-0.05,0,0]),
                "fixed_load_bbox": ti.Matrix.cols([
                    unnormalize_coords(0,0.93,0.475),
                    unnormalize_coords(1,0.95,0.525),
                ]),
                "color": ti.Vector([1,1,1]),
            },
        ],
    },
    
    {
        "def":{
            "mass": 1.0 * C_m,
            "law": force_law( [       # Hermite Spline Definition
                [0.9*C_d,  0.5*C_f,  0.0],
                [1.0*C_d,  0.0*C_f,  -1/0.2],
                [1.1*C_d,  -0.5*C_f,  0.0],
                [1.3*C_d,  0.0*C_f,  0.0],
                [1.4*C_d,  0.0*C_f,  0.0],
            ] ),
            "self_weighted": 0,
            "is_static": 0,
            "color": ti.Vector([0.3,0.8,0.5]),
        },
        "objs": [
            { # HexGrid Cantilevering Beam (Planar)
                "origin": ti.Vector(unnormalize_coords(0.3, 0.3, 0.0)),
                "bbox":   ti.Vector(unnormalize_coords(0.0, 0.1, 1.0)),
                "normal": Axis_Plane["X-YZ"],
                "grid_orientation": 0,
                "packing": PackingTypes["HexGrid"],
                "fixed_nodes_bbox": ti.Vector([1,0,0]),
                "fixed_load": ti.Vector([0,-0.006,0]),
                "fixed_load_bbox": ti.Matrix.cols([
                    unnormalize_coords(0,0,0.95),
                    unnormalize_coords(1,1,1.0),
                ]),
                "color": ti.Vector([1,1,1]),
            },
            { # HexGrid Supported Beam (Planar)
                "origin": ti.Vector(unnormalize_coords(0.3, 0.6, 0.0)),
                "bbox":   ti.Vector(unnormalize_coords(0.0, 0.1, 1.0)),
                "normal": Axis_Plane["X-YZ"],
                "grid_orientation": 0,
                "packing": PackingTypes["HexGrid"],
                "fixed_nodes_bbox": ti.Vector([-1,0,0]),
                "fixed_load": ti.Vector([0,-0.01,0]),
                "fixed_load_bbox": ti.Matrix.cols([
                    unnormalize_coords(0,0,0.475),
                    unnormalize_coords(1,1,0.525),
                ]),
                "color": ti.Vector([1,1,1]),
            },
            { # HexGrid Supported Beam (Planar)
                "origin": ti.Vector(unnormalize_coords(0.3, 0.9, 0.0)),
                "bbox":   ti.Vector(unnormalize_coords(0.0, 0.1, 1.0)),
                "normal": Axis_Plane["X-YZ"],
                "grid_orientation": 0,
                "packing": PackingTypes["HexGrid"],
                "fixed_nodes_bbox": ti.Vector([-1,0,0]),
                "fixed_load": ti.Vector([-0.05,0,0]),
                "fixed_load_bbox": ti.Matrix.cols([
                    unnormalize_coords(0,0.93,0.475),
                    unnormalize_coords(1,0.95,0.525),
                ]),
                "color": ti.Vector([1,1,1]),
            },
        ],
    },
    
    {
        "def":{
            "mass": 1.0 * C_m,
            "law": force_law( [       # Hermite Spline Definition
                [0.9*C_d,  1.0*C_f,  0.0],
                [1.0*C_d,  0.0*C_f,  -2/0.2],
                [1.1*C_d,  -1.0*C_f,  0.0],
                [1.3*C_d,  0.0*C_f,  0.0],
                [1.4*C_d,  0.0*C_f,  0.0],
            ] ),
            "self_weighted": 0,
            "is_static": 0,
            "color": ti.Vector([0.7,0.3,0.5]),
        },
        "objs": [
            { # HexGrid Cantilevering Beam (Planar)
                "origin": ti.Vector(unnormalize_coords(0.45, 0.3, 0.0)),
                "bbox":   ti.Vector(unnormalize_coords(0.0, 0.1, 1.0)),
                "normal": Axis_Plane["X-YZ"],
                "grid_orientation": 0,
                "packing": PackingTypes["HexGrid"],
                "fixed_nodes_bbox": ti.Vector([1,0,0]),
                "fixed_load": ti.Vector([0,-0.006,0]),
                "fixed_load_bbox": ti.Matrix.cols([
                    unnormalize_coords(0,0,0.95),
                    unnormalize_coords(1,1,1.0),
                ]),
                "color": ti.Vector([1,1,1]),
            },
            { # HexGrid Supported Beam (Planar)
                "origin": ti.Vector(unnormalize_coords(0.45, 0.6, 0.0)),
                "bbox":   ti.Vector(unnormalize_coords(0.0, 0.1, 1.0)),
                "normal": Axis_Plane["X-YZ"],
                "grid_orientation": 0,
                "packing": PackingTypes["HexGrid"],
                "fixed_nodes_bbox": ti.Vector([-1,0,0]),
                "fixed_load": ti.Vector([0,-0.01,0]),
                "fixed_load_bbox": ti.Matrix.cols([
                    unnormalize_coords(0,0,0.475),
                    unnormalize_coords(1,1,0.525),
                ]),
                "color": ti.Vector([1,1,1]),
            },
            { # HexGrid Supported Beam (Planar)
                "origin": ti.Vector(unnormalize_coords(0.45, 0.9, 0.0)),
                "bbox":   ti.Vector(unnormalize_coords(0.0, 0.1, 1.0)),
                "normal": Axis_Plane["X-YZ"],
                "grid_orientation": 0,
                "packing": PackingTypes["HexGrid"],
                "fixed_nodes_bbox": ti.Vector([-1,0,0]),
                "fixed_load": ti.Vector([-0.05,0,0]),
                "fixed_load_bbox": ti.Matrix.cols([
                    unnormalize_coords(0,0.93,0.475),
                    unnormalize_coords(1,0.95,0.525),
                ]),
                "color": ti.Vector([1,1,1]),
            },
        ],
    },

    {
        "def":{
            "mass": 1.0 * C_m,
            "law": force_law( [       # Hermite Spline Definition
                [0.9*C_d,  2.0*C_f,  0.0],
                [1.0*C_d,  0.0*C_f,  -4/0.2],
                [1.1*C_d,  -2.0*C_f,  0.0],
                [1.3*C_d,  0.0*C_f,  0.0],
                [1.4*C_d,  0.0*C_f,  0.0],
            ] ),
            "self_weighted": 0,
            "is_static": 0,
            "color": ti.Vector([0.3,0.5,0.8]),
        },
        "objs": [
            { # HexGrid Cantilevering Beam (Planar)
                "origin": ti.Vector(unnormalize_coords(0.6, 0.3, 0.0)),
                "bbox":   ti.Vector(unnormalize_coords(0.0, 0.1, 1.0)),
                "normal": Axis_Plane["X-YZ"],
                "grid_orientation": 0,
                "packing": PackingTypes["HexGrid"],
                "fixed_nodes_bbox": ti.Vector([1,0,0]),
                "fixed_load": ti.Vector([0,-0.006,0]),
                "fixed_load_bbox": ti.Matrix.cols([
                    unnormalize_coords(0,0,0.95),
                    unnormalize_coords(1,1,1.0),
                ]),
                "color": ti.Vector([1,1,1]),
            },
            { # HexGrid Supported Beam (Planar)
                "origin": ti.Vector(unnormalize_coords(0.6, 0.6, 0.0)),
                "bbox":   ti.Vector(unnormalize_coords(0.0, 0.1, 1.0)),
                "normal": Axis_Plane["X-YZ"],
                "grid_orientation": 0,
                "packing": PackingTypes["HexGrid"],
                "fixed_nodes_bbox": ti.Vector([-1,0,0]),
                "fixed_load": ti.Vector([0,-0.01,0]),
                "fixed_load_bbox": ti.Matrix.cols([
                    unnormalize_coords(0,0,0.475),
                    unnormalize_coords(1,1,0.525),
                ]),
                "color": ti.Vector([1,1,1]),
            },
            { # HexGrid Supported Beam (Planar)
                "origin": ti.Vector(unnormalize_coords(0.6, 0.9, 0.0)),
                "bbox":   ti.Vector(unnormalize_coords(0.0, 0.1, 1.0)),
                "normal": Axis_Plane["X-YZ"],
                "grid_orientation": 0,
                "packing": PackingTypes["HexGrid"],
                "fixed_nodes_bbox": ti.Vector([-1,0,0]),
                "fixed_load": ti.Vector([-0.05,0,0]),
                "fixed_load_bbox": ti.Matrix.cols([
                    unnormalize_coords(0,0.93,0.475),
                    unnormalize_coords(1,0.95,0.525),
                ]),
                "color": ti.Vector([1,1,1]),
            },
        ],
    },


    {
        "def": {
            "mass": 1.0 * C_m,
            "law": force_law( [       # Hermite Spline Definition
                [0.9*C_d,  2*C_f,  0.0],
                [1.0*C_d,  0.0*C_f,  -4/0.2],
                [1.1*C_d, -2*C_f,  0.0],
                [1.3*C_d,  4.0*C_f,  0.0],
                [1.4*C_d,  0.0*C_f,  0.0],
            ] ),
            "self_weighted": 0,
            "is_static": 0,
            "color": ti.Vector([0.9,0.6,0.4]),
        },
        "objs": [
            { # Supported simple cubic beam
                "origin": ti.Vector(unnormalize_coords(0.8,0.89,0)),
                "bbox":   ti.Vector(unnormalize_coords(0.05, 0.1, 1.0)),
                "normal": None,
                "grid_orientation": None,
                "packing": PackingTypes["SimpleCubic"],
                "fixed_load": ti.Vector([0, -0.02, 0]),
                "fixed_load_bbox": ti.Matrix.cols([
                    unnormalize_coords(0,0.0,0.45),
                    unnormalize_coords(1,1.0,0.55),
                ]),
                "fixed_nodes_bbox": ti.Vector([-1,0,0]),
                "color": ti.Vector([1,1,1]),
            },
            { # Supported simple cubic beam
                "origin": ti.Vector(unnormalize_coords(0.8,0.6,0)),
                "bbox":   ti.Vector(unnormalize_coords(0.05, 0.1, 1.0)),
                "normal": None,
                "grid_orientation": None,
                "packing": PackingTypes["SimpleCubic"],
                "fixed_load": ti.Vector([-0.02, 0, 0]),
                "fixed_load_bbox": ti.Matrix.cols([
                    unnormalize_coords(0,0.0,0.45),
                    unnormalize_coords(1,1.0,0.55),
                ]),
                "fixed_nodes_bbox": ti.Vector([-1,0,0]),
                "color": ti.Vector([1,1,1]),
            },
            { # Supported simple cubic beam
                "origin": ti.Vector(unnormalize_coords(0.8,0.3,0)),
                "bbox":   ti.Vector(unnormalize_coords(0.05, 0.1, 1.0)),
                "normal": None,
                "grid_orientation": None,
                "packing": PackingTypes["SimpleCubic"],
                "fixed_load": ti.Vector([0, -0.02, 0]),
                "fixed_load_bbox": ti.Matrix.cols([
                    unnormalize_coords(0,0,0.95),
                    unnormalize_coords(1,1,1),
                ]),
                "fixed_nodes_bbox": ti.Vector([1,0,0]),
                "color": ti.Vector([1,1,1]),
            },
        ]
    },
]

MATERIAL_CONFIG_CUBES = [
    {
        "def": {
            "mass": 1.0 * C_m,
            "law": force_law( [       # Hermite Spline Definition
                [0.9*C_d,  2.5*C_f,  0.0],
                [1.0*C_d,  0.0*C_f,  -5/0.2],
                [1.1*C_d,  -2.5*C_f,  0.0],
                [1.3*C_d,  0.3*C_f,  0.0],
                [1.4*C_d,  0.0*C_f,  0.0],
            ] ),
            "self_weighted": 0,
            "is_static": 1,
            "color": ti.Vector([0.3,0.7,0.5]),
        },
        "objs": [
            { # Ground Plane Collider
                "origin": ti.Vector(unnormalize_coords(0.001, 0.001, 0.001)),
                "bbox":   ti.Vector(unnormalize_coords(1, 0, 1)),
                "normal": Axis_Plane["Y-XZ"],
                "grid_orientation": 0,
                "packing": PackingTypes["HexGrid"],
                "color": ti.Vector([1,1,1]),
            },
        ],
    },
    {
        "def": {
            "mass": 1 * C_m,
            "law": force_law( [       # Hermite Spline Definition
                [0.9*C_d,  0.3*C_f,  0.0],
                [1.0*C_d,  0.0*C_f,  -0.3/0.1],
                [1.1*C_d,  -0.05*C_f,  0.0],
                [1.3*C_d,  0.05*C_f,  0.0],
                [1.4*C_d,  0.0*C_f,  0.0],
            ] ),
            "self_weighted": 1,
            "is_static": 0,
            "color": ti.Vector([0.6,0.2,0.7]),
        },
        "objs": [
            { # Simple Cube
                "origin": ti.Vector(unnormalize_coords(0.78, 0.7, 0.58)),
                "bbox":   ti.Vector([ 10*C_d, 10*C_d, 10*C_d ]),
                "normal": None,
                "grid_orientation": None,
                "packing": PackingTypes["SimpleCubic"],
                "fixed_nodes_bbox": ti.Vector([0,0,0]),
                "color": ti.Vector([1,1,1]),
            },
            { # Simple Cube
                "origin": ti.Vector(unnormalize_coords(0.48, 0.3, 0.58)),
                "bbox":   ti.Vector([ 10*C_d, 10*C_d, 10*C_d ]),
                "normal": None,
                "grid_orientation": None,
                "packing": PackingTypes["SimpleCubic"],
                "fixed_nodes_bbox": ti.Vector([0,0,0]),
                "color": ti.Vector([1,1,1]),
            },
            { # Simple Cube
                "origin": ti.Vector(unnormalize_coords(0.63, 0.4, 0.58)),
                "bbox":   ti.Vector([ 10*C_d, 10*C_d, 10*C_d ]),
                "normal": None,
                "grid_orientation": None,
                "packing": PackingTypes["SimpleCubic"],
                "fixed_nodes_bbox": ti.Vector([0,0,0]),
                "color": ti.Vector([1,1,1]),
            },
            { # Simple Cube
                "origin": ti.Vector(unnormalize_coords(0.55, 0.6, 0.58)),
                "bbox":   ti.Vector([ 10*C_d, 10*C_d, 10*C_d ]),
                "normal": None,
                "grid_orientation": None,
                "packing": PackingTypes["SimpleCubic"],
                "fixed_nodes_bbox": ti.Vector([0,0,0]),
                "color": ti.Vector([1,1,1]),
            },
        ],
    },
    {
        "def": {
            "mass": 1.0 * C_m,
            "law": force_law( [       # Hermite Spline Definition
                [0.85*C_d,  60*C_f,  0.0],
                [1.0*C_d,  0.0*C_f,  -60/0.05],
                [1.05*C_d,  -60.0*C_f,  0.0],
                [1.2*C_d,  200.0*C_f,  0.0],
                [1.37*C_d,  0.0*C_f,  0.0],
            ] ),
            "self_weighted": 1,
            "is_static": 0,
            "color": ti.Vector([0.5,0.7,0.4]),
        },
        "objs": [
            { # Simple Cube
                "origin": ti.Vector(unnormalize_coords(0.15, 0.6, 0.15)),
                "bbox":   ti.Vector([ 10*C_d, 10*C_d, 10*C_d ]),
                "normal": None,
                "grid_orientation": None,
                "packing": PackingTypes["SimpleCubic"],
                "fixed_nodes_bbox": ti.Vector([0,0,0]),
                "color": ti.Vector([1,1,1]),
            },
            { # Simple Cube
                "origin": ti.Vector(unnormalize_coords(0.2, 0.5, 0.2)),
                "bbox":   ti.Vector([ 10*C_d, 10*C_d, 10*C_d ]),
                "normal": None,
                "grid_orientation": None,
                "packing": PackingTypes["SimpleCubic"],
                "fixed_nodes_bbox": ti.Vector([0,0,0]),
                "fixed_load": ti.Vector([0, 0, 0]),
                "fixed_load_bbox": ti.Matrix.cols([
                    unnormalize_coords(0,0.59,0),
                    unnormalize_coords(0.21,0.6,1)
                ]),
                "color": ti.Vector([1,1,1]),
            },
            { # Simple Cube
                "origin": ti.Vector(unnormalize_coords(0.24, 0.4, 0.25)),
                "bbox":   ti.Vector([ 10*C_d, 10*C_d, 10*C_d ]),
                "normal": None,
                "grid_orientation": None,
                "packing": PackingTypes["SimpleCubic"],
                "fixed_nodes_bbox": ti.Vector([0,0,0]),
                "color": ti.Vector([1,1,1]),
            },
            { # Simple Cube
                "origin": ti.Vector(unnormalize_coords(0.14, 0.3, 0.2)),
                "bbox":   ti.Vector([ 10*C_d, 10*C_d, 10*C_d ]),
                "normal": None,
                "grid_orientation": None,
                "packing": PackingTypes["SimpleCubic"],
                "fixed_nodes_bbox": ti.Vector([0,0,0]),
                "color": ti.Vector([1,1,1]),
            },
        ]
    },
    {
        "def": {
            "mass": 1.0 * C_m,
            "law": force_law( [       # Hermite Spline Definition
                [0.9*C_d,  2*C_f,  0.0],
                [1.0*C_d,  0.0*C_f,  -4/0.2],
                [1.1*C_d,  -2.0*C_f,  0.0],
                [1.2*C_d,  1.0*C_f,  0.0],
                [1.4*C_d,  0.0*C_f,  0.0],
            ] ),
            "self_weighted": 1,
            "is_static": 0,
            "color": ti.Vector([0.3,0.5,0.7]),
        },
        "objs": [
            { # Simple Cube
                "origin": ti.Vector(unnormalize_coords(0.15, 0.6, 0.55)),
                "bbox":   ti.Vector([ 10*C_d, 10*C_d, 10*C_d ]),
                "normal": None,
                "grid_orientation": None,
                "packing": PackingTypes["SimpleCubic"],
                "fixed_nodes_bbox": ti.Vector([0,0,0]),
                "color": ti.Vector([1,1,1]),
            },
            { # Simple Cube
                "origin": ti.Vector(unnormalize_coords(0.2, 0.5, 0.6)),
                "bbox":   ti.Vector([ 10*C_d, 10*C_d, 10*C_d ]),
                "normal": None,
                "grid_orientation": None,
                "packing": PackingTypes["SimpleCubic"],
                "fixed_nodes_bbox": ti.Vector([0,0,0]),
                "fixed_load": ti.Vector([0, 0, 0]),
                "fixed_load_bbox": ti.Matrix.cols([
                    unnormalize_coords(0,0.59,0),
                    unnormalize_coords(0.21,0.6,1)
                ]),
                "color": ti.Vector([1,1,1]),
            },
            { # Simple Cube
                "origin": ti.Vector(unnormalize_coords(0.24, 0.4, 0.65)),
                "bbox":   ti.Vector([ 10*C_d, 10*C_d, 10*C_d ]),
                "normal": None,
                "grid_orientation": None,
                "packing": PackingTypes["SimpleCubic"],
                "fixed_nodes_bbox": ti.Vector([0,0,0]),
                "color": ti.Vector([1,1,1]),
            },
            { # Simple Cube
                "origin": ti.Vector(unnormalize_coords(0.14, 0.3, 0.6)),
                "bbox":   ti.Vector([ 10*C_d, 10*C_d, 10*C_d ]),
                "normal": None,
                "grid_orientation": None,
                "packing": PackingTypes["SimpleCubic"],
                "fixed_nodes_bbox": ti.Vector([0,0,0]),
                "color": ti.Vector([1,1,1]),
            },
        ],
    },
]

MATERIAL_CONFIG_FLUIDS = [
    {
        "def": {
            "mass": 1.0 * C_m,
            "law": force_law( [       # Hermite Spline Definition
                [0.9*C_d,  2.5*C_f,  0.0],
                [1.0*C_d,  0.0*C_f,  -5/0.2],
                [1.1*C_d,  -2.5*C_f,  0.0],
                [1.3*C_d,  0.3*C_f,  0.0],
                [1.4*C_d,  0.0*C_f,  0.0],
            ] ),
            "self_weighted": 0,
            "is_static": 1,
            "color": ti.Vector([0.3,0.7,0.5]),
        },
        "objs": [
            { # Ground Plane Collider
                "origin": ti.Vector(unnormalize_coords(0.01, 0.01, 0.01)),
                "bbox":   ti.Vector(unnormalize_coords(1, 0, 1)),
                "normal": Axis_Plane["Y-XZ"],
                "grid_orientation": 0,
                "packing": PackingTypes["HexGrid"],
                "color": ti.Vector([1,1,1]),
            },
            { # Ground Plane Collider
                "origin": ti.Vector(unnormalize_coords(0.01, 0.01, 0.01)),
                "bbox":   ti.Vector(unnormalize_coords(1, 1, 0)),
                "normal": Axis_Plane["Z-XY"],
                "grid_orientation": 0,
                "packing": PackingTypes["HexGrid"],
                "color": ti.Vector([1,1,1]),
            },
            { # Ground Plane Collider
                "origin": ti.Vector(unnormalize_coords(0.01, 0.01, 0.01)),
                "bbox":   ti.Vector(unnormalize_coords(0, 1, 1)),
                "normal": Axis_Plane["X-YZ"],
                "grid_orientation": 0,
                "packing": PackingTypes["HexGrid"],
                "color": ti.Vector([1,1,1]),
            },
            { # Ground Plane Collider
                "origin": ti.Vector(unnormalize_coords(0.99, 0.01, 0.01)),
                "bbox":   ti.Vector(unnormalize_coords(0, 1, 1)),
                "normal": Axis_Plane["X-YZ"],
                "grid_orientation": 0,
                "packing": PackingTypes["HexGrid"],
                "color": ti.Vector([1,1,1]),
            },
            { # Ground Plane Collider
                "origin": ti.Vector(unnormalize_coords(0.01, 0.01, 0.99)),
                "bbox":   ti.Vector(unnormalize_coords(1, 1, 0)),
                "normal": Axis_Plane["Z-XY"],
                "grid_orientation": 0,
                "packing": PackingTypes["HexGrid"],
                "color": ti.Vector([1,1,1]),
            },
        ],
    },
    {
        "def": {
            "mass": 1 * C_m,
            "law": force_law( [       # Hermite Spline Definition
                [0.5*C_d,  80.0*C_f,  0.0],
                [1.0*C_d,  0.0*C_f,  0],
                [1.1*C_d,  0.0*C_f,  0.0],
                [1.3*C_d,  0.0*C_f,  0.0],
                [1.4*C_d,  0.0*C_f,  0.0],
            ] ),
            "self_weighted": 1,
            "is_static": 0,
            "color": ti.Vector([0.2,0.2,0.7]),
        },
        "objs": [
            { 
                "origin": ti.Vector(unnormalize_coords(0.05, 0.05, 0.05)),
                "bbox":   ti.Vector(unnormalize_coords(0.9, 0.5, 0.9)),
                "normal": Axis_Plane["Z-XY"],
                "grid_orientation": 0,
                "packing": PackingTypes["SimpleCubic"],
                "color": ti.Vector([1,1,1]),
            },
        ],
    },
    {
        "def": {
            "mass": 1 * C_m,
            "law": force_law( [       # Hermite Spline Definition
                [0.9*C_d,  2*C_f,  0.0],
                [1.0*C_d,  0.0*C_f,  -4/0.2],
                [1.1*C_d,  -2.0*C_f,  0.0],
                [1.2*C_d,  1.0*C_f,  0.0],
                [1.4*C_d,  0.0*C_f,  0.0],
            ] ),
            "self_weighted": 1,
            "is_static": 0,
            "color": ti.Vector([0.3,0.5,0.7]),
        },
        "objs": [
            { # Simple Cube
                "origin": ti.Vector(unnormalize_coords(0.4, 3, 0.4)), # was 3
                "bbox":   ti.Vector([ 10*C_d, 10*C_d, 10*C_d ]),
                "normal": None,
                "grid_orientation": None,
                "packing": PackingTypes["SimpleCubic"],
                "fixed_nodes_bbox": ti.Vector([0,0,0]),
                "color": ti.Vector([1,1,1]),
            },
        ],
    }
]

MATERIAL_CONFIG = MATERIAL_CONFIG_FLUIDS
total_particles = 0
total_objs = 0

for mat_id, mat in enumerate(MATERIAL_CONFIG):
    mat["def"]["start_idx"] = total_particles
    n_particles_per_mat = 0
    for obj_id, obj in enumerate(mat["objs"]):
        obj["group_id"] = total_objs
        total_objs += 1
        obj["start_idx"] = total_particles
        bbox = obj["bbox"]
        el_count = 0
        if obj["packing"] == PackingTypes["SimpleCubic"]:
            el_count = 1
            spacing = obj["packing"]["spacing"]
            for i in range(3):
                if bbox[i] == 0:
                    continue
                el_count *= ti.floor(bbox[i] / spacing[i])
        elif obj["packing"] == PackingTypes["HexGrid"]:
            el_count = 1
            bounds = [0,0]
            cur = 0
            grid_or = obj["grid_orientation"]
            for i in range(3):
                if bbox[i] == 0:
                    continue
                bounds[cur] = bbox[i]
                cur += 1
            spacing = obj["packing"]["spacing"]
            for i in range(2):
                lut_idx = (i + grid_or) % 2
                el_count *= ti.floor(bounds[lut_idx] / spacing[lut_idx])
        elif obj["packing"] == PackingTypes["Random"]:
            el_count = 1
            spacing = obj["packing"]["spacing"]
            for i in range(3):
                if bbox[i] == 0:
                    continue
                el_count *= ti.floor(bbox[i] / spacing[i])
        obj["n_particles"] = el_count
        n_particles_per_mat += el_count
        total_particles += el_count
    mat["def"]["n_particles"] = n_particles_per_mat


"""
Dataclasses
"""

@ti.dataclass
class ParticleType:
    mass: float
    n_particles: int
    start_idx: int
    is_static: int
    law: force_law
    h_sizes: force_distances
    self_weighted: int
    color: vec3f

    # @ti.func
    def populate_h(self):
        # TODO: should this be a top-level fn? or converted into a func?
        # TODO: remove refs to n_law_bps by using self's law mat dim
        for i in range(0,N_LAW_BREAKPOINTS-1):
            self.h_sizes[i] = self.law[i+1, 0] - self.law[i, 0]

    @ti.func
    def eval_law(self, d: float) -> float:
        # TODO: get eraly returns working
        # force = self.law[0, 1]
        segment = 0
        while  d > self.law[segment+1, 0]:
            segment = segment + 1
            if segment == N_LAW_BREAKPOINTS-1:
                segment = N_LAW_BREAKPOINTS-2
                break
                # force = self.law[segment, 1]
        
        h = self.h_sizes[segment]
        t = ti.min(h,ti.max(0,(d - self.law[segment, 0])))

        """Hermite Interp"""
        # TODO: check that this is working correctly!
        phi_0 = (1 - 3 * t**2 / h**2 + 2 * t ** 3 / h**3) * self.law[segment, 1]
        phi_1 = (t - 2 * t**2 / h + t**3 / h**2) *          self.law[segment, 2]
        phi_2 = (3 * t**2 / h**2 - 2 * t ** 3 / h**3) *     self.law[segment + 1, 1]
        phi_3 = (-t**2 / h + t**3 / h**2) *                 self.law[segment + 1, 2]
        return phi_0 + phi_1 + phi_2 + phi_3

@ti.dataclass
class ParticleGroup:
    n_particles: int
    start_idx: int
    origin: vec3f
    bbox: vec3f
    normal: int
    packing: int
    grid_or: int
    fixed_nodes_bbox: vec3i16
    fixed_load: vec3f
    fixed_load_bbox: mat3f
    color: vec3f

@ti.dataclass
class Particle:
    pos: vec3f
    vel: vec3f
    acc: vec3f
    fixed_pos: ti.i8
    fixed_load: vec3f
    bin_addr: vec3i32
    el_type: int
    group_id: int

    @ti.func
    def verlet_vel_pos_update(self, dt: float):
        
        # if self.pos.z < domain[2,1]*0.05 or self.pos.z > domain[2,1]*1.95:
        if self.fixed_pos == 1 or particle_types[self.el_type].is_static == 1:
            """Fixed BC"""
            self.vel = ti.Vector([0.0, 0.0, 0.0])
        else:
            """Verlet Vel Update for t+dt/2"""
            self.vel += dt * self.acc / 2 # vel(t + dt/2)

        """Verlet Pos Update for t+dt"""
        self.pos += dt * self.vel     # pos(t+dt)

        """Update Bin Info"""
        bin_idx = ti.min(ti.max(ti.floor((self.pos - domain_mins)/domain_ranges * BINS_PER_AXIS, dtype=ti.i32),0),BINS_PER_AXIS-1)
        bin_counts[bin_idx[0], bin_idx[1], bin_idx[2]] += 1
        self.bin_addr = bin_idx
        # TODO: boundary collisions? or make bounds elements.
    
    @ti.func
    def create_fixed_boundary_from_initial_pos(self, bbox):
        is_fixed = True
        for i in range(3):
            if self.pos[i] < bbox[i, 0] or self.pos[i] > bbox[i,1]:
                is_fixed = False
        if is_fixed:
            self.fixed_pos = ti.cast(1,ti.i8)

    @ti.func
    def create_fixed_load_from_initial_pos(self, bbox, load):
        is_loaded = True
        for i in range(3):
            if self.pos[i] < bbox[i, 0] or self.pos[i] > bbox[i,1]:
                is_loaded = False
        if is_loaded:
            self.fixed_load = load
        





"""
Field Init
"""

# TODO: Allow variable number of particles per type
p_types = [ParticleType(**material["def"]) for material in MATERIAL_CONFIG]
particle_types = ParticleType.field(shape=(len(p_types)))
for i, p_type in enumerate(p_types):
    particle_types[i] = p_type
    particle_types[i].populate_h()

imf_force = ParticleType(**inter_particle_force_law)
imf_force.populate_h()

p_groups = []
for mat in MATERIAL_CONFIG:
    for obj in mat["objs"]:
        args = {key: ((val if val != None else -1) if key != "packing" else obj["packing"]["idx"]) for key,val in obj.items()}
        p_groups.append(ParticleGroup(**args))
particle_groups = ParticleGroup.field(shape=(len(p_groups)))
for i, obj in enumerate(p_groups):
    particle_groups[i] = obj

particles = Particle.field(shape=(total_particles))
particle_ids = ti.field(shape=(total_particles), dtype=int)
sphere_pts = ti.Vector.field(3,dtype=ti.f32, shape=(total_particles))
colors = ti.Vector.field(3,dtype=ti.f32, shape=(total_particles))
law_graph_pts = ti.field(dtype=float, shape=(len(p_types), 10000, 2))



"""
Kernels
"""

# TODO: get this kernel working!
# @ti.kernel
# def init_p_types():
#     for i in range(particle_types.shape[0]):
#         particle_types[i] = p_types[i]
#         particle_types[i].populate_h()

@ti.kernel
def populate_law_graph_pts():
    for type_id, i in ti.ndrange(law_graph_pts.shape[0], law_graph_pts.shape[1]):
        t = i / (law_graph_pts.shape[1] - 1) * 3*C_d
        law_graph_pts[type_id, i, 0] = t
        law_graph_pts[type_id, i, 1] = particle_types[type_id].eval_law(t)

z_low_bc = ti.Matrix.cols([
    unnormalize_coords(0,0,0),
    unnormalize_coords(1,1,0.05),
])

z_high_bc = ti.Matrix.cols([
    unnormalize_coords(0,0,0.95),
    unnormalize_coords(1,1,1),
])
@ti.kernel
def init_particles():
    for p_idx in particles:
        el_type = 0
        if particle_types.shape[0] > 1:
            while p_idx >= particle_types[el_type + 1].start_idx:
                el_type = el_type + 1
                if el_type == particle_types.shape[0] - 1:
                    break
        particles[p_idx].el_type = el_type
        p_idx_within_type = p_idx - particle_types[el_type].start_idx

        group_id = 0
        if particle_groups.shape[0] > 1:
            while p_idx >= particle_groups[group_id + 1].start_idx:
                group_id = group_id + 1
                if group_id == particle_groups.shape[0] - 1:
                    break
        particles[p_idx].group_id = group_id
        group_def = particle_groups[group_id]
        p_idx_within_group = p_idx - group_def.start_idx

        x_plane_idx = 0
        y_row_idx = 0
        z_col_idx = 0
        """SimpleCubic"""
        if group_def.packing == PackingTypes["SimpleCubic"]["idx"]:
            spacing = PackingTypes["SimpleCubic"]["spacing"]
            particles_per_axis = ti.floor(group_def.bbox / spacing)
            for i in range(3):
                if particles_per_axis[i] == 0:
                    particles_per_axis[i] = 1

            particles_per_depth = particles_per_axis[0]

            particles_per_col = particles_per_axis[1]
            particles_per_row = particles_per_axis[2]
            particles_per_plane = particles_per_col*particles_per_row

            x_plane_idx = ti.floor(p_idx_within_group / particles_per_plane)
            y_row_idx = ti.floor((p_idx_within_group - x_plane_idx*particles_per_plane) / particles_per_row)
            z_col_idx = (p_idx_within_group - x_plane_idx*particles_per_plane - y_row_idx * particles_per_row) 

            particles[p_idx].pos = ti.Vector([x_plane_idx, y_row_idx, z_col_idx])*C_d + group_def.origin
            particles[p_idx].vel = ti.Vector([0, 0, 0])
        elif group_def.packing == PackingTypes["HexGrid"]["idx"]:
            spacing = PackingTypes["HexGrid"]["spacing"]
            axes = ti.Vector([-1, -1], dt=int)
            if group_def.normal == Axis_Plane["X-YZ"]:
                axes = ti.Vector([Axis_Plane["Y-XZ"], Axis_Plane["Z-XY"]] )
            elif group_def.normal == Axis_Plane["Y-XZ"]:
                axes = ti.Vector([Axis_Plane["X-YZ"], Axis_Plane["Z-XY"]] )
            elif group_def.normal == Axis_Plane["Z-XY"]:
                axes = ti.Vector([Axis_Plane["X-YZ"], Axis_Plane["Y-XZ"]] )
            particles_per_line = ti.floor(group_def.bbox[axes[grid_or]] / spacing[0])
            linear_idx = p_idx_within_group % particles_per_line
            stack_idx = ti.floor(p_idx_within_group / particles_per_line)
            linear_shift = linear_idx * spacing[0] + (stack_idx % 2) * (C_d/2)
            stack_shift = stack_idx * spacing[1]
            particles[p_idx].pos = ti.Vector([0, 0, 0]) + group_def.origin
            particles[p_idx].pos[axes[grid_or]] += linear_shift
            particles[p_idx].pos[axes[1-grid_or]] += stack_shift
            particles[p_idx].vel = ti.Vector([0, 0, 0])
        elif group_def.packing == PackingTypes["Random"]["idx"]:
            particles[p_idx].pos = group_def.origin + group_def.bbox*ti.Vector([ti.random(), ti.random(), ti.random()])
            particles[p_idx].vel = ti.Vector([0, 0, 0])
        else:
            particles[p_idx].pos = domain_mins + domain_ranges*ti.Vector([ti.random(), ti.random(), ti.random()])
            particles[p_idx].fixed_pos = ti.cast(1, ti.i8)

        """Add fixed Boundaries"""
        fixed_bc_flags = group_def.fixed_nodes_bbox
        fixed_load = group_def.fixed_load
        fixed_load_bbox = group_def.fixed_load_bbox
    
        for i in range(3):
            bc_flag = fixed_bc_flags[i]
            if i == 0:
                if bc_flag == 1 or bc_flag == -1:
                    particles[p_idx].create_fixed_boundary_from_initial_pos(z_low_bc)
                if bc_flag == 2 or bc_flag == -1:
                    particles[p_idx].create_fixed_boundary_from_initial_pos(z_high_bc)
        
        """Apply Fixed Load"""
        particles[p_idx].create_fixed_load_from_initial_pos(fixed_load_bbox, fixed_load)

        if False:
            for p_type in range(particle_types.shape[0]):
                if particles[p_idx].el_type == p_type:
                    """Planar HexaGrid"""
                    pts_per_row = (domain[2,1]/ C_d)
                    row = ti.floor(p_idx_within_type / pts_per_row)
                    v_shift = row * (C_d/2) * ti.sqrt(3) + (p_type % 3)* 12*C_d
                    d = p_idx_within_type % pts_per_row
                    # TODO: this is using global C_d
                    h_shift = (row % 2)*C_d/2
                    d_shift = ti.floor(p_type / 3) * 10*C_d
                    particles[p_idx].pos = ti.Vector([domain[0,1]/2 + d_shift, v_shift + domain[1,1]/2, d*C_d+h_shift])
                    particles[p_idx].vel = ti.Vector([0.0, 0.0, 0.0])
                
                    """Apply fixed loads to certain points"""
                    # if particles[p_idx].pos.z >= 0.95*domain[2,1] and particles[p_idx].pos.z <= 1.0*domain[2,1]:
                    #     particles[p_idx].fixed_load = ti.Vector([0,-0.01/particle_types[particles[p_idx].el_type].mass, 0])

                    if particles[p_idx].pos.z >= 0.475*domain[2,1] and particles[p_idx].pos.z <= 0.525*domain[2,1]:
                        particles[p_idx].fixed_load = ti.Vector([0,-0.01/particle_types[particles[p_idx].el_type].mass, 0])

                    """Add fixed boundaries"""
                    bbox_a = ti.Matrix([
                        [domain[0,0], domain[0,1]],
                        [domain[1,0], domain[1,1]],
                        [domain[2,0], (domain[2,1]-domain[2,0])*0.05],
                    ])
                    particles[p_idx].create_fixed_boundary_from_initial_pos(bbox_a)

                    bbox_b = ti.Matrix([
                        [domain[0,0], domain[0,1]],
                        [domain[1,0], domain[1,1]],
                        [domain[2,1]-(domain[2,1]-domain[2,0])*0.05, domain[2,1]],
                    ])
                    particles[p_idx].create_fixed_boundary_from_initial_pos(bbox_b)

                # """Random Init"""
                # particles[p_idx].pos = ti.Vector([ti.random()*domain[0,1], ti.random()*domain[1,1], ti.random()*domain[2,1],])
                # particles[p_idx].vel = ti.Vector([ti.random(), ti.random(), ti.random()])*C_v*2-C_v
@ti.kernel
def init_colorfield():
    for p_group in particle_groups:
        particle_groups[p_group].color = ti.Vector([ti.random(), ti.random(), ti.random()])*0.1
    ti.sync()
    for p_idx in particles:
        group_id = particles[p_idx].group_id
        el_type = particles[p_idx].el_type
        color = particle_groups[group_id].color + particle_types[el_type].color
        colors[p_idx] = color
        if particles[p_idx].fixed_load.norm() > 0:
            colors[p_idx] = ti.Vector([1,0,0])
        if particles[p_idx].fixed_pos == 1:
            colors[p_idx] = ti.Vector([0,1,0])

@ti.kernel
def verlet_vel_pos_update(dt: float):
    bin_counts.fill(0)
    col_counts.fill(0)
    plane_counts.fill(0)

    """Verlet Update"""
    for p_idx in particles:
        particles[p_idx].verlet_vel_pos_update(dt)
    ti.sync()

    """----- CumSum -----"""
    """Collect columnar counts"""
    for i,j,k in bin_counts:
        col_counts[i,j] += bin_counts[i,j,k]
    ti.sync()
    
    """Collect planar counts"""
    for i,j in col_counts:
        plane_counts[i] += col_counts[i,j]
    ti.sync()

    """CumSum along planar axis"""
    plane_starts[0] = 0
    col_starts[0,0] = 0
    bin_starts[0,0,0] = col_starts[0,0]
    bin_cur[0,0,0] = bin_starts[0,0,0]
    bin_ends[0,0,0] = bin_starts[0,0,0] + bin_counts[0,0,0]
    ti.loop_config(serialize=True)
    for i in range(1, BINS_PER_AXIS):
        plane_starts[i] = plane_starts[i-1] + plane_counts[i-1]
        col_starts[i,0] = plane_starts[i]
        bin_starts[i,0,0] = col_starts[i,0]
        bin_cur[i,0,0] = bin_starts[i,0,0]
        bin_ends[i,0,0] = bin_starts[i,0,0] + bin_counts[i,0,0]
    ti.sync()
    
    """CumSum along columnar axis"""
    for i in plane_starts:
        for j in range(1, BINS_PER_AXIS):
            col_starts[i,j] = col_starts[i,j-1] + col_counts[i,j-1]
            bin_starts[i,j,0] = col_starts[i,j]
            bin_cur[i,j,0] = bin_starts[i,j,0]
            bin_ends[i,j,0] = bin_starts[i,j,0] + bin_counts[i,j,0]
    ti.sync()
    
    """CumSum for bins"""
    for i,j in col_starts:
        for k in range(1, BINS_PER_AXIS):
            bin_starts[i,j,k] = bin_ends[i,j,k-1] 
            bin_cur[i,j,k] = bin_starts[i,j,k]
            bin_ends[i,j,k] = bin_starts[i,j,k] + bin_counts[i,j,k]
    ti.sync()

    """----- Sort ----"""
    for p_idx in particles:
        bin_addr = particles[p_idx].bin_addr
        sort_idx = ti.atomic_add(bin_cur[bin_addr[0], bin_addr[1], bin_addr[2]], 1)  # NB: atomic add returns OLD index
        particle_ids[sort_idx] = p_idx
    ti.sync()

            
        

@ti.kernel
def verlet_acc_vel_update(dt: float):
    # TODO: explore performance impact of e.g. mass operation, which is unused
    for p_idx_a in particles:
        """Reset"""
        particles[p_idx_a].acc = ti.Vector([0.0,0.0,0.0])

        """Get ElType"""
        el_type = particles[p_idx_a].el_type
        is_static = particle_types[el_type].is_static
        self_weighted = particle_types[el_type].self_weighted
        mass = particle_types[el_type].mass

        """Load Field"""
        if particles[p_idx_a].pos[2] >= domain[2,1]*2.0 and particles[p_idx_a].pos[2] <= domain[2,1]*2.00:
            particles[p_idx_a].acc += ti.Vector([0.0,-0.1/mass, 0])
        
        if self_weighted == 1:
            particles[p_idx_a].acc += GRAV_VEC / mass

        """Fixed Loads"""
        particles[p_idx_a].acc += particles[p_idx_a].fixed_load
        
        """Force Law (Binned)"""
        pos_a = particles[p_idx_a].pos
        for i,j,k in ti.ndrange(3,3,3):
            bin_x = ti.max(ti.min(particles[p_idx_a].bin_addr.x-1 + i, BINS_PER_AXIS-1), 0)
            bin_y = ti.max(ti.min(particles[p_idx_a].bin_addr.y-1 + j, BINS_PER_AXIS-1), 0)
            bin_z = ti.max(ti.min(particles[p_idx_a].bin_addr.z-1 + k, BINS_PER_AXIS-1), 0)
            bin_start = bin_starts[bin_x, bin_y, bin_z]
            bin_end = bin_ends[bin_x, bin_y, bin_z]

            for sort_idx in range(bin_start, bin_end):
                p_idx_b = particle_ids[sort_idx]
                if p_idx_a > p_idx_b: # Prevent symmetric computation
                    if el_type == particles[p_idx_b].el_type:
                        """Handle ixs between particles of same type"""
                        if is_static:
                            pass
                        else:
                            pos_b = particles[p_idx_b].pos
                            d = ti.sqrt(ti.pow(pos_a.x - pos_b.x, 2) + ti.pow(pos_a.y - pos_b.y, 2) + ti.pow(pos_a.z - pos_b.z, 2))
                            a_mag = particle_types[el_type].eval_law(d) / mass #TODO: allow different mass per particles?
                            a = (pos_a-pos_b)*a_mag
                            particles[p_idx_a].acc += a
                            particles[p_idx_b].acc += -a
                    else:
                        """Handle ixs between particles of differing type"""
                        pos_b = particles[p_idx_b].pos
                        d = ti.sqrt(ti.pow(pos_a.x - pos_b.x, 2) + ti.pow(pos_a.y - pos_b.y, 2) + ti.pow(pos_a.z - pos_b.z, 2))
                        mag = ti.min(ti.max((1.0-d) * 10.0, 0.0), 20.0)
                        mag = ti.min(ti.max((1-d) * 100.0, 0.0), 1000.0)*2
                        # mag = imf_force.eval_law(d)  #TODO: allow different mass per particles?
                        a_mag = mag / mass
                        b_mag = mag / particle_types[particles[p_idx_b].el_type].mass
                        a = (pos_a-pos_b)*a_mag
                        b = (pos_b-pos_a)*b_mag
                        particles[p_idx_a].acc += a
                        particles[p_idx_b].acc += b

    ti.sync()

    """Finish Verlet Vel Update"""
    for p_idx in particles:
        particles[p_idx].vel += dt*particles[p_idx].acc/2
        particles[p_idx].vel *= VEL_DAMPING
    ti.sync()



@ti.kernel
def update_geo():
    for p_idx in particles:
        sphere_pts[p_idx] = particles[p_idx].pos

"""
GUI / Rendering
"""

def init_gui_scene():
  """ 
  Init GUI 
  """
  window = ti.ui.Window('Discrete Elements', res = (1000, 1000), pos = (50, 50))
  gui = window.get_gui()
  canvas = window.get_canvas()
  scene = ti.ui.Scene()
  camera = ti.ui.Camera()
  camera.position(domain[0,0]-domain[0,1], domain[1,1]/2, domain[2,1]/2)
  camera.lookat(domain_centers.x, domain_centers.y, domain_centers.z)
#   camera.lookat(domain[0,1]/2, domain[1,1]/2, domain[2,1]/2)
  camera.up(0,1,0)
  return window, canvas, scene, camera, gui

def build_cam_and_lights(window, camera, scene, t):
  """
  Add cameras and lights
  """
#   camera.track_user_inputs(window, movement_speed=0.03, hold_key=ti.ui.RMB)
  r = ti.max(domain_ranges[0], domain_ranges[2])*2.0
  c_x = r*ti.cos(t) + domain_centers[0]
  c_z = r*ti.sin(t) + domain_centers[2]
  camera.position(c_x, domain[1,1]*2.5, c_z)  
  scene.set_camera(camera)
  scene.ambient_light((0.8, 0.8, 0.8))
  scene.point_light(pos=(domain[0,1]/2, domain[1,1]+0.1*domain[1,1], domain[2,1]/2), color=(1, 1, 1))

def benchmark(n_tests=1000):
    import time
    print("Starting benchmark")
    verlet_vel_pos_update(0)
    verlet_acc_vel_update(0)
    s = time.time()
    for _ in range(n_tests):
        verlet_vel_pos_update(0.00000001)
        verlet_acc_vel_update(0.00000001)
    ti.sync()
    e = time.time()
    b_time = (e-s)/(n_tests*total_particles)
    print(f"benchmark took {b_time*1e9:0.3f} ns/particle")
    print(f"benchmark took {b_time*total_particles*1e3:0.3f} ms/particle field")
    print(f"approx {int(1/b_time)/1e6:0.3f}m particles/s")

def plot_force_laws():
    populate_law_graph_pts()
    vals = law_graph_pts.to_numpy()
    fig = plt.figure(figsize=(5,5))
    for type_id in range(law_graph_pts.shape[0]):
        d = vals[type_id, :, 0]
        y = vals[type_id, :, 1]
        plt.plot(d,y)
        plt.plot(d, np.zeros(shape=d.shape), linewidth=0.52)
    # plt.xlim(0,2.1)
    # plt.ylim(-1,1.1)
    plt.show()

def print_debugs(it):
    if (it % 1000) == 0:
        print(f"\n\nTotal count: {np.sum(bin_counts.to_numpy())}")
        print("\nPlane Counts:")
        print(plane_counts)
        print("\nPlane Starts:")
        print(plane_starts)
        print("\nCol Counts:")
        print(col_counts)
        print("\nCol Starts:")
        print(col_starts)
        print("\nBin Counts:")
        print(bin_counts)
        print("\nBin Starts:")
        print(bin_starts)
        print("\nBin Curs:")
        print(bin_cur)
        raise RuntimeError("STOP")

if __name__ == "__main__":
    import time
    import numpy as np
    import matplotlib.pyplot as plt

    RUN_BENCHMARK = True
    PLOT_FORCE_LAWS = False
    PRINT_DEBUG = False
    SAVE_IMAGE = True

    print(f"Total Particles: {total_particles}")
    print(f"Bins per axis: {BINS_PER_AXIS}")

    init_particles()
    init_colorfield()

    if RUN_BENCHMARK:
        benchmark()

    if PLOT_FORCE_LAWS:
        plot_force_laws()

    window, canvas, scene, camera, gui = init_gui_scene()

    N_UPDATES_PER_FRAME = 50
    it = 0
    ITS_PER_BENCHMARK = 100

    c_time = 0.0
    c_dt = 0.004
    s = time.time()
    while window.running:
        for i in range(N_UPDATES_PER_FRAME):
            verlet_vel_pos_update(dt)
            verlet_acc_vel_update(dt)

        build_cam_and_lights(window, camera, scene, c_time)
        update_geo()
        scene.particles(sphere_pts,  radius = 0.5*C_d, per_vertex_color=colors, index_offset=particle_types[1].start_idx)#color = (0.74,0.4,0.6))
        canvas.scene(scene)

        if PRINT_DEBUG:
            print_debugs(it)

        it += 1
        c_time += c_dt


        if it % ITS_PER_BENCHMARK == 0:
            e = time.time()
            b_time = (e-s)/(ITS_PER_BENCHMARK*N_UPDATES_PER_FRAME*total_particles)
            print(f"benchmark with rendering took {b_time*1e9:0.3f} ns/particle")
            print(f"benchmark with rendering took {b_time*total_particles*1e3:0.3f} ms/particle field")
            print(f"approx {int(1/b_time)/1e6:0.3f} m particles/s")
            s = time.time()



        if SAVE_IMAGE:
            window.save_image(f"./week_6_dem/images/fluid-box-4_{it:05d}.png")
        window.show()
