import dataclasses
from abc import ABCMeta, abstractmethod
from typing import Tuple, Optional

import numpy as np

import transformations as tr


@dataclasses.dataclass
class Pose2DEstimate:
    x: float
    y: float
    theta: float
    # time of the estimate
    time: float

    @property
    def q(self) -> np.ndarray:
        return tr.quaternion_from_euler(0, 0, self.theta)

    def copy(self) -> 'Pose2DEstimate':
        return Pose2DEstimate(**dataclasses.asdict(self))

    def __str__(self):
        return f"P(x={self.x}, y={self.y}, theta={self.theta})"


@dataclasses.dataclass
class Velocity2DEstimate:
    v: float
    w: float
    # time of the estimate
    time: float

    def copy(self) -> 'Velocity2DEstimate':
        return Velocity2DEstimate(**dataclasses.asdict(self))

    def __str__(self):
        return f"P(v={self.v}, w={self.w})"


class IWheelOdometer(metaclass=ABCMeta):

    def __init__(self,
                 ticks_per_meter: float,
                 wheel_baseline: float
                 ):
        """
        Performs odometry estimation using data from wheel encoders (aka deadreckoning).

        Args:
            ticks_per_meter (:obj:`int`):   Number of ticks generated by the encoder for
                                            each meter traveled by the wheel;

            wheel_baseline (:obj:`float`):  Distance between the center of the two wheels along
                                            the axle (in meters);

        """
        # store parameters
        self.ticks_per_meter: float = ticks_per_meter
        self.wheel_baseline: float = wheel_baseline

    @abstractmethod
    def initialize(self):
        pass

    @abstractmethod
    def update(self, left_ticks: int, right_ticks: int, timestamp: float = None):
        """
        Args:
            left_ticks `int`:   Number of ticks counted so far by the left wheel encoder.
            right_ticks `int`:  Number of ticks counted so far by the right wheel encoder.
            timestamp `float`:  Time in seconds when the reading happened, default is NOW.

        """

    @abstractmethod
    def get_estimate(self) -> Tuple[Optional[Pose2DEstimate], Optional[Velocity2DEstimate]]:
        """
        Returns a pose estimate and a velocity estimate based on the data processed so far.
        """


__all__ = [
    "IWheelOdometer",
    "Pose2DEstimate",
    "Velocity2DEstimate"
]
