from abc import abstractmethod

from ..pyrcf_component import PyRCFComponent
from ...core.types import GlobalMotionPlan, RobotState, LocalMotionPlan


class LocalPlannerBase(PyRCFComponent):
    """Base class defining the interface for all local planners that can be used in the control
    loop."""

    @abstractmethod
    def generate_local_plan(
        self,
        robot_state: RobotState,
        global_plan: GlobalMotionPlan,
        t: float,
        dt: float,
    ) -> LocalMotionPlan:
        """The main local plan generation method to be called in the loop.

        This method will be called after the global plan is generated in the loop.

        This method should be implemented by any local planner implementation.

        Args:
            robot_state (RobotState): The current state information from the robot.
            global_plan (GlobalMotionPlan): the latest global plan generated by the global planner
                used in the loop.
            t (float): the current time signature of the control loop. Defaults to None
                (controllers may or may not need this).
            dt (float, optional): the time since the last control loop. Defaults to None
                (controllers may or may not need this).

        Raises:
            NotImplementedError: Raised if this method is not implemented by the child class.

        Returns:
            LocalMotionPlan: The output local plan to be used by the controller in this iteration.
        """
        raise NotImplementedError("This method has to be implemented in the child class")


class DummyLocalPlanner(LocalPlannerBase):
    """Dummy local planner for testing pipeline."""

    def __init__(self, squawk: bool = True):
        """Dummy local planner for testing pipeline.

        Args:
            squawk (bool, optional): Verbose. Defaults to True.
        """
        self._squawk = squawk

    def generate_local_plan(
        self,
        robot_state: RobotState,
        global_plan: GlobalMotionPlan,
        t: float,
        dt: float,
    ) -> LocalMotionPlan:
        """This is a dummy method for sanity checking the control loop."""
        if self._squawk:
            print(f"{__class__.__name__}: Generating dummy local plan. time: {t}, dt: {dt}")
        return LocalMotionPlan()
