"""10장 실습: V(VAE) + M(MDN-RNN) + C(컨트롤러) — Ha & Schmidhuber 의 World Models.

하는 일
  1. 장난감: CMA-ES 가 2차원 함수에서 탐색 분포를 어떻게 바꾸는지 그림으로 본다.
  2. 8 · 9장의 V, M 을 불러오고, 컨트롤러 C 를 만든다.
        a_t = 2 · tanh(W [z_t ; h_t] + b)        (선형 + tanh, 매개변수 (32+256)+1 = 289 개)
  3. CMA-ES 로 C 를 학습한다.
        (A) 진짜 환경에서:  V, M 은 '특징 추출기', 리턴은 진짜 환경이 준다. (60세대, 후보당 에피소드 4개)
        (B) 꿈속에서만   :  M 이 다음 z 와 보상을 지어내고, 진짜 환경은 평가할 때만 쓴다. (150세대, 후보당 꿈 16개)
     꿈속 학습은 온도 τ 를 바꿔 가며 해 보고, '꿈속 리턴' 과 '진짜 리턴' 을 비교한다.
  4. 학습 곡선, 꿈 vs 진짜 비교, 학습한 컨트롤러가 진자를 세우는 장면을 그림으로 저장한다.

먼저 8 · 9장을 실행해 ch08_vae.pt, ch09_mdnrnn.pt, ch09_latents.npz 를 만들어 두어야 한다.

실행
  python ch10_world_models.py           # 본문 결과 (노트북 GPU 약 15분 / CPU 4스레드 약 1시간)
  python ch10_world_models.py --quick   # 연기 시험
"""

from __future__ import annotations

import argparse
import os
import time

import matplotlib

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

from ch08_vae import get_device, load_vae, to_float
from ch09_mdn_rnn import load_mdnrnn, mdn_sample
from wm_envs import PixelPendulum


