"""
=============================================================================
第 2 讲 · 代码 3：策略迭代（Policy Iteration）
=============================================================================

对应讲义：lectures/lecture02_mdp_planning.md 第 6、7 节

-----------------------------------------------------------------------------
【这个文件要解决什么问题？】
-----------------------------------------------------------------------------
从"评估"走向"控制"：**找到最优策略 π***。

最笨的办法是把所有 A^N 个确定性策略挨个评估一遍，挑最好的（火星车有 2^7 = 128 个，还行；
稍大一点的问题就是天文数字）。策略迭代用一个聪明得多的办法：

    1. 策略评估：算出当前策略 π 的价值 V^π
    2. 策略改进：在每个状态选 Q^π(s, a) 最大的动作，得到新策略 π'
    3. 如果 π' 和 π 一样，停止——π 就是最优策略；否则 π ← π'，回到 1

-----------------------------------------------------------------------------
【为什么这样做一定行？（讲义 7.3 节的策略改进定理）】
-----------------------------------------------------------------------------
    - 每次改进后，新策略在**每个状态**都不比旧策略差：V^{π'}(s) >= V^π(s)
    - 策略只有有限个，而且不会"绕回去"，所以一定在有限步内停止
    - 停止时满足贝尔曼最优方程，所以停在最优策略
本文件会用数值实验逐条验证这三点。

-----------------------------------------------------------------------------
【运行方式】
-----------------------------------------------------------------------------
    python 03_policy_iteration.py

【数据约定】P: (N, A, N)，R: (N, A)，确定性策略 policy: 长度 N 的整数数组
【下标约定】代码中的状态 i  <==>  讲义中的 s_{i+1}
"""

import itertools   # Python 标准库，这里用它的 product 函数枚举所有策略

import numpy as np

np.set_printoptions(precision=2, suppress=True)

TRY_LEFT, TRY_RIGHT = 0, 1
ARROWS = ["←", "→"]


# =============================================================================
# 第 0 部分：基础函数（精简版，详细注释见 01、02 文件）
# =============================================================================
def build_mars_rover_mdp(success_prob=0.8):
    """火星车 MDP：7 个状态，2 个动作；成功概率 success_prob，否则原地不动；s1 奖励 +1，s7 奖励 +10。"""
    n, n_actions = 7, 2
    P = np.zeros((n, n_actions, n))
    for s in range(n):
        for a in range(n_actions):
            target = min(max(s + (-1 if a == TRY_LEFT else 1), 0), n - 1)
            P[s, a, target] += success_prob
            P[s, a, s] += 1.0 - success_prob
    R = np.tile(np.array([1.0, 0, 0, 0, 0, 0, 10.0])[:, None], (1, n_actions))
    return P, R


def evaluate_deterministic_policy(P, R, policy, gamma):
    """
    精确评估一个**确定性**策略：V^π = (I - γ P_π)^{-1} R_π。

    【和 02 文件的区别】
        02 文件处理一般的随机策略，用 einsum 对动作求加权平均。
        确定性策略每个状态只选一个动作，所以不需要求平均，直接"挑出"那一行就行：
            P_π[s] = P[s, policy[s]]，   R_π[s] = R[s, policy[s]]

    【Python 小知识】花式索引 P[np.arange(n), policy]
        np.arange(n) = [0, 1, ..., n-1]，policy = [a_0, a_1, ..., a_{n-1}]，
        P[np.arange(n), policy] 取出 P[0, a_0], P[1, a_1], ..., 每个都是长度 n 的一维数组，
        堆起来就是 (n, n) 的矩阵 P_π。

    【参数】
        gamma : float，[0, 1)。

    【边界】
        γ = 1 → ValueError（I - P_π 奇异）。
    """
    if not (0.0 <= gamma < 1.0):
        raise ValueError(f"gamma 必须在 [0, 1) 内，但收到了 {gamma}")
    n = P.shape[0]
    idx = np.arange(n)
    P_pi = P[idx, policy]        # 形状 (n, n)
    R_pi = R[idx, policy]        # 形状 (n,)
    return np.linalg.solve(np.eye(n) - gamma * P_pi, R_pi)


def q_from_v(P, R, V, gamma):
    """Q(s, a) = R(s, a) + γ Σ_{s'} P(s'|s, a) V(s')。返回形状 (N, A)。"""
    return R + gamma * np.einsum("sat,t->sa", P, V)


