"""7장 (후반) · 확률적 앙상블 World Model 과 PETS.

무엇을 하나
  1. 1차원 장난감 문제로 '우연적(aleatoric)' 불확실성과 '인식적(epistemic)' 불확실성을 그림으로 본다.
  2. 진자에서 확률적 앙상블을 학습하고, 입자(particle)를 퍼뜨려 롤아웃의 불확실성이 시간에 따라
     어떻게 커지는지, 데이터가 덮지 못한 곳에서는 어떻게 흩어지는지 본다.
  3. PETS: 앙상블 + 궤적 표본 전파(TS∞) + CEM 으로 MPC 를 돌려 진자를 세운다.

실행
  python ch07_pets.py            # 본문 결과 (노트북 CPU 4스레드에서 약 20분, GPU 에서는 계획이 2-3배 빠르다)
  python ch07_pets.py --quick    # 연기 시험 (1분 안팎)
같은 폴더에 ch06_dynamics.py · ch07_mpc.py · wm_envs.py 가 있어야 한다.
"""

from __future__ import annotations

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
import torch.nn.functional as F

from ch06_dynamics import episodes_to_arrays, get_device, obs_to_angle, to_tensor
from ch07_mpc import CEMPlanner, RandomPolicy, run_episode
from wm_envs import Pendulum, collect_episodes, pendulum_reward


# --8<-- [start:ensemble]
class EnsembleLinear(nn.Module):
    """E 개의 Linear 층을 배치 행렬곱 한 번으로 계산한다. x (E, B, in) → (E, B, out)."""

    def __init__(self, n_members: int, in_dim: int, out_dim: int):
        super().__init__()
        bound = 1.0 / math.sqrt(in_dim)  # nn.Linear 와 같은 초기화 범위
        self.weight = nn.Parameter(torch.empty(n_members, in_dim, out_dim).uniform_(-bound, bound))
        self.bias = nn.Parameter(torch.empty(n_members, 1, out_dim).uniform_(-bound, bound))

    def forward(self, x):
        return torch.baddbmm(self.bias, x, self.weight)  # bias + x @ weight  (멤버마다 따로)


class GaussianEnsemble(nn.Module):
    """멤버 E 개가 각자 평균 μ 와 로그 분산 log σ² 을 내는 확률적 앙상블."""

    def __init__(self, in_dim: int, out_dim: int, n_members: int = 5, hidden: int = 128):
        super().__init__()
        self.E = n_members
        self.layers = nn.ModuleList([
            EnsembleLinear(n_members, in_dim, hidden),
            EnsembleLinear(n_members, hidden, hidden),
            EnsembleLinear(n_members, hidden, hidden),
            EnsembleLinear(n_members, hidden, 2 * out_dim),  # 평균 + 로그 분산
        ])
        # 로그 분산의 위 · 아래 한계 (이것도 학습한다)
        self.max_logvar = nn.Parameter(torch.full((out_dim,), 0.5))
        self.min_logvar = nn.Parameter(torch.full((out_dim,), -10.0))

    def forward(self, x):
        h = x
        for layer in self.layers[:-1]:
            h = F.silu(layer(h))
        mean, raw_logvar = self.layers[-1](h).chunk(2, dim=-1)
        # softplus 로 부드럽게 [min_logvar, max_logvar] 안에 가둔다
        logvar = self.max_logvar - F.softplus(self.max_logvar - raw_logvar)
        logvar = self.min_logvar + F.softplus(logvar - self.min_logvar)
        return mean, logvar
# --8<-- [end:ensemble]


# --8<-- [start:nll]
def gaussian_nll(mean, logvar, target):
    """가우시안 음의 로그 가능도에서 상수(½ log 2π)와 앞의 ½ 을 뺀 것: (y − μ)²/σ² + log σ²."""
    return (((target - mean) ** 2) * torch.exp(-logvar) + logvar).mean()


def train_ensemble(model, X, Y, steps=3000, batch_size=256, lr=1e-3, device="cpu", seed=0):
    """X (N, in), Y (N, out) 는 numpy 배열 (진자에서는 호출하는 쪽이 미리 표준화한다). 멤버마다 부트스트랩 표본으로 학습한다.

    돌려주는 값은 마지막 미니배치의 손실(NLL + 로그 분산 범위 벌점)이다.
    """
    g = torch.Generator().manual_seed(seed)
    N, E = len(X), model.E
    X, Y = to_tensor(X, device), to_tensor(Y, device)
    boot = torch.randint(N, (E, N), generator=g).to(device)  # 멤버 e 의 데이터 = X[boot[e]] (복원 추출)
    opt = torch.optim.Adam(model.parameters(), lr=lr)
    for step in range(steps):
        pos = torch.randint(N, (E, batch_size), generator=g).to(device)
        idx = torch.gather(boot, 1, pos)  # (E, B): 멤버마다 자기 부트스트랩 목록에서 뽑은 번호
        mean, logvar = model(X[idx])  # (E, B, out)
        loss = gaussian_nll(mean, logvar, Y[idx])
        loss = loss + 0.01 * (model.max_logvar.sum() - model.min_logvar.sum())  # 한계를 좁게 유지
        opt.zero_grad()
        loss.backward()
        opt.step()
    return loss.item()
