# LICENSE HEADER MANAGED BY add-license-header
#
# Copyright 2018 Kornia Team
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
#

"""Register two images by optimizing a homography or a similarity by gradient descent, coarse to fine.

:class:`ImageRegistrator` takes a pair of ``(1, C, H, W)`` images, builds a Gaussian pyramid of each with
:func:`~kornia.geometry.transform.build_pyramid` and, from the coarsest level to the finest, optimizes a transform
model on the loss of each image warped onto the other. The built-in models return a ``(1, 3, 3)`` destination→source
matrix in normalized :math:`[-1, 1]` coordinates (``align_corners=False``): the ``src_homo_dst`` of
:func:`~kornia.geometry.transform.homography_warp` and :class:`~kornia.geometry.transform.HomographyWarper`.

Which class to use:

- Register two images: :class:`ImageRegistrator`, with ``model_type`` ``'homography'``, ``'similarity'``,
  ``'translation'``, ``'rotation'`` or ``'scale'``.
- The models it optimizes: :class:`Homography` (a full homography) and :class:`Similarity` (a rotation, a scale and a
  shift, each one optional). A custom model subclasses :class:`BaseModel` and comes with a warper class, such as a
  subclass of :class:`~kornia.geometry.transform.BaseWarper`.
- To estimate a homography from matched keypoints rather than from pixel values, use
  :class:`~kornia.geometry.ransac.RANSAC` or :func:`~kornia.geometry.homography.find_homography_dlt`.
"""

from abc import ABC, abstractmethod
from typing import Any, Callable, Dict, List, Optional, Tuple, Type, Union

import torch
import torch.nn.functional as F
from torch import nn, optim

from kornia.core._compat import deprecated
from kornia.core.utils import _torch_inverse_cast
from kornia.geometry.conversions import angle_to_rotation_matrix, convert_affinematrix_to_homography

from .homography_warper import BaseWarper, HomographyWarper
from .pyramid import build_pyramid

__all__ = ["BaseModel", "Homography", "ImageRegistrator", "Similarity"]

_HALF_DTYPES = (torch.float16, torch.bfloat16)


def _float32_if_half(tensor: torch.Tensor) -> torch.Tensor:
    """Return a ``float16``/``bfloat16`` tensor as ``float32``, and any other tensor unchanged."""
    return tensor.float() if tensor.dtype in _HALF_DTYPES else tensor


class BaseModel(ABC, nn.Module):
    """Declare the interface of a transform model that ImageRegistrator optimizes.

    A subclass holds its transform parameters as :class:`~torch.nn.parameter.Parameter` and implements
    ``reset_model()``, which returns them to the identity, ``forward()``, the matrix with which its warper warps the
    source image into the destination, and ``forward_inverse()``, the matrix of the reverse warp.
    :class:`ImageRegistrator` takes an instance as ``model_type``, together with a ``warper`` class. The three methods
    are abstract: constructing ``BaseModel`` itself, or a subclass that does not implement all three, raises
    ``TypeError``, as for :class:`~kornia.geometry.transform.BaseWarper`. For the built-in models, use
    :class:`Homography` or :class:`Similarity`.

    See Also:
        - :class:`Homography`: the built-in model of a full homography.
        - :class:`Similarity`: the built-in model of a rotation, a scale and a shift.
        - :class:`ImageRegistrator`: optimizes a model of this interface.

    """

    @abstractmethod
    def reset_model(self) -> None:
        """Reset learnable registration parameters to the identity transform."""
        ...

    @abstractmethod
    def forward(self) -> torch.Tensor:
        """Return the transform used to warp the source image into the destination image.

        The built-in models map destination coordinates to source coordinates. A custom model's coordinates must
        match its supplied warper; see :class:`ImageRegistrator`.

        Returns:
            Transform tensor for the current model state, in the shape required by the warper.
        """
        ...

    @abstractmethod
    def forward_inverse(self) -> torch.Tensor:
        """Return the transform used to warp the destination image into the source image.

        Returns:
            Transform tensor for the inverse warp, in the coordinates expected by the warper.
        """
        ...


