"""World Model 튜토리얼 공용 환경 (numpy 만 사용한다).

이 파일 하나에 튜토리얼 전체에서 쓰는 '세상' 두 가지가 들어 있다.

1. Pendulum       : 진자 스윙업. 관측 = [cos θ, sin θ, θ̇] (숫자 3개).  5 ~ 7 · 13장
2. PixelPendulum  : 같은 진자를 64×64 흑백 그림으로 보는 환경.          4 · 8 ~ 12장

물리 법칙은 OpenAI Gym(지금의 Gymnasium) 의 Pendulum-v1 과 같다.
gymnasium 을 설치하지 않아도 되도록 직접 구현했고, 그 덕분에 '진짜 세상의 규칙'이
코드 몇 줄로 눈앞에 보인다. World Model 은 바로 이 규칙을 데이터만 보고 배우는 것이다.

인터페이스는 일부러 단순하게 했다.
    obs = env.reset()
    obs, reward, done = env.step(action)
(Gymnasium 은 step 이 (obs, reward, terminated, truncated, info) 다섯 개를 돌려준다.)
"""

from __future__ import annotations

import numpy as np

# --8<-- [start:angle]
def angle_normalize(x):
    """각도를 [-π, π) 범위로 접는다. 예: 3π/2 → -π/2."""
    return ((x + np.pi) % (2 * np.pi)) - np.pi
# --8<-- [end:angle]


# --8<-- [start:pendulum]
class Pendulum:
    """진자 스윙업 환경 (Gym Pendulum-v1 과 같은 물리).

    θ (theta)  : 막대의 각도. 0 이면 똑바로 서 있고(위), ±π 면 매달려 있다(아래).
    θ̇ (thdot)  : 각속도 [rad/s]. 1 초에 몇 라디안 도는지.
    행동 u     : 축에 거는 토크(돌리는 힘). [-2, 2] 로 잘린다.
    보상 r     : -(θ² + 0.1·θ̇² + 0.001·u²). 위에 가만히 서 있을 때 최댓값 0.
                 (여기서 θ 는 [-π, π) 로 접은 값, u 는 잘린 토크다.)

    토크 한계(2)가 중력이 막대를 돌리는 힘(토크, 최대 m·g·l/2 = 5)보다 약해서, 한 번에 들어 올릴 수 없다.
    그네 타듯 앞뒤로 흔들어 에너지를 모은 뒤 위에서 균형을 잡아야 한다.
    """

    max_speed = 8.0  # 각속도 한계 [rad/s]
    max_torque = 2.0  # 토크 한계
    dt = 0.05  # 한 스텝의 시간 [s] → 200 스텝 = 10 초
    g = 10.0  # 중력 가속도 [m/s²]
    m = 1.0  # 질량 [kg]
    l = 1.0  # 막대 길이 [m]

    obs_dim = 3
    act_dim = 1

    def __init__(self, seed: int | None = None, max_steps: int = 200):
        self.rng = np.random.default_rng(seed)
        self.max_steps = max_steps
        self.th = 0.0
        self.thdot = 0.0
        self.t = 0

    def reset(self) -> np.ndarray:
        """각도는 아무 곳에서나, 각속도는 [-1, 1] 에서 무작위로 시작한다."""
        self.th = float(self.rng.uniform(-np.pi, np.pi))
        self.thdot = float(self.rng.uniform(-1.0, 1.0))
        self.t = 0
        return self._obs()

    def _obs(self) -> np.ndarray:
        # θ 를 그대로 주지 않고 (cos θ, sin θ) 로 주는 까닭:
        # θ = π 와 θ = -π 는 같은 자세인데 숫자로는 2π 나 떨어져 있다.
        # cos · sin 은 이 '이음매' 가 없어서 신경망이 배우기 쉽다.
        return np.array([np.cos(self.th), np.sin(self.th), self.thdot], dtype=np.float32)

    def step(self, action) -> tuple[np.ndarray, float, bool]:
        u = float(np.clip(np.asarray(action, dtype=np.float64).reshape(-1)[0], -self.max_torque, self.max_torque))
        th, thdot = self.th, self.thdot

        # 보상은 '움직이기 전' 상태로 계산한다 (Gym 과 같다).
        cost = angle_normalize(th) ** 2 + 0.1 * thdot**2 + 0.001 * u**2

        # 물리: θ̈ = (3g / 2l)·sin θ + (3 / m l²)·u     (6장에서 유도한다)
        # 오일러 적분: 속도를 먼저 갱신하고, 새 속도로 각도를 갱신한다 (semi-implicit Euler).
        thdot_new = thdot + (3.0 * self.g / (2.0 * self.l) * np.sin(th) + 3.0 / (self.m * self.l**2) * u) * self.dt
        thdot_new = float(np.clip(thdot_new, -self.max_speed, self.max_speed))
        th_new = th + thdot_new * self.dt

        self.th, self.thdot = th_new, thdot_new
        self.t += 1
        done = self.t >= self.max_steps
        return self._obs(), float(-cost), done
