# Copyright 2019-present NAVER Corp.
# CC BY-NC-SA 3.0
# Available only for non-commercial use

import pdb

import numpy as np
import torch
import torch.nn as nn
import torch.nn.functional as F


""" Different samplers, each specifying how to sample pixels for the AP loss.
"""


class FullSampler(nn.Module):
    """ all pixels are selected
        - feats: keypoint descriptors
        - confs: reliability values
    """
    def __init__(self):
        nn.Module.__init__(self)
        self.mode = 'bilinear'
        self.padding = 'zeros'

    @staticmethod
    def _aflow_to_grid(aflow):
        H, W = aflow.shape[2:]
        grid = aflow.permute(0,2,3,1).clone()
        grid[:,:,:,0] *= 2/(W-1)
        grid[:,:,:,1] *= 2/(H-1)
        grid -= 1
        grid[torch.isnan(grid)] = 9e9 # invalids
        return grid
    
    def _warp(self, feats, confs, aflow):
        if isinstance(aflow, tuple): return aflow # result was precomputed
        feat1, feat2 = feats
        conf1, conf2 = confs if confs else (None,None)
    
        B, two, H, W = aflow.shape
        D = feat1.shape[1]
        assert feat1.shape == feat2.shape == (B, D, H, W) # D = 128, B = batch
        assert conf1.shape == conf2.shape == (B, 1, H, W) if confs else True

        # warp img2 to img1
        grid = self._aflow_to_grid(aflow)
        ones2 = feat2.new_ones(feat2[:,0:1].shape)
        feat2to1 = F.grid_sample(feat2, grid, mode=self.mode, padding_mode=self.padding)
        mask2to1 = F.grid_sample(ones2, grid, mode='nearest', padding_mode='zeros')
        conf2to1 = F.grid_sample(conf2, grid, mode=self.mode, padding_mode=self.padding) \
                   if confs else None
        return feat2to1, mask2to1.byte(), conf2to1

    def _warp_positions(self, aflow):
        B, two, H, W = aflow.shape
        assert two == 2
        
        Y = torch.arange(H, device=aflow.device)
        X = torch.arange(W, device=aflow.device)
        XY = torch.stack(torch.meshgrid(Y,X)[::-1], dim=0)
        XY = XY[None].expand(B, 2, H, W).float()
        
        grid = self._aflow_to_grid(aflow)
        XY2 = F.grid_sample(XY, grid, mode='bilinear', padding_mode='zeros')
        return XY, XY2



class SubSampler (FullSampler):
    """ pixels are selected in an uniformly spaced grid
    """
    def __init__(self, border, subq, subd, perimage=False):
        FullSampler.__init__(self)
        assert subq % subd == 0, 'subq must be multiple of subd'
        self.sub_q = subq
        self.sub_d = subd
        self.border = border
        self.perimage = perimage

    def __repr__(self):
        return "SubSampler(border=%d, subq=%d, subd=%d, perimage=%d)" % (
            self.border, self.sub_q, self.sub_d, self.perimage)

    def __call__(self, feats, confs, aflow):
        feat1, conf1 = feats[0], (confs[0] if confs else None)
        # warp with optical flow in img1 coords
        feat2, mask2, conf2 = self._warp(feats, confs, aflow)
        
        # subsample img1
        slq = slice(self.border, -self.border or None, self.sub_q)
        feat1 = feat1[:, :, slq, slq]
        conf1 = conf1[:, :, slq, slq] if confs else None
        # subsample img2
        sld = slice(self.border, -self.border or None, self.sub_d)
        feat2 = feat2[:, :, sld, sld]
        mask2 = mask2[:, :, sld, sld]
        conf2 = conf2[:, :, sld, sld] if confs else None
        
        B, D, Hq, Wq = feat1.shape
        B, D, Hd, Wd = feat2.shape
        
        # compute gt
        if self.perimage or self.sub_q != self.sub_d:
            # compute ground-truth by comparing pixel indices
            f = feats[0][0:1,0] if self.perimage else feats[0][:,0]
            idxs = torch.arange(f.numel(), dtype=torch.int64, device=feat1.device).view(f.shape)
            idxs1 = idxs[:, slq, slq].reshape(-1,Hq*Wq)
            idxs2 = idxs[:, sld, sld].reshape(-1,Hd*Wd)
            if self.perimage:
                gt = (idxs1[0].view(-1,1) == idxs2[0].view(1,-1))
                gt = gt[None,:,:].expand(B, Hq*Wq, Hd*Wd)
            else :
                gt = (idxs1.view(-1,1) == idxs2.view(1,-1)) 
        else:
            gt = torch.eye(feat1[:,0].numel(), dtype=torch.uint8, device=feat1.device) # always binary for AP loss
        
        # compute all images together
        queries  =  feat1.reshape(B,D,-1) # B x D x (Hq x Wq)
        database =  feat2.reshape(B,D,-1) # B x D x (Hd x Wd)
        if self.perimage:
            queries  =  queries.transpose(1,2) # B x (Hd x Wd) x D
            scores = torch.bmm(queries, database) # B x (Hq x Wq) x (Hd x Wd)
        else:
            queries  =  queries .transpose(1,2).reshape(-1,D) # (B x Hq x Wq) x D
            database =  database.transpose(1,0).reshape(D,-1) # D x (B x Hd x Wd)
            scores = torch.matmul(queries, database) # (B x Hq x Wq) x (B x Hd x Wd)

        # compute reliability
        qconf = (conf1 + conf2)/2 if confs else None

        assert gt.shape == scores.shape
        return scores, gt, mask2, qconf



