"""
=============================================================================
第 2 讲 · 代码 4：价值迭代（Value Iteration）与压缩映射
=============================================================================

对应讲义：lectures/lecture02_mdp_planning.md 第 8、9 节

-----------------------------------------------------------------------------
【这个文件要解决什么问题？】
-----------------------------------------------------------------------------
策略迭代每一轮都要**完整地**评估一个策略（解线性方程组或迭代到收敛），开销不小。
价值迭代提出一个问题：**能不能不维护显式的策略，直接逼近最优价值 V*？**

它只做一件事，反复做：
    V_{k+1}(s) = max_a [ R(s,a) + γ Σ_{s'} P(s'|s,a) V_k(s') ]
这一步叫"贝尔曼最优备份"。等 V 收敛后，再从 V 中"读出"贪心策略。

-----------------------------------------------------------------------------
【两种理解价值迭代的角度（讲义 8.1 节）】
-----------------------------------------------------------------------------
    角度 1（有限视界）：V_k 就是"只剩 k 步时的最优价值"。每迭代一次，多看一步。
    角度 2（截断的策略迭代）：策略评估只做一步备份就马上改进。

-----------------------------------------------------------------------------
【本文件的演示】
-----------------------------------------------------------------------------
    1. 价值迭代的逐步过程，以及"策略比价值收敛得早得多"
    2. 亲眼验证压缩性质：||BV - BV'||∞ <= γ ||V - V'||∞
    3. ε-最优停止准则：什么时候可以放心停下来？
    4. 有限视界：最优策略会随"剩余步数"而改变（非平稳策略）
    5. 价值迭代 vs 策略迭代：结果相同，代价不同
    6. 边界情况

-----------------------------------------------------------------------------
【运行方式】
-----------------------------------------------------------------------------
    python 04_value_iteration.py

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

import numpy as np

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

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


# =============================================================================
# 第 0 部分：基础函数（精简版，详细注释见 01–03 文件）
# =============================================================================
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_π。gamma 必须在 [0, 1)。"""
    n = P.shape[0]
    idx = np.arange(n)
    return np.linalg.solve(np.eye(n) - gamma * P[idx, policy], R[idx, policy])


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):
    return " ".join(ARROWS[a] for a in policy)


def greedy_from_q(Q, tol=1e-9):
    """
    从 Q 中读出贪心策略：每个状态选 Q 最大的动作；平局时选编号最小的。

    【为什么要 tol？】浮点误差可能让两个理论上相等的 Q 值相差 1e-15，
    用 tol 把"几乎相等"视为平局，结果更稳定、可复现。
    """
    return np.array([int(np.flatnonzero(q >= q.max() - tol)[0]) for q in Q])


# =============================================================================
# 第 1 部分：贝尔曼最优算子
# =============================================================================
def bellman_optimality_backup(P, R, V, gamma):
    """
    对所有状态同时做一次贝尔曼最优备份：(BV)(s) = max_a [ R(s,a) + γ Σ_{s'} P(s'|s,a) V(s') ]

    【和贝尔曼期望备份（02 文件）的唯一区别】
        期望备份：对动作按策略**取平均**   Σ_a π(a|s) [...]
        最优备份：对动作**取最大**         max_a [...]
        一个是"评估给定策略"，一个是"假设每一步都选最好的动作"。

    【参数】
        V : 长度 N 的数组，当前价值估计（可以是任意数组，不必是某个策略的价值）。

    【返回】
        (V_new, Q)：
        - V_new : 长度 N，备份后的价值 BV
        - Q     : 形状 (N, A)，中间结果 Q(s,a) = R + γ P V，顺便返回方便读出策略

    【Python 小知识】Q.max(axis=1) 对每一行（每个状态）取最大值，结果长度 N。
    """
    Q = q_from_v(P, R, V, gamma)
    return Q.max(axis=1), Q