# --8<-- [end:pendulum]


# --8<-- [start:reward]
def pendulum_reward(obs, action):
    """관측 · 행동 묶음으로 보상을 한꺼번에 계산한다 (numpy 배열이나 torch 텐서 모두 된다).

    obs    : (..., 3) = [cos θ, sin θ, θ̇]
    action : (..., 1)
    θ 는 atan2(sin θ, cos θ) 로 되살린다. atan2 의 결과는 이미 [-π, π] 안에 있다.
    """
    if hasattr(obs, "detach"):  # torch 텐서
        import torch

        th = torch.atan2(obs[..., 1], obs[..., 0])
        u = action[..., 0].clamp(-2.0, 2.0)
    else:
        th = np.arctan2(obs[..., 1], obs[..., 0])
        u = np.clip(action[..., 0], -2.0, 2.0)
    thdot = obs[..., 2]
    return -(th**2 + 0.1 * thdot**2 + 0.001 * u**2)
# --8<-- [end:reward]


# --8<-- [start:render]
def render_pendulum(th: float, size: int = 64) -> np.ndarray:
    """각도 θ 의 진자를 size×size 흑백 그림(uint8, 0~255)으로 그린다.

    그림의 가운데가 회전축, θ = 0 이면 막대가 위를 향한다.
    픽셀마다 '막대(선분)까지의 거리' 를 재서, 가까우면 밝게 칠한다.
    가장자리를 1 픽셀 정도 부드럽게 칠해(anti-aliasing) 작은 각도 변화도 그림에 드러나게 한다.
    """
    # 픽셀 중심 좌표를 [-0.5, 0.5] 로 맞춘다. x 는 오른쪽, y 는 아래쪽이 +.
    coords = (np.arange(size) + 0.5) / size - 0.5
    xs, ys = np.meshgrid(coords, coords)  # (size, size)
    rod_len, half_width, bob = 0.36, 0.045, 0.075
    tip_x, tip_y = rod_len * np.sin(th), -rod_len * np.cos(th)  # y 가 아래로 + 라서 위쪽은 -

    # 선분 [회전축(0,0), 끝(tip)] 까지의 거리: 투영 길이 t 를 [0,1] 로 잘라 가장 가까운 점을 찾는다.
    t = np.clip((xs * tip_x + ys * tip_y) / (tip_x**2 + tip_y**2), 0.0, 1.0)
    d_rod = np.hypot(xs - t * tip_x, ys - t * tip_y)
    d_bob = np.hypot(xs - tip_x, ys - tip_y)  # 끝에 달린 추(동그라미)

    px = 1.0 / size  # 픽셀 하나의 크기
    rod = np.clip((half_width - d_rod) / px + 0.5, 0.0, 1.0)
    ball = np.clip((bob - d_bob) / px + 0.5, 0.0, 1.0)
    pivot = np.clip((0.03 - np.hypot(xs, ys)) / px + 0.5, 0.0, 1.0) * 0.5
    img = np.maximum(np.maximum(rod * 0.85, ball), pivot)
    return (img * 255).astype(np.uint8)
