"""13장 실습: 아주 작은 '확산(diffusion) World Model'.

질문: "진자가 지금 (θ, θ̇) 에 있다. 앞으로 1초(20 스텝) 동안 모르는 토크 u 를 계속 건다면, 1초 뒤 막대 끝은 어디에 있을까?"
u 를 모르므로 답은 한 점이 아니라 '여러 가능성의 분포' 다. 특히 막대가 거의 서 있을 때는
어느 쪽으로 얼마나 쓰러질지 몰라 가능한 미래가 원 둘레 전체로 퍼진다.

- 평균 제곱 오차(MSE)로 학습한 회귀 모델은 가능한 미래들의 '평균' 하나를 내놓는다 → 원 둘레에 퍼진 점들의 평균,
  즉 실제로는 일어날 수 없는 곳(원 안쪽)을 예측한다. 그림을 예측하는 World Model 에서는 이것이 '흐릿한 그림' 이 된다.
- 확산 모델은 잡음에서 시작해 조금씩 잡음을 걷어 내며 '샘플' 을 만든다 → 가능한 미래들을 각각 또렷하게 뽑는다.

실행:
    python ch13_diffusion_toy.py            # 본문 결과 (CPU 4스레드 약 2분)
    python ch13_diffusion_toy.py --quick    # 연기 시험 (약 10초)
그림은 --fig-dir(기본 figures/)에 ch13-*.png 로 저장된다.
"""

import argparse
import math
import os
import time

import matplotlib

matplotlib.use("Agg")
import matplotlib.pyplot as plt
import numpy as np
import torch
import torch.nn as nn

from wm_envs import Pendulum

parser = argparse.ArgumentParser()
parser.add_argument("--quick", action="store_true", help="아주 짧게 돌려 오류만 확인")
parser.add_argument("--fig-dir", default="figures")
parser.add_argument("--seed", type=int, default=0)
args = parser.parse_args()

torch.manual_seed(args.seed)
rng = np.random.default_rng(args.seed)
device = "cuda" if torch.cuda.is_available() else ("mps" if torch.backends.mps.is_available() else "cpu")
os.makedirs(args.fig_dir, exist_ok=True)

HORIZON = 20  # 1초 = 20 스텝 뒤를 예측한다
N_DATA = 20_000 if args.quick else 200_000
TRAIN_STEPS = 300 if args.quick else 8_000


# --8<-- [start:data]
def simulate_batch(th, thdot, u, steps):
    """여러 진자를 한꺼번에 굴린다 (wm_envs.Pendulum.step 과 같은 물리, numpy 벡터 연산).

    th, thdot, u : (N,) 배열. u 는 steps 동안 그대로 유지하는 토크.
    """
    P = Pendulum
    u = np.clip(u, -P.max_torque, P.max_torque)
    for _ in range(steps):
        thdot = thdot + (3 * P.g / (2 * P.l) * np.sin(th) + 3.0 / (P.m * P.l**2) * u) * P.dt
        thdot = np.clip(thdot, -P.max_speed, P.max_speed)
        th = th + thdot * P.dt
    return th, thdot


def make_dataset(n):
    """조건 c = [cos θ, sin θ, θ̇/3] (지금), 목표 x = 1초 뒤 막대 끝 위치 [sin θ', cos θ'] (위쪽이 +y)."""
    th = rng.uniform(-np.pi, np.pi, n)
    thdot = rng.uniform(-3.0, 3.0, n)
    u = rng.uniform(-2.0, 2.0, n)  # 모르는 미래의 토크 → 미래가 여러 갈래가 되는 까닭
    th_f, _ = simulate_batch(th, thdot, u, HORIZON)
    cond = np.stack([np.cos(th), np.sin(th), thdot / 3.0], axis=1)  # θ̇ 는 대략 [-1, 1] 로 맞춘다
    target = np.stack([np.sin(th_f), np.cos(th_f)], axis=1)  # 막대 끝 (x, y)
    return cond.astype(np.float32), target.astype(np.float32)
# --8<-- [end:data]