def format_policy(policy):
    """把 [0, 1, 1] 显示成 "← → →"。"""
    return " ".join(ARROWS[a] if a < len(ARROWS) else str(a) for a in policy)


# =============================================================================
# 第 1 部分：策略改进
# =============================================================================
def greedy_policy(Q, old_policy=None, tol=1e-9):
    """
    策略改进：在每个状态选 Q 值最大的动作（"贪心"策略）。

        π'(s) = argmax_a Q(s, a)

    【为什么"贪心"就是改进？】
        Q^π(s, a) 是"在 s 先做 a、之后按 π 走"的价值。选 Q 最大的 a，
        就是"在这一步做出最好的选择，假设之后仍按老策略走"。
        策略改进定理（讲义 7.3 节）保证：把**所有**状态都这样换掉，新策略整体不会变差。

    【平局怎么办？——一个真实的坑】
        如果某个状态有多个动作的 Q 值（几乎）相等，选哪个都一样好。
        但如果每次随便选一个（比如随机选），策略可能在"同样好的动作之间"来回切换，
        导致"策略不再变化"这个停止条件永远满足不了，算法停不下来！
        解决办法：**如果旧策略的动作也在最优动作之列，就保留旧动作**；
        否则选编号最小的最优动作。这样平局不会引起无意义的变化。
        （演示 5 会亲眼看到不这样做的后果。）

    【参数】
        Q          : 形状 (N, A) 的动作价值。
        old_policy : 长度 N 的整数数组或 None。提供时用于打破平局。
        tol        : float，Q 值相差小于 tol 视为平局。
                     为什么需要？因为浮点计算有误差，两个理论上相等的 Q 值
                     可能算出来差 1e-15，直接比大小会被这种"噪声"左右。

    【返回】
        长度 N 的整数数组，新策略。
    """
    n = Q.shape[0]
    new_policy = np.zeros(n, dtype=int)
    for s in range(n):
        best_value = Q[s].max()
        # 所有"足够接近最大值"的动作都算最优
        best_actions = np.flatnonzero(Q[s] >= best_value - tol)
        if old_policy is not None and old_policy[s] in best_actions:
            new_policy[s] = old_policy[s]          # 平局时保留旧动作
        else:
            new_policy[s] = best_actions[0]        # 否则取编号最小的最优动作
    return new_policy


# =============================================================================
# 第 2 部分：策略迭代
# =============================================================================
def policy_iteration(P, R, gamma, init_policy=None, max_iters=1000):
    """
    策略迭代：交替进行"策略评估"和"策略改进"，直到策略不再变化。

    【伪代码】（讲义 7.4 节）
        π_0 ← 初始策略
        for i = 0, 1, 2, ...:
            V^{π_i}   ← 策略评估(π_i)
            Q^{π_i}   ← 由 V^{π_i} 算出
            π_{i+1}   ← greedy(Q^{π_i})
            if π_{i+1} == π_i: 停止，返回 π_i

    【参数】
        P, R        : MDP。
        gamma       : float，[0, 1)。
        init_policy : 长度 N 的整数数组；None 表示全部选动作 0。
                      初始策略**不影响最终结果**（都会收敛到最优价值 V*），但会影响迭代次数。
        max_iters   : int，最大迭代次数（保险）。理论上最多 A^N 次，实践中通常很少。

    【返回】
        (policy, V, history)：
        - policy  : 最优策略
        - V       : 最优价值 V*
        - history : list，每个元素是 (第 i 次迭代的策略, 它的价值) 二元组，用来观察过程

    【边界】
        - 达到 max_iters 仍未停止 → RuntimeError（正常情况下不会发生；如果发生，
          多半是平局处理有问题或数值误差，比如 tol 设得太小）。
    """
    n, n_actions, _ = P.shape
    policy = np.zeros(n, dtype=int) if init_policy is None else np.array(init_policy, dtype=int)
    history = []

    for _ in range(max_iters):
        V = evaluate_deterministic_policy(P, R, policy, gamma)   # 第 1 步：评估
        history.append((policy.copy(), V.copy()))
        Q = q_from_v(P, R, V, gamma)
        new_policy = greedy_policy(Q, old_policy=policy)          # 第 2 步：改进
        # np.array_equal 判断两个数组的形状和所有元素是否完全相同
        if np.array_equal(new_policy, policy):                    # 第 3 步：检查是否停止
            return policy, V, history
        policy = new_policy

    raise RuntimeError(f"策略迭代在 {max_iters} 次迭代内没有停止")


