"""Palette-constrained dithering.

Every routine takes the working image in CIELAB plus a per-pixel table of the
palette indices that pixel is *allowed* to use (because the VIC-II only lets a
given screen cell show a small set of colours), and returns an (H,W) image of
chosen palette indices (0..15).  Because the allowed set is per-pixel, error that
diffuses across a cell boundary is automatically re-clamped to the neighbour
cell's own colours -- exactly the constraint real C64 hardware imposes.
"""

from __future__ import annotations

import numpy as np

DITHER_MODES = ["bayer", "bluenoise", "yliluoma", "floyd", "atkinson", "stucki",
                "jarvis", "sierra", "sierra_lite", "burkes", "riemersma",
                "ostromoukhov", "none"]


def _bayer_int(n: int) -> np.ndarray:
    """Integer Bayer matrix with values 0..n*n-1 (n a power of two)."""
    if n == 1:
        return np.array([[0]])
    s = _bayer_int(n // 2)
    return np.block([
        [4 * s + 0, 4 * s + 2],
        [4 * s + 3, 4 * s + 1],
    ])


def bayer_matrix(n: int) -> np.ndarray:
    """Normalised (0..1) Bayer threshold matrix of size n x n (n power of two).

    Thresholds are centred at (i + 0.5) / n^2 so they span (0,1) symmetrically --
    normalising only once (not at every recursion level, which would compress the
    range and collapse the dither toward a single threshold)."""
    return (_bayer_int(n) + 0.5) / (n * n)


def _gather_colors(palette_lab: np.ndarray, allowed: np.ndarray) -> np.ndarray:
    # allowed: (H,W,K) palette indices -> (H,W,K,3) Lab
    return palette_lab[allowed]


def _ordered_core(img_lab, allowed, palette_lab, thr_full, strength=1.0):
    """Ordered dithering between each pixel's two best colours, using a supplied
    (H,W) threshold map (Bayer, blue-noise, ...).  For every pixel we take its
    nearest and second-nearest allowed colour, project the pixel onto the segment
    between them, and the threshold decides which of the two to emit -- smooth
    blends without ever leaving the cell's legal colour set."""
    H, W, _ = img_lab.shape
    colors = _gather_colors(palette_lab, allowed)            # (H,W,K,3)
    d = np.sum((img_lab[:, :, None, :] - colors) ** 2, axis=-1)  # (H,W,K)

    i1 = np.argmin(d, axis=-1)
    d2 = np.array(d)
    np.put_along_axis(d2, i1[..., None], np.inf, axis=-1)
    i2 = np.argmin(d2, axis=-1)

    yy, xx = np.indices((H, W))
    c1 = colors[yy, xx, i1]                                  # (H,W,3)
    c2 = colors[yy, xx, i2]
    seg = c2 - c1
    seg_len2 = np.sum(seg * seg, axis=-1) + 1e-9
    t = np.sum((img_lab - c1) * seg, axis=-1) / seg_len2     # projection 0..1
    t = np.clip(t * strength, 0.0, 1.0)

    chosen = np.where(t > thr_full, i2, i1)
    return np.take_along_axis(allowed, chosen[..., None], axis=-1)[..., 0]


def quantize_ordered(img_lab, allowed, palette_lab, strength=1.0, n=8):
    """Ordered (Bayer) dithering."""
    H, W, _ = img_lab.shape
    yy, xx = np.indices((H, W))
    thr = bayer_matrix(n)
    return _ordered_core(img_lab, allowed, palette_lab, thr[yy % n, xx % n], strength)


_BLUENOISE = {}        # cached void-and-cluster matrices, keyed by size


def bluenoise_matrix(n: int = 64, sigma: float = 1.9) -> np.ndarray:
    """Tileable n x n blue-noise threshold matrix (0..1), via void-and-cluster
    (Ulichney).  Like Bayer but with no regular cross-hatch grid -- the dot
    pattern is isotropic and organic, so gradients look far cleaner."""
    if n in _BLUENOISE:
        return _BLUENOISE[n]
    rng = np.random.default_rng(12345)
    # toroidal Gaussian centred at (0,0); rolling it centres it on any pixel
    ax = np.minimum(np.arange(n), n - np.arange(n))
    g = np.exp(-(ax[:, None] ** 2 + ax[None, :] ** 2) / (2 * sigma ** 2))

    def roll(p):
        r, c = divmod(p, n)
        return np.roll(np.roll(g, r, axis=0), c, axis=1)

    binary = np.zeros((n, n), bool)
    ones = max(1, (n * n) // 10)
    for p in rng.choice(n * n, ones, replace=False):
        binary.flat[p] = True
    e = np.zeros((n, n))
    for p in np.flatnonzero(binary):
        e += roll(p)

    # phase 1: even out the initial pattern (move tightest cluster -> largest void)
    while True:
        tc = int(np.argmax(np.where(binary.flat, e.flat, -np.inf)))
        binary.flat[tc] = False; e -= roll(tc)
        lv = int(np.argmin(np.where(binary.flat, np.inf, e.flat)))
        binary.flat[lv] = True; e += roll(lv)
        if lv == tc:
            break

    rank = np.full(n * n, -1, np.int64)
    proto = binary.copy(); ep = e.copy()
    # phase 2a: remove tightest clusters, ranking downward
    b = proto.copy(); e = ep.copy(); cnt = int(b.sum())
    for r in range(cnt - 1, -1, -1):
        tc = int(np.argmax(np.where(b.flat, e.flat, -np.inf)))
        b.flat[tc] = False; e -= roll(tc); rank[tc] = r
    # phase 2b: fill largest voids, ranking upward
    b = proto.copy(); e = ep.copy()
    for r in range(cnt, n * n):
        lv = int(np.argmin(np.where(b.flat, np.inf, e.flat)))
        b.flat[lv] = True; e += roll(lv); rank[lv] = r

    m = (rank.reshape(n, n) + 0.5) / (n * n)
    _BLUENOISE[n] = m
    return m


def quantize_bluenoise(img_lab, allowed, palette_lab, n=64):
    """Ordered dithering with a blue-noise mask instead of Bayer (no grid)."""
    H, W, _ = img_lab.shape
    yy, xx = np.indices((H, W))
    thr = bluenoise_matrix(n)
    return _ordered_core(img_lab, allowed, palette_lab, thr[yy % n, xx % n])


def _quantize_diffusion(img_lab, allowed, palette_lab, kernel, divisor):
    """Generic serpentine error-diffusion constrained to per-pixel allowed sets."""
    H, W, _ = img_lab.shape
    work = img_lab.astype(np.float64).copy()
    out = np.zeros((H, W), dtype=np.int64)
    pal = palette_lab
    for y in range(H):
        cols = range(W) if (y % 2 == 0) else range(W - 1, -1, -1)
        flip = 1 if (y % 2 == 0) else -1
        for x in cols:
            allow = allowed[y, x]
            cand = pal[allow]
            diff = cand - work[y, x]
            k = int(allow[np.argmin(np.sum(diff * diff, axis=-1))])
            out[y, x] = k
            err = work[y, x] - pal[k]
            for dx, dy, w in kernel:
                nx, ny = x + dx * flip, y + dy
                if 0 <= nx < W and 0 <= ny < H:
                    work[ny, nx] += err * (w / divisor)
    return out


# (dx, dy, weight) relative to current pixel, assuming left-to-right scan.
_FLOYD = [(1, 0, 7), (-1, 1, 3), (0, 1, 5), (1, 1, 1)]
_ATKINSON = [(1, 0, 1), (2, 0, 1), (-1, 1, 1), (0, 1, 1), (1, 1, 1), (0, 2, 1)]
# Larger kernels spread error further -> smoother gradients (best for grayscale).
_STUCKI = [(1, 0, 8), (2, 0, 4),
           (-2, 1, 2), (-1, 1, 4), (0, 1, 8), (1, 1, 4), (2, 1, 2),
           (-2, 2, 1), (-1, 2, 2), (0, 2, 4), (1, 2, 2), (2, 2, 1)]
_JARVIS = [(1, 0, 7), (2, 0, 5),
           (-2, 1, 3), (-1, 1, 5), (0, 1, 7), (1, 1, 5), (2, 1, 3),
           (-2, 2, 1), (-1, 2, 3), (0, 2, 5), (1, 2, 3), (2, 2, 1)]
# Sierra-3: like Jarvis but lighter third row -> a touch less smearing.
_SIERRA = [(1, 0, 5), (2, 0, 3),
           (-2, 1, 2), (-1, 1, 4), (0, 1, 5), (1, 1, 4), (2, 1, 2),
           (-1, 2, 2), (0, 2, 3), (1, 2, 2)]
# Sierra-Lite: tiny 3-tap kernel -> crisp, fast, minimal bleed across cells.
_SIERRA_LITE = [(1, 0, 2), (-1, 1, 1), (0, 1, 1)]
# Burkes: two-row Stucki relative -> smooth gradients, cheaper than Stucki.
_BURKES = [(1, 0, 8), (2, 0, 4),
           (-2, 1, 2), (-1, 1, 4), (0, 1, 8), (1, 1, 4), (2, 1, 2)]


def quantize_floyd(img_lab, allowed, palette_lab):
    return _quantize_diffusion(img_lab, allowed, palette_lab, _FLOYD, 16)


def quantize_atkinson(img_lab, allowed, palette_lab):
    return _quantize_diffusion(img_lab, allowed, palette_lab, _ATKINSON, 8)


def quantize_stucki(img_lab, allowed, palette_lab):
    return _quantize_diffusion(img_lab, allowed, palette_lab, _STUCKI, 42)


def quantize_jarvis(img_lab, allowed, palette_lab):
    return _quantize_diffusion(img_lab, allowed, palette_lab, _JARVIS, 48)


def quantize_sierra(img_lab, allowed, palette_lab):
    return _quantize_diffusion(img_lab, allowed, palette_lab, _SIERRA, 32)


def quantize_sierra_lite(img_lab, allowed, palette_lab):
    return _quantize_diffusion(img_lab, allowed, palette_lab, _SIERRA_LITE, 4)


def quantize_burkes(img_lab, allowed, palette_lab):
    return _quantize_diffusion(img_lab, allowed, palette_lab, _BURKES, 32)


def _hilbert_path(W: int, H: int):
    """Sequence of (x,y) covering a WxH grid along a Hilbert space-filling curve
    (points outside the grid are skipped).  Used by Riemersma dithering so the
    1-D error trail stays spatially compact in 2-D."""
    order = 1
    while (1 << order) < max(W, H):
        order += 1
    side = 1 << order
    pts = []
    for d in range(side * side):
        rx = ry = 0
        t = d
        x = y = 0
        s = 1
        while s < side:
            rx = 1 & (t // 2)
            ry = 1 & (t ^ rx)
            if ry == 0:                       # rotate quadrant
                if rx == 1:
                    x = s - 1 - x
                    y = s - 1 - y
                x, y = y, x
            x += s * rx
            y += s * ry
            t //= 4
            s <<= 1
        if x < W and y < H:
            pts.append((x, y))
    return pts


def quantize_riemersma(img_lab, allowed, palette_lab, qlen=16, ratio=1.0 / 16):
    """Riemersma dithering: error diffusion along a Hilbert curve with a decaying
    memory of recent errors (geometric weights).  Spreads error in every
    direction without the directional grain of raster diffusion."""
    H, W, _ = img_lab.shape
    pal = palette_lab
    out = np.zeros((H, W), dtype=np.int64)
    # geometric weights, most-recent first, summing to 1 (error-conserving)
    w = ratio ** (np.arange(qlen) / max(1, qlen - 1))
    w = w / w.sum()
    hist = np.zeros((qlen, 3), dtype=np.float64)
    for x, y in _hilbert_path(W, H):
        target = img_lab[y, x] + w @ hist
        allow = allowed[y, x]
        cand = pal[allow]
        diff = cand - target
        k = int(allow[np.argmin(np.sum(diff * diff, axis=-1))])
        out[y, x] = k
        hist[1:] = hist[:-1]
        hist[0] = img_lab[y, x] - pal[k]
    return out


# Ostromoukhov variable-coefficient error diffusion ("A Simple and Efficient
# Error-Diffusion Algorithm", SIGGRAPH 2001).  The three forward coefficients
# (right, below-left, below) vary with the *tone* being quantised, which breaks
# up the worm artefacts of fixed-kernel diffusion.  Table given at breakpoints
# (luminance 0..255) and linearly interpolated; symmetric about 127.5.
_OSTRO_KNOTS = np.array([
    (0,   13, 0,  5),  (1,   13, 0,  5),  (2,   21, 0, 10),  (3,    7, 0,  4),
    (4,    8, 0,  5),  (10,  47, 3, 28),  (15,  23, 3, 13),  (16,  15, 3, 11),
    (32,  43, 7, 36),  (64,  21, 8, 21),  (96,  39, 7, 35),  (112, 19, 7, 17),
    (127, 38, 8, 35),
], dtype=np.float64)


def _ostro_coeffs():
    """Build the 256x3 (normalised) Ostromoukhov coefficient table by
    interpolating the knot table and mirroring it about the midpoint."""
    if "tab" in _BLUENOISE:           # reuse the module cache dict
        return _BLUENOISE["tab"]
    xs = _OSTRO_KNOTS[:, 0]
    c = np.empty((256, 3))
    half = np.arange(128)
    for j in range(3):
        c[:128, j] = np.interp(half, xs, _OSTRO_KNOTS[:, j + 1])
    c[128:] = c[:128][::-1]           # symmetric for the upper half
    c /= c.sum(axis=1, keepdims=True)
    _BLUENOISE["tab"] = c
    return c


def quantize_ostromoukhov(img_lab, allowed, palette_lab):
    H, W, _ = img_lab.shape
    work = img_lab.astype(np.float64).copy()
    out = np.zeros((H, W), dtype=np.int64)
    pal = palette_lab
    coeffs = _ostro_coeffs()
    for y in range(H):
        ltr = (y % 2 == 0)
        cols = range(W) if ltr else range(W - 1, -1, -1)
        flip = 1 if ltr else -1
        for x in cols:
            allow = allowed[y, x]
            cand = pal[allow]
            diff = cand - work[y, x]
            k = int(allow[np.argmin(np.sum(diff * diff, axis=-1))])
            out[y, x] = k
            err = work[y, x] - pal[k]
            tone = int(np.clip(work[y, x, 0] * 2.55, 0, 255))  # L* (0..100) -> 0..255
            cr, cdl, cd = coeffs[tone]
            for dx, dy, wgt in ((1, 0, cr), (-1, 1, cdl), (0, 1, cd)):
                nx, ny = x + dx * flip, y + dy
                if 0 <= nx < W and 0 <= ny < H:
                    work[ny, nx] += err * wgt
    return out


def quantize_yliluoma(img_lab, allowed, palette_lab, plan=16, n=8):
    """Yliluoma-style ordered dithering.  For each pixel we greedily build a
    'mixing plan' -- a list of `plan` palette entries (with repeats) from its
    allowed set whose running average best matches the target -- then index into
    the plan, sorted by lightness, with the Bayer threshold.  Mixes more than two
    colours per cell, so flat-palette platforms get far richer apparent colour."""
    H, W, _ = img_lab.shape
    colors = _gather_colors(palette_lab, allowed)        # (H,W,K,3)
    yy, xx = np.indices((H, W))

    running = np.zeros((H, W, 3))                          # sum of chosen Lab
    chosen = np.empty((H, W, plan), dtype=np.int64)       # local index into allowed
    for t in range(plan):
        # average if we appended each candidate next
        avg = (running[:, :, None, :] + colors) / (t + 1)
        d = np.sum((avg - img_lab[:, :, None, :]) ** 2, axis=-1)   # (H,W,K)
        pick = np.argmin(d, axis=-1)                       # (H,W)
        chosen[:, :, t] = pick
        running = running + colors[yy, xx, pick]

    # sort each pixel's plan by the lightness (L*) of its colours
    plan_L = colors[yy[..., None], xx[..., None], chosen, 0]       # (H,W,plan)
    order = np.argsort(plan_L, axis=-1)
    sorted_plan = np.take_along_axis(chosen, order, axis=-1)

    thr = bayer_matrix(n)
    idx = np.clip((thr[yy % n, xx % n] * plan).astype(np.int64), 0, plan - 1)
    local = np.take_along_axis(sorted_plan, idx[..., None], axis=-1)[..., 0]
    return np.take_along_axis(allowed, local[..., None], axis=-1)[..., 0]


def quantize_none(img_lab, allowed, palette_lab):
    colors = _gather_colors(palette_lab, allowed)
    d = np.sum((img_lab[:, :, None, :] - colors) ** 2, axis=-1)
    i1 = np.argmin(d, axis=-1)
    return np.take_along_axis(allowed, i1[..., None], axis=-1)[..., 0]


def quantize(img_lab, allowed, palette_lab, mode="bayer"):
    if mode == "bayer":
        return quantize_ordered(img_lab, allowed, palette_lab)
    if mode == "bluenoise":
        return quantize_bluenoise(img_lab, allowed, palette_lab)
    if mode == "yliluoma":
        return quantize_yliluoma(img_lab, allowed, palette_lab)
    if mode == "floyd":
        return quantize_floyd(img_lab, allowed, palette_lab)
    if mode == "atkinson":
        return quantize_atkinson(img_lab, allowed, palette_lab)
    if mode == "stucki":
        return quantize_stucki(img_lab, allowed, palette_lab)
    if mode == "jarvis":
        return quantize_jarvis(img_lab, allowed, palette_lab)
    if mode == "sierra":
        return quantize_sierra(img_lab, allowed, palette_lab)
    if mode == "sierra_lite":
        return quantize_sierra_lite(img_lab, allowed, palette_lab)
    if mode == "burkes":
        return quantize_burkes(img_lab, allowed, palette_lab)
    if mode == "riemersma":
        return quantize_riemersma(img_lab, allowed, palette_lab)
    if mode == "ostromoukhov":
        return quantize_ostromoukhov(img_lab, allowed, palette_lab)
    return quantize_none(img_lab, allowed, palette_lab)