#!/usr/bin/env python3

# Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S.
# Government sponsorship acknowledged. All rights reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0

r"""Reprojects pixel observations from one model to another

SYNOPSIS

  $ < points-in.vnl
    mrcal-reproject-points
      from.cameramodel to.cameramodel
    > points-out.vnl

This tool takes a set of pixel observations of points captured by one camera
model, and transforms them into observations of the same points captured by
another model. This is similar to mrcal-reproject-image, but acts on discrete
points, rather than on whole images. The two sets of intrinsics are always used.
The translation component of the extrinsics is always ignored; the rotation is
ignored as well if --intrinsics-only.

This allows one to combine multiple image-processing techniques that expect
different projections. For instance, planes projected using a pinhole projection
have some nice properties, and we can use those after running this tool.

The input data comes in on standard input, and the output data is written to
standard output. Both are vnlog data: human-readable text with 2 columns: x and
y pixel coords. Comments are allowed, and start with the '#' character.

"""

import sys
import argparse


def parse_args():
    parser = argparse.ArgumentParser(
        description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter
    )

    parser.add_argument(
        "--intrinsics-only",
        action="store_true",
        help="""By default, the relative camera rotation is used in the transformation. If we
                        want to use the intrinsics ONLY, pass --intrinsics-only.
                        Note that relative translation is ALWAYS ignored""",
    )

    parser.add_argument(
        "model-from", type=str, help="""Camera model for the input points."""
    )

    parser.add_argument(
        "model-to", type=str, help="""Camera model for the output points."""
    )

    return parser.parse_args()


args = parse_args()

# arg-parsing is done before the imports so that --help works without building
# stuff, so that I can generate the manpages and README


import numpy as np
import numpysane as nps

import mrcal
import time

model_from = mrcal.cameramodel(getattr(args, "model-from"))
model_to = mrcal.cameramodel(getattr(args, "model-to"))


p = nps.atleast_dims(np.genfromtxt(sys.stdin), -2)
v = mrcal.unproject(p, *model_from.intrinsics())

print(
    "## generated on {} with   {}".format(
        time.strftime("%Y-%m-%d %H:%M:%S"),
        " ".join(shlex.quote(s) for s in sys.argv),
    )
)
if not args.intrinsics_only:
    Rt_to_from = mrcal.compose_Rt(
        model_to.extrinsics_Rt_fromref(), model_from.extrinsics_Rt_toref()
    )

    if nps.norm2(Rt_to_from[3, :]) > 1e-6:
        print(
            f"## WARNING: {sys.argv[0]} ignores relative translations, which were non-zero here. t_to_from = {Rt_to_from[3, :]}"
        )

    v = nps.matmult(Rt_to_from[:3, :], nps.dummy(v, -1))[..., 0]

p = mrcal.project(v, *model_to.intrinsics())
np.savetxt(sys.stdout, p, fmt="%f", header="x y")
