#!/usr/bin/env python
"""
Main script for doing object capture
"""

from ecto_image_pipeline.io.source import add_camera_group
from object_recognition_capture import openni_capture
import argparse
import math
import sys
import textwrap

def parse_args():
    parser = argparse.ArgumentParser(formatter_class=argparse.RawDescriptionHelpFormatter,
                                    description=textwrap.dedent(
'''Captures data appropriate for training object recognition pipelines.
Assumes that there is a known fiducial in the scene, and captures views of the 
object sparsely, depending on the angle_thresh setting.'''),
                                    fromfile_prefix_chars='@')

    parser.add_argument('-o', '--output', metavar='BAG_FILE', dest='bag', type=str,
                       default='',
                       help='A bagfile to write to.')
    parser.add_argument('-a', '--angle_thresh', metavar='RADIANS', dest='angle_thresh', type=float,
                       default= 10 * math.pi / 180, #10 degrees
                       help='The delta angular threshold in pose.'
                            'Frames will not be recorded unless they are not closer to any other pose by this amount. default(%(default)s)')
    parser.add_argument('-n', '--nviews', metavar='NVIEWS', dest='nviews', type=int,
                       default=36,
                       help='Number of desired views. default(%(default)s)')
    parser.add_argument('-p', '--preview', dest='preview', action='store_true',
                        default=False, help='Preview the pose estimator.')
    parser.add_argument('-i', '--input', dest='input', type=str, help='The directory of the template to use. If empty, it uses '
                        'the opposite dot pattern', default='')
    parser.add_argument('-m', '--matches', dest='matches', action='store_true',
                        default=False, help='Visualize the matches.')

    add_camera_group(parser)
    #parser.add_argument('--use_turn_table', dest='use_turn_table', action='store_true',
    #                    default=False, help='Use an E106 servo based turntable.')
    from ecto.opts import scheduler_options, cell_options
    from ecto_opencv.rgbd import OnPlaneClustererCylinder
    segmentation_factory = cell_options(parser, OnPlaneClustererCylinder, 'seg')

    #add ecto scheduler args.
    group = parser.add_argument_group('Scheduler Options')
    scheduler_options(group)
    args = parser.parse_args()
    if not args.preview and len(args.bag) < 1:
        parser.print_help()
        print '\nYou must supply a bag name, or run in --preview mode'
        sys.exit(1)
    args.segmentation_factory = segmentation_factory
    return args

if "__main__" == __name__:
    argv = sys.argv[:]
    #ecto_ros.strip_ros_args(sys.argv)
    options = parse_args()
    #ecto_ros.init(argv, "object_capture", False)
    segmentation = options.segmentation_factory(options)

    (plasm, segmentation) = openni_capture.create_capture_plasm(bag_name=options.bag,
                                            angle_thresh=options.angle_thresh,
                                            segmentation_cell=segmentation,
                                            n_desired=options.nviews,
                                            preview=options.preview,
                                            orb_template=options.input,
                                            orb_matches=options.matches,
                                            use_turn_table=False,
                                            res=options.res,
                                            fps=options.fps
                                            )
    from ecto.opts import run_plasm
    run_plasm(options, plasm, locals=dict(plasm=plasm, segmentation=segmentation))
