from itertools import product

import matplotlib.pyplot as plt
import numpy as np


def check_line_voxel(px, py, pz, dx, dy, dz, vx, vy, vz, c):
    """Check if a line intersects a voxel.

    Parameters:
    - px, py, pz: voxel coner coordinates
    - dx, dy, dz: line origin coordinates
    - vx, vy, vz: line direction coordinates
    - c: voxel size
    """
    # Compute the intersection bounds
    kx1 = (px - dx) / vx
    ky1 = (py - dy) / vy
    kz1 = (pz - dz) / vz
    kx2 = (px - dx + c) / vx
    ky2 = (py - dy + c) / vy
    kz2 = (pz - dz + c) / vz

    # Order the bounds
    kxmin = np.min(np.concatenate([kx1[:, np.newaxis], kx2[:, np.newaxis]], axis=1), axis=1)
    kymin = np.min(np.concatenate([ky1[:, np.newaxis], ky2[:, np.newaxis]], axis=1), axis=1)
    kzmin = np.min(np.concatenate([kz1[:, np.newaxis], kz2[:, np.newaxis]], axis=1), axis=1)
    kxmax = np.max(np.concatenate([kx1[:, np.newaxis], kx2[:, np.newaxis]], axis=1), axis=1)
    kymax = np.max(np.concatenate([ky1[:, np.newaxis], ky2[:, np.newaxis]], axis=1), axis=1)
    kzmax = np.max(np.concatenate([kz1[:, np.newaxis], kz2[:, np.newaxis]], axis=1), axis=1)

    # Check if the bounds overlap
    kmax = np.min(np.concatenate([kxmax[:, np.newaxis], kymax[:, np.newaxis], kzmax[:, np.newaxis]], axis=1), axis=1)
    kmin = np.max(np.concatenate([kxmin[:, np.newaxis], kymin[:, np.newaxis], kzmin[:, np.newaxis]], axis=1), axis=1)
    return kmin <= kmax


c = 1.0
points = np.array(
    [[x, y, z] for x, y, z in product(np.arange(-5.0, 4.0, c), np.arange(-5.0, 4.0, c), np.arange(-5.0, 4.0, c))]
)
while True:

    fig = plt.figure()
    ax = plt.axes(projection="3d")

    d = np.random.rand(3) * 1 - 0.5
    v = np.random.rand(3) * 1 - 0.5

    px, py, pz = points[:, 0], points[:, 1], points[:, 2]
    dx, dy, dz = d
    vx, vy, vz = v

    bool_vect = check_line_voxel(px, py, pz, dx, dy, dz, vx, vy, vz, c)

    # plot cube
    for i, (px, py, pz) in enumerate(points):
        if not bool_vect[i]:
            continue

        ax.plot([px, px + c], [py, py], [pz, pz], "b")
        ax.plot([px, px + c], [py, py], [pz + c, pz + c], "b")
        ax.plot([px, px + c], [py + c, py + c], [pz, pz], "b")
        ax.plot([px, px + c], [py + c, py + c], [pz + c, pz + c], "b")
        ax.plot([px, px], [py, py + c], [pz, pz], "b")
        ax.plot([px, px], [py, py + c], [pz + c, pz + c], "b")
        ax.plot([px + c, px + c], [py, py + c], [pz, pz], "b")
        ax.plot([px + c, px + c], [py, py + c], [pz + c, pz + c], "b")
        ax.plot([px, px], [py, py], [pz, pz + c], "b")
        ax.plot([px, px], [py + c, py + c], [pz, pz + c], "b")
        ax.plot([px + c, px + c], [py, py], [pz, pz + c], "b")
        ax.plot([px + c, px + c], [py + c, py + c], [pz, pz + c], "b")

    # plot line
    ax.plot([dx, dx + vx], [dy, dy + vy], [dz, dz + vz], "g")

    plt.show()