"""6장 · 첫 번째 World Model: 진자의 동역학 모델을 데이터로 배운다.

무엇을 하나
  1. 무작위 행동으로 진자 에피소드를 모은다 (학습용 · 검증용을 따로).
  2. (o_t, a_t) → (Δo_t = o_{t+1} − o_t, r_t) 를 예측하는 MLP 를 학습한다.
  3. 1스텝 예측이 얼마나 정확한지, 여러 스텝을 이어서 상상(열린 고리 롤아웃)하면
     오차가 어떻게 쌓이는지 잰다.
  4. 1스텝 손실로 배운 모델과 다스텝 손실로 배운 모델의 오차 누적을 비교한다.

실행
  python ch06_dynamics.py            # 본문 결과 (노트북 CPU 4스레드에서 약 46초)
  python ch06_dynamics.py --quick    # 연기 시험 (5초 남짓)
그림은 --fig-dir (기본 figures/) 에, 학습한 모델은 ch06_model.pt 로 저장된다.
7장(ch07_mpc.py)이 이 파일의 DynamicsModel · train_model 을 그대로 가져다 쓴다.
"""

from __future__ import annotations

import argparse
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, angle_normalize, collect_episodes


def get_device() -> str:
    if torch.cuda.is_available():
        return "cuda"
    if torch.backends.mps.is_available():
        return "mps"
    return "cpu"


# --8<-- [start:data]
def episodes_to_arrays(episodes):
    """에피소드 목록 → 전이(transition) 배열 네 개.

    에피소드 하나의 obs 는 (T+1, 3) 이라서, 앞의 T 개가 o_t, 뒤의 T 개가 o_{t+1} 이 된다.
    """
    obs = np.concatenate([ep["obs"][:-1] for ep in episodes])  # (N, 3)  o_t
    act = np.concatenate([ep["action"] for ep in episodes])  # (N, 1)  a_t
    next_obs = np.concatenate([ep["obs"][1:] for ep in episodes])  # (N, 3)  o_{t+1}
    rew = np.concatenate([ep["reward"] for ep in episodes])  # (N,)    r_t
    return obs, act, next_obs, rew


def make_inputs_targets(obs, act, next_obs, rew):
    """신경망의 입력 x = [o_t, a_t], 정답 y = [o_{t+1} − o_t, r_t]."""
    inputs = np.concatenate([obs, act], axis=1)  # (N, 4)
    targets = np.concatenate([next_obs - obs, rew[:, None]], axis=1)  # (N, 4)
    return inputs.astype(np.float32), targets.astype(np.float32)
# --8<-- [end:data]


# --8<-- [start:model]
class DynamicsModel(nn.Module):
    """(o_t, a_t) → (Δo_t, r_t) 를 예측하는 MLP. 이것이 우리의 첫 World Model 이다."""

    def __init__(self, obs_dim: int = 3, act_dim: int = 1, hidden: int = 128):
        super().__init__()
        self.obs_dim, self.act_dim = obs_dim, act_dim
        self.net = nn.Sequential(
            nn.Linear(obs_dim + act_dim, hidden),
            nn.SiLU(),
            nn.Linear(hidden, hidden),
            nn.SiLU(),
            nn.Linear(hidden, hidden),
            nn.SiLU(),
            nn.Linear(hidden, obs_dim + 1),  # Δo 3개 + 보상 1개
        )
        # 표준화 통계. buffer 로 두면 학습은 되지 않지만 .to(device) · 저장 때 함께 따라간다.
        self.register_buffer("in_mean", torch.zeros(obs_dim + act_dim))
        self.register_buffer("in_std", torch.ones(obs_dim + act_dim))
        self.register_buffer("out_mean", torch.zeros(obs_dim + 1))
        self.register_buffer("out_std", torch.ones(obs_dim + 1))

    def set_normalizer(self, inputs: np.ndarray, targets: np.ndarray) -> None:
        """학습 데이터로 열마다 평균 μ 와 표준편차 σ 를 구해 둔다 (x̃ = (x − μ) / σ)."""
        dev = self.in_mean.device
        self.in_mean.copy_(torch.as_tensor(inputs.mean(0), device=dev))
        self.in_std.copy_(torch.as_tensor(inputs.std(0) + 1e-6, device=dev))
        self.out_mean.copy_(torch.as_tensor(targets.mean(0), device=dev))
        self.out_std.copy_(torch.as_tensor(targets.std(0) + 1e-6, device=dev))

    def forward_normalized(self, obs: torch.Tensor, act: torch.Tensor) -> torch.Tensor:
        """표준화된 공간에서의 예측 ỹ (학습할 때 쓴다). obs (B, 3), act (B, 1) → (B, 4)."""
        x = torch.cat([obs, act], dim=-1)
        x = (x - self.in_mean) / self.in_std
        return self.net(x)

    def predict(self, obs: torch.Tensor, act: torch.Tensor):
        """원래 단위의 예측: 다음 관측 ô_{t+1} = o_t + Δô_t 와 보상 r̂_t."""
        y = self.forward_normalized(obs, act) * self.out_std + self.out_mean
        next_obs = obs + y[..., : self.obs_dim]
        reward = y[..., self.obs_dim]
        return next_obs, reward