def check_physics():
    """벡터로 다시 쓴 물리가 wm_envs.Pendulum 과 정확히 같은지 확인한다."""
    env = Pendulum(seed=1)
    env.reset()
    th0, thdot0 = env.th, env.thdot
    for _ in range(HORIZON):
        env.step([0.7])
    th_b, thdot_b = simulate_batch(np.array([th0]), np.array([thdot0]), np.array([0.7]), HORIZON)
    assert abs(th_b[0] - env.th) < 1e-9 and abs(thdot_b[0] - env.thdot) < 1e-9
    print("물리 확인: simulate_batch 와 Pendulum.step 결과가 같습니다.")


# --8<-- [start:schedule]
T = 100  # 잡음 단계 수
betas = torch.linspace(1e-4, 0.15, T)  # β_1 … β_T : 단계마다 섞는 잡음의 양
alphas = 1.0 - betas  # α_t = 1 - β_t
alpha_bars = torch.cumprod(alphas, dim=0)  # ᾱ_t = α_1 · α_2 · … · α_t


def q_sample(x0, t, eps):
    """닫힌 꼴로 한 번에 x_t 를 만든다: x_t = sqrt(ᾱ_t) x_0 + sqrt(1 - ᾱ_t) ε."""
    ab = alpha_bars.to(x0.device)[t].unsqueeze(1)  # (B, 1)
    return ab.sqrt() * x0 + (1 - ab).sqrt() * eps
# --8<-- [end:schedule]


# --8<-- [start:model]
def time_embedding(t, dim=32):
    """잡음 단계 t(정수)를 사인 · 코사인 벡터로 바꾼다 (Transformer 의 위치 임베딩과 같은 방법)."""
    half = dim // 2
    freqs = torch.exp(-math.log(1000.0) * torch.arange(half, device=t.device) / half)
    angles = t.float().unsqueeze(1) * freqs.unsqueeze(0)  # (B, half)
    return torch.cat([angles.sin(), angles.cos()], dim=1)  # (B, dim)


class Denoiser(nn.Module):
    """ε_θ(x_t, t, c): 잡음 섞인 미래 x_t, 단계 t, 조건 c(지금 상태)를 보고 섞인 잡음 ε 을 맞힌다."""

    def __init__(self, x_dim=2, c_dim=3, hidden=256):
        super().__init__()
        self.net = nn.Sequential(
            nn.Linear(x_dim + c_dim + 32, hidden), nn.SiLU(),
            nn.Linear(hidden, hidden), nn.SiLU(),
            nn.Linear(hidden, hidden), nn.SiLU(),
            nn.Linear(hidden, x_dim),
        )

    def forward(self, x_t, t, c):
        return self.net(torch.cat([x_t, c, time_embedding(t)], dim=1))


class Regressor(nn.Module):
    """비교용: 조건 c 만 보고 미래 x 를 한 점으로 예측하는 보통의 회귀 모델 (MSE 로 학습)."""

    def __init__(self, x_dim=2, c_dim=3, hidden=256):
        super().__init__()
        self.net = nn.Sequential(
            nn.Linear(c_dim, hidden), nn.SiLU(),
            nn.Linear(hidden, hidden), nn.SiLU(),
            nn.Linear(hidden, x_dim),
        )

    def forward(self, c):
        return self.net(c)
# --8<-- [end:model]


