"""7장 · 상상으로 계획하기: 학습한 World Model 안에서 MPC (랜덤 슈팅 · CEM) 로 진자를 세운다.

무엇을 하나
  1. 무작위 행동으로 진자 에피소드 몇 개를 모은다 (시드 데이터).
  2. 되풀이: 6장의 동역학 모델을 학습 → 그 모델 안에서 CEM 으로 계획하며 진짜 환경에서 한 에피소드 실행
             → 새 경험을 데이터에 더한다.
  3. 마지막 모델로 무작위 정책 · 랜덤 슈팅 MPC · CEM MPC 를 같은 시작 상태에서 비교한다.

실행
  python ch07_mpc.py            # 본문 결과 (노트북 CPU 4스레드에서 약 6-10분)
  python ch07_mpc.py --quick    # 연기 시험 (수십 초)
같은 폴더에 ch06_dynamics.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

from ch06_dynamics import DynamicsModel, get_device, obs_to_angle, train_model
from wm_envs import Pendulum, collect_episodes, pendulum_reward

MAX_U = Pendulum.max_torque  # 행동(토크) 범위 [-2, 2]


# --8<-- [start:evaluate]
@torch.no_grad()
def evaluate_sequences(model, obs, actions):
    """후보 행동열 N 개를 모델 속에서 굴려, 각 행동열의 예측 총 보상 J 를 계산한다.

    obs     : (3,)        지금의 진짜 관측 o_t (여기서 상상을 시작한다)
    actions : (N, H, 1)   후보 행동열 N 개, 각각 길이 H
    반환    : (N,)        J = Σ_k r(ô_{t+k}, a_{t+k})
    """
    N, H, _ = actions.shape
    o = obs.expand(N, -1)  # (N, 3): 모든 후보가 같은 곳에서 출발
    total = torch.zeros(N, device=actions.device)
    for k in range(H):
        a = actions[:, k]  # (N, 1)
        total += pendulum_reward(o, a)  # 알려진 보상 함수로 점수를 매긴다
        o, _ = model.predict(o, a)  # 상상 속 다음 관측
    return total
# --8<-- [end:evaluate]


# --8<-- [start:random_shooting]
class RandomShootingPlanner:
    """랜덤 슈팅: 무작위 행동열 N 개를 뽑아 모델 속에서 시험해 보고, 가장 좋은 것의 첫 행동을 고른다."""

    def __init__(self, model, horizon=25, n_candidates=1000, device="cpu"):
        self.model, self.H, self.N, self.device = model, horizon, n_candidates, device

    def reset(self):
        pass  # 기억할 것이 없다

    def __call__(self, obs):
        obs = torch.as_tensor(obs, dtype=torch.float32, device=self.device)
        actions = torch.empty(self.N, self.H, 1, device=self.device).uniform_(-MAX_U, MAX_U)
        returns = evaluate_sequences(self.model, obs, actions)
        best = returns.argmax()
        return actions[best, 0].cpu().numpy()  # 첫 행동만 실행한다 (MPC)
# --8<-- [end:random_shooting]


# --8<-- [start:cem]
class CEMPlanner:
    """교차 엔트로피 방법(CEM): 가우시안에서 행동열을 뽑고, 정예(elite)에 가우시안을 다시 맞추기를 되풀이한다."""

    def __init__(self, model, horizon=25, n_candidates=400, n_elites=40, n_iters=5,
                 alpha=0.1, init_std=1.0, device="cpu"):
        self.model, self.H, self.N, self.K = model, horizon, n_candidates, n_elites
        self.n_iters, self.alpha, self.init_std, self.device = n_iters, alpha, init_std, device
        self.reset()

    def reset(self):
        # 에피소드가 시작될 때 평균 행동열을 0 으로 (= 아무 정보 없음)
        self.mean = torch.zeros(self.H, 1, device=self.device)

    def evaluate(self, obs, actions):
        # 후보 행동열의 점수. 확률적 모델(ch07_pets.py)은 이 메서드만 바꿔 끼운다.
        return evaluate_sequences(self.model, obs, actions)

    def __call__(self, obs):
        obs = torch.as_tensor(obs, dtype=torch.float32, device=self.device)
        mean = self.mean.clone()  # (H, 1) μ
        std = torch.full_like(mean, self.init_std)  # (H, 1) σ
        for _ in range(self.n_iters):
            # 1) 표본 뽑기: a = μ + σ ⊙ ε,  ε ~ N(0, I). 범위 밖은 잘라낸다.
            eps = torch.randn(self.N, self.H, 1, device=self.device)
            actions = (mean + std * eps).clamp(-MAX_U, MAX_U)  # (N, H, 1)
            # 2) 모델 속에서 점수 매기기
            returns = self.evaluate(obs, actions)  # (N,)
            # 3) 점수 상위 K 개 = 정예
            elites = actions[returns.topk(self.K).indices]  # (K, H, 1)
            # 4) 정예에 가우시안을 최대우도로 맞춘다: 평균 = 표본 평균, 분산 = 표본 분산
            new_mean = elites.mean(dim=0)
            new_std = elites.std(dim=0, unbiased=False)
            # 5) 평활: 한 번에 너무 크게 바뀌지 않게 옛 값을 α 만큼 섞는다
            mean = self.alpha * mean + (1 - self.alpha) * new_mean
            std = self.alpha * std + (1 - self.alpha) * new_std
        # 웜 스타트: 다음 시각의 계획은 이번 계획을 한 칸 당긴 것에서 시작한다
        self.mean = torch.cat([mean[1:], torch.zeros(1, 1, device=self.device)])
        return mean[0].cpu().numpy()  # 첫 행동만 실행한다 (MPC)
# --8<-- [end:cem]


# --8<-- [start:run_episode]
def run_episode(env, planner):
    """진짜 환경에서 한 에피소드를 돈다. 매 스텝 '계획 → 첫 행동 실행 → 새 관측으로 다시 계획' (MPC)."""
    obs = env.reset()
    planner.reset()
    obs_list, act_list, rew_list = [obs], [], []
    done = False
    while not done:
        action = np.asarray(planner(obs), dtype=np.float32).reshape(1)
        obs, reward, done = env.step(action)
        obs_list.append(obs)
        act_list.append(action)
        rew_list.append(reward)
    episode = dict(obs=np.stack(obs_list), action=np.stack(act_list),
                   reward=np.asarray(rew_list, dtype=np.float32))
    return episode, float(episode["reward"].sum())
# --8<-- [end:run_episode]


class RandomPolicy:
    def __init__(self, seed=0):
        self.rng = np.random.default_rng(seed)

    def reset(self):
        pass

    def __call__(self, obs):
        return self.rng.uniform(-MAX_U, MAX_U, size=(1,))


def fit_model(data, device, seed, train_steps=3000, batch_size=256):
    """데이터 크기와 관계없이 기울기 스텝 수가 비슷하도록 에폭 수를 정해 처음부터 학습한다."""
    torch.manual_seed(seed)
    n = sum(len(ep["action"]) for ep in data)
    epochs = max(5, math.ceil(train_steps / math.ceil(n / batch_size)))
    model = DynamicsModel()
    hist = train_model(model, data, epochs=epochs, batch_size=batch_size, device=device, verbose=False)
    model.eval()
    return model, hist["train"][-1]


def main():
    parser = argparse.ArgumentParser(description="7장: 학습한 모델 안에서 MPC 로 진자 세우기")
    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("--seed-episodes", type=int, default=1, help="처음에 모을 무작위 에피소드 수")
    parser.add_argument("--iterations", type=int, default=8, help="학습 → 계획 → 수집 되풀이 횟수")
    parser.add_argument("--eval-episodes", type=int, default=10)
    parser.add_argument("--horizon", type=int, default=25)
    args = parser.parse_args()
    if args.quick:
        args.iterations, args.eval_episodes, args.horizon = 1, 1, 10
    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()
    train_steps = 300 if args.quick else 3000

    # --8<-- [start:loop]
    # 1) 시드 데이터: 무작위 행동 에피소드 몇 개
    env = Pendulum(seed=args.seed)
    data = collect_episodes(env, args.seed_episodes, seed=args.seed)
    print(f"seed data: {args.seed_episodes} random episodes, "
          f"mean return {np.mean([ep['reward'].sum() for ep in data]):.1f}")

    # 2) 되풀이: 모델 학습 → 그 모델로 계획하며 진짜 환경에서 한 에피소드 → 데이터에 더하기
    returns_per_iter = []
    for it in range(1, args.iterations + 1):
        model, loss = fit_model(data, device, seed=args.seed + it, train_steps=train_steps)
        planner = CEMPlanner(model, horizon=args.horizon, 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 | model loss {loss:.5f} | "
              f"CEM-MPC return {ret:8.1f} | {time.time() - start:5.0f}s")
    # --8<-- [end:loop]

    # 3) 마지막 데이터로 모델을 한 번 더 학습하고, 세 가지 방법을 같은 시작 상태에서 비교
    model, _ = fit_model(data, device, seed=args.seed + 999, train_steps=train_steps)
    methods = {
        "random policy": lambda i: RandomPolicy(seed=args.seed + i),
        "random shooting MPC": lambda i: RandomShootingPlanner(model, horizon=args.horizon, device=device),
        "CEM MPC": lambda i: CEMPlanner(model, horizon=args.horizon, device=device),
    }
    results, trajectories = {}, {}
    for name, make in methods.items():
        rets, trajs = [], []
        for i in range(args.eval_episodes):
            torch.manual_seed(args.seed + i)
            ep, ret = run_episode(Pendulum(seed=10_000 + i), make(i))  # 같은 i → 같은 시작 상태
            rets.append(ret)
            trajs.append(ep)
        results[name], trajectories[name] = np.array(rets), trajs
        print(f"{name:20s}: 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")

    # ── 그림 ──────────────────────────────────────────────
    # (a) 되풀이에 따른 리턴
    fig, ax = plt.subplots(figsize=(7.5, 3.6))
    its = np.arange(1, len(returns_per_iter) + 1)
    ax.plot(its, returns_per_iter, "o-", label="CEM-MPC episode (collected)")
    ax.axhline(results["random policy"].mean(), color="gray", ls="--", label="random policy (mean)")
    ax.set_xlabel("iteration (one new real episode per iteration)")
    ax.set_ylabel("episode return")
    ax.set_title("Model-based RL loop: learn model -> plan with CEM -> collect")
    ax.grid(alpha=0.3)
    ax.legend(fontsize=9)
    fig.tight_layout()
    fig.savefig(f"{args.fig_dir}/ch07-learning-curve.png", dpi=120)
    plt.close(fig)

    # (b) 각도 · 행동 궤적
    fig, axes = plt.subplots(2, 3, figsize=(11, 5.0), sharex=True, sharey="row")
    t = np.arange(201) * Pendulum.dt
    for col, name in enumerate(methods):
        for ep in trajectories[name]:
            th, _ = obs_to_angle(ep["obs"])
            axes[0, col].plot(t, th, lw=1.2)
            axes[1, col].plot(t[:-1], ep["action"][:, 0], lw=0.8)
        axes[0, col].set_title(f"{name}\nmean return {results[name].mean():.0f}", fontsize=10)
        axes[0, col].axhline(0, color="k", lw=0.6)
        axes[0, col].grid(alpha=0.3)
        axes[1, col].grid(alpha=0.3)
        axes[1, col].set_xlabel("time [s]")
    axes[0, 0].set_ylabel("angle θ [rad]\n(0 = upright)")
    axes[1, 0].set_ylabel("torque u")
    fig.tight_layout()
    fig.savefig(f"{args.fig_dir}/ch07-trajectories.png", dpi=120)
    plt.close(fig)
    print(f"figures saved to {args.fig_dir}/")


if __name__ == "__main__":
    main()
