# 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.
#

import copy
from typing import Any, Dict, Optional, Tuple, Union, cast

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

from kornia.core.check import KORNIA_CHECK_SHAPE
from kornia.core.download import load_state_dict_from_url
from kornia.core.utils import dataclass_to_dict, dict_to_dataclass
from kornia.feature.sold2.structures import DetectorCfg, LineMatcherCfg

from .backbones import SOLD2Net
from .sold2_detector import LineSegmentDetectionModule, line_map_to_segments, prob_to_junctions, urls


class SOLD2(nn.Module):
    r"""Detect, describe and match line segments in grayscale images with SOLD2.

    ``sold2 = SOLD2()`` builds the network with its pretrained weights, in eval mode, and ``outputs = sold2(images)``
    takes grayscale images ``(B, 1, H, W)`` in :math:`[0, 1]`, with ``H`` and ``W`` multiples of 8, and returns a dict:
    ``"line_segments"``, one ``(N, 2, 2)`` tensor of ``(row, col)`` endpoints in pixels per image, the
    ``"junction_heatmap"`` and ``"line_heatmap"``, ``(B, H, W)`` each, and the ``"dense_desc"`` map
    ``(B, 128, H / 4, W / 4)``. ``matches = sold2.match(line_seg1, line_seg2, desc1, desc2)`` then matches the
    segments of two images by their descriptors sampled along each segment, with ``desc1`` and ``desc2`` the
    ``(1, 128, H / 4, W / 4)`` maps of one image each, and returns for each segment of ``line_seg1`` the index of its
    match in ``line_seg2``, or ``-1``. To detect segments only, use :class:`SOLD2_detector` instead, which builds no
    descriptor head. This is based on the original code from the paper "SOLD²: Self-supervised Occlusion-aware Line
    Detector and Descriptor". See :cite:`SOLD22021` for more details.

    Convention:
        - :meth:`forward` takes the input and returns the heatmaps and the ``(row, col)`` line segments of
          :class:`~kornia.feature.SOLD2_detector`, whose Convention block gives their frame and order, and also
          ``dense_desc``, ``(B, 128, H / 4, W / 4)``, which is not normalized.
        - ``match`` takes segments as :meth:`forward` returns them and the descriptor map of one image each. It
          returns an int64 tensor ``(N_1,)``: for each segment of ``line_seg1``, the index of its match in
          ``line_seg2``, or ``-1``. It is int64 also when either set of segments is empty: an empty tensor when
          ``line_seg1`` is, ``-1`` for every segment of ``line_seg1`` when ``line_seg2`` is.
        - Known defects: on CPU, ``match`` passes float16 and bfloat16 descriptor maps to torch's ``grid_sample``
          without an upcast: the half-precision kernel can return wrong values, NaN or overflow, and where torch has
          none the call raises (`#5810 <https://github.com/kornia/kornia/issues/5810>`_).

    Args:
        pretrained: If True, download and set pretrained weights to the model, and return it in eval mode; otherwise
            the weights are random and the model is in training mode.
        config: a ``kornia.feature.sold2.structures.DetectorCfg``, or a dict of its fields, specifying parameters.
            None will load the default parameters, which are tuned for images in the range 400~800 px.
            The model works on a copy with ``use_descriptor`` set to True; the object passed in is not modified.

    Returns:
        The raw junction and line heatmaps, the semi-dense descriptor map,
        as well as the list of detected line segments (ij coordinates convention).

    See Also:
        - :class:`SOLD2_detector`: the same line detection, without the descriptor map or matching.

    Example:
        >>> images = torch.rand(2, 1, 64, 64)
        >>> sold2 = SOLD2()
        >>> outputs = sold2(images)
        >>> line_seg1 = outputs["line_segments"][0]
        >>> line_seg2 = outputs["line_segments"][1]
        >>> desc1 = outputs["dense_desc"][0]
        >>> desc2 = outputs["dense_desc"][1]
        >>> matches = sold2.match(line_seg1, line_seg2, desc1[None], desc2[None])

    """

    def __init__(self, pretrained: bool = True, config: Optional[Union[DetectorCfg, Dict[str, Any]]] = None) -> None:
        if isinstance(config, dict):
            config = dict_to_dataclass(cast(Dict[str, Any], config), DetectorCfg)
        super().__init__()
        # Initialize some parameters
        # Copy so the descriptor head flag never leaks into the caller's object.
        self.config = copy.deepcopy(config) if config is not None else DetectorCfg()
        self.config.use_descriptor = True  # Only difference to SOLD2_detector DetectorCfg
        self.grid_size = self.config.grid_size
        self.junc_detect_thresh = self.config.detection_thresh
        self.max_num_junctions = self.config.max_num_junctions

        # Load the pre-trained model
        self.model = SOLD2Net(dataclass_to_dict(self.config))
        if pretrained:
            pretrained_dict = load_state_dict_from_url(urls["wireframe"], map_location=torch.device("cpu"))
            state_dict = self.adapt_state_dict(pretrained_dict["model_state_dict"])
            self.model.load_state_dict(state_dict)

        # Initialize the line detector
        self.line_detector = LineSegmentDetectionModule(self.config.line_detector_cfg)

        # Initialize the line matcher
        self.line_matcher = WunschLineMatcher(self.config.line_matcher_cfg)
        if pretrained:
            self.eval()

    def forward(self, img: torch.Tensor) -> Dict[str, Any]:
        """Run forward.

        Args:
            img: batched images with shape ``(B, 1, H, W)``.

        Returns:
            line_segments: a list of ``B`` tensors ``(N, 2, 2)``, the N line segments of each image.
            junction_heatmap: raw junction heatmap of shape ``(B, H, W)``.
            line_heatmap: raw line heatmap of shape ``(B, H, W)``.
            dense_desc: the semi-dense descriptor map of shape ``(B, 128, H/4, W/4)``.

        """
        KORNIA_CHECK_SHAPE(img, ["B", "1", "H", "W"])
        outputs = {}

        # Forward pass of the CNN backbone
        net_outputs = self.model(img)
        outputs["junction_heatmap"] = net_outputs["junctions"]
        outputs["line_heatmap"] = net_outputs["heatmap"]
        outputs["dense_desc"] = net_outputs["descriptors"]

        # Loop through all images
        lines = []
        for junc_prob, heatmap in zip(net_outputs["junctions"], net_outputs["heatmap"]):
            # Get the junctions
            junctions = prob_to_junctions(junc_prob, self.grid_size, self.junc_detect_thresh, self.max_num_junctions)

            # Run the line detector
            line_map, junctions, _ = self.line_detector.detect(junctions, heatmap)
            lines.append(line_map_to_segments(junctions, line_map))
        outputs["line_segments"] = lines

        return outputs

    def match(
        self, line_seg1: torch.Tensor, line_seg2: torch.Tensor, desc1: torch.Tensor, desc2: torch.Tensor
    ) -> torch.Tensor:
        """Find the best matches between two sets of line segments and their corresponding descriptors.

        Each descriptor map is sampled in its own dtype, so the segments may be in another floating-point dtype.

        Args:
            line_seg1: list of line segments in image 1, with shape [num_lines, 2, 2].
            line_seg2: list of line segments in image 2, with shape [num_lines, 2, 2].
            desc1: semi-dense descriptor map of image 1, with shape [1, 128, H/4, W/4].
            desc2: semi-dense descriptor map of image 2, with shape [1, 128, H/4, W/4].

        Returns:
            An integer tensor of size [num_lines1] indicating the index in line_seg2 of the matched line,
            for each line in line_seg1. -1 means that the line is not matched.

        """
        return self.line_matcher(line_seg1, line_seg2, desc1, desc2)

    def adapt_state_dict(self, state_dict: Dict[str, Any]) -> Dict[str, Any]:
        """Adapt pretrained checkpoint keys to this module implementation.

        Args:
            state_dict: Checkpoint state dictionary whose keys are adapted to this module layout.

        Returns:
            State dictionary with checkpoint keys renamed or removed so it can be loaded by the current module.
        """
        del state_dict["w_junc"]
        del state_dict["w_heatmap"]
        del state_dict["w_desc"]
        state_dict["heatmap_decoder.conv_block_lst.2.0.weight"] = state_dict["heatmap_decoder.conv_block_lst.2.weight"]
        state_dict["heatmap_decoder.conv_block_lst.2.0.bias"] = state_dict["heatmap_decoder.conv_block_lst.2.bias"]
        del state_dict["heatmap_decoder.conv_block_lst.2.weight"]
        del state_dict["heatmap_decoder.conv_block_lst.2.bias"]
        return state_dict