# --8<-- [end:render]


# --8<-- [start:pixel]
class PixelPendulum(Pendulum):
    """같은 진자를 그림으로만 보는 환경.

    obs = (size, size) uint8 흑백 그림. 숫자 θ, θ̇ 는 알려 주지 않는다.
    그림 한 장으로는 각도는 보여도 '얼마나 빨리 도는지(θ̇)' 는 알 수 없다.
    → 부분 관측(POMDP). 여러 장면을 기억해야 하므로 RNN 같은 '기억' 이 필요하다 (9 · 11장).

    action_repeat=k 면 같은 행동을 k 번 되풀이하고 보상을 더해서 돌려준다.
    (PlaNet · Dreamer 가 쓰는 기법. 결정 횟수가 줄어 학습이 쉬워진다.)
    """

    def __init__(self, seed: int | None = None, max_steps: int = 200, size: int = 64, action_repeat: int = 1):
        super().__init__(seed=seed, max_steps=max_steps)
        self.size = size
        self.action_repeat = action_repeat

    def reset(self) -> np.ndarray:
        super().reset()
        return self.render()

    def render(self) -> np.ndarray:
        return render_pendulum(self.th, self.size)

    def state(self) -> np.ndarray:
        """정답 상태 [cos θ, sin θ, θ̇] (학습에는 쓰지 않고, 확인 · 그림 그리기에만 쓴다)."""
        return self._obs()

    def step(self, action) -> tuple[np.ndarray, float, bool]:
        total, done = 0.0, False
        for _ in range(self.action_repeat):
            _, r, done = super().step(action)
            total += r
            if done:
                break
        return self.render(), total, done
# --8<-- [end:pixel]


def collect_episodes(env, n_episodes: int, policy=None, seed: int | None = None):
    """정책(없으면 무작위)으로 에피소드를 모아 리스트로 돌려준다.

    에피소드 하나 = dict(obs=(T+1, ...), action=(T, act_dim), reward=(T,))
    obs 는 시작 관측을 포함하므로 행동 · 보상보다 하나 길다.
    """
    rng = np.random.default_rng(seed)
    episodes = []
    for _ in range(n_episodes):
        obs = env.reset()
        obs_list, act_list, rew_list = [obs], [], []
        done = False
        while not done:
            if policy is None:
                action = rng.uniform(-env.max_torque, env.max_torque, size=(env.act_dim,)).astype(np.float32)
            else:
                action = np.asarray(policy(obs), dtype=np.float32).reshape(env.act_dim)
            obs, reward, done = env.step(action)
            obs_list.append(obs)
            act_list.append(action)
            rew_list.append(reward)
        episodes.append(
            dict(
                obs=np.stack(obs_list),
                action=np.stack(act_list).astype(np.float32),
                reward=np.asarray(rew_list, dtype=np.float32),
            )
        )
    return episodes


if __name__ == "__main__":
    # 간단한 확인: 무작위 행동으로 한 에피소드를 돌리고 몇 장면을 저장한다.
    env = Pendulum(seed=0)
    ep = collect_episodes(env, 1, seed=0)[0]
    print("상태 관측", ep["obs"].shape, "행동", ep["action"].shape, "보상 합", round(float(ep["reward"].sum()), 1))

    penv = PixelPendulum(seed=0)
    pep = collect_episodes(penv, 1, seed=0)[0]
    print("그림 관측", pep["obs"].shape, pep["obs"].dtype, "값 범위", pep["obs"].min(), pep["obs"].max())

    try:
        import matplotlib.pyplot as plt

        fig, axes = plt.subplots(1, 8, figsize=(12, 1.8))
        for ax, t in zip(axes, range(0, 200, 25)):
            ax.imshow(pep["obs"][t], cmap="gray", vmin=0, vmax=255)
            ax.set_title(f"t={t}", fontsize=9)
            ax.axis("off")
        fig.tight_layout()
        fig.savefig("pixel_pendulum.png", dpi=120)
        print("pixel_pendulum.png 저장")
    except ImportError:
        pass
