from vectors import *
from mpl_toolkits.mplot3d import Axes3D
from matplotlib import cm
from matplotlib.ticker import LinearLocator, FormatStrFormatter
import matplotlib.pyplot as plt
import numpy as np

# Given a line with coordinates 'start' and 'end' and the
# coordinates of a point 'pnt' the proc returns the shortest
# distance from pnt to the line and the coordinates of the
# nearest point on the line.
#
# 1  Convert the line segment to a vector ('line_vec').
# 2  Create a vector connecting start to pnt ('pnt_vec').
# 3  Find the length of the line vector ('line_len').
# 4  Convert line_vec to a unit vector ('line_unitvec').
# 5  Scale pnt_vec by line_len ('pnt_vec_scaled').
# 6  Get the dot product of line_unitvec and pnt_vec_scaled ('t').
# 7  Ensure t is in the range 0 to 1.
# 8  Use t to get the nearest location on the line to the end
#    of vector pnt_vec_scaled ('nearest').
# 9  Calculate the distance from nearest to pnt_vec_scaled.
# 10 Translate nearest back to the start/end line.
# Malcolm Kesson 16 Dec 2012

points=[(14,12,0),
        (18,27,0),
        (23,35,0),
        (32,35,0),
        (35,25,0),
        (37,14,0),
        (45,15,0),
        (49,29,0),
        (55,36,0)
        ]
goal=(55,36,0)

def pnt2line(pnt, start, end):
    line_vec = vector(start, end)
    pnt_vec = vector(start, pnt)
    line_len = length(line_vec)
    line_unitvec = unit(line_vec)
    pnt_vec_scaled = scale(pnt_vec, 1.0/line_len)
    t = dot(line_unitvec, pnt_vec_scaled)
    if t < 0.0:
        t = 0.0
    elif t > 1.0:
        t = 1.0
    nearest = scale(line_vec, t)
    dist = distance(nearest, pnt_vec)
    nearest = add(nearest, start)
    return (dist, nearest)

def getMinimumDistance(point):
    shortestDistance=float("Inf")
    for i in range(8):
        distance=pnt2line(point,points[i],points[i+1])[0]
        if shortestDistance>=distance:
            shortestDistance=distance
    return shortestDistance


def show(x,y,z):
    fig = plt.figure()
    ax = fig.gca(projection='3d')
    surf = ax.plot_surface(x, y, z, rstride=1, cstride=1, cmap=cm.coolwarm,
            linewidth=0, antialiased=False)
    ax.set_zlim(0, 100)

    ax.zaxis.set_major_locator(LinearLocator(10))
    ax.zaxis.set_major_formatter(FormatStrFormatter('%.02f'))

    fig.colorbar(surf, shrink=0.5, aspect=5)

    plt.show()
    return 0

def calculateRiver():
    river=[[getMinimumDistance([x,y,0])+distance((x,y,0),goal) for x in range(60)] for y in range(60)]
    x = np.arange(0, 60, 1)
    y = np.arange(0, 60, 1)
    x, y = np.meshgrid(x, y)
    #show(x,y,river)
    return river

def getLowestPoint(current,river):
    lowestPoint=current
    for x in range(-1,2):
        for y in range(-1,2):
            next=(current[0]+x,current[1]+y)
            print next,"=",river[next[0]][next[1]]
            if river[next[0]][next[1]]<river[lowestPoint[0]][lowestPoint[1]]:
                lowestPoint=next
    return lowestPoint



river=calculateRiver()
currentPoint=points[0]
nextPoint=getLowestPoint(currentPoint,river)
while currentPoint[0]!=nextPoint[0] or currentPoint[1]!=nextPoint[1]:
    print currentPoint, nextPoint
    print river[currentPoint[0]][currentPoint[1]],river[nextPoint[0]][nextPoint[1]]
    currentPoint=nextPoint
    nextPoint=getLowestPoint(currentPoint,river)

print nextPoint