# --8<-- [start:train]
def train(cond, target):
    cond_t = torch.from_numpy(cond).to(device)
    x_t0 = torch.from_numpy(target).to(device)
    denoiser, regressor = Denoiser().to(device), Regressor().to(device)
    opt_d = torch.optim.Adam(denoiser.parameters(), lr=1e-3)
    opt_r = torch.optim.Adam(regressor.parameters(), lr=1e-3)
    sched_d = torch.optim.lr_scheduler.CosineAnnealingLR(opt_d, TRAIN_STEPS)
    losses = []
    for step in range(1, TRAIN_STEPS + 1):
        idx = torch.randint(0, len(cond_t), (1024,), device=device)
        c, x0 = cond_t[idx], x_t0[idx]

        # (1) 확산 모델: 단계 t 를 고르고, 잡음 ε 을 섞어 x_t 를 만든 뒤, ε 을 맞히게 한다.
        t = torch.randint(0, T, (len(idx),), device=device)
        eps = torch.randn_like(x0)
        x_noisy = q_sample(x0, t, eps)
        loss_d = ((denoiser(x_noisy, t, c) - eps) ** 2).mean()  # ‖ε - ε_θ‖² 의 평균
        opt_d.zero_grad()
        loss_d.backward()
        opt_d.step()
        sched_d.step()

        # (2) 회귀 모델: 미래 위치를 바로 맞히게 한다 (MSE).
        loss_r = ((regressor(c) - x0) ** 2).mean()
        opt_r.zero_grad()
        loss_r.backward()
        opt_r.step()

        losses.append(loss_d.item())
        if step % max(1, TRAIN_STEPS // 8) == 0:
            print(f"step {step:5d}  확산 손실 {np.mean(losses[-200:]):.4f}  회귀 MSE {loss_r.item():.4f}")
    return denoiser, regressor, losses
# --8<-- [end:train]


# --8<-- [start:sample]
@torch.no_grad()
def sample(denoiser, c, n, keep=()):
    """역방향: x_T ~ N(0, I) 에서 시작해 t = T … 1 순서로 잡음을 걷어 낸다 (DDPM 샘플러).

    x_{t-1} = (x_t - β_t / sqrt(1 - ᾱ_t) · ε_θ(x_t, t, c)) / sqrt(α_t) + σ_t z,   σ_t² = β_t
    """
    c = torch.as_tensor(c, dtype=torch.float32, device=device).expand(n, -1)
    x = torch.randn(n, 2, device=device)
    trace = []
    for t in reversed(range(T)):
        tt = torch.full((n,), t, device=device, dtype=torch.long)
        eps_hat = denoiser(x, tt, c)
        b, a, ab = betas[t].item(), alphas[t].item(), alpha_bars[t].item()
        x = (x - b / math.sqrt(1 - ab) * eps_hat) / math.sqrt(a)
        if t > 0:
            x = x + math.sqrt(b) * torch.randn_like(x)  # 마지막 단계에는 잡음을 더하지 않는다
        if t in keep:  # 보고 싶은 단계의 x_t 를 남겨 둔다 (그림용)
            trace.append((t, x.cpu().numpy().copy()))
    return x.cpu().numpy(), trace
# --8<-- [end:sample]


def true_futures(th, thdot, n):
    u = rng.uniform(-2.0, 2.0, n)
    th_f, _ = simulate_batch(np.full(n, th), np.full(n, thdot), u, HORIZON)
    return np.stack([np.sin(th_f), np.cos(th_f)], axis=1)


def cond_of(th, thdot):
    return torch.tensor([[math.cos(th), math.sin(th), thdot / 3.0]])


def main():
    check_physics()
    print(f"장치: {device},  ᾱ_T = {alpha_bars[-1].item():.2e}  (0 에 가까울수록 x_T 가 순수한 잡음)")

    t0 = time.time()
    cond, target = make_dataset(N_DATA)
    print(f"데이터 {len(cond):,}개 만들기: {time.time() - t0:.1f}초")

    # 그림 1: 정방향 과정 — 진짜 미래 위치(원 위의 점들)에 잡음을 점점 섞으면 둥근 가우시안 덩어리가 된다.
    x0 = torch.from_numpy(target[:3000])
    fig, axes = plt.subplots(1, 5, figsize=(12, 2.8))
    for ax, t in zip(axes, [0, 10, 25, 50, 99]):
        xt = x0 if t == 0 else q_sample(x0, torch.full((len(x0),), t), torch.randn_like(x0))
        ax.scatter(xt[:, 0], xt[:, 1], s=1, alpha=0.4, color="#2f6fdb")
        ax.set_title(f"t = {t + 1 if t else 0}" + ("  (data)" if t == 0 else ""), fontsize=10)
        ax.set_xlim(-3, 3), ax.set_ylim(-3, 3), ax.set_aspect("equal"), ax.set_xticks([]), ax.set_yticks([])
    fig.suptitle("Forward process q(x_t | x_0): data on the circle -> Gaussian noise", fontsize=11)
    fig.tight_layout()
    fig.savefig(os.path.join(args.fig_dir, "ch13-forward.png"), dpi=110)
    plt.close(fig)

    t0 = time.time()
    denoiser, regressor, losses = train(cond, target)
    print(f"학습: {time.time() - t0:.1f}초")

    # 그림 2: 세 가지 '지금 상태' 에 대해 1초 뒤 막대 끝의 분포 — 진짜 / 확산 샘플 / 회귀 예측
    cases = [(0.1, 0.0, "near top, still"), (math.pi, 0.0, "hanging, still"), (math.pi / 2, -2.0, "horizontal, moving")]
    fig, axes = plt.subplots(1, 3, figsize=(12, 4.2))
    for ax, (th, thdot, name) in zip(axes, cases):
        real = true_futures(th, thdot, 600)
        gen, _ = sample(denoiser, cond_of(th, thdot), 600)
        reg = regressor(cond_of(th, thdot).to(device)).detach().cpu().numpy()[0]
        circle = np.linspace(0, 2 * np.pi, 200)
        ax.plot(np.cos(circle), np.sin(circle), color="#cccccc", lw=1)
        ax.scatter(gen[:, 0], gen[:, 1], s=6, alpha=0.35, color="#2f6fdb", label="diffusion samples")
        ax.scatter(real[:, 0], real[:, 1], s=3, alpha=0.6, color="#1f8a5b", label="true futures")
        ax.scatter([reg[0]], [reg[1]], s=140, marker="X", color="#b03a6a", label="MSE regression", zorder=5)
        ax.scatter([math.sin(th)], [math.cos(th)], s=60, marker="o", facecolors="none", edgecolors="k", label="now")
        ax.set_title(f"{name}: theta={th:.2f}, thdot={thdot:.1f}", fontsize=10)
        ax.set_xlim(-1.35, 1.35), ax.set_ylim(-1.35, 1.35), ax.set_aspect("equal")
        print(f"[{name}] 회귀 예측 = ({reg[0]:+.3f}, {reg[1]:+.3f}), 원점에서 거리 {np.hypot(*reg):.3f}"
              f"  |  확산 샘플의 원점 거리 평균 {np.hypot(gen[:, 0], gen[:, 1]).mean():.3f}")
    axes[0].legend(loc="lower left", fontsize=8, framealpha=0.9)
    fig.suptitle("Where is the rod tip 1 s later (unknown constant torque)?", fontsize=11)
    fig.tight_layout()
    fig.savefig(os.path.join(args.fig_dir, "ch13-futures.png"), dpi=110)
    plt.close(fig)

    # 그림 3: 역방향 과정 — '거의 서 있는' 경우에 잡음에서 가능한 미래들(원 위의 점들)이 생겨나는 모습
    _, trace = sample(denoiser, cond_of(0.1, 0.0), 1000, keep=(60, 30, 15, 8, 3, 0))
    fig, axes = plt.subplots(1, len(trace), figsize=(12, 2.8))
    for ax, (t, xs) in zip(axes, trace):
        ax.scatter(xs[:, 0], xs[:, 1], s=1, alpha=0.4, color="#2f6fdb")
        ax.set_title(f"t = {t}", fontsize=10)
        ax.set_xlim(-3, 3), ax.set_ylim(-3, 3), ax.set_aspect("equal"), ax.set_xticks([]), ax.set_yticks([])
    fig.suptitle("Reverse process (near-top start): noise -> possible futures on the circle", fontsize=11)
    fig.tight_layout()
    fig.savefig(os.path.join(args.fig_dir, "ch13-reverse.png"), dpi=110)
    plt.close(fig)
    print("그림 저장: ch13-forward.png, ch13-futures.png, ch13-reverse.png")


if __name__ == "__main__":
    main()
