#!/usr/bin/env python

import os
import sys
import argparse

import triangulation

def main():
    parser = argparse.ArgumentParser()
    parser.add_argument('dataset', type=str)
    parser.add_argument('output', type=str)
    parser.add_argument('--format', choices=['vgg', 'matlab'], required=True)
    parser.add_argument('--algorithm', choices=triangulation.algorithms.keys(), required=True)
    parser.add_argument('-v', '--verbose', action='store_true', default=False)
    parser.add_argument('-q', '--quiet', action='store_true', default=False)
    args = parser.parse_args()

    if args.format == 'vgg':
        bundle = triangulation.load_vgg_dataset(args.dataset)
    elif args.format == 'matlab':
        bundle = triangulation.load_matlab_dataset(args.dataset)

    num_successful = 0
    num_too_short = 0
    num_errors = 0
    with open(args.output, "w") as f:
        for i, track in enumerate(bundle.tracks):
            observations = []
            poses = []
            for observation in track:
                camera = bundle.cameras[observation.frame_index]
                observations.append(observation.feature)
                poses.append(camera.pose)

            if len(poses) < 2:
                num_too_short += 1
                if args.verbose:
                    print("Track %d has only %d observations" % (i, len(poses)))

            try:
                landmark = triangulation.triangulate(observations, poses, algorithm=args.algorithm)
                f.write("%d %s\n" % (i, ' '.join(['%.12f' % xi for xi in landmark])))
                num_successful += 1
            except Exception as ex:
                num_errors += 1
                if args.verbose:
                    print("Failed to triangulate track %d: %s" % (i, ex))

        if not args.quiet:
            print("Triangulated %d landmarks, found %d tracks that were too short, and encountered %d errors" %
                (num_successful, num_too_short, num_errors))


if __name__ == '__main__':
    main()
