#!/usr/bin/env python

import sys

import matplotlib.mlab as pylab
import matplotlib.pyplot as plt
import numpy as np

import cv
import utils
import cvutils
import ubc_utils
import moving

# use something like getopt to manage arguments if necessary

if len(sys.argv) < 3:
    print('Usage: {0} <video-filename> <n-objects>'.format(sys.argv[0]))
    sys.exit()

if sys.argv[1].endswith('.avi'):
    videoFilenamePrefix = utils.removeExtension(sys.argv[1],'.')
else:
    videoFilenamePrefix = sys.argv[1]

objectNum = int(sys.argv[2])

objects = ubc_utils.loadTrajectories(videoFilenamePrefix+'-objects.txt', objectNum+1)
obj = objects[objectNum]
features = ubc_utils.loadTrajectories(videoFilenamePrefix+'-features.txt', max(obj.featureNumbers)+1)
h = np.loadtxt(videoFilenamePrefix+'-homography.txt')

invh = cvutils.invertHomography(h)

yCoordinates = -np.ones((len(obj.featureNumbers),int(obj.length())))
for i,fnum in enumerate(obj.featureNumbers):
    traj = features[fnum].getPositions().asArray()
    imgTraj = cvutils.projectArray(invh, traj)
    yCoordinates[i,features[fnum].getFirstInstant()-obj.getFirstInstant():features[fnum].getLastInstant()+1-obj.getFirstInstant()] = imgTraj[1,:]

indices = argmax(yCoordinates,0)
newTraj = moving.Trajectory()
for j,idx in enumerate(indices):
    newTraj.addPosition(features[obj.featureNumbers[idx]].getPositionAtInstant(j+obj.getFirstInstant()))

# TODO
# use kalman filter over the resulting trajectory
# estimate the error terms using actual features