# --8<-- [start:cmaes]
class CMAES:
    """CMA-ES (Hansen 의 튜토리얼 기본값을 따른 최소 구현). 목적 함수를 '최대화' 한다.

    사용법:  es = CMAES(x0, sigma0, popsize)
            while ...:
                xs = es.ask()                 # (λ, n) 후보들
                es.tell(xs, fitness)          # fitness: (λ,) 클수록 좋다
    """

    def __init__(self, x0, sigma0: float, popsize: int | None = None, seed: int = 0):
        self.rng = np.random.default_rng(seed)
        n = len(x0)
        self.n = n
        self.mean = np.array(x0, dtype=np.float64)  # 분포의 평균 m
        self.sigma = float(sigma0)  # 전체 스텝 크기 σ
        self.lam = popsize or 4 + int(3 * np.log(n))  # 한 세대의 후보 수 λ
        self.mu = self.lam // 2  # 부모(상위) 수 μ
        w = np.log(self.mu + 0.5) - np.log(np.arange(1, self.mu + 1))  # 1등이 가장 큰 가중치
        self.weights = w / w.sum()
        self.mu_eff = 1.0 / np.sum(self.weights**2)  # 유효 부모 수
        # 학습률들 (기본값)
        self.c_sigma = (self.mu_eff + 2) / (n + self.mu_eff + 5)
        self.d_sigma = 1 + 2 * max(0.0, np.sqrt((self.mu_eff - 1) / (n + 1)) - 1) + self.c_sigma
        self.c_c = (4 + self.mu_eff / n) / (n + 4 + 2 * self.mu_eff / n)
        self.c_1 = 2 / ((n + 1.3) ** 2 + self.mu_eff)
        self.c_mu = min(1 - self.c_1, 2 * (self.mu_eff - 2 + 1 / self.mu_eff) / ((n + 2) ** 2 + self.mu_eff))
        self.chi_n = np.sqrt(n) * (1 - 1 / (4 * n) + 1 / (21 * n**2))  # E‖N(0, I)‖
        # 상태: 공분산 C = B diag(D²) Bᵀ, 진화 경로 p_σ, p_c
        self.C = np.eye(n)
        self.B = np.eye(n)
        self.D = np.ones(n)
        self.p_sigma = np.zeros(n)
        self.p_c = np.zeros(n)
        self.gen = 0

    def ask(self):
        """x_k = m + σ · B D ε_k,  ε_k ~ N(0, I)   →  x_k ~ N(m, σ² C)."""
        eps = self.rng.standard_normal((self.lam, self.n))
        self.y = (eps * self.D) @ self.B.T  # y_k ~ N(0, C)
        return self.mean + self.sigma * self.y

    def tell(self, xs, fitness):
        order = np.argsort(-np.asarray(fitness))  # 큰 것부터
        y_best = self.y[order[: self.mu]]  # 상위 μ 개의 스텝 (μ, n)
        y_w = self.weights @ y_best  # 가중 평균 스텝
        # (1) 평균 갱신: 좋은 후보 쪽으로 옮긴다
        self.mean = self.mean + self.sigma * y_w
        # (2) 스텝 크기 σ: 진화 경로 p_σ 가 '우연' 보다 길면 키우고, 짧으면 줄인다
        C_inv_sqrt = self.B @ np.diag(1 / self.D) @ self.B.T
        self.p_sigma = (1 - self.c_sigma) * self.p_sigma + np.sqrt(
            self.c_sigma * (2 - self.c_sigma) * self.mu_eff) * (C_inv_sqrt @ y_w)
        norm_ps = np.linalg.norm(self.p_sigma)
        h_sigma = norm_ps / np.sqrt(1 - (1 - self.c_sigma) ** (2 * (self.gen + 1))) < (1.4 + 2 / (self.n + 1)) * self.chi_n
        # (3) 공분산 C: 진화 경로(rank-1) + 상위 스텝들의 가중 공분산(rank-μ)
        self.p_c = (1 - self.c_c) * self.p_c + h_sigma * np.sqrt(self.c_c * (2 - self.c_c) * self.mu_eff) * y_w
        rank_one = np.outer(self.p_c, self.p_c)
        rank_mu = (y_best.T * self.weights) @ y_best
        delta = (1 - h_sigma) * self.c_c * (2 - self.c_c)
        self.C = (1 - self.c_1 - self.c_mu) * self.C + self.c_1 * (rank_one + delta * self.C) + self.c_mu * rank_mu
        self.sigma *= np.exp((self.c_sigma / self.d_sigma) * (norm_ps / self.chi_n - 1))
        # C = B D² Bᵀ 고윳값 분해 (대칭 · 양의 정부호 유지)
        self.C = np.triu(self.C) + np.triu(self.C, 1).T
        eigvals, self.B = np.linalg.eigh(self.C)
        self.D = np.sqrt(np.maximum(eigvals, 1e-20))
        self.gen += 1
# --8<-- [end:cmaes]