# =============================================================================
# 第 2 部分：价值迭代
# =============================================================================
def value_iteration(P, R, gamma, epsilon=1e-6, max_iters=100_000, V_init=None):
    """
    价值迭代：反复做贝尔曼最优备份，直到满足 ε-最优停止准则，然后读出贪心策略。

    【伪代码】（讲义 8.2 节）
        V_0 ← 0
        repeat:
            V_{k+1}(s) ← max_a [ R(s,a) + γ Σ_{s'} P(s'|s,a) V_k(s') ]    对所有 s
        until ||V_{k+1} - V_k||∞ < ε (1-γ) / (2γ)
        π(s) ← argmax_a [ R(s,a) + γ Σ_{s'} P(s'|s,a) V_{k+1}(s') ]

    【停止准则从哪来？】（讲义 8.5 节）
        可以证明：若相邻两次迭代的变化 < ε(1-γ)/(2γ)，则
            (1) 当前 V 与真正的 V* 相差不超过 ε/2；
            (2) 由当前 V 读出的贪心策略是 **ε-最优**的：它的真实价值与 V* 相差不超过 ε。
        直觉：γ 越接近 1，"小的变化"越不能说明"离终点近"（因为剩下的误差还会累积很久），
        所以要求的变化阈值 ε(1-γ)/(2γ) 越严格。

    【参数】
        P, R      : MDP。
        gamma     : float，(0, 1)。γ = 0 时一步就收敛（直接特殊处理）；γ = 1 见边界。
        epsilon   : float，> 0。希望返回的策略离最优最多差多少。
        max_iters : int，最大迭代次数。
        V_init    : 长度 N 的数组或 None（默认全 0）。
                    由于压缩性质，**从任何初始值出发都会收敛到同一个 V***，只是步数不同。

    【返回】
        dict，包含：
        - "V"         : 最后的价值估计
        - "policy"    : 贪心策略
        - "n_iters"   : 迭代次数
        - "deltas"    : 每次迭代的 ||V_{k+1} - V_k||∞
        - "history"   : 每次迭代后的 V（列表），用于观察过程
        - "converged" : 是否在 max_iters 内满足停止准则

    【边界】
        - γ = 1：停止阈值为 0，永远无法满足；对没有终止状态的 MDP 价值也会无限增长。
          我们直接拒绝 γ >= 1。（有终止状态时 γ = 1 可以用，见 05_gridworld.py 的讨论。）
        - γ = 0：V* = max_a R(s,a)，一步到位。
    """
    if not (0.0 <= gamma < 1.0):
        raise ValueError(f"gamma 必须在 [0, 1) 内，但收到了 {gamma}")
    if epsilon <= 0:
        raise ValueError(f"epsilon 必须 > 0，但收到了 {epsilon}")

    n = P.shape[0]
    V = np.zeros(n) if V_init is None else np.array(V_init, dtype=float)
    threshold = epsilon * (1 - gamma) / (2 * gamma) if gamma > 0 else np.inf
    deltas, history = [], [V.copy()]
    converged = False

    for k in range(1, max_iters + 1):
        V_new, _ = bellman_optimality_backup(P, R, V, gamma)
        delta = float(np.abs(V_new - V).max())
        deltas.append(delta)
        V = V_new
        history.append(V.copy())
        if delta < threshold:
            converged = True
            break

    policy = greedy_from_q(q_from_v(P, R, V, gamma))
    return {"V": V, "policy": policy, "n_iters": k, "deltas": deltas,
            "history": history, "converged": converged}


# =============================================================================
# 第 3 部分：有限视界价值迭代
# =============================================================================
def finite_horizon_value_iteration(P, R, horizon, gamma=1.0):
    """
    有限视界 MDP：一共只有 horizon 步。计算"剩余 k 步时"的最优价值和最优动作，k = 1..horizon。

    【思路】（讲义 8.6 节）
        剩余 0 步：什么都拿不到，V_0 = 0。
        剩余 k 步：先走一步（拿即时奖励），然后还剩 k-1 步，而剩 k-1 步的最优价值已经算好了：
            V_k(s)  = max_a [ R(s,a) + γ Σ_{s'} P(s'|s,a) V_{k-1}(s') ]
            π_k(s)  = argmax_a [ ... ]
        这和无限视界的价值迭代**公式一模一样**！区别在于解读：
        这里的每一个 V_k 和 π_k 都有精确的含义，而且 π_k 会随 k 变化——
        "还剩很多步"和"只剩最后一步"时，最优动作可能不同。这叫**非平稳策略**。

    【参数】
        horizon : int，>= 1。
        gamma   : float，[0, 1]。**有限视界时 γ = 1 完全没问题**（有限项求和不会发散）。

    【返回】
        (V_list, policy_list)：
        - V_list[k]      : 剩余 k 步时的最优价值，k = 0..horizon（V_list[0] 全为 0）
        - policy_list[k] : 剩余 k 步时的最优动作，k = 1..horizon（policy_list[0] 为 None，占位）
    """
    if horizon < 1:
        raise ValueError(f"horizon 必须 >= 1，但收到了 {horizon}")
    if not (0.0 <= gamma <= 1.0):
        raise ValueError(f"gamma 必须在 [0, 1] 内，但收到了 {gamma}")
    n = P.shape[0]
    V_list = [np.zeros(n)]
    policy_list = [None]
    for k in range(1, horizon + 1):
        V_new, Q = bellman_optimality_backup(P, R, V_list[-1], gamma)
        V_list.append(V_new)
        policy_list.append(greedy_from_q(Q))
    return V_list, policy_list