# --8<-- [end:model]


def to_tensor(x, device):
    return torch.as_tensor(np.asarray(x), dtype=torch.float32, device=device)


# --8<-- [start:train]
def train_model(model, train_eps, val_eps=None, epochs=100, batch_size=256, lr=1e-3,
                device="cpu", log_every=10, verbose=True):
    """1스텝 MSE 손실로 동역학 모델을 학습한다. 에폭마다의 학습 · 검증 손실을 돌려준다."""
    obs, act, next_obs, rew = episodes_to_arrays(train_eps)
    inputs, targets = make_inputs_targets(obs, act, next_obs, rew)
    model.to(device)
    model.set_normalizer(inputs, targets)  # 표준화 통계는 '학습' 데이터로만 구한다

    o, a = to_tensor(obs, device), to_tensor(act, device)
    y = (to_tensor(targets, device) - model.out_mean) / model.out_std  # 표준화된 정답 ỹ
    if val_eps is not None:
        v = episodes_to_arrays(val_eps)
        vo, va = to_tensor(v[0], device), to_tensor(v[1], device)
        vy = (to_tensor(make_inputs_targets(*v)[1], device) - model.out_mean) / model.out_std

    opt = torch.optim.Adam(model.parameters(), lr=lr)
    history = {"train": [], "val": []}
    n = len(o)
    for epoch in range(1, epochs + 1):
        model.train()
        perm = torch.randperm(n, device=device)  # 매 에폭 순서를 섞는다
        total = 0.0
        for i in range(0, n, batch_size):
            idx = perm[i : i + batch_size]
            pred = model.forward_normalized(o[idx], a[idx])  # (B, 4)
            loss = ((pred - y[idx]) ** 2).mean()  # MSE
            opt.zero_grad()
            loss.backward()
            opt.step()
            total += loss.item() * len(idx)
        history["train"].append(total / n)

        if val_eps is not None:
            model.eval()
            with torch.no_grad():
                history["val"].append(((model.forward_normalized(vo, va) - vy) ** 2).mean().item())
        if verbose and (epoch == 1 or epoch % log_every == 0):
            msg = f"epoch {epoch:4d} | train MSE {history['train'][-1]:.5f}"
            if val_eps is not None:
                msg += f" | val MSE {history['val'][-1]:.5f}"
            print(msg)
    return history
# --8<-- [end:train]


# --8<-- [start:rollout]
@torch.no_grad()
def rollout(model, obs0, actions):
    """열린 고리(open-loop) 롤아웃: 시작 관측과 행동열만 주고, 모델의 예측을 다시 입력으로 넣어 H 스텝을 상상한다.

    obs0 (B, 3), actions (B, H, 1) → 예측 관측 (B, H+1, 3), 예측 보상 (B, H)
    진짜 관측은 시작할 때 한 번만 본다. 그 뒤로는 모델이 만든 '상상' 만 이어진다.
    """
    o = obs0
    obs_seq, rew_seq = [o], []
    for k in range(actions.shape[1]):
        o, r = model.predict(o, actions[:, k])
        obs_seq.append(o)
        rew_seq.append(r)
    return torch.stack(obs_seq, dim=1), torch.stack(rew_seq, dim=1)
# --8<-- [end:rollout]


# --8<-- [start:horizon]
def obs_to_angle(obs):
    """[cos θ, sin θ, θ̇] → (θ, θ̇). atan2 는 cos · sin 이 조금 어긋나도 각도를 안정적으로 되살린다."""
    return np.arctan2(obs[..., 1], obs[..., 0]), obs[..., 2]


