#! /usr/bin/env python
''' Generic utilities.'''

#from numpy import *
#from pylab import *

from moving import Point,Interval

__metaclass__ = type

commentChar = '#';

#########################
# maths section
#########################

def segmentIntersection(p1, p2, p3, p4):
    '''Returns the intersecting point of the segments [p1, p2] and [p3, p4], None otherwise
    
    >>> segmentIntersection(Point(0,0),Point(1,1), Point(0,1), Point(1,2))
    >>> segmentIntersection(Point(0,1),Point(1,0), Point(0,2), Point(2,1))
    >>> segmentIntersection(Point(0,0),Point(2,0), Point(1,-1),Point(1,1))
    (1.000000,0.000000)
    >>> segmentIntersection(Point(0,1),Point(2,0),Point(1,1),Point(1,2))
    '''
    from numpy import matrix
    from numpy.linalg import linalg, det

    dp1 = p2-p1#[s1[0][1]-s1[0][0], s1[1][1]-s1[1][0]]
    dp2 = p4-p3#[s2[0][1]-s2[0][0], s2[1][1]-s2[1][0]]

    A = matrix([[dp1.y, -dp1.x],
                [dp2.y, -dp2.x]])
    B = matrix([[dp1.y*p1.x-dp1.x*p1.y],
                [dp2.y*p3.x-dp2.x*p3.y]])

    if linalg.det(A) == 0:#crossProduct(ds1, ds2) == 0:
        return None
    else:
        intersection = linalg.solve(A,B)
        if (Interval(p1.x, p2.x, True).contains(intersection[0,0])
            and Interval(p3.x, p4.x, True).contains(intersection[0,0])
            and Interval(p1.y, p2.y, True).contains(intersection[1,0])
            and Interval(p3.y, p4.y, True).contains(intersection[1,0])):
            return Point(intersection[0,0], intersection[1,0])
        else:
            return None

def crossProduct(l1, l2):
    return l1[0]*l2[1]-l1[1]*l2[0]

#########################
# file I/O section
#########################

def openCheck(filename, option = 'r', quit = False):
    '''Open file filename in read mode by default
    and checks it is open

    >>> f = openCheck('non_existant_file.txt')
    File non_existant_file.txt could not be opened.
    '''
    try:
        return open(filename, option)
    except IOError:
        print 'File %s could not be opened.' % filename
        if quit:
            from sys import exit
            exit()
        return None

def readline(f):
    '''Modified readline function to skip comments.'''
    s = f.readline()
    while (len(s) > 0) and s.startswith(commentChar):
        s = f.readline()
    return s.strip()

def removeExtension(filename, delimiter = '.'):
    '''Returns the filename minus the extension (all characters after last .)
    >>> removeExtension('test-adfasdf.asdfa.txt')
    'test-adfasdf.asdfa'
    >>> removeExtension('test-adfasdf')
    'test-adfasdf'
    '''
    i = filename.rfind(delimiter)
    if i>0:
        return filename[:i]
    else:
        return filename

def listfiles(dirname, extension, remove = False):
    '''Returns the list of files with the extension in the directory dirname
    If remove is True, the filenames are stripped from the extension'''
    from os import listdir
    tmp = [f for f in listdir(dirname) if f.endswith(extension)]
    tmp.sort()
    if remove:
        return [removeExtension(f, extension) for f in tmp]
    else:
        return tmp

def removeFile(filename):
    '''Deletes the file while avoiding raising an error 
    if the file does not exist'''
    if (os.path.exists(filename)):
        os.remove(filename)

def invertHomography(homography):
    'Returns an inverted homography'
    from numpy.linalg.linalg import inv
    invH = inv(homography)
    invH /= invH[2,2]
    return invH

def project(homography, p):
    '''Returns the coordinates of the projection of the point p
    through homography'''
    from numpy.core._dotblas import dot
    from numpy.core.multiarray import array
    from numpy.lib.function_base import insert
    if (homography!=None) and (len(homography)>0):
        pAugmented = insert(array(p), [2],[1], axis=0)
        projected = dot(homography, pAugmented)
        projected[0] /= projected[2]
        projected[1] /= projected[2]
    else:
        projected = p
    return projected[:2]

def projectTrajectory(homography, trajectory):
    '''Projects a series of points in the format
    [[x1, x2, ...],
    [y1, y2, ...]]

    Warning: not optimized, calls project()'''
    projected = [[],[]]
    for x, y in zip(trajectory[0], trajectory[1]):
        p = [x,y]
        pp = project(homography, p)
        projected[0].append(pp[0])
        projected[1].append(pp[1])
    return projected

def plotPolygon(poly, options = ''):
    from numpy.core.multiarray import array
    from matplotlib.pyplot import plot
    from shapely.geometry import Polygon

    tmp = array(poly.exterior)
    plot(tmp[:,0], tmp[:,1], options)

if __name__ == "__main__":
    import doctest
    import unittest
    #suite = doctest.DocFileSuite('tests/ubc_utils.txt')
    suite = doctest.DocTestSuite()
    unittest.TextTestRunner().run(suite)
    #doctest.testmod()
    #doctest.testfile("example.txt")