#! /usr/bin/env python
# -*- coding: utf-8 -*-

# This script calculates and saves a homography matrix from 
# the point-correspondences.txt generated by the homography web tool

# Dependencies : 
# - opencv 2.4.11
# - numpy, matplotlib
# - utm 
# - argparse

# belisle.francois@gmail.com
# 20150917

def homography_matrix(pc_filename, save = True, homography_filename = 'homography.txt'):
 
    world, image = create_point_correspondences(pc_filename)

    import cv2
    homography, mask = cv2.findHomography(image, world)
    
    if save == True:
        import numpy as np
        np.savetxt(homography_filename, homography)
    return homography

def latlong2utm(point):
    """
    This function converts a point from lat long to utm
    Input : point : (lat,long)
    Output : utm point : (x,y,z, n)
    """
    import utm
    return utm.from_latlon(point[0],point[1])

def rel_coords(points):
    """
    This function returns the relative coordinats of points relative
    to the first point given ; it also rounds coords to centimeters

    Expected coords in UTM
    """
    first = points[0]
    rel = [(round(p[0]-first[0], ndigits=2),
            round(p[1]-first[1], ndigits=2)) for p in points]

    import numpy as np
    return np.array(rel, dtype=np.float32)

def convert(points):
    """
    Converts a series of points from lat,long to utm in meters
    
    """
    import numpy as np
    point_conv = [latlong2utm(p) for p in points]
    return np.array([(p[0],p[1]) for p in point_conv])

def create_point_correspondences(pc_filename):
    """Loads and returns the corresponding points in world in relative meters
    (first 2 lines x,y ) and image spaces (last 2 lines lat,long)"""

    import numpy as np

    image = np.loadtxt(pc_filename, usecols=(0, 1), dtype=np.float32)
    world_lat_long = np.loadtxt(pc_filename, usecols=(2, 3), dtype=np.float32)
    world_utm = convert(world_lat_long)
    rel_utm = rel_coords(world_utm)

    return (rel_utm, image)


if __name__ == '__main__':

    import argparse
    parser = argparse.ArgumentParser(description=
                                     "This scripts computes a homography matrix.")
    parser.add_argument('-p', 
                        dest = 'point_correspondences_filename', 
                        help = 'name of the text file containing the points',
                        default = 'sample/point-correspondences.txt')
    parser.add_argument('-o', 
                        dest = 'homography_filename', 
                        help = 'filename of the homography matrix to be saved', 
                        default = 'results/homography.txt')
    parser.add_argument('-i', 
                        dest = 'image_filename',
                        help = 'Image of a frame from the video', 
                        default = 'capture.png')

    args = parser.parse_args()
    
    homography = homography_matrix(args.point_correspondences_filename,
								   save = True,
                                   homography_filename = args.homography_filename) 
        

    

