# 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 math
from typing import Tuple

import torch


def arange_sequence(ranges: torch.Tensor) -> torch.Tensor:
    """Return a sequence of the ranges specified by the argument.

    Example:
    [2, 5, 1, 2] -> [0, 1, 0, 1, 2, 3, 4, 0, 0, 1]

    """
    maxcnt = torch.max(ranges).item()
    numuni = ranges.shape[0]
    complete_ranges = torch.arange(maxcnt, device=ranges.device).unsqueeze(0).expand(numuni, -1)

    return complete_ranges[complete_ranges < ranges.unsqueeze(-1)]


def dist_matrix(d1: torch.Tensor, d2: torch.Tensor, is_normalized: bool = False) -> torch.Tensor:
    """Distance between two tensors."""
    if is_normalized:
        return 2 - 2.0 * d1 @ d2.t()
    x_norm = (d1**2).sum(1).view(-1, 1)
    y_norm = (d2**2).sum(1).view(1, -1)
    # print(x_norm, y_norm)
    return x_norm + y_norm - 2.0 * d1 @ d2.t()
    # distmat[torch.isnan(distmat)] = np.inf


def orientation_diff(o1: torch.Tensor, o2: torch.Tensor) -> torch.Tensor:
    """Orientation difference between two tensors."""
    diff = o2 - o1
    diff = torch.where(diff < -180, diff + 360, diff)
    return torch.where(diff >= 180, diff - 360, diff)


def piecewise_arange(piecewise_idxer: torch.Tensor) -> torch.Tensor:
    """Count repeated indices.

    Example:
    [0, 0, 0, 3, 3, 3, 3, 1, 1, 2] -> [0, 1, 2, 0, 1, 2, 3, 0, 1, 0]
    """
    _, counts = torch.unique_consecutive(piecewise_idxer, return_counts=True)
    return arange_sequence(counts)


def batch_2x2_inv(m: torch.Tensor, check_dets: bool = False) -> torch.Tensor:
    """Return inverse of batch of 2x2 matrices."""
    a = m[..., 0, 0]
    b = m[..., 0, 1]
    c = m[..., 1, 0]
    d = m[..., 1, 1]
    minv = torch.empty_like(m)
    det = a * d - b * c
    if check_dets:
        det[torch.abs(det) < 1e-10] = 1e-10
    minv[..., 0, 0] = d
    minv[..., 1, 1] = a
    minv[..., 0, 1] = -b
    minv[..., 1, 0] = -c
    return minv / det.unsqueeze(-1).unsqueeze(-1)


def batch_2x2_Q(m: torch.Tensor) -> torch.Tensor:
    """Return Q of batch of 2x2 matrices."""
    return batch_2x2_inv(batch_2x2_invQ(m), check_dets=True)


def batch_2x2_invQ(m: torch.Tensor) -> torch.Tensor:
    """Return inverse Q of batch of 2x2 matrices."""
    return m @ m.transpose(-1, -2)


def batch_2x2_det(m: torch.Tensor) -> torch.Tensor:
    """Return determinant of batch of 2x2 matrices."""
    a = m[..., 0, 0]
    b = m[..., 0, 1]
    c = m[..., 1, 0]
    d = m[..., 1, 1]
    return a * d - b * c


def batch_2x2_eigenvals(m: torch.Tensor, *, eps: float = 0.0) -> torch.Tensor:
    """Return eigenvalues of ``m @ m.mT`` for a batch of 2x2 matrices."""
    am = m[..., 0, 0]
    bm = m[..., 0, 1]
    cm = m[..., 1, 0]
    dm = m[..., 1, 1]

    a = am * am + bm * bm
    b = am * cm + bm * dm
    d = cm * cm + dm * dm

    trh = 0.5 * (a + d)
    diff = 0.5 * (a - d)

    # stable hypot
    sqrtdisc = torch.hypot(diff, b)

    min_val = eps if eps > 0 else 0.0
    e1 = (trh + sqrtdisc).clamp(min=min_val)
    e2 = (trh - sqrtdisc).clamp(min=min_val)
    return torch.stack([e1, e2], dim=-1)


def batch_2x2_ellipse(m: torch.Tensor, *, eps: float = 0.0) -> Tuple[torch.Tensor, torch.Tensor]:
    """Return Eigenvalues and Eigenvectors of batch of 2x2 matrices."""
    eigenvals = batch_2x2_eigenvals(m, eps=eps)
    am = m[..., 0, 0]
    bm = m[..., 0, 1]
    cm = m[..., 1, 0]
    dm = m[..., 1, 1]

    a = am * am + bm * bm
    b = am * cm + bm * dm
    d = cm * cm + dm * dm

    theta = 0.5 * torch.atan2(2.0 * b, a - d)
    c = torch.cos(theta)
    s = torch.sin(theta)

    ev1 = torch.stack([c, s], dim=-1)  # (...,2)
    ev2 = torch.stack([-s, c], dim=-1)  # orthogonal (...,2)
    eigenvecs = torch.stack([ev1, ev2], dim=-1)  # (...,2,2) columns are eigenvectors
    return eigenvals, eigenvecs


def draw_first_k_couples(k: int, rdims: torch.Tensor, dv: torch.device) -> torch.Tensor:
    """Return first k couples.

    Exhaustive search over the first n samples:
     * n(n+1)/2 = n2/2 + n/2 couples
    Max n for which we can exhaustively sample with k couples:
    * n2/2 + n/2 = k
    * n = sqrt(1/4 + 2k)-1/2 = (sqrt(8k+1)-1)/2

    The couples are ``(1, 0), (2, 0), (2, 1), (3, 0), ...``: the ``k`` distinct pairs ``(i, j)`` with ``j < i`` in
    that order, so the residual couples after the complete rows continue with ``(n + 1, 0), (n + 1, 1), ...``. The
    reference AdaLAM implementation used the residual count as that first index, which repeated couples already
    drawn (8 of the default 128) instead of trying new ones. Each region then takes the indices modulo its number of
    samples in ``rdims``, so the couples stay distinct only in a region with enough samples for ``k`` distinct
    couples, at least 17 for ``k = 128``; in a smaller region they wrap, and can repeat or pair a sample with itself.
    """
    max_exhaustive_search = int(math.sqrt(2 * k + 0.25) - 0.5)
    # The strict lower triangle is already in pair order. Its size depends only on Python k, avoiding the
    # repeat_interleave output-size read, max().item(), and boolean compaction of GPU-generated row lengths.
    size = max_exhaustive_search + 2
    idx_sequence = torch.tril_indices(size, size, offset=-1, device=dv)[:, :k].t()
    return torch.remainder(idx_sequence.unsqueeze(-1), rdims)


def random_samples_indices(iters: int, rdims: torch.Tensor, dv: torch.device) -> torch.Tensor:
    """Randomly sample indices of torch.Tensor."""
    rands = torch.rand(size=(iters, 2, rdims.shape[0]), device=dv)
    scaled_rands = rands * (rdims - 1e-8).float()
    return scaled_rands.long()