class Homography(BaseModel):
    r"""Hold a learnable 3x3 homography for ImageRegistrator, starting from the identity.

    ``model = Homography()`` holds one ``(3, 3)`` :class:`~torch.nn.parameter.Parameter`, initialized to the
    identity. ``model()`` returns it as a ``(1, 3, 3)`` matrix scaled so that its ``[2, 2]`` entry is 1, and
    ``model.forward_inverse()`` its inverse, up to scale. ``ImageRegistrator('homography')`` optimizes it as the
    destination→source homography in normalized coordinates. For a rotation, a scale and a shift only, use
    :class:`Similarity`.

    See Also:
        - :class:`Similarity`: four degrees of freedom instead of eight, each of rotation, scale and shift optional.
        - :class:`ImageRegistrator`: optimizes this model with ``model_type='homography'``.

    """

    def __init__(self) -> None:
        super().__init__()
        self.model = nn.Parameter(torch.eye(3))
        self.reset_model()

    def __repr__(self) -> str:
        return f"{self.__class__.__name__}({self.model})"

    def reset_model(self) -> None:
        """Initialize the model with identity transform."""
        torch.nn.init.eye_(self.model)

    def forward(self) -> torch.Tensor:
        r"""Single-batch homography, scaled so that its ``[2, 2]`` entry is 1.

        Returns:
            Homography matrix with shape ``(1, 3, 3)``.

        """
        return torch.unsqueeze(self.model / self.model[2, 2], dim=0)  # 1x3x3

    def forward_inverse(self) -> torch.Tensor:
        r"""Inverted single-batch homography.

        Returns:
            Homography matrix with shape ``(1, 3, 3)``.

        """
        return torch.unsqueeze(_torch_inverse_cast(self.model), dim=0)