def cmaes_toy(fig_dir):
    """비스듬히 놓인 길쭉한 그릇 모양 함수에서 CMA-ES 의 탐색 분포가 바뀌는 모습."""
    angle = np.deg2rad(35)
    R = np.array([[np.cos(angle), -np.sin(angle)], [np.sin(angle), np.cos(angle)]])
    H = R @ np.diag([1.0, 25.0]) @ R.T
    target = np.array([2.0, 1.0])

    def f(x):  # 최대화할 함수 (= 그릇 함수에 - 를 붙임)
        d = x - target
        return -np.einsum("...i,ij,...j->...", d, H, d)

    es = CMAES([-3.0, -2.0], sigma0=0.5, popsize=12, seed=3)
    snaps = {}
    for g in range(41):
        xs = es.ask()
        if g in (0, 4, 10, 25):
            snaps[g] = (xs.copy(), es.mean.copy(), es.sigma**2 * es.C.copy())
        es.tell(xs, f(xs))
    gx, gy = np.meshgrid(np.linspace(-4.5, 4, 200), np.linspace(-3.5, 3.5, 200))
    fz = f(np.stack([gx, gy], -1))
    fig, axes = plt.subplots(1, 4, figsize=(12, 3.3))
    t = np.linspace(0, 2 * np.pi, 100)
    for ax, (g, (xs, m, cov)) in zip(axes, snaps.items()):
        ax.contour(gx, gy, -fz, levels=np.geomspace(0.5, 400, 12), colors="#c6ccd4", linewidths=0.8)
        ax.scatter(xs[:, 0], xs[:, 1], s=14, color="#2f6fdb", zorder=3, label="samples")
        vals, vecs = np.linalg.eigh(cov)
        ell = vecs @ (2 * np.sqrt(vals)[:, None] * np.stack([np.cos(t), np.sin(t)]))
        ax.plot(m[0] + ell[0], m[1] + ell[1], color="#d1495b", lw=1.6, label="2-sigma of N(m, sigma^2 C)")
        ax.plot(*m, "x", color="#d1495b", ms=8)
        ax.plot(*target, "*", color="#1f8a5b", ms=12, label="optimum")
        ax.set_title(f"generation {g}")
        ax.set_xlim(-4.5, 4)
        ax.set_ylim(-3.5, 3.5)
        ax.set_aspect("equal")
        ax.set_xticks([])
        ax.set_yticks([])
    axes[0].legend(loc="lower right", fontsize=7)
    fig.tight_layout()
    fig.savefig(os.path.join(fig_dir, "ch10-cmaes-toy.png"), dpi=110, facecolor="white")
    plt.close(fig)


# --8<-- [start:controller]
class Controllers:
    """후보 P 개의 선형 컨트롤러를 한꺼번에 계산한다.

    params: (P, n) numpy, n = (z_dim + hidden) · 1 + 1   (가중치 W 와 편향 b 를 이어 붙인 것)
    act(feat): feat (P, K, F) → 행동 (P, K, 1) = 2 · tanh(feat · W + b)
    """

    def __init__(self, params, feat_dim: int, device):
        p = torch.as_tensor(params, dtype=torch.float32, device=device)
        self.W = p[:, :feat_dim].unsqueeze(-1)  # (P, F, 1)
        self.b = p[:, feat_dim:].unsqueeze(1)  # (P, 1, 1)

    def act(self, feat):
        return 2.0 * torch.tanh(torch.bmm(feat, self.W) + self.b)  # (P, K, 1)
# --8<-- [end:controller]


# --8<-- [start:rollout-real]
@torch.no_grad()
def rollout_real(params, seeds, vae, mdn, device, record=False):
    """진짜 환경에서 후보 P 개 × 시작 상태 K 개를 동시에 돌린다. 반환: 리턴 (P, K).

    매 스텝: 그림 o_t → V: z_t = μ_φ(o_t) → C: a_t = C([z_t ; h_t]) → 환경 · M 갱신 h_{t+1}
    """
    P, K = len(params), len(seeds)
    ctrl = Controllers(params, mdn.z_dim + mdn.hidden, device)
    envs = [PixelPendulum(seed=int(s)) for _ in range(P) for s in seeds]
    obs = np.stack([env.reset() for env in envs])  # (P·K, 64, 64)
    state = mdn.initial_state(P * K, device)
    returns = np.zeros(P * K)
    frames, thetas = [], []
    for t in range(200):
        z, _ = vae.encode(to_float(obs, device))  # (P·K, D)  평균 μ 를 z 로 쓴다
        h = state[0][0]  # (P·K, H)  행동 전의 기억 h_t
        feat = torch.cat([z, h], dim=-1).view(P, K, -1)
        a = ctrl.act(feat).view(P * K, 1)  # (P·K, 1)
        a_np = a.cpu().numpy()
        if record:
            frames.append(obs[0].copy())
            thetas.append([env.th for env in envs])
        results = [env.step(a_np[i]) for i, env in enumerate(envs)]
        obs = np.stack([r[0] for r in results])
        returns += np.array([r[1] for r in results])
        _, _, _, _, state = mdn(z.unsqueeze(1), a.unsqueeze(1), state)  # M 이 기억을 갱신
    if record:
        return returns.reshape(P, K), np.stack(frames), np.array(thetas)
    return returns.reshape(P, K)
