import atexit
import time
from math import floor, pi

from pitop.core.mixins import Recreatable, Stateful

from .encoder_motor_controller import EncoderMotorController
from .parameters import BrakingType, Direction, ForwardDirection


class EncoderMotor(Stateful, Recreatable):
    """Represents a pi-top motor encoder component.

    Note that pi-top motor encoders use a built-in closed-loop control system, that feeds the readings
    from an encoder sensor to an PID controller. This controller will actively modify the motor's current to move at the desired
    speed or position, even if a load is applied to the shaft.

    This internal controller is used when moving the motor through :class:`set_target_rpm` or :class:`set_target_speed`
    methods, while using the :class:`set_power` method will make the motor work in open-loop, not using the controller.

    .. note::
        Note that some methods allow to use distance and speed settings in meters and meters per second. These will only make
        sense when using a wheel attached to the shaft of the motor.

        The conversions between angle, rotations and RPM used by the motor to meters and meters/second are performed considering
        the :data:`wheel_diameter` parameter. This parameter defaults to the diameter of the wheel included with MMK.
        If a wheel of different dimmensions is attached to the motor, you'll need to measure it's diameter, in order for these
        methods to work properly.

    :type port_name: str
    :param port_name:
        The ID for the port to which this component is connected.

    :type forward_direction: ForwardDirection
    :param forward_direction:
        The type of rotation of the motor shaft that corresponds to forward motion.

    :type braking_type: BrakingType
    :param braking_type:
        The braking type of the motor. Defaults to coast.

    :type wheel_diameter: int or float
    :param wheel_diameter:
        The diameter of the wheel attached to the motor.

    :type name: str
    :param name:
        Component name, defaults to `encoder_motor`. Used to access this component when added to a :class:`pitop.Pitop` object.
    """

    MMK_STANDARD_GEAR_RATIO = 41.8
    MAX_DC_MOTOR_RPM = 4800

    def __init__(
        self,
        port_name,
        forward_direction,
        braking_type=BrakingType.BRAKE,
        wheel_diameter=0.070,
        name="encoder_motor",
    ):
        self.name = name
        self._pma_port = port_name

        if isinstance(braking_type, BrakingType):
            braking_type = braking_type.value

        self.__motor_core = EncoderMotorController(self._pma_port, braking_type)
        self.__forward_direction = forward_direction
        self.__wheel_diameter = wheel_diameter

        self.__prev_time_dist_cnt = time.time()
        self.__previous_reading_odometer = 0

        atexit.register(self.stop)

        Stateful.__init__(self)
        Recreatable.__init__(
            self,
            config_dict={
                "port_name": port_name,
                "name": name,
                "forward_direction": lambda: self.forward_direction,
                "braking_type": lambda: self.braking_type,
                "wheel_diameter": lambda: self.wheel_diameter,
            },
        )

    @property
    def own_state(self):
        return {
            "current_rpm": lambda: self.current_rpm,
            "current_speed": lambda: self.current_speed,
            "distance": lambda: self.distance,
            "wheel_diameter": lambda: self.wheel_diameter,
            "forward_direction": lambda: self.forward_direction,
            "braking_type": lambda: self.braking_type,
        }

    @property
    def forward_direction(self):
        """Represents the forward direction setting used by the motor.

        Setting this property will determine on which direction the
        motor will turn whenever a movement in a particular direction is
        requested.

        :type forward_direction: ForwardDirection
        :param forward_direction: The direction that corresponds to
            forward motion.
        """
        return ForwardDirection(self.__forward_direction)

    @forward_direction.setter
    def forward_direction(self, forward_direction):
        self.__forward_direction = forward_direction

    @property
    def braking_type(self):
        """Returns the type of braking used by the motor when it's stopping
        after a movement.

        Setting this property will change the way the motor stops a movement:

        - :data:`BrakingType.COAST` will make the motor coast to a halt when stopped.
        - :data:`BrakingType.BRAKE` will cause the motor to actively brake when stopped.

        :type braking_type: BrakingType
        :param braking_type:
            The braking type of the motor.
        """
        return BrakingType(self.__motor_core.braking_type())

    @braking_type.setter
    def braking_type(self, braking_type):
        self.__motor_core.set_braking_type(braking_type.value)

    def set_power(self, power, direction=Direction.FORWARD):
        """Turn the motor on at the power level provided, in the range -1.0 to.

        +1.0, where:

        - 1.0: motor will turn with full power in the :data:`direction` provided as argument.
        - 0.0: motor will not move.
        - -1.0: motor will turn with full power in the direction contrary to :data:`direction`.

        .. warning::
            Setting a :data:`.power` value out of range will cause the method to raise an
            exception.

        :type power: int or float
        :param power:
            Motor power, in the range -1.0 to +1.0

        :type direction: Direction
        :param direction:
            Direction to rotate the motor
        """

        if not (-1.0 <= power <= 1.0):
            raise ValueError("Power value must be between -1.0 and +1.0 (inclusive)")

        power_mapping = int(round(power * 1000) * self.__forward_direction * direction)

        self.__motor_core.set_power(power_mapping)

    def power(self):
        """Get the current power of the motor.

        Returns a value from -1.0 to +1.0, assuming the user is
        controlling the motor using the :class:`set_power` method (motor
        is in control mode 0). If this is not the case, returns None.
        """

        power = self.__motor_core.power()
        if power:
            return power / 1000.0
        return None

    def set_target_rpm(
        self, target_rpm, direction=Direction.FORWARD, total_rotations=0.0
    ):
        """Run the motor at the specified :data:`.target_rpm` RPM.

        If desired, a number of full or partial rotations can also be set through the :data:`total_rotations`
        parameter. Once reached, the motor will stop. Setting :data:`total_rotations` to 0 will set the
        motor to run indefinitely until stopped.

        If the desired RPM setting cannot be achieved, :class:`torque_limited` will be set to :data:`True`
        and the motor will run at the maximum possible RPM it is capable of for the instantaneous torque.
        This means that if the torque lowers, then the RPM will continue to rise until it meets the
        desired level.

        Care needs to be taken here if you want to drive a vehicle forward in a straight line, as the motors
        are not guaranteed to spin at the same rate if they are torque-limited.

        .. warning::
            Setting a :data:`.target_rpm` higher than the maximum allowed will cause the
            method to throw an exception. To determine what the maximum possible target RPM for the motor
            is, use the :class:`max_rpm` method.

        :type target_rpm: int or float
        :param target_rpm:
            Desired RPM of output shaft

        :type direction: Direction
        :param direction:
            Direction to rotate the motor. Defaults to forward.

        :type total_rotations: int or float
        :param total_rotations:
            Total number of rotations to be execute. Set to 0 to run indefinitely.
        """

        if not (-self.max_rpm <= abs(target_rpm) <= self.max_rpm):
            raise ValueError(
                f"Target RPM value must be between {-self.max_rpm} and {self.max_rpm} (inclusive)"
            )

        dc_motor_rpm = int(
            round(target_rpm * self.MMK_STANDARD_GEAR_RATIO)
            * self.__forward_direction
            * direction
        )
        dc_motor_rotations = int(
            round(total_rotations * self.MMK_STANDARD_GEAR_RATIO)
            * self.__forward_direction
            * direction
        )

        if not (-self.MAX_DC_MOTOR_RPM <= dc_motor_rpm <= self.MAX_DC_MOTOR_RPM):
            raise ValueError(
                f"DC motor RPM value must be between {-self.MAX_DC_MOTOR_RPM} and {self.MAX_DC_MOTOR_RPM} (inclusive)"
            )

        if dc_motor_rotations == 0:
            self.__motor_core.set_rpm_control(dc_motor_rpm)
        else:
            dc_motor_rotation_counter = self.__motor_core.odometer()
            rotations_offset_to_send = int(
                dc_motor_rotations + dc_motor_rotation_counter
            )
            self.__motor_core.set_rpm_with_rotations(
                dc_motor_rpm, rotations_offset_to_send
            )

    def target_rpm(self):
        """Get the desired RPM of the motor output shaft, assuming the user is
        controlling the motor using :class:`set_target_rpm` (motor is in
        control mode 1).

        If this is not the case, returns None.
        """

        rpm = self.__motor_core.rpm_control()
        if rpm:
            return rpm / self.MMK_STANDARD_GEAR_RATIO * self.__forward_direction
        return None

    def stop(self):
        """Stop the motor in all circumstances."""
        self.__motor_core.stop()

    @property
    def current_rpm(self):
        """Returns the actual RPM currently being achieved at the output shaft,
        measured by the encoder sensor.

        This value might differ from the target RPM set through
        :class:`set_target_rpm`.
        """

        dc_motor_rpm_actual = self.__motor_core.tachometer() * self.__forward_direction
        output_shaft_rpm_actual = dc_motor_rpm_actual / self.MMK_STANDARD_GEAR_RATIO

        return output_shaft_rpm_actual

    @property
    def rotation_counter(self):
        """Returns the total or partial number of rotations performed by the
        motor shaft.

        Rotations will increment when moving forward, and decrement when
        moving backward. This value is a float with many decimal points
        of accuracy, so can be used to monitor even very small turns of
        the output shaft.
        """

        dc_motor_rotation_counter = (
            self.__motor_core.odometer() * self.__forward_direction
        )
        output_shaft_rotation_counter = round(
            dc_motor_rotation_counter / self.MMK_STANDARD_GEAR_RATIO, 1
        )

        return output_shaft_rotation_counter

    @property
    def torque_limited(self):
        """Check if the actual motor speed or RPM does not match the target
        speed or RPM.

        Returns a boolean value, :data:`True` if the motor is torque-
        limited and :data:`False` if it is not.
        """

        return False

    @property
    def max_rpm(self):
        """Returns the approximate maximum RPM capable given the motor and gear
        ratio."""

        return floor(self.MAX_DC_MOTOR_RPM / self.MMK_STANDARD_GEAR_RATIO)

    @property
    def wheel_diameter(self):
        """Represents the diameter of the wheel attached to the motor in
        meters.

        This parameter is important if using library functions to measure speed or distance, as these rely on
        knowing the diameter of the wheel in order to function correctly.
        Use the predefined pi-top wheel measurement, or define your own wheel size.

        .. note::
            The wheel diameter changes slightly when in contact with a surface due to rubber compression.

            For this application, the key measurement is the diameter when the wheel rests on a flat surface.
            Based on this, the default wheel diameter used is 0.070m.

        :type wheel_diameter: int or float
        :param wheel_diameter:
            Wheel diameter in meters.
        """
        return self.__wheel_diameter

    @wheel_diameter.setter
    def wheel_diameter(self, wheel_diameter):
        if wheel_diameter <= 0.0:
            raise ValueError("Wheel diameter must be higher than 0")

        self.__wheel_diameter = wheel_diameter

    @property
    def wheel_circumference(self):
        return self.wheel_diameter * pi

    def set_target_speed(self, target_speed, direction=Direction.FORWARD, distance=0.0):
        """Run the wheel at the specified target speed in meters per second.

        If desired, a :data:`distance` to travel can also be specified in meters, after which the motor
        will stop. Setting :data:`distance` to 0 will set the motor to run indefinitely until stopped.

        .. warning::
            Setting a :data:`.target_speed` higher than the maximum allowed will cause the
            method to throw an exception. To determine what the maximum possible target speed for the motor
            is, use the :class:`max_speed` method.

        .. note::
            Note that for this method to move the wheel the expected :data:`distance`, the correct
            :data:`wheel_diameter` value needs to be used.

        :type target_speed: int or float
        :param target_speed:
            Desired speed in m/s

        :type direction: Direction
        :param direction:
            Direction to rotate the motor. Defaults to forward.

        :type distance: int or float
        :param distance:
            Total distance to travel in m. Set to 0 to run indefinitely.
        """
        if not (-self.max_speed <= target_speed <= self.max_speed):
            raise ValueError(
                f"Wheel speed value must be between {-self.max_speed} and {self.max_speed} (inclusive)"
            )

        rpm = 60.0 * (target_speed / self.wheel_circumference)
        total_rotations = distance / self.wheel_circumference

        self.set_target_rpm(rpm, direction, total_rotations)

    def forward(self, target_speed, distance=0.0):
        """Run the wheel forward at the desired speed in meters per second.

        This method is a simple interface to move the motor that wraps a call to :data:`set_target_speed`,
        specifying the forward direction.

        If desired, a :data:`distance` to travel can also be specified in meters, after which the motor
        will stop. Setting :data:`distance` to 0 will set the motor to run indefinitely until stopped.

        .. note::
            Note that for this method to move the wheel the expected :data:`distance`, the correct
            :data:`wheel_circumference` value needs to be used.

        :type target_speed: int or float
        :param target_speed:
            Desired speed in m/s

        :type distance: int or float
        :param distance:
            Total distance to travel in m. Set to 0 to run indefinitely.
        """

        self.set_target_speed(target_speed, Direction.FORWARD, distance)

    def backward(self, target_speed, distance=0.0):
        """Run the wheel backwards at the desired speed in meters per second.

        This method is a simple interface to move the wheel that wraps a call to :data:`set_target_speed`,
        specifying the back direction.

        If desired, a :data:`distance` to travel can also be specified in meters, after which the motor
        will stop. Setting :data:`distance` to 0 will set the motor to run indefinitely until stopped.

        .. note::
            Note that for this method to move the wheel the expected :data:`distance`, the correct
            :data:`wheel_circumference` value needs to be used.

        :type target_speed: int or float
        :param target_speed:
            Desired speed in m/s

        :type distance: int or float
        :param distance:
            Total distance to travel in m. Set to 0 to run indefinitely.
        """

        self.set_target_speed(target_speed, Direction.BACK, distance)

    @property
    def current_speed(self):
        """Returns the speed currently being achieved by the motor in meters
        per second.

        This value may differ from the target speed set through
        :class:`set_target_speed`.
        """

        return (self.current_rpm / 60.0) * self.wheel_circumference

    @property
    def distance(self):
        """Returns the distance the wheel has travelled in meters.

        This value depends on the correct :data:`wheel_circumference`
        value being set.
        """

        return self.wheel_circumference * self.rotation_counter

    @property
    def max_speed(self):
        """The approximate maximum speed possible for the wheel attached to the
        motor shaft, given the motor specs, gear ratio and wheel circumference.

        This value depends on the correct :data:`wheel_circumference`
        value being set.
        """

        return self.max_rpm / 60.0 * self.wheel_circumference