# =============================================================================
# 第 3 部分：暴力枚举（用来验证策略迭代的结果）
# =============================================================================
def brute_force_all_policies(P, R, gamma):
    """
    把所有 A^N 个确定性策略全部评估一遍。只适用于非常小的问题！

    【思路】
        itertools.product(range(A), repeat=N) 会生成所有长度为 N、
        每个位置取值 0..A-1 的组合。例如 A=2, N=3 时生成：
            (0,0,0), (0,0,1), (0,1,0), (0,1,1), (1,0,0), (1,0,1), (1,1,0), (1,1,1)
        共 2^3 = 8 个，每个就是一个确定性策略。

    【返回】
        (policies, values)：
        - policies : 形状 (A^N, N) 的整数数组，每行一个策略
        - values   : 形状 (A^N, N) 的数组，每行是对应策略的 V^π

    【边界】
        A^N 很大时会耗尽内存和时间。这里加了一个上限检查：超过 10 万个策略就拒绝运行。
    """
    n, n_actions, _ = P.shape
    total = n_actions ** n
    if total > 100_000:
        raise ValueError(f"一共有 {total} 个策略，太多了，不适合暴力枚举")
    policies = np.array(list(itertools.product(range(n_actions), repeat=n)), dtype=int)
    values = np.array([evaluate_deterministic_policy(P, R, pol, gamma) for pol in policies])
    return policies, values


# =============================================================================
# 第 4 部分：演示
# =============================================================================
def print_row(label, values):
    print(f"  {label:<14s}" + "".join(f"{v:8.2f}" for v in values))


