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


from PIL import Image

from tools import common
from tools.dataloader import norm_RGB
from nets.patchnet import *
from os import path

from extract import load_network, NonMaxSuppression, extract_multiscale

# Kapture is a pivot file format, based on text and binary files, used to describe SfM (Structure From Motion)
# and more generally sensor-acquired data
# it can be installed with
# pip install kapture
# for more information check out https://github.com/naver/kapture
import kapture
from kapture.io.records import get_image_fullpath
from kapture.io.csv import kapture_from_dir
from kapture.io.csv import get_feature_csv_fullpath, keypoints_to_file, descriptors_to_file
from kapture.io.features import get_keypoints_fullpath, keypoints_check_dir, image_keypoints_to_file
from kapture.io.features import get_descriptors_fullpath, descriptors_check_dir, image_descriptors_to_file
from kapture.io.csv import get_all_tar_handlers


def extract_kapture_keypoints(args):
    """
    Extract r2d2 keypoints and descritors to the kapture format directly
    """
    print('extract_kapture_keypoints...')
    with get_all_tar_handlers(args.kapture_root,
                              mode={kapture.Keypoints: 'a',
                                    kapture.Descriptors: 'a',
                                    kapture.GlobalFeatures: 'r',
                                    kapture.Matches: 'r'}) as tar_handlers:
        kdata = kapture_from_dir(args.kapture_root, None,
                                 skip_list=[kapture.GlobalFeatures,
                                            kapture.Matches,
                                            kapture.Points3d,
                                            kapture.Observations],
                                 tar_handlers=tar_handlers)

        assert kdata.records_camera is not None
        image_list = [filename for _, _, filename in kapture.flatten(kdata.records_camera)]
        if args.keypoints_type is None:
            args.keypoints_type = path.splitext(path.basename(args.model))[0]
            print(f'keypoints_type set to {args.keypoints_type}')
        if args.descriptors_type is None:
            args.descriptors_type = path.splitext(path.basename(args.model))[0]
            print(f'descriptors_type set to {args.descriptors_type}')

        if kdata.keypoints is not None and args.keypoints_type in kdata.keypoints \
                and kdata.descriptors is not None and args.descriptors_type in kdata.descriptors:
            print('detected already computed features of same keypoints_type/descriptors_type, resuming extraction...')
            image_list = [name
                          for name in image_list
                          if name not in kdata.keypoints[args.keypoints_type] or
                          name not in kdata.descriptors[args.descriptors_type]]

        if len(image_list) == 0:
            print('All features were already extracted')
            return
        else:
            print(f'Extracting r2d2 features for {len(image_list)} images')

        iscuda = common.torch_set_gpu(args.gpu)

        # load the network...
        net = load_network(args.model)
        if iscuda:
            net = net.cuda()

        # create the non-maxima detector
        detector = NonMaxSuppression(
            rel_thr=args.reliability_thr,
            rep_thr=args.repeatability_thr)

        if kdata.keypoints is None:
            kdata.keypoints = {}
        if kdata.descriptors is None:
            kdata.descriptors = {}

        if args.keypoints_type not in kdata.keypoints:
            keypoints_dtype = None
            keypoints_dsize = None
        else:
            keypoints_dtype = kdata.keypoints[args.keypoints_type].dtype
            keypoints_dsize = kdata.keypoints[args.keypoints_type].dsize
        if args.descriptors_type not in kdata.descriptors:
            descriptors_dtype = None
            descriptors_dsize = None
        else:
            descriptors_dtype = kdata.descriptors[args.descriptors_type].dtype
            descriptors_dsize = kdata.descriptors[args.descriptors_type].dsize

        for image_name in image_list:
            img_path = get_image_fullpath(args.kapture_root, image_name)
            print(f"\nExtracting features for {img_path}")
            img = Image.open(img_path).convert('RGB')
            W, H = img.size
            img = norm_RGB(img)[None]
            if iscuda:
                img = img.cuda()

            # extract keypoints/descriptors for a single image
            xys, desc, scores = extract_multiscale(net, img, detector,
                                                   scale_f=args.scale_f,
                                                   min_scale=args.min_scale,
                                                   max_scale=args.max_scale,
                                                   min_size=args.min_size,
                                                   max_size=args.max_size,
                                                   verbose=True)

            xys = xys.cpu().numpy()
            desc = desc.cpu().numpy()
            scores = scores.cpu().numpy()
            idxs = scores.argsort()[-args.top_k or None:]

            xys = xys[idxs]
            desc = desc[idxs]
            if keypoints_dtype is None or descriptors_dtype is None:
                keypoints_dtype = xys.dtype
                descriptors_dtype = desc.dtype

                keypoints_dsize = xys.shape[1]
                descriptors_dsize = desc.shape[1]

                kdata.keypoints[args.keypoints_type] = kapture.Keypoints('r2d2', keypoints_dtype, keypoints_dsize)
                kdata.descriptors[args.descriptors_type] = kapture.Descriptors('r2d2', descriptors_dtype,
                                                                               descriptors_dsize,
                                                                               args.keypoints_type, 'L2')
                keypoints_config_absolute_path = get_feature_csv_fullpath(kapture.Keypoints,
                                                                          args.keypoints_type,
                                                                          args.kapture_root)
                descriptors_config_absolute_path = get_feature_csv_fullpath(kapture.Descriptors,
                                                                            args.descriptors_type,
                                                                            args.kapture_root)
                keypoints_to_file(keypoints_config_absolute_path, kdata.keypoints[args.keypoints_type])
                descriptors_to_file(descriptors_config_absolute_path, kdata.descriptors[args.descriptors_type])
            else:
                assert kdata.keypoints[args.keypoints_type].dtype == xys.dtype
                assert kdata.descriptors[args.descriptors_type].dtype == desc.dtype
                assert kdata.keypoints[args.keypoints_type].dsize == xys.shape[1]
                assert kdata.descriptors[args.descriptors_type].dsize == desc.shape[1]
                assert kdata.descriptors[args.descriptors_type].keypoints_type == args.keypoints_type
                assert kdata.descriptors[args.descriptors_type].metric_type == 'L2'

            keypoints_fullpath = get_keypoints_fullpath(args.keypoints_type, args.kapture_root,
                                                        image_name, tar_handlers)
            print(f"Saving {xys.shape[0]} keypoints to {keypoints_fullpath}")
            image_keypoints_to_file(keypoints_fullpath, xys)
            kdata.keypoints[args.keypoints_type].add(image_name)

            descriptors_fullpath = get_descriptors_fullpath(args.descriptors_type, args.kapture_root,
                                                            image_name, tar_handlers)
            print(f"Saving {desc.shape[0]} descriptors to {descriptors_fullpath}")
            image_descriptors_to_file(descriptors_fullpath, desc)
            kdata.descriptors[args.descriptors_type].add(image_name)

        if not keypoints_check_dir(kdata.keypoints[args.keypoints_type], args.keypoints_type,
                                   args.kapture_root, tar_handlers) or \
                not descriptors_check_dir(kdata.descriptors[args.descriptors_type], args.descriptors_type,
                                          args.kapture_root, tar_handlers):
            print('local feature extraction ended successfully but not all files were saved')


if __name__ == '__main__':
    import argparse
    parser = argparse.ArgumentParser(
        "Extract r2d2 local features for all images in a dataset stored in the kapture format")
    parser.add_argument("--model", type=str, required=True, help='model path')
    parser.add_argument('--keypoints-type', default=None,  help='keypoint type_name, default is filename of model')
    parser.add_argument('--descriptors-type', default=None,  help='descriptors type_name, default is filename of model')

    parser.add_argument("--kapture-root", type=str, required=True, help='path to kapture root directory')

    parser.add_argument("--top-k", type=int, default=5000, help='number of keypoints')

    parser.add_argument("--scale-f", type=float, default=2**0.25)
    parser.add_argument("--min-size", type=int, default=256)
    parser.add_argument("--max-size", type=int, default=1024)
    parser.add_argument("--min-scale", type=float, default=0)
    parser.add_argument("--max-scale", type=float, default=1)

    parser.add_argument("--reliability-thr", type=float, default=0.7)
    parser.add_argument("--repeatability-thr", type=float, default=0.7)

    parser.add_argument("--gpu", type=int, nargs='+', default=[0], help='use -1 for CPU')
    args = parser.parse_args()

    extract_kapture_keypoints(args)