class NghSampler (FullSampler):
    """ all pixels in a small neighborhood
    """
    def __init__(self, ngh, subq=1, subd=1, ignore=1, border=None):
        FullSampler.__init__(self)
        assert 0 <= ignore < ngh
        self.ngh = ngh
        self.ignore = ignore
        assert subd <= ngh
        self.sub_q = subq
        self.sub_d = subd
        if border is None: border = ngh
        assert border >= ngh, 'border has to be larger than ngh'
        self.border = border

    def __repr__(self):
        return "NghSampler(ngh=%d, subq=%d, subd=%d, ignore=%d, border=%d)" % (
            self.ngh, self.sub_q, self.sub_d, self.ignore, self.border)

    def trans(self, arr, i, j):
        s = lambda i: slice(self.border+i, i-self.border or None, self.sub_q)
        return arr[:,:,s(j),s(i)]

    def __call__(self, feats, confs, aflow):
        feat1, conf1 = feats[0], (confs[0] if confs else None)
        # warp with optical flow in img1 coords
        feat2, mask2, conf2 = self._warp(feats, confs, aflow)
        
        qfeat = self.trans(feat1,0,0)
        qconf = (self.trans(conf1,0,0) + self.trans(conf2,0,0)) / 2 if confs else None
        mask2 = self.trans(mask2,0,0)
        scores_at = lambda i,j: (qfeat * self.trans(feat2,i,j)).sum(dim=1)
        
        # compute scores for all neighbors
        B, D = feat1.shape[:2]
        min_d = self.ignore**2
        max_d = self.ngh**2
        rad = (self.ngh//self.sub_d) * self.ngh # make an integer multiple
        negs = []
        offsets = []
        for j in range(-rad, rad+1, self.sub_d):
          for i in range(-rad, rad+1, self.sub_d):
            if not(min_d < i*i + j*j <= max_d): 
                continue # out of scope
            offsets.append((i,j)) # Note: this list is just for debug
            negs.append( scores_at(i,j) )
        
        scores = torch.stack([scores_at(0,0)] + negs, dim=-1)
        gt = scores.new_zeros(scores.shape, dtype=torch.uint8)
        gt[..., 0] = 1 # only the center point is positive

        return scores, gt, mask2, qconf



class FarNearSampler (FullSampler):
    """ Sample pixels from *both* a small neighborhood *and* far-away pixels.
        
    How it works?
        1) Queries are sampled from img1,
            - at least `border` pixels from borders and 
            - on a grid with step = `subq`
            
        2) Close database pixels 
            - from the corresponding image (img2),
            - within a `ngh` distance radius 
            - on a grid with step = `subd_ngh`
            - ignored if distance to query is >0 and <=`ignore`
            
        3) Far-away database pixels from ,
            - from all batch images in `img2`
            - at least `border` pixels from borders
            - on a grid with step = `subd_far`
    """
    def __init__(self, subq, ngh, subd_ngh, subd_far, border=None, ignore=1, 
                       maxpool_ngh=False ):
        FullSampler.__init__(self)
        border = border or ngh
        assert ignore < ngh < subd_far, 'neighborhood needs to be smaller than far step'
        self.close_sampler = NghSampler(ngh=ngh, subq=subq, subd=subd_ngh, 
                ignore=not(maxpool_ngh), border=border)
        self.faraway_sampler = SubSampler(border=border, subq=subq, subd=subd_far)
        self.maxpool_ngh = maxpool_ngh

    def __repr__(self):
        c,f = self.close_sampler, self.faraway_sampler
        res = "FarNearSampler(subq=%d, ngh=%d" % (c.sub_q, c.ngh)
        res += ", subd_ngh=%d, subd_far=%d" % (c.sub_d, f.sub_d)
        res += ", border=%d, ign=%d" % (f.border, c.ignore)
        res += ", maxpool_ngh=%d" % self.maxpool_ngh
        return res+')'

    def __call__(self, feats, confs, aflow):
        # warp with optical flow in img1 coords
        aflow = self._warp(feats, confs, aflow)

        # sample ngh pixels
        scores1, gt1, msk1, conf1 = self.close_sampler(feats, confs, aflow)
        scores1, gt1 = scores1.view(-1,scores1.shape[-1]), gt1.view(-1,gt1.shape[-1])
        if self.maxpool_ngh:
            # we consider all scores from ngh as potential positives
            scores1, self._cached_maxpool_ngh = scores1.max(dim=1,keepdim=True)
            gt1 = gt1[:, 0:1]

        # sample far pixels
        scores2, gt2, msk2, conf2 = self.faraway_sampler(feats, confs, aflow)
        # assert (msk1 == msk2).all()
        # assert (conf1 == conf2).all()

        return (torch.cat((scores1,scores2),dim=1), 
                torch.cat((gt1,    gt2),    dim=1), 
                msk1, conf1 if confs else None)


class NghSampler2 (nn.Module):
    """ Similar to NghSampler, but doesnt warp the 2nd image.
    Distance to GT =>  0 ... pos_d ... neg_d ... ngh
    Pixel label    =>  + + + + + + 0 0 - - - - - - -
    
    Subsample on query side: if > 0, regular grid
                                < 0, random points 
    In both cases, the number of query points is = W*H/subq**2
    """
    def __init__(self, ngh, subq=1, subd=1, pos_d=0, neg_d=2, border=None,
                       maxpool_pos=True, subd_neg=0):
        nn.Module.__init__(self)
        assert 0 <= pos_d < neg_d <= (ngh if ngh else 99)
        self.ngh = ngh
        self.pos_d = pos_d
        self.neg_d = neg_d
        assert subd <= ngh or ngh == 0
        assert subq != 0
        self.sub_q = subq
        self.sub_d = subd
        self.sub_d_neg = subd_neg
        if border is None: border = ngh
        assert border >= ngh, 'border has to be larger than ngh'
        self.border = border
        self.maxpool_pos = maxpool_pos
        self.precompute_offsets()

    def precompute_offsets(self):
        pos_d2 = self.pos_d**2
        neg_d2 = self.neg_d**2
        rad2 = self.ngh**2
        rad = (self.ngh//self.sub_d) * self.ngh # make an integer multiple
        pos = []
        neg = []
        for j in range(-rad, rad+1, self.sub_d):
          for i in range(-rad, rad+1, self.sub_d):
            d2 = i*i + j*j
            if d2 <= pos_d2:
                pos.append( (i,j) )
            elif neg_d2 <= d2 <= rad2: 
                neg.append( (i,j) )

        self.register_buffer('pos_offsets', torch.LongTensor(pos).view(-1,2).t())
        self.register_buffer('neg_offsets', torch.LongTensor(neg).view(-1,2).t())

    def gen_grid(self, step, aflow):
        B, two, H, W = aflow.shape
        dev = aflow.device
        b1 = torch.arange(B, device=dev)
        if step > 0:
            # regular grid
            x1 = torch.arange(self.border, W-self.border, step, device=dev)
            y1 = torch.arange(self.border, H-self.border, step, device=dev)
            H1, W1 = len(y1), len(x1)
            x1 = x1[None,None,:].expand(B,H1,W1).reshape(-1)
            y1 = y1[None,:,None].expand(B,H1,W1).reshape(-1)
            b1 = b1[:,None,None].expand(B,H1,W1).reshape(-1)
            shape = (B, H1, W1)
        else:
            # randomly spread
            n = (H - 2*self.border) * (W - 2*self.border) // step**2
            x1 = torch.randint(self.border, W-self.border, (n,), device=dev)
            y1 = torch.randint(self.border, H-self.border, (n,), device=dev)
            x1 = x1[None,:].expand(B,n).reshape(-1)
            y1 = y1[None,:].expand(B,n).reshape(-1)
            b1 = b1[:,None].expand(B,n).reshape(-1)
            shape = (B, n)
        return b1, y1, x1, shape

    def forward(self, feats, confs, aflow, **kw):
        B, two, H, W = aflow.shape
        assert two == 2
        feat1, conf1 = feats[0], (confs[0] if confs else None)
        feat2, conf2 = feats[1], (confs[1] if confs else None)
        
        # positions in the first image
        b1, y1, x1, shape = self.gen_grid(self.sub_q, aflow)

        # sample features from first image
        feat1 = feat1[b1, :, y1, x1]
        qconf = conf1[b1, :, y1, x1].view(shape) if confs else None
        
        #sample GT from second image
        b2 = b1
        xy2 = (aflow[b1, :, y1, x1] + 0.5).long().t()
        mask = (0 <= xy2[0]) * (0 <= xy2[1]) * (xy2[0] < W) * (xy2[1] < H)
        mask = mask.view(shape)
        
        def clamp(xy):
            torch.clamp(xy[0], 0, W-1, out=xy[0])
            torch.clamp(xy[1], 0, H-1, out=xy[1])
            return xy
        
        # compute positive scores
        xy2p = clamp(xy2[:,None,:] + self.pos_offsets[:,:,None])
        pscores = (feat1[None,:,:] * feat2[b2, :, xy2p[1], xy2p[0]]).sum(dim=-1).t()
#        xy1p = clamp(torch.stack((x1,y1))[:,None,:] + self.pos_offsets[:,:,None])
#        grid = FullSampler._aflow_to_grid(aflow)
#        feat2p = F.grid_sample(feat2, grid, mode='bilinear', padding_mode='border')
#        pscores = (feat1[None,:,:] * feat2p[b1,:,xy1p[1], xy1p[0]]).sum(dim=-1).t()
        if self.maxpool_pos:
            pscores, pos = pscores.max(dim=1, keepdim=True)
            if confs: 
                sel = clamp(xy2 + self.pos_offsets[:,pos.view(-1)])
                qconf = (qconf + conf2[b2, :, sel[1], sel[0]].view(shape))/2
        
        # compute negative scores
        xy2n = clamp(xy2[:,None,:] + self.neg_offsets[:,:,None])
        nscores = (feat1[None,:,:] * feat2[b2, :, xy2n[1], xy2n[0]]).sum(dim=-1).t()

        if self.sub_d_neg:
            # add distractors from a grid
            b3, y3, x3, _ = self.gen_grid(self.sub_d_neg, aflow)
            distractors = feat2[b3, :, y3, x3]
            dscores = torch.matmul(feat1, distractors.t())
            del distractors
            
            # remove scores that corresponds to positives or nulls
            dis2 = (x3 - xy2[0][:,None])**2 + (y3 - xy2[1][:,None])**2
            dis2 += (b3 != b2[:,None]).long() * self.neg_d**2
            dscores[dis2 < self.neg_d**2] = 0
            
            scores = torch.cat((pscores, nscores, dscores), dim=1)
        else:
            # concat everything
            scores = torch.cat((pscores, nscores), dim=1)

        gt = scores.new_zeros(scores.shape, dtype=torch.uint8)
        gt[:, :pscores.shape[1]] = 1

        return scores, gt, mask, qconf