"""
(*)~---------------------------------------------------------------------------
Pupil - eye tracking platform
Copyright (C) 2012-2019 Pupil Labs

Distributed under the terms of the GNU
Lesser General Public License (LGPL v3.0).
See COPYING and COPYING.LESSER for license details.
---------------------------------------------------------------------------~(*)
"""
from abc import abstractmethod, abstractproperty
from collections import deque
from math import floor
from typing import Sequence, Optional

import numpy as np
from sortedcontainers import SortedList

from .camera import CameraModel
from .constants import _EYE_RADIUS_DEFAULT
from .geometry.primitives import Ellipse, Line
from .geometry.projections import project_line_into_image_plane, unproject_ellipse


class Observation(object):
    def __init__(
        self, ellipse: Ellipse, confidence: float, timestamp: float, focal_length: float
    ):
        self.ellipse = ellipse
        self.confidence_2d = confidence
        self.confidence = 0.0
        self.timestamp = timestamp

        self.circle_3d_pair = None
        self.gaze_3d_pair = None
        self.gaze_2d = None
        self.aux_2d = None
        self.aux_3d = None
        self.invalid = True

        circle_3d_pair = unproject_ellipse(ellipse, focal_length)
        if not circle_3d_pair:
            # unprojecting ellipse failed, invalid observation!
            return

        self.invalid = False
        self.confidence = self.confidence_2d
        self.circle_3d_pair = circle_3d_pair

        self.gaze_3d_pair = [
            Line(
                circle_3d_pair[i].center,
                circle_3d_pair[i].center + circle_3d_pair[i].normal,
            )
            for i in [0, 1]
        ]
        self.gaze_2d = project_line_into_image_plane(self.gaze_3d_pair[0], focal_length)
        self.gaze_2d_line = np.array([*self.gaze_2d.origin, *self.gaze_2d.direction])

        self.aux_2d = np.empty((2, 3))
        v = np.reshape(self.gaze_2d.direction, (2, 1))
        self.aux_2d[:, :2] = np.eye(2) - v @ v.T
        self.aux_2d[:, 2] = (np.eye(2) - v @ v.T) @ self.gaze_2d.origin

        self.aux_3d = np.empty((2, 3, 4))
        for i in range(2):
            Dierkes_line = self.get_Dierkes_line(i)
            v = np.reshape(Dierkes_line.direction, (3, 1))
            self.aux_3d[i, :3, :3] = np.eye(3) - v @ v.T
            self.aux_3d[i, :3, 3] = (np.eye(3) - v @ v.T) @ Dierkes_line.origin

    def get_Dierkes_line(self, i):
        origin = (
            self.circle_3d_pair[i].center
            - _EYE_RADIUS_DEFAULT * self.circle_3d_pair[i].normal
        )
        direction = self.circle_3d_pair[i].center
        return Line(origin, direction)


class ObservationStorage:
    @abstractmethod
    def add(self, observation: Observation):
        pass

    @abstractproperty
    def observations(self) -> Sequence[Observation]:
        pass

    @abstractmethod
    def clear(self):
        pass

    @abstractmethod
    def count(self) -> int:
        pass


class BasicStorage(ObservationStorage):
    def __init__(self):
        self._storage = []

    def add(self, observation: Observation):
        if observation.invalid:
            return
        self._storage.append(observation)

    @property
    def observations(self) -> Sequence[Observation]:
        return self._storage

    def clear(self):
        self._storage.clear()

    def count(self) -> int:
        return len(self._storage)


class BufferedObservationStorage(ObservationStorage):
    def __init__(self, confidence_threshold: float, buffer_length: int):
        self.confidence_threshold = confidence_threshold
        self._storage = deque(maxlen=buffer_length)

    def add(self, observation: Observation):
        if observation.invalid:
            return
        if observation.confidence < self.confidence_threshold:
            return

        self._storage.append(observation)

    @property
    def observations(self) -> Sequence[Observation]:
        return list(self._storage)

    def clear(self):
        self._storage.clear()

    def count(self) -> int:
        return len(self._storage)