class Similarity(BaseModel):
    """Hold a learnable rotation, scale and shift for ImageRegistrator, each one optional.

    ``model = Similarity(rotation, scale, shift)`` holds an angle ``rot`` in degrees, a ``scale`` and a ``(1, 2, 1)``
    ``shift`` in normalized units, each a :class:`~torch.nn.parameter.Parameter` when its flag is ``True`` and
    otherwise a constant buffer (0, 1 and 0). ``model()`` returns the ``(1, 3, 3)`` matrix described below and
    ``model.forward_inverse()`` its inverse. :class:`ImageRegistrator` uses it for ``'similarity'`` (all three),
    ``'translation'`` (the shift), ``'rotation'`` and ``'scale'``. For a full homography, use :class:`Homography`.

    Convention:
        - ``forward()`` is ``[[scale * R, shift], [0, 0, 1]]`` in the normalized :math:`[-1, 1]` coordinates of
          :class:`ImageRegistrator` (``align_corners=False``), where ``R`` comes from
          :func:`~kornia.geometry.conversions.angle_to_rotation_matrix` of ``rot`` in degrees.
        - After :meth:`set_image_shape` with the image height ``H`` and width ``W``, ``R`` is
          ``diag(H / W, 1) @ angle_to_rotation_matrix(rot) @ diag(W / H, 1)``. The normalized frame scales x by
          ``2 / W`` and y by ``2 / H``, so in pixels the linear part is then ``scale * angle_to_rotation_matrix(rot)``
          about the image centre, for any aspect ratio. :meth:`ImageRegistrator.register` sets the shape of
          ``dst_img``. ``shift`` stays in normalized units.
        - Without an image shape, ``R`` is ``angle_to_rotation_matrix(rot)``, which rotates the pixels only on a
          square image. The shape is not part of ``state_dict()``: after ``load_state_dict``, call
          :meth:`set_image_shape` again before using the model on a non-square image.

    Args:
        rotation: if True, the rotation is optimizable, else constant zero.
        scale: if True, the scale is optimizable, else constant one.
        shift: if True, the shift is optimizable, else constant zero.

    See Also:
        - :class:`Homography`: a full homography, eight degrees of freedom.
        - :class:`ImageRegistrator`: optimizes this model for every ``model_type`` but ``'homography'``.

    """

    def __init__(self, rotation: bool = True, scale: bool = True, shift: bool = True) -> None:
        super().__init__()
        self.height: Optional[int] = None
        self.width: Optional[int] = None
        if rotation:
            self.rot = nn.Parameter(torch.zeros(1))
        else:
            self.register_buffer("rot", torch.zeros(1))
        if shift:
            self.shift = nn.Parameter(torch.zeros(1, 2, 1))
        else:
            self.register_buffer("shift", torch.zeros(1, 2, 1))
        if scale:
            self.scale = nn.Parameter(torch.ones(1))
        else:
            self.register_buffer("scale", torch.ones(1))
        self.reset_model()

    def __repr__(self) -> str:
        return f"{self.__class__.__name__}(angle = {self.rot},\n shift={self.shift}, \n scale={self.scale})"

    def reset_model(self) -> None:
        """Initialize the model with identity transform."""
        torch.nn.init.zeros_(self.rot)
        torch.nn.init.zeros_(self.shift)
        torch.nn.init.ones_(self.scale)

    def set_image_shape(self, height: int, width: int) -> None:
        """Set the image size in pixels, which makes ``rot`` and ``scale`` a rotation and scaling of the pixels.

        Args:
            height: image height ``H``.
            width: image width ``W``.

        """
        self.height = height
        self.width = width

    def forward(self) -> torch.Tensor:
        r"""Single-batch similarity transform.

        Returns:
            Similarity with shape ``(1, 3, 3)``

        """
        rot = self.scale * angle_to_rotation_matrix(self.rot)
        if self.height is not None and self.width is not None:
            # diag(H / W, 1) @ rot @ diag(W / H, 1): the pixel rotation in the normalized frame, whose x unit is W / 2
            # pixels and y unit H / 2 pixels. Only the off-diagonal entries change, so the diagonal stays exact.
            aspect = self.height / self.width
            rot = rot * rot.new_tensor([[1.0, aspect], [1.0 / aspect, 1.0]])
        return convert_affinematrix_to_homography(torch.cat([rot, self.shift], dim=2))

    def forward_inverse(self) -> torch.Tensor:
        r"""Single-batch inverse similarity transform.

        Returns:
            Similarity with shape ``(1, 3, 3)``

        """
        return _torch_inverse_cast(self.forward())