def main():
    P, R = build_mars_rover_mdp(success_prob=0.8)
    n, n_actions, _ = P.shape
    gamma = 0.9
    header = "  " + " " * 14 + "".join(f"{f's{j + 1}':>8s}" for j in range(n))

    # ---------------------------------------------------------------------
    print("=" * 78)
    print(f"演示 1：策略迭代的全过程（从\"一直向左\"出发，γ = {gamma}）")
    print("=" * 78)
    always_left = np.zeros(n, dtype=int)
    pi_star, V_star, history = policy_iteration(P, R, gamma, init_policy=always_left)
    print(header)
    for i, (pol, V) in enumerate(history):
        print(f"  π_{i} = {format_policy(pol)}")
        print_row(f"   V^π_{i}", V)
    print(f"\n  共 {len(history)} 次策略评估后停止（最后一次改进没有改变策略）。")
    print(f"  最优策略 π* = {format_policy(pi_star)}")
    print("\n  观察：改进是\"从右往左\"逐步发生的——第一次改进只把 s6、s7 改成向右，")
    print("        因为只有它们\"看得到\" +10；等它们的价值变高后，s5、s4 才发现向右更好……")

    # ---------------------------------------------------------------------
    print("\n" + "=" * 78)
    print("演示 2：验证策略改进定理——每一步在每个状态都不变差")
    print("=" * 78)
    for i in range(len(history) - 1):
        pol, V = history[i]
        _, V_next = history[i + 1]
        Q = q_from_v(P, R, V, gamma)
        # 定理的证明链：V^π(s) <= max_a Q^π(s,a) <= V^{π'}(s)
        chain_ok = np.all(V <= Q.max(axis=1) + 1e-9) and np.all(Q.max(axis=1) <= V_next + 1e-9)
        # np.round(..., 2) 保留两位小数；再加 0.0 是一个小技巧：把浮点误差产生的 "-0.0" 变成 "0.0"，
        # 否则打印出来会看到 -0.，让人误以为变差了（其实只是 -1e-15 这样的舍入误差）。
        improvement = np.round(V_next - V, 2) + 0.0
        print(f"  π_{i} → π_{i + 1}：每个状态的提升量 = {improvement}")
        print(f"             V^π(s) <= max_a Q^π(s,a) <= V^π'(s) 对所有 s 成立？{chain_ok}")
    print("  → 提升量全部 >= 0：新策略在每一个状态都不比旧策略差（单调改进）。")

    # ---------------------------------------------------------------------
    print("\n" + "=" * 78)
    print("演示 3：暴力枚举全部 2^7 = 128 个策略，验证策略迭代找到的确实是最优")
    print("=" * 78)
    policies, values = brute_force_all_policies(P, R, gamma)
    # 对每个状态，在所有 128 个策略中取价值的最大值
    best_per_state = values.max(axis=0)
    print(header)
    print_row("max_π V^π(s)", best_per_state)
    print_row("V^π*(s)", V_star)
    print(f"\n  两者一致？{np.allclose(best_per_state, V_star)}")
    print("  → 这说明了一件并不显然的事：存在**一个**策略，它在**每一个**状态都同时达到最优。")
    print("    （在每个状态分别挑最好的策略，挑出来的竟然是同一个策略！讲义 6.1 节解释了为什么。）")
    n_optimal = int(np.sum(np.all(np.isclose(values, V_star), axis=1)))
    print(f"  128 个策略中，达到最优价值的策略有 {n_optimal} 个。")
    print(f"  策略迭代只评估了 {len(history)} 个策略，暴力枚举评估了 {len(policies)} 个。")

    # ---------------------------------------------------------------------
    print("\n" + "=" * 78)
    print("演示 4：γ 改变最优策略；初始策略不改变最终结果")
    print("=" * 78)
    for g in [0.3, 0.5, 0.7, 0.9, 0.99]:
        pol, V, hist = policy_iteration(P, R, g)
        print(f"  γ = {g:<5}  π* = {format_policy(pol)}   （{len(hist)} 次评估）")
    print("  → γ 小（短视）时，靠近 s1 的状态宁可去拿近处的 +1；")
    print("    γ 大（有耐心）时，所有状态都愿意长途跋涉去拿 +10。")
    print("    \"最优\"是相对于 γ 而言的：γ 是问题定义的一部分。\n")

    rng = np.random.default_rng(0)
    for trial in range(3):
        init = rng.integers(0, n_actions, size=n)
        pol, V, hist = policy_iteration(P, R, gamma, init_policy=init)
        print(f"  随机初始策略 {format_policy(init)}  →  π* = {format_policy(pol)}，"
              f"{len(hist)} 次评估，V* 相同：{np.allclose(V, V_star)}")

    # ---------------------------------------------------------------------
    print("\n" + "=" * 78)
    print("演示 5：边界情况——平局处理不当，策略迭代会停不下来")
    print("=" * 78)
    # 构造一个有"重复动作"的 MDP：第 3 个动作和 TryRight 完全一样
    # 【Python 小知识】np.concatenate([a, b], axis=1) 沿第 1 维（动作维）拼接两个数组。
    #   P[:, 1:2, :] 用切片 1:2 而不是 1，是为了保留这一维（形状 (7,1,7) 而不是 (7,7)）。
    P3 = np.concatenate([P, P[:, 1:2, :]], axis=1)
    R3 = np.concatenate([R, R[:, 1:2]], axis=1)
    print(f"  新 MDP 有 {P3.shape[1]} 个动作：←、→、→'（→' 与 → 完全相同）")

    pol, V, hist = policy_iteration(P3, R3, gamma)
    print(f"  正确的平局处理（保留旧动作）：{len(hist)} 次评估后停止，π* = {format_policy(pol)}")

    # 错误做法：平局时随机选
    rng = np.random.default_rng(0)
    policy = np.zeros(n, dtype=int)
    for it in range(1, 1001):
        V = evaluate_deterministic_policy(P3, R3, policy, gamma)
        Q = q_from_v(P3, R3, V, gamma)
        new_policy = np.array([rng.choice(np.flatnonzero(Q[s] >= Q[s].max() - 1e-9)) for s in range(n)])
        if np.array_equal(new_policy, policy):
            break
        policy = new_policy
    print(f"  错误的平局处理（随机选）    ：{it} 次评估后才碰巧停下")
    print("  → 价值早就是最优了，但策略在 → 和 →' 之间来回跳，\"策略不变\"的停止条件迟迟满足不了。")
    print("    7 个状态每个都有 2 个等价的最优动作，随机选时策略恰好不变的概率只有 (1/2)^7 = 1/128。")


if __name__ == "__main__":
    main()