def horizon_errors(model, episodes, horizon=50, n_starts=300, seed=0, device="cpu"):
    """기록된 에피소드의 아무 시점에서 출발해, 기록된 행동으로 H 스텝 롤아웃하고 진짜와 비교한다.

    돌려주는 값: 각 스텝 k(1..H) 에서의 각도 오차 |θ̂ − θ| 와 각속도 오차 |θ̂̇ − θ̇|, 모양 (n_starts, H).
    """
    rng = np.random.default_rng(seed)
    T = len(episodes[0]["action"])
    ep_idx = rng.integers(len(episodes), size=n_starts)
    t0 = rng.integers(T - horizon + 1, size=n_starts)
    true_obs = np.stack([episodes[e]["obs"][t : t + horizon + 1] for e, t in zip(ep_idx, t0)])  # (S, H+1, 3)
    acts = np.stack([episodes[e]["action"][t : t + horizon] for e, t in zip(ep_idx, t0)])  # (S, H, 1)

    pred_obs, _ = rollout(model, to_tensor(true_obs[:, 0], device), to_tensor(acts, device))
    pred_obs = pred_obs.cpu().numpy()
    th_hat, thdot_hat = obs_to_angle(pred_obs[:, 1:])
    th, thdot = obs_to_angle(true_obs[:, 1:])
    angle_err = np.abs(angle_normalize(th_hat - th))  # 각도 차이는 [-π, π) 로 접어서 잰다
    vel_err = np.abs(thdot_hat - thdot)
    return angle_err, vel_err
# --8<-- [end:horizon]


# --8<-- [start:multistep]
def make_windows(episodes, k):
    """에피소드를 길이 k 의 조각으로 자른다: 관측 (N, k+1, 3), 행동 (N, k, 1), 보상 (N, k)."""
    obs_w, act_w, rew_w = [], [], []
    for ep in episodes:
        T = len(ep["action"])
        for t in range(T - k + 1):
            obs_w.append(ep["obs"][t : t + k + 1])
            act_w.append(ep["action"][t : t + k])
            rew_w.append(ep["reward"][t : t + k])
    return np.stack(obs_w), np.stack(act_w), np.stack(rew_w)


def train_multistep(model, train_eps, k=5, epochs=100, batch_size=256, lr=1e-3,
                    device="cpu", log_every=10, verbose=True):
    """다스텝 손실: 모델 자신의 예측을 다시 넣어 k 스텝 굴리고, 매 스텝의 오차를 모두 더한다.

    k = 1 이면 train_model 과 같은 1스텝 손실이 된다. 오차는 Δo 의 표준편차 단위로 잰다.
    """
    obs, act, next_obs, rew = episodes_to_arrays(train_eps)
    model.to(device)
    model.set_normalizer(*make_inputs_targets(obs, act, next_obs, rew))
    ow, aw, rw = (to_tensor(x, device) for x in make_windows(train_eps, k))
    scale_o, scale_r = model.out_std[:3], model.out_std[3]

    opt = torch.optim.Adam(model.parameters(), lr=lr)
    history = []
    n = len(ow)
    for epoch in range(1, epochs + 1):
        perm = torch.randperm(n, device=device)
        total = 0.0
        for i in range(0, n, batch_size):
            idx = perm[i : i + batch_size]
            o = ow[idx, 0]  # 진짜 관측은 첫 스텝에서만 쓴다
            loss = 0.0
            for j in range(k):
                o, r = model.predict(o, aw[idx, j])  # 예측을 다음 입력으로 (기울기도 그대로 흐른다)
                err_o = (o - ow[idx, j + 1]) / scale_o
                err_r = (r - rw[idx, j]) / scale_r
                loss = loss + (err_o.pow(2).sum(-1) + err_r.pow(2)).mean() / 4
            loss = loss / k
            opt.zero_grad()
            loss.backward()
            opt.step()
            total += loss.item() * len(idx)
        history.append(total / n)
        if verbose and (epoch == 1 or epoch % log_every == 0):
            print(f"[{k}-step] epoch {epoch:4d} | train loss {history[-1]:.5f}")
    return history
# --8<-- [end:multistep]


