import math
import tempfile
import unittest
from pathlib import Path

from scripts.wall_fit_core import (
    DEFAULT_REFERENCE_POINTS,
    DEFAULT_WALL_FIT,
    fit_wall_config_from_captures,
    load_wall_fit,
    localize_from_scan,
    map_points_to_scan,
    predict_pose_from_odom_delta,
    save_wall_fit_atomic,
    wall_bounds,
)


class WallFitCoreTests(unittest.TestCase):
    def test_reference_points_use_lower_left_map_frame(self):
        self.assertEqual(DEFAULT_REFERENCE_POINTS["P"]["map"], {"x": 0.56, "y": 0.18})
        self.assertEqual(DEFAULT_REFERENCE_POINTS["task_release"]["map"], {"x": 4.5, "y": 1.81})
        self.assertEqual(DEFAULT_REFERENCE_POINTS["passage"]["map"], {"x": 2.5, "y": 2.6})
        self.assertEqual(DEFAULT_REFERENCE_POINTS["clinic"]["map"], {"x": 2.8, "y": 3.77})

    def test_predict_pose_from_odom_delta_advances_wall_pose(self):
        previous_wall_pose = {"x": 1.0, "y": 2.0, "theta": math.radians(170.0)}
        previous_odom_pose = {"x": 0.56, "y": 0.18, "theta": math.radians(175.0)}
        current_odom_pose = {"x": 0.86, "y": 0.08, "theta": math.radians(-175.0)}

        predicted = predict_pose_from_odom_delta(previous_wall_pose, previous_odom_pose, current_odom_pose)

        self.assertAlmostEqual(predicted["x"], 1.3, places=6)
        self.assertAlmostEqual(predicted["y"], 1.9, places=6)
        self.assertAlmostEqual(predicted["theta"], math.radians(-180.0), places=6)
        self.assertEqual(predicted["prediction_source"], "odom_delta")

    def test_wall_bounds_can_extend_outside_map_image(self):
        config = dict(DEFAULT_WALL_FIT)
        config["walls"] = [
            {"name": "left", "a": 1.0, "b": 0.0, "c": 0.08, "color": "#2ca02c"},
            {"name": "right", "a": -1.0, "b": 0.0, "c": 5.12, "color": "#ff7f0e"},
            {"name": "bottom", "a": 0.0, "b": 1.0, "c": 0.05, "color": "#1f77b4"},
            {"name": "top", "a": 0.0, "b": -1.0, "c": 5.2, "color": "#d62728"},
        ]

        bounds = wall_bounds(config)

        self.assertEqual(bounds, {"x_min": -0.08, "x_max": 5.12, "y_min": -0.05, "y_max": 5.2})

    def test_localize_from_scan_corrects_xy_and_theta(self):
        map_points = []
        for y in [0.6, 1.2, 1.8, 2.4, 3.0, 3.6, 4.2]:
            map_points.append({"x": 0.0, "y": y})
            map_points.append({"x": 5.0, "y": y})
        for x in [0.6, 1.2, 1.8, 2.4, 3.0, 3.6, 4.2]:
            map_points.append({"x": x, "y": 0.0})
            map_points.append({"x": x, "y": 5.0})
        true_pose = {"x": 1.6, "y": 1.5, "theta": math.radians(8.0)}
        predicted_pose = {"x": 1.55, "y": 1.45, "theta": 0.0}
        scan = {"points": map_points_to_scan(map_points, true_pose)}

        result = localize_from_scan(scan, DEFAULT_WALL_FIT, predicted_pose)

        self.assertTrue(result["ok"])
        self.assertAlmostEqual(result["pose"]["x"], true_pose["x"], delta=0.08)
        self.assertAlmostEqual(result["pose"]["y"], true_pose["y"], delta=0.08)
        self.assertAlmostEqual(result["pose"]["theta"], true_pose["theta"], delta=math.radians(1.1))

    def test_fit_wall_config_from_captures_uses_outer_points_beyond_map_edges(self):
        captures = []
        true_bounds = {"x_min": -0.08, "x_max": 5.12, "y_min": -0.05, "y_max": 5.2}
        map_points = []
        for y in [0.7, 1.4, 2.1, 2.8, 3.5, 4.2]:
            map_points.append({"x": true_bounds["x_min"], "y": y})
            map_points.append({"x": true_bounds["x_max"], "y": y})
        for x in [0.7, 1.4, 2.1, 2.8, 3.5, 4.2, 4.9]:
            map_points.append({"x": x, "y": true_bounds["y_min"]})
            map_points.append({"x": x, "y": true_bounds["y_max"]})
        for key, reference in DEFAULT_REFERENCE_POINTS.items():
            pose = {"x": reference["map"]["x"], "y": reference["map"]["y"], "theta": 0.0}
            captures.append({"key": key, "map": reference["map"], "odom_pose": pose, "scan": {"points": map_points_to_scan(map_points, pose)}})

        config = fit_wall_config_from_captures(captures, DEFAULT_WALL_FIT)
        bounds = wall_bounds(config)

        self.assertAlmostEqual(bounds["x_min"], true_bounds["x_min"], delta=0.04)
        self.assertAlmostEqual(bounds["x_max"], true_bounds["x_max"], delta=0.04)
        self.assertAlmostEqual(bounds["y_min"], true_bounds["y_min"], delta=0.04)
        self.assertAlmostEqual(bounds["y_max"], true_bounds["y_max"], delta=0.04)

    def test_save_wall_fit_atomic_round_trips_json(self):
        with tempfile.TemporaryDirectory() as temp_dir:
            path = Path(temp_dir) / "wall_fit.json"
            save_wall_fit_atomic(path, DEFAULT_WALL_FIT)

            loaded = load_wall_fit(path)

        self.assertEqual(loaded["coordinate_frame"]["origin"], "map_lower_left")
        self.assertEqual(loaded["reference_points"]["P"]["map"], {"x": 0.56, "y": 0.18})


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