"""
Performance Benchmarks for Agent ROS Bridge
Week 2 Deliverable: Performance Benchmarks

Measures:
- Intent parsing latency
- Safety validation latency
- Motion planning latency
"""

import json
import statistics
import sys
import time
from collections.abc import Callable
from dataclasses import asdict, dataclass, field
from datetime import datetime
from pathlib import Path
from typing import Any

# Add parent directories to path
sys.path.insert(0, str(Path(__file__).parent.parent.parent))


@dataclass
class BenchmarkResult:
    """Result of a single benchmark run"""

    name: str
    operation: str
    iterations: int
    total_time: float
    min_time: float
    max_time: float
    avg_time: float
    median_time: float
    p95_time: float
    p99_time: float
    std_dev: float
    timestamp: str = field(default_factory=lambda: datetime.now().isoformat())
    metadata: dict[str, Any] = field(default_factory=dict)


class LatencyBenchmark:
    """Benchmarks for measuring system latencies"""

    def __init__(self, iterations: int = 1000):
        self.iterations = iterations
        self.results: list[BenchmarkResult] = []

    def benchmark_operation(
        self, name: str, operation: Callable, setup: Callable = None, teardown: Callable = None
    ) -> BenchmarkResult:
        """Benchmark a single operation"""

        times = []

        for i in range(self.iterations):
            # Setup
            if setup:
                setup()

            # Measure
            start = time.perf_counter()
            operation()
            end = time.perf_counter()

            times.append((end - start) * 1000)  # Convert to ms

            # Teardown
            if teardown:
                teardown()

        # Calculate statistics
        times_sorted = sorted(times)
        n = len(times_sorted)

        p95_idx = int(n * 0.95)
        p99_idx = int(n * 0.99)

        result = BenchmarkResult(
            name=name,
            operation=operation.__name__,
            iterations=self.iterations,
            total_time=sum(times),
            min_time=min(times),
            max_time=max(times),
            avg_time=statistics.mean(times),
            median_time=statistics.median(times),
            p95_time=times_sorted[p95_idx] if p95_idx < n else times_sorted[-1],
            p99_time=times_sorted[p99_idx] if p99_idx < n else times_sorted[-1],
            std_dev=statistics.stdev(times) if len(times) > 1 else 0.0,
        )

        self.results.append(result)
        return result

    def benchmark_intent_parsing(self) -> BenchmarkResult:
        """Benchmark intent parsing latency"""

        # Sample utterances
        utterances = [
            "go to kitchen",
            "pick up the red block",
            "navigate to position 5 5",
            "stop",
            "move forward 2 meters",
            "turn left",
            "go back to home",
            "what is your status",
            "help",
            "cancel current task",
        ]

        def parse_intent():
            """Simulate intent parsing"""
            # In real implementation, this would call the intent parser
            # For benchmarking, we simulate the processing
            utterance = utterances[int(time.time() * 1000) % len(utterances)]

            # Simulate parsing work
            tokens = utterance.split()
            intent_type = self._classify_intent(tokens)
            entities = self._extract_entities(tokens)

            return {"intent": intent_type, "entities": entities, "confidence": 0.95}

        return self.benchmark_operation("intent_parsing", parse_intent)

    def benchmark_safety_validation(self) -> BenchmarkResult:
        """Benchmark safety validation latency"""

        def validate_safety():
            """Simulate safety validation"""
            # Simulate trajectory validation
            trajectory = self._generate_sample_trajectory()

            # Check velocity limits
            max_vel = max(p.get("velocity", 0) for p in trajectory)
            if max_vel > 1.5:
                return {"approved": False, "reason": "velocity_exceeded"}

            # Check workspace bounds
            for point in trajectory:
                if abs(point.get("x", 0)) > 10 or abs(point.get("y", 0)) > 10:
                    return {"approved": False, "reason": "out_of_bounds"}

            # Generate safety certificate
            certificate = {
                "id": f"cert_{int(time.time()*1000)}",
                "expires": time.time() + 30,
                "constraints_checked": ["velocity", "workspace"],
            }

            return {"approved": True, "certificate": certificate}

        return self.benchmark_operation("safety_validation", validate_safety)

    def benchmark_motion_planning(self) -> BenchmarkResult:
        """Benchmark motion planning latency"""

        def plan_motion():
            """Simulate motion planning"""
            # Generate start and goal
            start = {"x": 0, "y": 0, "yaw": 0}
            goal = {"x": 5, "y": 5, "yaw": 1.57}

            # Simulate path planning (A* or similar)
            path = self._plan_path(start, goal)

            # Convert to trajectory
            trajectory = self._path_to_trajectory(path)

            return {
                "path_length": len(path),
                "trajectory_points": len(trajectory),
                "estimated_duration": len(trajectory) * 0.1,
            }

        return self.benchmark_operation("motion_planning", plan_motion)

    def benchmark_context_resolution(self) -> BenchmarkResult:
        """Benchmark context resolution latency"""

        def resolve_context():
            """Simulate context resolution"""
            # Sample references
            references = ["kitchen", "there", "it", "home", "charging station"]
            reference = references[int(time.time() * 1000) % len(references)]

            # Resolve reference
            if reference == "kitchen":
                return {"type": "location", "coordinates": {"x": 5, "y": 3}}
            elif reference == "there":
                return {"type": "deictic", "requires_pointing": True}
            elif reference == "it":
                return {"type": "anaphoric", "refers_to": "last_mentioned_object"}
            else:
                return {"type": "location", "name": reference}

        return self.benchmark_operation("context_resolution", resolve_context)

    def benchmark_end_to_end(self) -> BenchmarkResult:
        """Benchmark end-to-end pipeline latency"""

        def end_to_end():
            """Simulate full pipeline"""
            # 1. Parse intent
            intent = {"type": "NAVIGATE", "target": "kitchen"}

            # 2. Resolve context
            target = {"x": 5, "y": 3}

            # 3. Plan motion
            path = self._plan_path({"x": 0, "y": 0}, target)
            trajectory = self._path_to_trajectory(path)

            # 4. Validate safety
            valid = all(p.get("velocity", 0) < 1.5 for p in trajectory)

            return {"intent": intent, "target": target, "path_length": len(path), "valid": valid}

        return self.benchmark_operation("end_to_end", end_to_end)

    # Helper methods for simulation
    def _classify_intent(self, tokens: list[str]) -> str:
        """Classify intent from tokens"""
        if "go" in tokens or "navigate" in tokens:
            return "NAVIGATE"
        elif "pick" in tokens or "grasp" in tokens:
            return "MANIPULATE"
        elif "stop" in tokens or "cancel" in tokens:
            return "SAFETY"
        else:
            return "QUERY"

    def _extract_entities(self, tokens: list[str]) -> list[dict]:
        """Extract entities from tokens"""
        entities = []
        locations = ["kitchen", "home", "office", "lab"]
        colors = ["red", "blue", "green", "yellow"]

        for token in tokens:
            if token in locations:
                entities.append({"type": "LOCATION", "value": token})
            elif token in colors:
                entities.append({"type": "COLOR", "value": token})

        return entities

    def _generate_sample_trajectory(self) -> list[dict]:
        """Generate a sample trajectory"""
        import random

        points = []
        for i in range(10):
            points.append({"x": i * 0.5, "y": i * 0.3, "velocity": random.uniform(0.1, 1.0)})
        return points

    def _plan_path(self, start: dict, goal: dict) -> list[dict]:
        """Simulate path planning"""
        import random

        path = [start]

        # Simple interpolation
        steps = 20
        for i in range(1, steps):
            t = i / steps
            path.append(
                {
                    "x": start["x"] + (goal["x"] - start["x"]) * t + random.uniform(-0.1, 0.1),
                    "y": start["y"] + (goal["y"] - start["y"]) * t + random.uniform(-0.1, 0.1),
                }
            )

        path.append(goal)
        return path

    def _path_to_trajectory(self, path: list[dict]) -> list[dict]:
        """Convert path to trajectory with velocities"""
        trajectory = []
        for i, point in enumerate(path):
            trajectory.append(
                {
                    "x": point["x"],
                    "y": point["y"],
                    "velocity": 0.5 + (i % 3) * 0.2,  # Varying velocity
                }
            )
        return trajectory

    def run_all_benchmarks(self) -> list[BenchmarkResult]:
        """Run all benchmarks"""
        print(f"Running benchmarks with {self.iterations} iterations each...\n")

        benchmarks = [
            ("Intent Parsing", self.benchmark_intent_parsing),
            ("Context Resolution", self.benchmark_context_resolution),
            ("Safety Validation", self.benchmark_safety_validation),
            ("Motion Planning", self.benchmark_motion_planning),
            ("End-to-End Pipeline", self.benchmark_end_to_end),
        ]

        for name, benchmark_func in benchmarks:
            print(f"Benchmarking: {name}...")
            result = benchmark_func()
            self._print_result(result)
            print()

        return self.results

    def _print_result(self, result: BenchmarkResult):
        """Print benchmark result"""
        print(f"  Iterations: {result.iterations}")
        print(f"  Total time: {result.total_time:.2f} ms")
        print(f"  Average:    {result.avg_time:.3f} ms")
        print(f"  Median:     {result.median_time:.3f} ms")
        print(f"  Min:        {result.min_time:.3f} ms")
        print(f"  Max:        {result.max_time:.3f} ms")
        print(f"  P95:        {result.p95_time:.3f} ms")
        print(f"  P99:        {result.p99_time:.3f} ms")
        print(f"  Std Dev:    {result.std_dev:.3f} ms")

    def generate_report(self) -> dict[str, Any]:
        """Generate benchmark report"""
        return {
            "timestamp": datetime.now().isoformat(),
            "iterations_per_benchmark": self.iterations,
            "benchmarks": [asdict(r) for r in self.results],
            "summary": {
                "total_benchmarks": len(self.results),
                "avg_intent_parsing_ms": next(
                    (r.avg_time for r in self.results if r.name == "intent_parsing"), 0
                ),
                "avg_safety_validation_ms": next(
                    (r.avg_time for r in self.results if r.name == "safety_validation"), 0
                ),
                "avg_motion_planning_ms": next(
                    (r.avg_time for r in self.results if r.name == "motion_planning"), 0
                ),
                "avg_end_to_end_ms": next(
                    (r.avg_time for r in self.results if r.name == "end_to_end"), 0
                ),
            },
        }

    def save_report(self, output_dir: str = "results") -> Path:
        """Save benchmark report to file"""
        output_path = Path(output_dir)
        output_path.mkdir(exist_ok=True)

        report_file = output_path / f"benchmark_report_{int(time.time())}.json"

        report = self.generate_report()

        with open(report_file, "w") as f:
            json.dump(report, f, indent=2)

        print(f"\nBenchmark report saved to: {report_file}")
        return report_file

    def generate_text_report(self) -> str:
        """Generate human-readable text report"""
        lines = [
            "=" * 70,
            "PERFORMANCE BENCHMARK REPORT",
            "=" * 70,
            f"Generated: {datetime.now().isoformat()}",
            f"Iterations per benchmark: {self.iterations}",
            "",
            "LATENCY REQUIREMENTS",
            "-" * 70,
            "Intent Parsing:       < 10 ms (target)",
            "Context Resolution:   < 5 ms (target)",
            "Safety Validation:    < 10 ms (target)",
            "Motion Planning:      < 50 ms (target)",
            "End-to-End:           < 100 ms (target)",
            "",
            "BENCHMARK RESULTS",
            "-" * 70,
        ]

        for result in self.results:
            status = "✓" if result.avg_time < self._get_target(result.name) else "✗"
            target = self._get_target(result.name)

            lines.extend(
                [
                    "",
                    f"{status} {result.name.upper().replace('_', ' ')}",
                    f"  Average:  {result.avg_time:>8.3f} ms (target: {target} ms)",
                    f"  Median:   {result.median_time:>8.3f} ms",
                    f"  P95:      {result.p95_time:>8.3f} ms",
                    f"  P99:      {result.p99_time:>8.3f} ms",
                    f"  Min/Max:  {result.min_time:>8.3f} / {result.max_time:.3f} ms",
                ]
            )

        lines.extend(["", "=" * 70, "END OF REPORT", "=" * 70])

        return "\n".join(lines)

    def _get_target(self, benchmark_name: str) -> float:
        """Get target latency for benchmark"""
        targets = {
            "intent_parsing": 10.0,
            "context_resolution": 5.0,
            "safety_validation": 10.0,
            "motion_planning": 50.0,
            "end_to_end": 100.0,
        }
        return targets.get(benchmark_name, 100.0)