def one_step_report(model, episodes, device):
    """검증 데이터에서 1스텝 예측의 RMSE 와 R² (설명된 분산의 비율)을 차원별로 출력한다."""
    obs, act, next_obs, rew = episodes_to_arrays(episodes)
    _, targets = make_inputs_targets(obs, act, next_obs, rew)
    with torch.no_grad():
        nxt, r = model.predict(to_tensor(obs, device), to_tensor(act, device))
    pred = np.concatenate([nxt.cpu().numpy() - obs, r.cpu().numpy()[:, None]], axis=1)
    names = ["d_cos", "d_sin", "d_thdot", "reward"]
    for i, name in enumerate(names):
        rmse = np.sqrt(np.mean((pred[:, i] - targets[:, i]) ** 2))
        r2 = 1 - np.sum((pred[:, i] - targets[:, i]) ** 2) / np.sum((targets[:, i] - targets[:, i].mean()) ** 2)
        print(f"  {name:8s} RMSE {rmse:.5f}  (target std {targets[:, i].std():.4f})  R^2 {r2:.5f}")
    return pred, targets


def main():
    parser = argparse.ArgumentParser(description="6장: 진자 동역학 모델 학습")
    parser.add_argument("--quick", action="store_true", help="아주 작게 돌려 오류만 확인")
    parser.add_argument("--fig-dir", default="figures")
    parser.add_argument("--seed", type=int, default=0)
    parser.add_argument("--episodes", type=int, default=50, help="학습용 무작위 에피소드 수")
    parser.add_argument("--epochs", type=int, default=100)
    args = parser.parse_args()
    if args.quick:
        args.episodes, args.epochs = 8, 4

    import os

    os.makedirs(args.fig_dir, exist_ok=True)
    torch.manual_seed(args.seed)
    np.random.seed(args.seed)
    device = get_device()
    print(f"device: {device}")
    start = time.time()

    # --8<-- [start:main_data]
    # 1) 데이터: 무작위 행동으로 학습용 · 검증용 에피소드를 따로 모은다 (시드가 달라 겹치지 않는다).
    train_eps = collect_episodes(Pendulum(seed=args.seed), args.episodes, seed=args.seed)
    val_eps = collect_episodes(Pendulum(seed=args.seed + 1000), 10, seed=args.seed + 1000)
    print(f"train transitions: {sum(len(e['action']) for e in train_eps)}, "
          f"val transitions: {sum(len(e['action']) for e in val_eps)}")
    # --8<-- [end:main_data]

    # --8<-- [start:main_train]
    # 2) 학습: 1스텝 MSE
    model = DynamicsModel()
    hist = train_model(model, train_eps, val_eps, epochs=args.epochs, device=device)
    print("one-step accuracy on validation data:")
    pred, targets = one_step_report(model, val_eps, device)
    # --8<-- [end:main_train]
    torch.save(model.state_dict(), "ch06_model.pt")

    # 3) 다스텝 손실로 같은 크기의 모델을 하나 더 학습해 비교한다.
    torch.manual_seed(args.seed)
    model_ms = DynamicsModel()
    train_multistep(model_ms, train_eps, k=10, epochs=args.epochs, device=device, log_every=25)

    # 4) 오차 누적: 지평 H 에 따른 오차
    H = 50
    ae1, ve1 = horizon_errors(model, val_eps, horizon=H, device=device)
    aem, vem = horizon_errors(model_ms, val_eps, horizon=H, device=device)
    for k in [1, 5, 10, 20, 30, 50]:
        print(f"  horizon {k:2d}: 1-step model angle err {np.median(ae1[:, k-1]):.4f} rad, "
              f"vel err {np.median(ve1[:, k-1]):.4f} | 10-step model angle {np.median(aem[:, k-1]):.4f}, "
              f"vel {np.median(vem[:, k-1]):.4f}   (median)")
    print(f"elapsed: {time.time() - start:.1f}s")

    # ── 그림 ──────────────────────────────────────────────
    # (a) 학습 곡선
    fig, ax = plt.subplots(figsize=(7.5, 3.6))
    ep = np.arange(1, len(hist["train"]) + 1)
    ax.plot(ep, hist["train"], label="train MSE")
    ax.plot(ep, hist["val"], label="validation MSE")
    ax.set_yscale("log")
    ax.set_xlabel("epoch")
    ax.set_ylabel("MSE (normalized targets)")
    ax.set_title("Learning curve of the dynamics model")
    ax.grid(alpha=0.3)
    ax.legend()
    fig.tight_layout()
    fig.savefig(f"{args.fig_dir}/ch06-learning-curve.png", dpi=120)
    plt.close(fig)

    # (b) 1스텝 예측 산점도
    fig, axes = plt.subplots(1, 4, figsize=(11, 3.0))
    titles = ["Δcos θ", "Δsin θ", "Δθ̇ (angular velocity)", "reward r"]
    sel = np.random.default_rng(0).choice(len(pred), size=min(1500, len(pred)), replace=False)
    for i, ax in enumerate(axes):
        t, p = targets[sel, i], pred[sel, i]
        ax.scatter(t, p, s=4, alpha=0.4)
        lo, hi = min(t.min(), p.min()), max(t.max(), p.max())
        ax.plot([lo, hi], [lo, hi], "k--", lw=1)
        ax.set_title(titles[i], fontsize=10)
        ax.set_xlabel("true")
        if i == 0:
            ax.set_ylabel("predicted")
        ax.grid(alpha=0.3)
    fig.suptitle("One-step predictions on validation data (dashed line = perfect)", fontsize=11)
    fig.tight_layout()
    fig.savefig(f"{args.fig_dir}/ch06-one-step.png", dpi=120)
    plt.close(fig)

    # (c) 한 에피소드의 열린 고리 롤아웃 (100 스텝)
    ep0 = val_eps[3]
    L = 100
    o0 = to_tensor(ep0["obs"][None, 0], device)
    acts = to_tensor(ep0["action"][None, :L], device)
    p1, _ = rollout(model, o0, acts)
    pm, _ = rollout(model_ms, o0, acts)
    th_true, thd_true = obs_to_angle(ep0["obs"][: L + 1])
    th1, thd1 = obs_to_angle(p1[0].cpu().numpy())
    thm, thdm = obs_to_angle(pm[0].cpu().numpy())
    t = np.arange(L + 1) * Pendulum.dt
    fig, axes = plt.subplots(2, 1, figsize=(8.5, 5.0), sharex=True)
    axes[0].plot(t, th_true, "k", lw=2, label="true (environment)")
    axes[0].plot(t, th1, "--", label="model, 1-step loss")
    axes[0].plot(t, thm, ":", lw=2, label="model, 10-step loss")
    axes[0].set_ylabel("angle θ [rad]")
    axes[0].legend(fontsize=8, loc="upper right")
    axes[0].grid(alpha=0.3)
    axes[1].plot(t, thd_true, "k", lw=2)
    axes[1].plot(t, thd1, "--")
    axes[1].plot(t, thdm, ":", lw=2)
    axes[1].set_ylabel("angular velocity θ̇ [rad/s]")
    axes[1].set_xlabel("time [s]  (open-loop: the model never sees the true state after t = 0)")
    axes[1].grid(alpha=0.3)
    fig.suptitle("Open-loop rollout: same start, same actions", fontsize=11)
    fig.tight_layout()
    fig.savefig(f"{args.fig_dir}/ch06-rollout.png", dpi=120)
    plt.close(fig)

    # (d) 지평에 따른 오차 (중앙값과 25–75% 구간)
    ks = np.arange(1, H + 1)
    fig, axes = plt.subplots(1, 2, figsize=(10, 3.6))
    for ax, e1, em, name in [(axes[0], ae1, aem, "angle error |Δθ| [rad]"),
                             (axes[1], ve1, vem, "velocity error |Δθ̇| [rad/s]")]:
        for e, label, style in [(e1, "1-step loss", "-"), (em, "10-step loss", "--")]:
            med = np.median(e, axis=0)
            ax.plot(ks, med, style, label=label)
            ax.fill_between(ks, np.percentile(e, 25, axis=0), np.percentile(e, 75, axis=0), alpha=0.2)
        ax.set_yscale("log")
        ax.set_xlabel("rollout step k")
        ax.set_ylabel(name)
        ax.grid(alpha=0.3, which="both")
        ax.legend(fontsize=9)
    fig.suptitle("Compounding error: error grows with the rollout horizon (median, 25-75% band)", fontsize=11)
    fig.tight_layout()
    fig.savefig(f"{args.fig_dir}/ch06-horizon-error.png", dpi=120)
    plt.close(fig)
    print(f"figures saved to {args.fig_dir}/")


if __name__ == "__main__":
    main()