# --8<-- [end:nll]


# --8<-- [start:prob_dynamics]
class ProbabilisticDynamics(nn.Module):
    """앙상블로 만든 진자 World Model: [o_t, a_t] → Δo_t 의 분포 N(μ, σ²) (보상은 알려진 함수를 쓴다)."""

    def __init__(self, n_members: int = 5, hidden: int = 128):
        super().__init__()
        self.E = n_members
        self.ens = GaussianEnsemble(4, 3, n_members, hidden)
        for name, dim in [("in_mean", 4), ("in_std", 4), ("out_mean", 3), ("out_std", 3)]:
            self.register_buffer(name, torch.zeros(dim) if "mean" in name else torch.ones(dim))

    def fit(self, episodes, steps=3000, device="cpu", seed=0):
        obs, act, next_obs, _ = episodes_to_arrays(episodes)
        X = np.concatenate([obs, act], axis=1)
        Y = next_obs - obs
        self.to(device)
        for buf, val in [(self.in_mean, X.mean(0)), (self.in_std, X.std(0) + 1e-6),
                         (self.out_mean, Y.mean(0)), (self.out_std, Y.std(0) + 1e-6)]:
            buf.copy_(to_tensor(val, device))
        Xn = (X - X.mean(0)) / (X.std(0) + 1e-6)
        Yn = (Y - Y.mean(0)) / (Y.std(0) + 1e-6)
        return train_ensemble(self.ens, Xn, Yn, steps=steps, device=device, seed=seed)

    def sample_next(self, obs, act):
        """obs (E, B, 3), act (E, B, 1) → 멤버 e 가 자기 몫의 입자 B 개를 한 스텝 표본 추출한다."""
        x = (torch.cat([obs, act], dim=-1) - self.in_mean) / self.in_std
        mean, logvar = self.ens(x)
        delta = mean + torch.exp(0.5 * logvar) * torch.randn_like(mean)  # μ + σ ⊙ ε
        return obs + delta * self.out_std + self.out_mean
# --8<-- [end:prob_dynamics]