def main():
    """Main entry point for benchmarks"""
    import argparse

    parser = argparse.ArgumentParser(description="Run performance benchmarks")
    parser.add_argument(
        "--iterations", type=int, default=1000, help="Number of iterations per benchmark"
    )
    parser.add_argument("--output", default="results", help="Output directory for reports")
    parser.add_argument(
        "--benchmark",
        choices=["intent", "context", "safety", "planning", "e2e", "all"],
        default="all",
        help="Specific benchmark to run",
    )

    args = parser.parse_args()

    benchmark = LatencyBenchmark(iterations=args.iterations)

    if args.benchmark == "all":
        benchmark.run_all_benchmarks()
    elif args.benchmark == "intent":
        result = benchmark.benchmark_intent_parsing()
        benchmark._print_result(result)
    elif args.benchmark == "context":
        result = benchmark.benchmark_context_resolution()
        benchmark._print_result(result)
    elif args.benchmark == "safety":
        result = benchmark.benchmark_safety_validation()
        benchmark._print_result(result)
    elif args.benchmark == "planning":
        result = benchmark.benchmark_motion_planning()
        benchmark._print_result(result)
    elif args.benchmark == "e2e":
        result = benchmark.benchmark_end_to_end()
        benchmark._print_result(result)

    # Save report
    benchmark.save_report(args.output)

    # Print text report
    print("\n" + benchmark.generate_text_report())


if __name__ == "__main__":
    main()
