"""P3 选点器验收：保证接收、后验收敛、清除证书阈值、与解析证书衔接。"""

import math
import unittest
from dataclasses import replace
from unittest import TestCase

from mcm26b.belief import CandidateRegion
from mcm26b.config import ProblemConfig
from mcm26b.geometry.localization import Measurement
from mcm26b.geometry.planning import first_bearing_candidate_disk
from mcm26b.planner import (_worst_posterior_radius, bearing_deg,
                            clear_certificate, select_measurement_point,
                            select_second_point)
from mcm26b.protocol.client import SimulatorClient
from mcm26b.harness import LocalSimTransport, LocalSimulator

CONFIG = replace(ProblemConfig(),
                 search=replace(ProblemConfig().search,
                                bearing_rounding_margin_deg=0.005))
FIRST_STATION = (0.0, 0.0)
FIRST_BEARING = 30.0


def _k1_region() -> CandidateRegion:
    region = CandidateRegion.initial(FIRST_STATION, CONFIG)
    region.clip_wedge(FIRST_STATION, FIRST_BEARING,
                      CONFIG.field.bearing_error_half_deg
                      + CONFIG.search.bearing_rounding_margin_deg)
    return region


class SelectionGuaranteeTests(TestCase):
    def test_selected_point_receives_every_sector_candidate(self):
        """安全盘内选点对解析角域 K1 内任意候选源的距离 <= 1000m。

        直接对截断角域 K1 = Ω ∩ W(s1, θ±δ) ∩ B(s1,1500) 稠密采样验证，
        不依赖外包多边形本身。
        """
        region = _k1_region()
        plan = select_measurement_point(region, FIRST_STATION, CONFIG)
        half = 1.005
        for step_phi in range(41):
            phi = math.radians(FIRST_BEARING - half + 2 * half * step_phi / 40)
            for step_t in range(60):
                t = 5.0 + (1500.0 - 5.0) * step_t / 59
                g = (t * math.cos(phi), t * math.sin(phi))
                self.assertLessEqual(
                    math.dist(plan.position, g), 1000.0 + 1e-6,
                    f"候选源 {g} 距选点超过最小接收半径，将浪费一次 no_signal")

    def test_worst_posterior_shrinks_region(self):
        region = _k1_region()
        plan = select_measurement_point(region, FIRST_STATION, CONFIG)
        self.assertLess(plan.worst_posterior_radius_m, region.mec()[1],
                        "最坏后验半径必须优于当前候选集，否则选点无进展")
        self.assertGreater(plan.worst_posterior_radius_m, 0.0)

    def test_posterior_probe_retains_witness(self):
        """后验探针（读数取区间端点）仍包含生成它的边界点：保守性抽查。"""
        region = _k1_region()
        plan = select_measurement_point(region, FIRST_STATION, CONFIG)
        half = CONFIG.field.bearing_error_half_deg \
            + CONFIG.search.bearing_rounding_margin_deg
        witnesses = region.witnesses(300.0)
        self.assertGreater(len(witnesses), 4)
        for g in witnesses:
            phi = bearing_deg(plan.position, g)
            for reading in (phi - half, phi + half):
                probe = CandidateRegion(list(region.polygon), region.slack_m)
                probe.clip_wedge(plan.position, reading, half)
                self.assertTrue(probe.polygon and probe.contains(g, tol_m=0.05),
                                f"后验剪掉了生成读数的边界点 {g}")

    def test_posterior_scores_near_branch(self):
        """后验评分必须包含 near 分支，否则低报最坏后验半径。

        候选集为线段 [-4,4]×{0}、测点为原点时，任意合法读数必为 near
        （源距原点 <=4 < 5m），后验仍为整段线段（半径 4m）；只枚举
        direction 端点会错误报出 2m。
        """
        region = CandidateRegion([(-4.0, 0.0), (4.0, 0.0)])
        half = 1.005
        witnesses = region.witnesses(60.0)
        self.assertEqual(witnesses, [(-4.0, 0.0), (4.0, 0.0)])
        worst = _worst_posterior_radius(region, (0.0, 0.0), witnesses, half,
                                        CONFIG.field.near_radius_m,
                                        CONFIG.search.polygon_sides)
        self.assertGreaterEqual(worst, 4.0 - 1e-6,
                                "near 分支后验半径应逼近候选集半径 4m")
        self.assertLessEqual(worst, 4.0 + region.slack_m + 0.1)

    def test_move_cost_recorded_and_reason_explains(self):
        region = _k1_region()
        plan = select_measurement_point(region, (500.0, 500.0), CONFIG)
        expected = math.dist((500.0, 500.0), plan.position) / CONFIG.cost.speed_mps
        self.assertAlmostEqual(plan.move_cost_s, expected, places=6)
        self.assertIn("安全盘", plan.reason)


class ClearCertificateTests(TestCase):
    def test_near_region_is_certified(self):
        region = CandidateRegion.disk((500.0, 500.0), 5.0, 192)
        cert = clear_certificate(region, 7, CONFIG)
        self.assertTrue(cert.certified)
        self.assertLess(cert.limit_m, CONFIG.field.clear_radius_m)
        self.assertGreater(cert.limit_m, 19.0)

    def test_first_bearing_region_is_not_certified(self):
        cert = clear_certificate(_k1_region(), 7, CONFIG)
        self.assertFalse(cert.certified)
        self.assertGreater(cert.radius_m, cert.limit_m)


class SelectSecondPointTests(TestCase):
    def test_plan_lies_in_analytic_safe_disk_or_is_equivalent(self):
        """P3 入口输出与 geometry 解析证书一致（或提供同等保证）。"""
        simulator = LocalSimulator([], robot_id="T001")
        client = SimulatorClient("T001", transport=LocalSimTransport(simulator))
        client.enter()
        plan = select_second_point(client, FIRST_STATION, FIRST_BEARING, CONFIG)
        # 逐点保证：对解析角域采样，接收距离全部成立（测试见上）；此处只
        # 核对输出位置确实落在解析证书安全盘的合理邻域内（两者中心口径不同，
        # 允许差一个外包裕量）。
        cert = first_bearing_candidate_disk(
            Measurement(FIRST_STATION[0], FIRST_STATION[1], FIRST_BEARING,
                        1.0 + CONFIG.search.bearing_rounding_margin_deg))
        disk = cert["sufficient_candidate_disk"]
        self.assertLessEqual(math.dist(plan.position, tuple(disk["center"])),
                             disk["radius_m"] + 10.0)

    def test_exploration_fallback_when_rho_over_1000(self):
        """ρ>1000 的一般候选集退化为探索 MEC 圆心，并如实标注可能 no_signal。"""
        wide = CandidateRegion.disk((0.0, 0.0), 1200.0, 192)
        plan = select_measurement_point(wide, (0.0, 0.0), CONFIG)
        self.assertAlmostEqual(plan.position[0], 0.0, places=6)
        self.assertAlmostEqual(plan.position[1], 0.0, places=6)
        self.assertIn("探索", plan.reason)


if __name__ == "__main__":
    unittest.main()