# --8<-- [start:ts]
@torch.no_grad()
def evaluate_sequences_ts(dyn, obs, actions, n_particles=10):
    """궤적 표본 전파(TS∞): 후보마다 입자 P 개를 띄우고, 입자마다 앙상블 멤버 하나를 끝까지 고정해 굴린다.

    obs (3,), actions (N, H, 1) → 기대 총 보상 (N,) = 그 후보에 속한 입자들의 총 보상 평균
    배치 모양: 멤버 E 개 × (후보 N × 멤버당 입자 m) — 후보 n 의 입자는 행 n·m … n·m+m−1.
    """
    N, H, _ = actions.shape
    E = dyn.E
    m = max(1, n_particles // E)  # 후보 하나에서 멤버 하나가 맡는 입자 수
    o = obs.expand(E, N * m, 3).clone()  # (E, N·m, 3) 모든 입자가 지금 관측에서 출발
    total = torch.zeros(E, N * m, device=actions.device)
    for k in range(H):
        a = actions[:, k].repeat_interleave(m, dim=0).expand(E, N * m, 1)  # 후보의 행동을 그 입자들에게
        total += pendulum_reward(o, a)
        o = dyn.sample_next(o, a)
    return total.view(E, N, m).mean(dim=(0, 2))  # 입자 평균 = 기대 리턴의 몬테카를로 추정


class PETSPlanner(CEMPlanner):
    """CEM 은 그대로 두고, 점수 매기기만 TS 입자 평균으로 바꾼 계획기."""

    def __init__(self, dyn, n_particles=10, **kwargs):
        super().__init__(dyn, **kwargs)
        self.P = n_particles

    def evaluate(self, obs, actions):
        return evaluate_sequences_ts(self.model, obs, actions, self.P)
# --8<-- [end:ts]


def toy_uncertainty_figure(fig_path, device, steps):
    """1차원 회귀로 두 가지 불확실성을 보인다."""
    rng = np.random.default_rng(0)
    x = np.concatenate([rng.uniform(-4, -1, 100), rng.uniform(1.5, 4, 100)])
    noise = np.where(x > 0, 0.25, 0.04)  # 오른쪽 데이터가 더 시끄럽다 (우연적 불확실성이 크다)
    y = np.sin(1.3 * x) + noise * rng.standard_normal(len(x))
    torch.manual_seed(0)
    ens = GaussianEnsemble(1, 1, n_members=5, hidden=64).to(device)
    train_ensemble(ens, x[:, None], y[:, None], steps=steps, device=device)
    xs = np.linspace(-6.5, 6.5, 400)
    with torch.no_grad():
        mu, lv = ens(to_tensor(xs, device)[None, :, None].expand(5, -1, -1))
    mu, var = mu[..., 0].cpu().numpy(), lv.exp()[..., 0].cpu().numpy()  # (E, 400)
    mean = mu.mean(0)
    aleatoric = var.mean(0)  # 멤버들이 말하는 잡음 분산의 평균
    epistemic = mu.var(0)  # 멤버들 평균끼리의 흩어짐
    fig, axes = plt.subplots(1, 2, figsize=(11, 3.8), sharey=True)
    for ax in axes:
        ax.scatter(x, y, s=6, color="k", alpha=0.5, label="data")
        ax.plot(xs, np.sin(1.3 * xs), color="gray", lw=1, ls=":", label="true function")
        ax.axvspan(-1, 1.5, color="gold", alpha=0.12)
        ax.set_xlabel("x")
        ax.grid(alpha=0.3)
        ax.set_ylim(-3.2, 3.2)
    for e in range(5):
        axes[0].plot(xs, mu[e], lw=1, alpha=0.8, label="ensemble members" if e == 0 else None)
    axes[0].set_title("5 members: they agree on data, disagree elsewhere", fontsize=10)
    axes[0].legend(fontsize=8, loc="lower left")
    axes[1].plot(xs, mean, color="C0", label="ensemble mean")
    tot = np.sqrt(aleatoric + epistemic)
    axes[1].fill_between(xs, mean - 2 * tot, mean + 2 * tot, color="C1", alpha=0.25,
                         label="±2σ total (aleatoric + epistemic)")
    axes[1].fill_between(xs, mean - 2 * np.sqrt(aleatoric), mean + 2 * np.sqrt(aleatoric), color="C0",
                         alpha=0.3, label="±2σ aleatoric only")
    axes[1].set_title("Uncertainty: noise (aleatoric) vs lack of data (epistemic)", fontsize=10)
    axes[1].legend(fontsize=8, loc="lower left")
    fig.tight_layout()
    fig.savefig(fig_path, dpi=120)
    plt.close(fig)
    for x0 in [-2.5, 0.25, 2.75, 6.0]:
        i = np.argmin(np.abs(xs - x0))
        print(f"  toy x={x0:5.2f}: aleatoric std {np.sqrt(aleatoric[i]):.3f}, epistemic std {np.sqrt(epistemic[i]):.3f}")


@torch.no_grad()
def particle_rollout(dyn, obs0, actions, n_particles=50):
    """TS∞ 로 입자 n_particles 개를 굴린다. obs0 (3,), actions (H, 1) → (P, H+1, 3)."""
    E = dyn.E
    m = n_particles // E
    o = obs0.expand(E, m, 3).clone()
    seq = [o]
    for k in range(len(actions)):
        o = dyn.sample_next(o, actions[k].expand(E, m, 1))
        seq.append(o)
    return torch.stack(seq, dim=2).reshape(E * m, len(actions) + 1, 3)


class BottomStartPendulum(Pendulum):
    """늘 매달린 자세 근처에서 시작하는 진자. 무작위 행동으로는 위쪽 절반에 한 번도 가지 못한다."""

    def reset(self):
        super().reset()
        self.th = float(np.pi + self.rng.uniform(-0.3, 0.3))
        self.thdot = float(self.rng.uniform(-0.5, 0.5))
        return self._obs()


def true_rollout(th, thdot, actions):
    """진짜 환경에서 (θ, θ̇) 로부터 행동열을 실행한 관측 (H+1, 3)."""
    env = Pendulum(max_steps=10**9)
    env.th, env.thdot = th, thdot
    obs = [env._obs()]
    for a in actions:
        obs.append(env.step(a)[0])
    return np.stack(obs)


def main():
    parser = argparse.ArgumentParser(description="7장 후반: 확률적 앙상블과 PETS")
    parser.add_argument("--quick", action="store_true")
    parser.add_argument("--fig-dir", default="figures")
    parser.add_argument("--seed", type=int, default=0)
    parser.add_argument("--iterations", type=int, default=8)
    parser.add_argument("--eval-episodes", type=int, default=10)
    parser.add_argument("--horizon", type=int, default=25)
    parser.add_argument("--candidates", type=int, default=200)
    parser.add_argument("--particles", type=int, default=10)
    args = parser.parse_args()
    steps = 3000
    if args.quick:
        args.iterations, args.eval_episodes, args.horizon, args.candidates = 1, 1, 10, 100
        steps = 300
    os.makedirs(args.fig_dir, exist_ok=True)
    np.random.seed(args.seed)
    torch.manual_seed(args.seed)
    device = get_device()
    print(f"device: {device}")
    start = time.time()

    # 1) 장난감 문제
    print("toy 1D problem:")
    toy_uncertainty_figure(f"{args.fig_dir}/ch07-uncertainty-toy.png", device, steps)

    # 2) 진자: 데이터가 없는 곳을 상상하면 입자가 퍼진다
    th0, thdot0 = 0.4, 0.0  # 거의 서 있는 자세에서 출발 (아래쪽 데이터만 본 모델에게는 처음 보는 곳)
    act_rng = np.random.default_rng(4242)
    test_actions = act_rng.uniform(-2, 2, size=(60, 1)).astype(np.float32)
    truth = true_rollout(th0, thdot0, test_actions)  # (61, 3)
    datasets = {
        "bottom-start data only": collect_episodes(BottomStartPendulum(seed=args.seed), 20, seed=args.seed),
        "uniform-start data": collect_episodes(Pendulum(seed=args.seed), 20, seed=args.seed),
    }
    fig, axes = plt.subplots(2, 2, figsize=(10, 5.2), sharex=True, sharey="row")
    t = np.arange(61) * Pendulum.dt
    for col, (label, eps) in enumerate(datasets.items()):
        dyn = ProbabilisticDynamics()
        dyn.fit(eps, steps=steps, device=device, seed=args.seed)
        parts = particle_rollout(dyn, to_tensor(truth[0], device), to_tensor(test_actions, device)).cpu().numpy()
        spread = parts[:, :, 2].std(0)  # 입자들의 θ̇ 표준편차, 시간별
        print(f"  {label:24s}: particle std of θ̇ at step 10/30/60 = "
              f"{spread[10]:.3f} / {spread[30]:.3f} / {spread[60]:.3f}")
        for row, (dim, name) in enumerate([(0, "cos θ (1 = upright)"), (2, "angular velocity θ̇")]):
            ax = axes[row, col]
            for p in parts[:, :, dim]:
                ax.plot(t, p, color="C1", lw=0.6, alpha=0.35)
            ax.plot(t, truth[:, dim], "k", lw=2, label="true")
            ax.grid(alpha=0.3)
            if col == 0:
                ax.set_ylabel(name)
            if row == 0:
                ax.set_title(f"trained on {label} (20 episodes)", fontsize=10)
            if row == 1:
                ax.set_xlabel("time [s]")
    axes[0, 0].plot([], [], color="C1", label="50 particles (TS∞)")
    axes[0, 0].legend(fontsize=8, loc="lower left")
    fig.suptitle("Particle rollouts from a near-upright start: spread = model uncertainty", fontsize=11)
    fig.tight_layout()
    fig.savefig(f"{args.fig_dir}/ch07-particles.png", dpi=120)
    plt.close(fig)

    # --8<-- [start:pets_loop]
    # 3) PETS: 확률적 앙상블 + TS∞ + CEM 으로 MPC (7장 전반의 되풀이 고리와 같다)
    env = Pendulum(seed=args.seed)
    data = collect_episodes(env, 1, seed=args.seed)
    returns_per_iter = []
    for it in range(1, args.iterations + 1):
        dyn = ProbabilisticDynamics()
        loss = dyn.fit(data, steps=steps, device=device, seed=args.seed + it)
        planner = PETSPlanner(dyn, n_particles=args.particles, horizon=args.horizon,
                              n_candidates=args.candidates, n_elites=30, device=device)
        episode, ret = run_episode(env, planner)
        data.append(episode)
        returns_per_iter.append(ret)
        print(f"iter {it:2d} | data {len(data):2d} episodes | NLL {loss:7.3f} | "
              f"PETS return {ret:8.1f} | {time.time() - start:5.0f}s")
    # --8<-- [end:pets_loop]

    dyn = ProbabilisticDynamics()
    dyn.fit(data, steps=steps, device=device, seed=args.seed + 999)
    rets = []
    for i in range(args.eval_episodes):
        torch.manual_seed(args.seed + i)
        planner = PETSPlanner(dyn, n_particles=args.particles, horizon=args.horizon,
                              n_candidates=args.candidates, n_elites=30, device=device)
        _, ret = run_episode(Pendulum(seed=10_000 + i), planner)  # ch07_mpc.py 와 같은 시작 상태
        rets.append(ret)
    print(f"PETS (eval)         : mean return {np.mean(rets):8.1f} ± {np.std(rets):6.1f}  "
          f"(episodes: {', '.join(f'{r:.0f}' for r in rets)})")
    print(f"elapsed: {time.time() - start:.1f}s")
    print(f"figures saved to {args.fig_dir}/")


if __name__ == "__main__":
    main()