# --8<-- [end:rollout-real]


# --8<-- [start:rollout-dream]
@torch.no_grad()
def rollout_dream(params, z0, mdn, device, tau: float, steps: int = 200):
    """꿈속 롤아웃: 진짜 환경 없이 M 이 다음 z 와 보상을 지어낸다. 반환: 꿈속 리턴 (P, K).

    z0: (K, D) 시작 그림들의 z (진짜 에피소드의 첫 장면에서 가져온다)
    """
    P, K = len(params), len(z0)
    ctrl = Controllers(params, mdn.z_dim + mdn.hidden, device)
    z = torch.as_tensor(z0, device=device).repeat(P, 1)  # (P·K, D)
    state = mdn.initial_state(P * K, device)
    returns = torch.zeros(P * K, device=device)
    for t in range(steps):
        h = state[0][0]
        a = ctrl.act(torch.cat([z, h], dim=-1).view(P, K, -1)).view(P * K, 1)
        log_pi, mu, log_sigma, r_hat, state = mdn(z.unsqueeze(1), a.unsqueeze(1), state)
        returns += r_hat[:, 0]  # 보상도 M 이 지어낸 것
        z = mdn_sample(log_pi[:, 0], mu[:, 0], log_sigma[:, 0], tau)  # 다음 z 를 뽑아 다시 입력
    return returns.view(P, K).cpu().numpy()
# --8<-- [end:rollout-dream]


# --8<-- [start:train]
def train_controller(mode, vae, mdn, z_starts, device, generations=60, popsize=32, rollouts=4, sigma0=0.1,
                     tau=1.0, eval_every=5, eval_seeds=None, seed=0, log=print):
    """CMA-ES 로 컨트롤러를 학습한다. mode = 'real' (진짜 환경) 또는 'dream' (꿈속).

    매 세대: 후보 popsize 개 × 시작 상태 rollouts 개를 돌려 '평균 리턴' 을 적합도로 쓴다.
    eval_every 세대마다 '현재 평균 m' 을 진짜 환경의 고정 시작 상태들(eval_seeds)로 평가한다.
    """
    n = mdn.z_dim + mdn.hidden + 1
    es = CMAES(np.zeros(n), sigma0, popsize, seed=seed)
    rng = np.random.default_rng(seed + 100)
    history = []  # (세대, 적합도 평균, 적합도 최고, 진짜 평가 리턴 또는 nan)
    for g in range(generations + 1):
        xs = es.ask()
        if mode == "real":
            seeds = rng.integers(0, 10**6, rollouts)  # 세대마다 새 시작 상태, 후보끼리는 같게
            fit = rollout_real(xs, seeds, vae, mdn, device).mean(axis=1)
        else:
            z0 = z_starts[rng.integers(0, len(z_starts), rollouts)]
            fit = rollout_dream(xs, z0, mdn, device, tau).mean(axis=1)
        es.tell(xs, fit)
        real_eval = np.nan
        if g % eval_every == 0 or g == generations:
            real_eval = rollout_real(es.mean[None], eval_seeds, vae, mdn, device).mean()
            log(f"[{mode}{'' if mode == 'real' else f' τ={tau:g}'}] 세대 {g:3d} | 적합도 평균 {fit.mean():8.1f} "
                f"최고 {fit.max():8.1f} | σ {es.sigma:.3f} | 진짜 환경 평가(평균 m) {real_eval:8.1f}")
        history.append((g, fit.mean(), fit.max(), real_eval))
    return es.mean, np.array(history)
# --8<-- [end:train]