class ImageRegistrator(nn.Module):
    r"""Register two images by optimizing a transform model on their Gaussian pyramids, coarse to fine.

    Build ``registrator = ImageRegistrator(model_type)``, then ``M = registrator.register(src_img, dst_img)`` takes two
    ``(1, C, H, W)`` images (the built-in models hold one transform, and a larger batch raises). From the coarsest of
    ``pyramid_levels`` Gaussian levels to the finest, it runs up to ``num_iterations`` steps of ``optimizer`` on the
    masked loss of ``src_img`` warped onto ``dst_img`` plus ``dst_img`` warped back onto ``src_img``, and returns the
    model's matrix, ``(1, 3, 3)`` for the built-in models (with the matrix after each level as well, when
    ``output_intermediate_models=True``); :meth:`warp_src_into_dst` then warps an image by it. With
    more than one level, both sides need at least ``2 ** (pyramid_levels - 1)`` pixels, 16 for the default 5. To
    align images from matched keypoints instead, use :class:`~kornia.geometry.ransac.RANSAC`.

    Convention:
        - :meth:`register` returns ``self.model()`` unchanged. For the built-in string ``model_type`` values, this
          maps **destination** coordinates to **source** coordinates, normalized to :math:`[-1, 1]` with
          ``align_corners=False``: the ``src_homo_dst`` argument of
          :func:`~kornia.geometry.transform.homography_warp`. :meth:`warp_src_into_dst` uses this model, and
          :meth:`warp_dst_into_src` uses ``self.model.forward_inverse()``. A custom ``model_type`` and ``warper``
          determine their own coordinate system; their forward and inverse mappings must agree with that warper.
        - For the built-in models, ``denormalize_homography(M, (H, W), (H, W), align_corners=False)`` converts the
          returned ``M`` to pixels: content that moves by :math:`(t_x, t_y)` pixels from ``src_img`` to ``dst_img``
          gives a shift of :math:`(-t_x, -t_y)`.
        - :meth:`register` always starts from the identity, so a loaded ``state_dict`` drives the warps but is not a
          warm start.
        - A ``float16``/``bfloat16`` model or image is optimized in ``float32``: during :meth:`register` both images,
          also when ``allow_shape_mismatch`` resizes ``src_img``, and the model's half parameters and buffers are
          ``float32``, and afterwards those tensors and their gradients return to their own dtype, also when
          :meth:`register` raises. Adam's second-moment estimate ``0.001 * g**2`` and its ``eps`` of ``1e-8`` are
          both 0 in ``float16`` once ``|g| < 0.0055``, so a ``float16`` optimization divides by zero and ends in NaN,
          and ``bfloat16`` steps round to the dtype's coarse spacing. For a half model the intermediate models of
          ``output_intermediate_models`` are computed in ``float32`` and rounded to the model's dtype, so the last
          one can differ by an ulp from the returned ``self.model()``, which is computed in the model's dtype.
        - A ``float16``, ``bfloat16`` or ``float32`` model therefore registers images of any of these dtypes.
          :meth:`warp_src_into_dst` and :meth:`warp_dst_into_src` sample a model and an image of two different ones
          of them in ``float32`` too and return the image's dtype; a model and an image of the same dtype go to the
          warper unchanged, as before.

    Args:
        model_type: Geometrical model for registration: ``'homography'``, ``'similarity'``, or ``'translation'``,
            ``'rotation'`` or ``'scale'`` (a :class:`Similarity` that optimizes only that parameter), or a
            :class:`BaseModel` instance together with ``warper``.
        optimizer: optimizer class used for the optimization.
        loss_fn: torch loss function, called with ``reduction='none'``.
        pyramid_levels: number of scale pyramid levels.
        lr: learning rate for optimization.
        num_iterations: maximum number of iterations at each pyramid level, from coarse to fine.
        tolerance: stop a pyramid level when successive losses at that level differ by less than this value.
            The first iteration at each level always runs. Default 1e-4.
        warper: the warper class, called as ``warper(height, width)``. Required when ``model_type`` is a module;
            a string ``model_type`` uses :class:`~kornia.geometry.transform.HomographyWarper`.
        allow_shape_mismatch: if True, :meth:`register` resizes ``src_img`` bilinearly to the height and width of
            ``dst_img``, and a different batch or channel size still raises ``ValueError``; if False, images of
            different shapes raise ``ValueError``.

    See Also:
        - :class:`Homography`: the model of ``model_type='homography'``.
        - :class:`Similarity`: the model of the other string ``model_type`` values.
        - :class:`~kornia.geometry.transform.HomographyWarper`: the warper of the built-in models.
        - :class:`~kornia.geometry.ransac.RANSAC`: a homography from matched keypoints, robust to outliers.

    Example:
        >>> from kornia.geometry import ImageRegistrator
        >>> img_src = torch.rand(1, 1, 32, 32)
        >>> img_dst = torch.rand(1, 1, 32, 32)
        >>> registrator = ImageRegistrator('similarity')
        >>> homo = registrator.register(img_src, img_dst)

    """

    # TODO: resolve better type, potentially using factory.
    def __init__(
        self,
        model_type: Union[str, BaseModel] = "homography",
        optimizer: Type[optim.Optimizer] = optim.Adam,
        loss_fn: Callable[..., torch.Tensor] = F.l1_loss,
        pyramid_levels: int = 5,
        lr: float = 1e-3,
        num_iterations: int = 100,
        tolerance: float = 1e-4,
        warper: Optional[Type[BaseWarper]] = None,
        allow_shape_mismatch: bool = False,
    ) -> None:
        super().__init__()
        self.known_models = ["homography", "similarity", "translation", "scale", "rotation"]
        # We provide pre-defined combinations or allow user to supply model
        # together with warper
        if not isinstance(model_type, str):
            if warper is None:
                raise ValueError("You must supply warper together with custom model")
            self.warper = warper
            self.model = model_type
        elif model_type.lower() == "homography":
            self.warper = HomographyWarper
            self.model = Homography()
        elif model_type.lower() == "similarity":
            self.warper = HomographyWarper
            self.model = Similarity(True, True, True)
        elif model_type.lower() == "translation":
            self.warper = HomographyWarper
            self.model = Similarity(False, False, True)
        elif model_type.lower() == "rotation":
            self.warper = HomographyWarper
            self.model = Similarity(True, False, False)
        elif model_type.lower() == "scale":
            self.warper = HomographyWarper
            self.model = Similarity(False, True, False)
        else:
            raise ValueError(f"{model_type} is not supported. Try {self.known_models}")
        self.pyramid_levels = pyramid_levels
        self.optimizer = optimizer
        self.lr = lr
        self.loss_fn = loss_fn
        self.num_iterations = num_iterations
        self.tolerance = tolerance
        self.allow_shape_mismatch = allow_shape_mismatch

    def get_single_level_loss(
        self, img_src: torch.Tensor, img_dst: torch.Tensor, transform_model: torch.Tensor
    ) -> torch.Tensor:
        """Warp img_src into img_dst with transform_model and returns loss."""
        # ToDo: Make possible registration of images of different shape
        if img_src.shape != img_dst.shape:
            raise ValueError(f"Cannot register images of different shapes {img_src.shape} {img_dst.shape}")
        _height, _width = img_dst.shape[-2:]
        warper = self.warper(_height, _width)
        img_src_to_dst = warper(img_src, transform_model)
        # compute and mask loss
        loss = self.loss_fn(img_src_to_dst, img_dst, reduction="none")  # 1xCxHxW
        ones_tensor = warper(torch.ones_like(img_src), transform_model)
        return loss.masked_select(ones_tensor > 0.9).mean()

    def reset_model(self) -> None:
        """Call model reset function."""
        self.model.reset_model()

    def register(
        self,
        src_img: torch.Tensor,
        dst_img: torch.Tensor,
        verbose: bool = False,
        output_intermediate_models: bool = False,
    ) -> Union[torch.Tensor, Tuple[torch.Tensor, List[torch.Tensor]]]:
        r"""Estimate the transformation which warps src_img into dst_img by gradient descent.

        Args:
            src_img: Input image torch.Tensor.
            dst_img: Input image torch.Tensor.
            verbose: if True, outputs loss every 10 iterations.
            output_intermediate_models: if True with intermediate models

        Returns:
            the transformation between two images, shape depends on the model,
            typically [1x3x3] torch.Tensor for string model_types.

        """
        self.reset_model()
        if src_img.shape != dst_img.shape and not self.allow_shape_mismatch:
            raise ValueError(
                f"Cannot register images of different shapes {src_img.shape} {dst_img.shape}. "
                "Consider setting `allow_shape_mismatch = True`"
            )
        # Half precision is optimized in float32 (see the class Convention), the resize of src_img included. Every
        # promotion runs inside the region that gives the model's tensors their dtype back; restoring a tensor that an
        # error left unpromoted is a no-op.
        restore = [(t, t.dtype) for t in (*self.model.parameters(), *self.model.buffers()) if t.dtype in _HALF_DTYPES]
        try:
            for tensor, _ in restore:
                tensor.data = tensor.data.float()
            src_img, dst_img = _float32_if_half(src_img), _float32_if_half(dst_img)
            if src_img.shape != dst_img.shape:
                src_img = F.interpolate(src_img, size=dst_img.shape[-2:], mode="bilinear", align_corners=False)
            aux_models = self._optimize(src_img, dst_img, verbose, output_intermediate_models)
        finally:
            for tensor, dtype in restore:
                tensor.data = tensor.data.to(dtype)
                if tensor.grad is not None:
                    tensor.grad = tensor.grad.to(dtype)
        model = self.model()
        if output_intermediate_models:
            return model, [aux.to(model.dtype) for aux in aux_models]
        return model

    def _optimize(
        self, src_img: torch.Tensor, dst_img: torch.Tensor, verbose: bool, output_intermediate_models: bool
    ) -> List[torch.Tensor]:
        """Run the coarse-to-fine optimization of :meth:`register` and return the model after each level, if asked."""
        # ToDo: better parameter passing to optimizer
        _opt_args: Dict[str, Any] = {}
        _opt_args["lr"] = self.lr
        opt = self.optimizer(self.model.parameters(), **_opt_args)

        # compute the gaussian pyramids
        # [::-1] because we have to register from coarse to fine
        img_src_pyr = build_pyramid(src_img, self.pyramid_levels)[::-1]
        img_dst_pyr = build_pyramid(dst_img, self.pyramid_levels)[::-1]
        aux_models = []
        if len(img_dst_pyr) != len(img_src_pyr):
            raise ValueError("Cannot register images of different sizes")
        if isinstance(self.model, Similarity):
            # every pyramid level spans the same image, so the full-resolution shape is the pixel aspect ratio of each
            # level; a level's own rounded shape is not
            self.model.set_image_shape(dst_img.shape[-2], dst_img.shape[-1])
        for img_src_level, img_dst_level in zip(img_src_pyr, img_dst_pyr):
            # tolerance compares successive losses of one level; a loss from the coarser level is not one of them
            prev_loss: Optional[float] = None
            for i in range(self.num_iterations):
                # compute gradient and update optimizer parameters
                opt.zero_grad()
                loss = self.get_single_level_loss(img_src_level, img_dst_level, self.model())
                loss += self.get_single_level_loss(img_dst_level, img_src_level, self.model.forward_inverse())
                current_loss = loss.item()
                if prev_loss is not None and abs(current_loss - prev_loss) < self.tolerance:
                    break
                prev_loss = current_loss
                loss.backward()
                if verbose and (i % 10 == 0):
                    print(f"Loss = {current_loss:.4f}, iter={i}")  # noqa: T201 - only with verbose=True
                opt.step()
            if output_intermediate_models:
                aux_models.append(self.model().clone().detach())
        return aux_models

    def _warp(self, img: torch.Tensor, model: torch.Tensor) -> torch.Tensor:
        """Warp ``img`` by ``model`` at the image's size, in the image's dtype (see the class Convention)."""
        _height, _width = img.shape[-2:]
        warper = self.warper(_height, _width)
        if model.dtype == img.dtype:
            return warper(img, model)
        # register() optimizes a half model or image in float32, so the two can differ in dtype; such a pair is
        # sampled in float32 as well
        return warper(_float32_if_half(img), _float32_if_half(model)).to(img.dtype)

    def warp_src_into_dst(self, src_img: torch.Tensor) -> torch.Tensor:
        r"""Warp src_img with estimated model."""
        return self._warp(src_img, self.model())

    def warp_dst_into_src(self, dst_img: torch.Tensor) -> torch.Tensor:
        r"""Warp dst_img with inverted estimated model."""
        return self._warp(dst_img, self.model.forward_inverse())

    @deprecated(replace_with="ImageRegistrator.warp_dst_into_src", version="0.9.0")
    def warp_dst_inro_src(self, dst_img: torch.Tensor) -> torch.Tensor:
        r"""Deprecated alias for :meth:`warp_dst_into_src`."""
        return self.warp_dst_into_src(dst_img)