class WunschLineMatcher(nn.Module):
    """Class matching two sets of line segments with the Needleman-Wunsch algorithm.

    TODO: move it later in kornia.feature.matching
    """

    def __init__(self, config: Optional[LineMatcherCfg] = None) -> None:
        super().__init__()
        # Initialize the parameters
        if config is None:
            config = LineMatcherCfg()
        self.config = config
        self.cross_check = self.config.cross_check
        self.num_samples = self.config.num_samples
        self.min_dist_pts = self.config.min_dist_pts
        self.top_k_candidates = self.config.top_k_candidates
        self.grid_size = self.config.grid_size
        self.line_score = self.config.line_score

    def forward(
        self, line_seg1: torch.Tensor, line_seg2: torch.Tensor, desc1: torch.Tensor, desc2: torch.Tensor
    ) -> torch.Tensor:
        """Find the best matches between two sets of line segments and their corresponding descriptors."""
        KORNIA_CHECK_SHAPE(line_seg1, ["N", "2", "2"])
        KORNIA_CHECK_SHAPE(line_seg2, ["N", "2", "2"])
        KORNIA_CHECK_SHAPE(desc1, ["B", "D", "H", "H"])
        KORNIA_CHECK_SHAPE(desc2, ["B", "D", "H", "H"])
        device = desc1.device
        img_size1 = (desc1.shape[2] * self.grid_size, desc1.shape[3] * self.grid_size)
        img_size2 = (desc2.shape[2] * self.grid_size, desc2.shape[3] * self.grid_size)

        # Default case when an image has no lines
        if len(line_seg1) == 0:
            return torch.empty(0, dtype=torch.long, device=device)
        if len(line_seg2) == 0:
            return -torch.ones(len(line_seg1), dtype=torch.long, device=device)

        # Sample points regularly along each line
        line_points1, valid_points1 = self.sample_line_points(line_seg1)
        line_points2, valid_points2 = self.sample_line_points(line_seg2)
        line_points1 = line_points1.reshape(-1, 2)
        line_points2 = line_points2.reshape(-1, 2)

        # Extract the descriptors for each point. F.grid_sample needs the grid in the dtype of the map it samples,
        # and the segments need not share it (SOLD2.forward returns float32 segments whatever the model dtype).
        grid1 = keypoints_to_grid(line_points1, img_size1).to(desc1.dtype)
        grid2 = keypoints_to_grid(line_points2, img_size2).to(desc2.dtype)
        desc1 = F.normalize(F.grid_sample(desc1, grid1, align_corners=False)[0, :, :, 0], dim=0)
        desc2 = F.normalize(F.grid_sample(desc2, grid2, align_corners=False)[0, :, :, 0], dim=0)

        # Precompute the distance between line points for every pair of lines
        # Assign a score of -1 for invalid points
        scores = desc1.t() @ desc2
        scores[~valid_points1.flatten()] = -1
        scores[:, ~valid_points2.flatten()] = -1
        scores = scores.reshape(len(line_seg1), self.num_samples, len(line_seg2), self.num_samples)
        scores = scores.permute(0, 2, 1, 3)
        # scores.shape = (n_lines1, n_lines2, num_samples, num_samples)

        # Pre-filter the line candidates and find the best match for each line
        matches = self.filter_and_match_lines(scores)

        # [Optionally] filter matches with mutual nearest neighbor filtering
        if self.cross_check:
            matches2 = self.filter_and_match_lines(scores.permute(1, 0, 3, 2))
            mutual = matches2[matches] == torch.arange(len(line_seg1), device=device)
            matches[~mutual] = -1

        return matches

    def sample_line_points(self, line_seg: torch.Tensor) -> Tuple[torch.Tensor, torch.Tensor]:
        """Regularly sample points along each line segments, with a minimal distance between each point.

        Pad the remaining points.

        Args:
            line_seg: an Nx2x2 torch.Tensor.

        Returns:
            line_points: an N x num_samples x 2 torch.Tensor.
            valid_points: a boolean N x num_samples torch.Tensor.
        """
        _N, _, _ = line_seg.shape
        M = self.num_samples
        dev = line_seg.device

        lengths = torch.norm(line_seg[:, 0] - line_seg[:, 1], dim=1)
        num_pts = torch.clamp((lengths / self.min_dist_pts).floor().int(), min=2, max=M)  # (N,)

        orig = line_seg[:, 0].unsqueeze(1)
        dirs = (line_seg[:, 1] - line_seg[:, 0]).unsqueeze(1)
        idx = torch.arange(M, device=dev).unsqueeze(0)
        denom = (num_pts - 1).unsqueeze(1)
        alpha = idx / denom
        pts = orig + dirs * alpha.unsqueeze(-1)
        valid = idx < num_pts.unsqueeze(1)
        pts = pts.masked_fill(~valid.unsqueeze(-1), 0.0)

        return pts, valid

    def filter_and_match_lines(self, scores: torch.Tensor) -> torch.Tensor:
        """Use scores to keep the top k best lines.

        Compute the Needleman- Wunsch algorithm on each candidate pairs, and keep the highest score.

        Args:
            scores: a (N, M, n, n) torch.Tensor containing the pairwise scores
                    of the elements to match.

        Returns:
            matches: a (N) torch.Tensor containing the indices of the best match
        """
        KORNIA_CHECK_SHAPE(scores, ["M", "N", "n", "n"])

        # Pre-filter the pairs and keep the top k best candidate lines
        line_scores1 = scores.max(3)[0]
        valid_scores1 = line_scores1 != -1
        line_scores1 = (line_scores1 * valid_scores1).sum(2) / valid_scores1.sum(2)
        line_scores2 = scores.max(2)[0]
        valid_scores2 = line_scores2 != -1
        line_scores2 = (line_scores2 * valid_scores2).sum(2) / valid_scores2.sum(2)
        line_scores = (line_scores1 + line_scores2) / 2
        topk_lines = torch.argsort(line_scores, dim=1)[:, -self.top_k_candidates :]
        # topk_lines.shape = (n_lines1, top_k_candidates)

        top_scores = torch.take_along_dim(scores, topk_lines[:, :, None, None], dim=1)

        # Consider the reversed line segments as well
        top_scores = torch.cat([top_scores, torch.flip(top_scores, dims=[-1])], 1)

        # Compute the line distance matrix with Needleman-Wunsch algo and
        # retrieve the closest line neighbor
        n_lines1, top2k, n, m = top_scores.shape
        top_scores = top_scores.reshape((n_lines1 * top2k, n, m))
        nw_scores = self.needleman_wunsch(top_scores)
        nw_scores = nw_scores.reshape(n_lines1, top2k)
        matches = torch.remainder(torch.argmax(nw_scores, dim=1), top2k // 2)
        return topk_lines[torch.arange(n_lines1), matches]

    def needleman_wunsch(self, scores: torch.Tensor) -> torch.Tensor:
        """Batched implementation of the Needleman-Wunsch algorithm.

        The cost of the InDel operation is set to 0 by subtracting the gap
        penalty to the scores.

        Args:
            scores: a (B, N, M) torch.Tensor containing the pairwise scores
                    of the elements to match.
        """
        KORNIA_CHECK_SHAPE(scores, ["B", "N", "M"])
        # Recalibrate the scores to get a gap score of 0
        gap = 0.1
        B, N, M = scores.shape
        dp = torch.zeros(B, N + 1, M + 1, device=scores.device, dtype=scores.dtype)
        S = scores - gap
        for k in range(2, N + M + 1):
            i_min = max(1, k - M)
            i_max = min(N, k - 1)
            i = torch.arange(i_min, i_max + 1, device=scores.device)
            j = k - i
            up = dp[:, i - 1, j]
            left = dp[:, i, j - 1]
            diag = dp[:, i - 1, j - 1] + S[:, i - 1, j - 1]
            dp[:, i, j] = torch.max(torch.max(up, left), diag)
        return dp[:, -1, -1]


def keypoints_to_grid(keypoints: torch.Tensor, img_size: Tuple[int, int]) -> torch.Tensor:
    """Convert a list of keypoints into a grid in [-1, 1]² that can be used in torch.nn.functional.grid_sample.

    The normalization is the reference implementation's ``keypoints * 2 / img_size - 1``
    (cvg/SOLD2, ``sold2/misc/geometry_utils.py``), the convention the pretrained weights were trained
    with. It pairs with the ``align_corners=False`` sampling in :class:`WunschLineMatcher`: image pixel
    ``p`` on an axis of size ``S`` lands on descriptor-map coordinate ``p * S_desc / S - 0.5``, which is
    ``p / grid_size - 0.5`` whatever ``img_size`` is, so ``p = grid_size * (i + 0.5)`` reads descriptor
    ``i`` exactly. It is deliberately not
    :func:`kornia.geometry.conversions.normalize_pixel_coordinates`, whose corner-aligned mapping
    ``2p / (S - 1) - 1`` drifts by ``p / (S - 1)`` image pixels toward the far edge under this sampler.
    It is also not the half-pixel mapping ``(2p + 1) / S - 1``, from which it differs by a constant
    half image pixel at every ``p``. Changing either the formula or the ``align_corners`` of the
    sampler would break compatibility with the pretrained weights (compare
    :class:`~kornia.feature.XFeat` and its ``normgrid``, which keep their own trained pairing).

    Args:
        keypoints: a torch.Tensor [N, 2] of N keypoints (ij coordinates convention).
        img_size: the original image size (H, W)

    """
    KORNIA_CHECK_SHAPE(keypoints, ["N", "2"])
    n_points = len(keypoints)
    return (keypoints * 2.0 / torch.tensor(img_size, device=keypoints.device) - 1.0)[:, [1, 0]].view(-1, n_points, 1, 2)


def batched_linspace(start: torch.Tensor, end: torch.Tensor, step: int, dim: int) -> torch.Tensor:
    """Batch version of torch.linspace (similar to the numpy one)."""
    intervals = ((end - start) / (step - 1)).unsqueeze(dim)
    broadcast_size = [1] * len(intervals.shape)
    broadcast_size[dim] = step
    samples = torch.arange(step, dtype=torch.float, device=start.device).reshape(broadcast_size)
    return start.unsqueeze(dim) + samples * intervals
