#!/usr/bin/python3

r'''Displays the calibration-time geometry: the cameras and the observed objects

SYNOPSIS

  $ mrcal-show-geometry *.cameramodel
  ... a plot pops up showing the camera arrangement

This tool visualizes the relative geometry between several cameras and the
calibration objects they observed when computing the calibration.

'''


import sys
import argparse
import re
import os

def parse_args():

    parser = \
        argparse.ArgumentParser(description = __doc__,
                                formatter_class=argparse.RawDescriptionHelpFormatter)

    parser.add_argument('--axis-scale',
                        type=float,
                        help='''Scale for the camera axes. By default a
                        reasonable default is chosen (see mrcal.show_geometry()
                        for the logic)''')
    parser.add_argument('--title',
                        type=str,
                        default = None,
                        help='''Title string for the plot''')
    parser.add_argument('--hardcopy',
                        type=str,
                        help='''Write the output to disk, instead of making an
                        interactive plot. The output filename is given in the
                        option''')
    parser.add_argument('--terminal',
                        type=str,
                        help=r'''The gnuplotlib terminal. The default is almost
                        always right, so most people don't need this
                        option''')

    parser.add_argument('--show-calobjects',
                        action='store_true',
                        help='''If given, draw the calibration object
                        observations from the FIRST given camera model that
                        contains the optimization_inputs''')

    parser.add_argument('--transforms',
                        type=str,
                        help='''Optional transforms.txt. This is a legacy file representing an extra
                        transformation for each camera. Most usages will omit
                        this''')

    parser.add_argument('--set',
                        type=str,
                        action='append',
                        help='''Extra 'set' directives to pass to gnuplotlib. May be given multiple
                        times''')

    parser.add_argument('--unset',
                        type=str,
                        action='append',
                        help='''Extra 'unset' directives to pass to gnuplotlib. May be given multiple
                        times''')

    parser.add_argument('models',
                        type = str,
                        nargs= '+',
                        help='''Camera models to visualize. Any N cameras can be given''')

    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 gnuplotlib as gp
import mrcal



models = [mrcal.cameramodel(m) for m in args.models]

cameras_Rt_plot_ref = None
if args.transforms is not None:
    import mrcal.cahvor
    transforms = mrcal.cahvor.read_transforms(args.transforms)

    def get_pair(icam):
        f = args.models[icam]
        m = re.search("camera([0-9]+)", f)
        return int(m.group(1))
    def Rt_plot_ref(icam):
        try:
            pair = get_pair(icam)
            Rt_ins_ref = transforms['ins_from_camera'][pair]
            return Rt_ins_ref
        except:
            return None
    cameras_Rt_plot_ref = [ Rt_plot_ref(icam) for icam in range(len(models))]

plotkwargs = {}
if args.title    is not None: plotkwargs['title'   ] = args.title
if args.hardcopy is not None: plotkwargs['hardcopy'] = args.hardcopy
if args.terminal is not None: plotkwargs['terminal'] = args.terminal

if args.set   is not None:
    gp.add_plot_option( plotkwargs, 'set',   args.set)
if args.unset is not None:
    gp.add_plot_option( plotkwargs, 'unset', args.unset)

plot = mrcal.show_geometry(models,
                           cameranames         = args.models,
                           cameras_Rt_plot_ref = cameras_Rt_plot_ref,
                           show_calobjects     = args.show_calobjects,
                           axis_scale          = args.axis_scale,
                           **plotkwargs)

if args.hardcopy is None:
    plot.wait()