class BinBufferedObservationStorage(ObservationStorage):
    def __init__(
        self,
        camera: CameraModel,
        confidence_threshold: float,
        n_bins_horizontal: int,
        bin_buffer_length: int,
        forget_min_observations: Optional[int] = None,
        forget_min_time: Optional[float] = None,
    ):
        self.camera = camera
        self.confidence_threshold = confidence_threshold
        self.bin_buffer_length = bin_buffer_length
        self.forget_min_observations = forget_min_observations
        self.forget_min_time = forget_min_time
        self.pixels_per_bin = self.camera.resolution[0] / n_bins_horizontal
        self.w = n_bins_horizontal
        self.h = int(round(self.camera.resolution[1] / self.pixels_per_bin))

        self._by_time = SortedList(key=lambda obs: obs.timestamp)
        self._by_bin = dict()

    def add(self, observation: Observation):
        if observation.invalid:
            return
        if observation.confidence < self.confidence_threshold:
            return

        idx = self._get_bin(observation)
        if idx < 0 or idx >= self.w * self.h:
            print(f"INDEX OUT OF BOUNDS: {idx}")
            return

        if idx not in self._by_bin:
            self._by_bin[idx] = SortedList(key=lambda obs: obs.timestamp)

        # add to both lookup structures
        _bin: SortedList = self._by_bin[idx]
        _bin.add(observation)
        self._by_time.add(observation)

        # manage within-bin forgetting
        while len(_bin) > self.bin_buffer_length:
            old = _bin.pop(0)
            self._by_time.remove(old)

        # manage across-bin forgetting
        if self.forget_min_observations is None or self.forget_min_time is None:
            return

        while self.count() > self.forget_min_observations:
            oldest_age = observation.timestamp - self._by_time[0].timestamp
            if oldest_age < self.forget_min_time:
                break

            # forget oldest entry
            old = self._by_time.pop(0)
            idx = self._get_bin(old)
            _bin = self._by_bin[idx]
            _bin.remove(old)
            # make sure to remove bin if empty for bin-counting to work
            if len(_bin) == 0:
                self._by_bin.pop(idx)

    @property
    def observations(self) -> Sequence[Observation]:
        return list(self._by_time)

    def clear(self):
        self._by_time.clear()
        self._by_bin.clear()

    def count(self) -> int:
        return len(self._by_time)

    def get_bin_counts(self) -> np.ndarray:
        dense_1d = np.zeros((self.w * self.h,))
        for idx, _bin in self._by_bin.items():
            dense_1d[idx] = len(_bin)
        return np.reshape(dense_1d, (self.w, self.h))

    def _get_bin(self, observation: Observation) -> int:
        x, y = (
            floor((ellipse_center + resolution / 2) / self.pixels_per_bin)
            for ellipse_center, resolution in zip(
                observation.ellipse.center, self.camera.resolution
            )
        )
        # convert to 1D bin index
        return x + y * self.h

"""
(*)~---------------------------------------------------------------------------
Pupil - eye tracking platform
Copyright (C) 2012-2019 Pupil Labs

Distributed under the terms of the GNU
Lesser General Public License (LGPL v3.0).
See COPYING and COPYING.LESSER for license details.
---------------------------------------------------------------------------~(*)
"""
from abc import abstractmethod, abstractproperty
from collections import deque
from math import floor
from typing import Sequence, Optional

import numpy as np
from sortedcontainers import SortedList

from .camera import CameraModel
from .constants import _EYE_RADIUS_DEFAULT
from .geometry.primitives import Ellipse, Line
from .geometry.projections import project_line_into_image_plane, unproject_ellipse


class Observation(object):
    def __init__(
        self, ellipse: Ellipse, confidence: float, timestamp: float, focal_length: float
    ):
        self.ellipse = ellipse
        self.confidence_2d = confidence
        self.confidence = 0.0
        self.timestamp = timestamp

        self.circle_3d_pair = None
        self.gaze_3d_pair = None
        self.gaze_2d = None
        self.aux_2d = None
        self.aux_3d = None
        self.invalid = True

        circle_3d_pair = unproject_ellipse(ellipse, focal_length)
        if not circle_3d_pair:
            # unprojecting ellipse failed, invalid observation!
            return

        self.invalid = False
        self.confidence = self.confidence_2d
        self.circle_3d_pair = circle_3d_pair

        self.gaze_3d_pair = [
            Line(
                circle_3d_pair[i].center,
                circle_3d_pair[i].center + circle_3d_pair[i].normal,
            )
            for i in [0, 1]
        ]
        self.gaze_2d = project_line_into_image_plane(self.gaze_3d_pair[0], focal_length)
        self.gaze_2d_line = np.array([*self.gaze_2d.origin, *self.gaze_2d.direction])

        self.aux_2d = np.empty((2, 3))
        v = np.reshape(self.gaze_2d.direction, (2, 1))
        self.aux_2d[:, :2] = np.eye(2) - v @ v.T
        self.aux_2d[:, 2] = (np.eye(2) - v @ v.T) @ self.gaze_2d.origin

        self.aux_3d = np.empty((2, 3, 4))
        for i in range(2):
            Dierkes_line = self.get_Dierkes_line(i)
            v = np.reshape(Dierkes_line.direction, (3, 1))
            self.aux_3d[i, :3, :3] = np.eye(3) - v @ v.T
            self.aux_3d[i, :3, 3] = (np.eye(3) - v @ v.T) @ Dierkes_line.origin

    def get_Dierkes_line(self, i):
        origin = (
            self.circle_3d_pair[i].center
            - _EYE_RADIUS_DEFAULT * self.circle_3d_pair[i].normal
        )
        direction = self.circle_3d_pair[i].center
        return Line(origin, direction)