# =============================================================================
# 第 4 部分：演示
# =============================================================================
def row_string(label, values):
    """把一行价值格式化成字符串：标签占 10 列，每个数占 8 列、保留 2 位小数。"""
    return f"  {label:<10s}" + "".join(f"{v:8.2f}" for v in values)


def print_row(label, values):
    print(row_string(label, values))


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

    # ---------------------------------------------------------------------
    print("=" * 78)
    print(f"演示 1：价值迭代的逐步过程（γ = {gamma}）")
    print("=" * 78)
    result = value_iteration(P, R, gamma, epsilon=1e-6)
    print(header + "    贪心策略")
    for k in [0, 1, 2, 3, 4, 5, 6, 7, 10, 30]:
        V_k = result["history"][k]
        pol_k = greedy_from_q(q_from_v(P, R, V_k, gamma))
        print(row_string(f"V_{k}", V_k) + "    " + format_policy(pol_k))
    print_row("V*", result["V"])
    print(f"\n  共迭代 {result['n_iters']} 次满足 ε=1e-6 的停止准则；最终策略 {format_policy(result['policy'])}")

    # 找出贪心策略"最后一次改变"是在第几次迭代
    last_change = 0
    prev = None
    for k, V_k in enumerate(result["history"]):
        pol_k = greedy_from_q(q_from_v(P, R, V_k, gamma))
        if prev is not None and not np.array_equal(pol_k, prev):
            last_change = k
        prev = pol_k
    print(f"\n  观察 1：贪心策略在第 {last_change} 次迭代后就再也没变过，但价值还要再迭代 "
          f"{result['n_iters'] - last_change} 次才收敛。")
    print("          → 策略比价值收敛得早得多。我们真正需要的是策略，所以实践中往往不必等价值完全收敛。")
    print("  观察 2：V_k 的含义是\"只剩 k 步时的最优价值\"。V_1 = R；V_2 时 s6 才知道右边有 +10……")
    print("          V_k 的贪心策略在早期（k 小）时，左边的状态向左走——因为 k 步之内根本走不到 s7。")

    # ---------------------------------------------------------------------
    print("\n" + "=" * 78)
    print("演示 2：亲眼验证压缩性质 ||BV - BV'||∞ <= γ ||V - V'||∞")
    print("=" * 78)
    rng = np.random.default_rng(0)
    print("  随机生成 5 对价值函数 V、V'，比较备份前后的距离：")
    for _ in range(5):
        # rng.normal(0, 50, size=n)：生成 n 个均值 0、标准差 50 的随机数
        V1 = rng.normal(0, 50, size=n)
        V2 = rng.normal(0, 50, size=n)
        BV1, _ = bellman_optimality_backup(P, R, V1, gamma)
        BV2, _ = bellman_optimality_backup(P, R, V2, gamma)
        before = np.abs(V1 - V2).max()
        after = np.abs(BV1 - BV2).max()
        print(f"    备份前距离 {before:7.2f}   备份后距离 {after:7.2f}   比值 {after / before:.3f} <= γ = {gamma}")
    print("  → 无论 V、V' 是什么，一次备份后它们的距离至少缩小到 γ 倍。")
    print("    所以从任何初始值出发，反复备份后都会被\"挤\"到同一个点——唯一的不动点 V*。")

    V_far = value_iteration(P, R, gamma, V_init=rng.normal(0, 1000, size=n))
    print(f"\n  从一个离谱的随机初始值（标准差 1000）出发：迭代 {V_far['n_iters']} 次，"
          f"与从 0 出发的 V* 最大差 {np.abs(V_far['V'] - result['V']).max():.1e}")

    # ---------------------------------------------------------------------
    print("\n" + "=" * 78)
    print("演示 3：ε-最优停止准则——停下来的时候，策略离最优有多远？")
    print("=" * 78)
    pi_star_V = evaluate_deterministic_policy(P, R, result["policy"], gamma)
    for eps in [10.0, 1.0, 0.1, 1e-3]:
        r = value_iteration(P, R, gamma, epsilon=eps)
        V_pol = evaluate_deterministic_policy(P, R, r["policy"], gamma)
        gap = np.abs(pi_star_V - V_pol).max()
        err = np.abs(pi_star_V - r["V"]).max()
        print(f"  ε = {eps:<6}  迭代 {r['n_iters']:3d} 次   ||V - V*|| = {err:7.4f} (保证 <= ε/2)"
              f"   策略价值差距 = {gap:.4f} (保证 <= ε)")
    print("  → 实际误差远小于理论保证：这个界是\"最坏情况\"下的，通常很保守。")

    # ---------------------------------------------------------------------
    print("\n" + "=" * 78)
    print("演示 4：有限视界——最优动作随\"剩余步数\"而改变（γ = 1，最多 10 步）")
    print("=" * 78)
    V_list, pol_list = finite_horizon_value_iteration(P, R, horizon=10, gamma=1.0)
    print("  剩余步数   各状态的最优动作     各状态的最优价值（s1 ... s7）")
    for k in range(1, 11):
        values = " ".join(f"{v:6.2f}" for v in V_list[k])
        print(f"    k = {k:2d}    {format_policy(pol_list[k])}    {values}")
    print("  → 只剩几步时，靠近 s1 的状态选择向左（反正走不到 s7，不如去拿 +1）；")
    print("    剩余步数足够多时，越来越多的状态选择长途跋涉去 s7。")
    print("    同一个状态、不同的剩余步数，最优动作不同——有限视界的最优策略是\"非平稳\"的。")
    print("    （k=1 时所有动作都一样好，因为奖励只取决于当前格子，平局时取编号最小的 ←。）")

    # ---------------------------------------------------------------------
    print("\n" + "=" * 78)
    print("演示 5：价值迭代 vs 策略迭代（结果相同，代价不同）")
    print("=" * 78)
    # 简单实现一个策略迭代用于对比（完整注释见 03 文件）
    policy = np.zeros(n, dtype=int)
    n_pi_iters = 0
    while True:
        n_pi_iters += 1
        V_pi = evaluate_deterministic_policy(P, R, policy, gamma)
        Q = q_from_v(P, R, V_pi, gamma)
        new_policy = np.array([policy[s] if Q[s, policy[s]] >= Q[s].max() - 1e-9 else int(Q[s].argmax())
                               for s in range(n)])
        if np.array_equal(new_policy, policy):
            break
        policy = new_policy
    print(f"  策略迭代：{n_pi_iters:3d} 次迭代，每次要解一个 {n}×{n} 线性方程组（O(N³)）")
    print(f"  价值迭代：{result['n_iters']:3d} 次迭代，每次只做一次备份（O(N²A)）")
    print(f"  两者的最优策略相同？{np.array_equal(policy, result['policy'])}；"
          f"价值最大差 {np.abs(V_pi - result['V']).max():.1e}")
    print("  → 策略迭代：迭代次数少，但每次贵；价值迭代：每次便宜，但次数多。")

    # ---------------------------------------------------------------------
    print("\n" + "=" * 78)
    print("演示 6：边界情况")
    print("=" * 78)
    try:
        value_iteration(P, R, gamma=1.0)
    except ValueError as e:
        print(f"  [无限视界 γ=1] ValueError: {e}")
    r0 = value_iteration(P, R, gamma=0.0)
    print(f"  [γ=0]          迭代 {r0['n_iters']} 次，V* = {r0['V']}（= 各状态最大即时奖励）")
    for g in [0.5, 0.9, 0.99]:
        r = value_iteration(P, R, g, epsilon=1e-6)
        print(f"  [γ={g:<4}]       迭代 {r['n_iters']:5d} 次收敛（γ 越接近 1 越慢，约 ∝ 1/(1-γ)）")


if __name__ == "__main__":
    main()