def main():
    parser = argparse.ArgumentParser(description="10장: World Models (V + M + C)")
    parser.add_argument("--quick", action="store_true")
    parser.add_argument("--generations", type=int, default=60, help="진짜 환경 학습의 세대 수")
    parser.add_argument("--popsize", type=int, default=32)
    parser.add_argument("--rollouts", type=int, default=4, help="진짜 환경에서 후보 하나를 평가할 에피소드 수")
    parser.add_argument("--dream-generations", type=int, default=150, help="꿈속 학습의 세대 수 (꿈은 싸므로 더 많이)")
    parser.add_argument("--dream-rollouts", type=int, default=16, help="꿈속에서 후보 하나를 평가할 에피소드 수")
    parser.add_argument("--sigma0", type=float, default=0.1)
    parser.add_argument("--taus", default="0.1,1.0,1.5", help="꿈속 학습에 쓸 온도들")
    parser.add_argument("--seed", type=int, default=0)
    parser.add_argument("--data", default=".", help="8 · 9장 결과가 있는 폴더")
    parser.add_argument("--fig-dir", default="figures")
    args = parser.parse_args()
    if args.quick:
        args.generations, args.popsize, args.rollouts, args.taus = 2, 6, 2, "1.0"
        args.dream_generations, args.dream_rollouts = 2, 2
    taus = [float(t) for t in args.taus.split(",")]
    os.makedirs(args.fig_dir, exist_ok=True)
    torch.manual_seed(args.seed)
    device = get_device()
    print(f"장치: {device}")

    cmaes_toy(args.fig_dir)

    vae = load_vae(os.path.join(args.data, "ch08_vae.pt"), device)
    mdn = load_mdnrnn(os.path.join(args.data, "ch09_mdnrnn.pt"), device)
    lat = np.load(os.path.join(args.data, "ch09_latents.npz"))
    z_starts = lat["mu"][:, 0].astype(np.float32)  # 진짜 에피소드들의 첫 장면 z (꿈의 시작점)
    n_params = mdn.z_dim + mdn.hidden + 1
    print(f"컨트롤러 매개변수 수: ({mdn.z_dim} + {mdn.hidden}) × 1 + 1 = {n_params}")
    eval_seeds = np.arange(1000, 1000 + (4 if args.quick else 20))  # 평가용 고정 시작 상태 20개

    # 기준선: 무작위 행동, 아무것도 안 하기(토크 0)
    rand_ret = lat["reward"].sum(axis=1).mean()
    zero_ret = rollout_real(np.zeros((1, n_params)), eval_seeds, vae, mdn, device).mean()
    print(f"기준선 | 무작위 행동 {rand_ret:.1f} | 토크 0 {zero_ret:.1f}")

    results = {}
    t0 = time.time()
    params_real, hist_real = train_controller(
        "real", vae, mdn, z_starts, device, args.generations, args.popsize, args.rollouts, args.sigma0,
        eval_seeds=eval_seeds, seed=args.seed)
    n_real = (args.generations + 1) * args.popsize * args.rollouts
    print(f"(A) 진짜 환경 학습 {time.time() - t0:.0f}초 | 학습에 쓴 진짜 에피소드 {n_real}개 ({n_real * 200:,} 스텝)")
    results["real"] = (params_real, hist_real)

    for tau in taus:
        t0 = time.time()
        params_d, hist_d = train_controller(
            "dream", vae, mdn, z_starts, device, args.dream_generations, args.popsize, args.dream_rollouts,
            args.sigma0, tau=tau, eval_every=10, eval_seeds=eval_seeds, seed=args.seed)
        print(f"(B) 꿈속 학습 τ={tau:g} {time.time() - t0:.0f}초")
        results[f"dream {tau:g}"] = (params_d, hist_d)

    # 최종 비교: 같은 컨트롤러를 꿈속(학습 때의 τ, real 은 τ=1)과 진짜 환경에서 각각 평가
    print("\n최종 평가 (진짜 환경: 고정 시작 상태 20개 평균 ± 표준편차, 꿈속: 학습 때와 같은 τ, 시작 상태 64개)")
    rng = np.random.default_rng(7)
    z_eval = z_starts[rng.integers(0, len(z_starts), 64)]
    summary = []
    for name, (params, _) in results.items():
        real = rollout_real(params[None], eval_seeds, vae, mdn, device)[0]
        tau = 1.0 if name == "real" else float(name.split()[1])
        dream_ret = rollout_dream(params[None], z_eval, mdn, device, tau)[0]
        summary.append((name, dream_ret.mean(), real.mean(), real.std()))
        print(f"  {name:10s} | 꿈속 리턴 {dream_ret.mean():8.1f} | 진짜 리턴 {real.mean():8.1f} ± {real.std():5.1f}")

    # 그림 1: 학습 곡선 (진짜 환경 평가 리턴)
    fig, ax = plt.subplots(figsize=(8, 3.6))
    colors = ["#222222", "#2f6fdb", "#1f8a5b", "#b7620c", "#7b4fd1"]
    for (name, (_, hist)), c in zip(results.items(), colors):
        m = ~np.isnan(hist[:, 3])
        label = "trained in real env" if name == "real" else f"trained in dream (tau={name.split()[1]})"
        ax.plot(hist[m, 0], hist[m, 3], "o-", color=c, ms=3, label=label)
    ax.axhline(rand_ret, color="#999999", ls="--", lw=1, label="random actions")
    ax.set_xlabel("CMA-ES generation")
    ax.set_ylabel("return in REAL env (mean of 20)")
    ax.legend(fontsize=8, loc="lower right")
    fig.tight_layout()
    fig.savefig(os.path.join(args.fig_dir, "ch10-curves.png"), dpi=110, facecolor="white")
    plt.close(fig)

    # 그림 2: 꿈속 리턴 vs 진짜 리턴 막대
    fig, ax = plt.subplots(figsize=(7.5, 3.2))
    x = np.arange(len(summary))
    ax.bar(x - 0.18, [s[1] for s in summary], 0.36, color="#b7620c", label="return in dream (tau as trained; tau=1 for real-env one)")
    ax.bar(x + 0.18, [s[2] for s in summary], 0.36, color="#2f6fdb", label="return in real env")
    ax.set_xticks(x)
    ax.set_xticklabels(["real env" if s[0] == "real" else f"dream tau={s[0].split()[1]}" for s in summary])
    ax.set_ylabel("episode return")
    ax.axhline(0, color="black", lw=0.8)
    ax.legend(fontsize=8, loc="lower right")
    fig.tight_layout()
    fig.savefig(os.path.join(args.fig_dir, "ch10-dream-vs-real.png"), dpi=110, facecolor="white")
    plt.close(fig)

    # 그림 3: 가장 좋은 꿈속 컨트롤러가 진짜 환경에서 진자를 세우는 모습
    best = max((s for s in summary if s[0] != "real"), key=lambda s: s[2])[0]
    params = results[best][0]
    rets, frames, thetas = rollout_real(params[None], np.array([1003]), vae, mdn, device, record=True)
    show = np.arange(0, 200, 20)
    fig = plt.figure(figsize=(11, 3.6))
    for i, t in enumerate(show):
        ax = fig.add_subplot(2, len(show), i + 1)
        ax.imshow(frames[t], cmap="gray", vmin=0, vmax=255)
        ax.set_title(f"t={t}", fontsize=8)
        ax.axis("off")
    ax = fig.add_subplot(2, 1, 2)
    th = (thetas[:, 0] + np.pi) % (2 * np.pi) - np.pi
    ax.plot(np.arange(200) * 0.05, th, color="#2f6fdb")
    ax.axhline(0, color="#1f8a5b", ls="--", lw=1)
    ax.set_xlabel("time (s)")
    ax.set_ylabel("angle (rad), 0 = upright")
    ax.set_title(f"controller trained only in dream ({best}) acting in the real env, return {rets[0, 0]:.0f}", fontsize=9)
    fig.tight_layout()
    fig.savefig(os.path.join(args.fig_dir, "ch10-swingup.png"), dpi=110, facecolor="white")
    plt.close(fig)
    print("그림 저장:", args.fig_dir)


if __name__ == "__main__":
    main()