class ObservationStorage:
    @abstractmethod
    def add(self, observation: Observation):
        pass

    @abstractproperty
    def observations(self) -> Sequence[Observation]:
        pass

    @abstractmethod
    def clear(self):
        pass

    @abstractmethod
    def count(self) -> int:
        pass


class BasicStorage(ObservationStorage):
    def __init__(self):
        self._storage = []

    def add(self, observation: Observation):
        if observation.invalid:
            return
        self._storage.append(observation)

    @property
    def observations(self) -> Sequence[Observation]:
        return self._storage

    def clear(self):
        self._storage.clear()

    def count(self) -> int:
        return len(self._storage)


class BufferedObservationStorage(ObservationStorage):
    def __init__(self, confidence_threshold: float, buffer_length: int):
        self.confidence_threshold = confidence_threshold
        self._storage = deque(maxlen=buffer_length)

    def add(self, observation: Observation):
        if observation.invalid:
            return
        if observation.confidence < self.confidence_threshold:
            return

        self._storage.append(observation)

    @property
    def observations(self) -> Sequence[Observation]:
        return list(self._storage)

    def clear(self):
        self._storage.clear()

    def count(self) -> int:
        return len(self._storage)


class BinBufferedObservationStorage(ObservationStorage):
    def __init__(
        self,
        camera: CameraModel,
        confidence_threshold: float,
        n_bins_horizontal: int,
        bin_buffer_length: int,
        forget_min_observations: Optional[int] = None,
        forget_min_time: Optional[float] = None,
    ):
        self.camera = camera
        self.confidence_threshold = confidence_threshold
        self.bin_buffer_length = bin_buffer_length
        self.forget_min_observations = forget_min_observations
        self.forget_min_time = forget_min_time
        self.pixels_per_bin = self.camera.resolution[0] / n_bins_horizontal
        self.w = n_bins_horizontal
        self.h = int(round(self.camera.resolution[1] / self.pixels_per_bin))

        self._by_time = SortedList(key=lambda obs: obs.timestamp)
        self._by_bin = dict()

    def add(self, observation: Observation):
        if observation.invalid:
            return
        if observation.confidence < self.confidence_threshold:
            return

        idx = self._get_bin(observation)
        if idx < 0 or idx >= self.w * self.h:
            print(f"INDEX OUT OF BOUNDS: {idx}")
            return

        if idx not in self._by_bin:
            self._by_bin[idx] = SortedList(key=lambda obs: obs.timestamp)

        # add to both lookup structures
        _bin: SortedList = self._by_bin[idx]
        _bin.add(observation)
        self._by_time.add(observation)

        # manage within-bin forgetting
        while len(_bin) > self.bin_buffer_length:
            old = _bin.pop(0)
            self._by_time.remove(old)

        # manage across-bin forgetting
        if self.forget_min_observations is None or self.forget_min_time is None:
            return

        while self.count() > self.forget_min_observations:
            oldest_age = observation.timestamp - self._by_time[0].timestamp
            if oldest_age < self.forget_min_time:
                break

            # forget oldest entry
            old = self._by_time.pop(0)
            idx = self._get_bin(old)
            _bin = self._by_bin[idx]
            _bin.remove(old)
            # make sure to remove bin if empty for bin-counting to work
            if len(_bin) == 0:
                self._by_bin.pop(idx)

    @property
    def observations(self) -> Sequence[Observation]:
        return list(self._by_time)

    def clear(self):
        self._by_time.clear()
        self._by_bin.clear()

    def count(self) -> int:
        return len(self._by_time)

    def get_bin_counts(self) -> np.ndarray:
        dense_1d = np.zeros((self.w * self.h,))
        for idx, _bin in self._by_bin.items():
            dense_1d[idx] = len(_bin)
        return np.reshape(dense_1d, (self.w, self.h))

    def _get_bin(self, observation: Observation) -> int:
        x, y = (
            floor((ellipse_center + resolution / 2) / self.pixels_per_bin)
            for ellipse_center, resolution in zip(
                observation.ellipse.center, self.camera.resolution
            )
        )
        # convert to 1D bin index
        return x + y * self.h

(*)~--------------------------------------------------------------------------- Pupil - eye tracking platform Copyright (C) 2012-2019 Pupil Labs  Distributed under the terms of the GNU Lesser General Public License (LGPL v3.0). See COPYING and COPYING.LESSER for license details. ---------------------------------------------------------------------------~(*) # unprojecting ellipse failed, invalid observation! # add to both lookup structures # manage within-bin forgetting # manage across-bin forgetting # forget oldest entry # make sure to remove bin if empty for bin-counting to work # convert to 1D bin index