# Pathfinding stuff

import math
import copy

from locals import *

# This is an implementation of A*
# The following pseudo code is from the Wikipedia page on the A* algorithm,
# which was used as a reference
"""
function A*(start,goal)
    var closed := the empty set
    var q := make_queue(path(start))
    while q is not empty
        var p := remove_first(q)
        var x := the last node of p
        if x in closed
            continue
        if x = goal
            return p
        add x to closed
        foreach y in successors(p)
            if y not in closed
                enqueue(q, extend_path(p,y))
    return failure
"""
def find_path(map_, from_pos, to_pos, from_obj = None, to_obj = None):
    #If there is a direct line of sight to the target, the path is also just a direct path between these points. This is now disabled because of bugs.
    """if line_of_sight(map_, from_pos, to_pos, from_obj, to_obj):
       p = []
       i = 0.0
       dist = math.sqrt((to_pos[0] - from_pos[0])**2 + (to_pos[1] - from_pos[1])**2)
       step = 1.0 / (dist / FAST_PATH_DIVISION)
       while i < 1.0:
           pointx = to_pos[0] * i + from_pos[0] * (1.0-i)
           pointy = to_pos[1] * i + from_pos[1] * (1.0-i)
           p.append((pointx, pointy))
           i += step
       #print "fast pathfinding for direct line of sight applied"

       return p"""
    closed = []
    q = [[(math.floor(from_pos[0]) + 0.5, math.floor(from_pos[1]) + 0.5)]]
    closest = [from_pos]
    #q = [[from_pos]]
    while len(q) != 0:
        p = q[0]
        q.remove(p)
        x = p[-1]
        if x in closed:
            continue
        if math.floor(x[0]) == math.floor(to_pos[0]) and math.floor(x[1]) == math.floor(to_pos[1]):
            p.append(to_pos)
# We return p[1:] because the current system is a bit inaccurate and it would
# result in the character walking backwards sometimes, which looks dumb.
            return p[1:]
        closed.append(x)
        if heuristic(closest[-1],to_pos) > heuristic(x,to_pos):
            closest = p

        """
        unit = 1.0
        for y in ((x[0]-unit, x[1]), (x[0]+unit, x[1]), (x[0], x[1]-unit), (x[0], x[1]+unit)):
            if y[0] >= 0 and y[1] >= 0 and y[0] < map_.width and y[1] < map_.height:
                if line_of_sight(map_, x, y, from_obj, to_obj):
                    if y not in closed:
                        enqueue(q, extend_path(p, y), to_pos)
        """
        y = (x[0]-1, x[1])
        if y[0] >= 0 and y[1] >= 0 and y[0] < map_.width and y[1] < map_.height:
	    if y not in closed:
		if map_.get_tile(y).type and not map_.get_tile(x).leftwall or map_.get_tile(x).leftwall == 7:
		    #if obj_line_of_sight(map_, x, y, from_obj, to_obj):
		    if objs_not_in_tile(map_, y, from_obj, to_obj):
			y = (int(y[0]) + 0.5, int(y[1]) + 0.5)
			enqueue(q, extend_path(p, y), to_pos)
        y = (x[0], x[1]-1)
        if y[0] >= 0 and y[1] >= 0 and y[0] < map_.width and y[1] < map_.height:
	    if y not in closed:
	       	if map_.get_tile(y).type and not map_.get_tile(x).rightwall or map_.get_tile(x).rightwall == 7:
		    #if obj_line_of_sight(map_, x, y, from_obj, to_obj):
		    if objs_not_in_tile(map_, y, from_obj, to_obj):
			y = (int(y[0]) + 0.5, int(y[1]) + 0.5)
			enqueue(q, extend_path(p, y), to_pos)
        y = (x[0]+1, x[1])
        if y[0] >= 0 and y[1] >= 0 and y[0] < map_.width and y[1] < map_.height:
	    if y not in closed:
	       	if map_.get_tile(y).type and not map_.get_tile(y).leftwall or map_.get_tile(y).leftwall == 7:
		    #if obj_line_of_sight(map_, x, y, from_obj, to_obj):
		    if objs_not_in_tile(map_, y, from_obj, to_obj):
			y = (int(y[0]) + 0.5, int(y[1]) + 0.5)
			enqueue(q, extend_path(p, y), to_pos)
        y = (x[0], x[1]+1)
        if y[0] >= 0 and y[1] >= 0 and y[0] < map_.width and y[1] < map_.height:
	    if y not in closed:
	       	if map_.get_tile(y).type and not map_.get_tile(y).rightwall or map_.get_tile(y).rightwall == 7:
	    	    #if obj_line_of_sight(map_, x, y, from_obj, to_obj):
		    if objs_not_in_tile(map_, y, from_obj, to_obj):
		   	y = (int(y[0]) + 0.5, int(y[1]) + 0.5)
		      	enqueue(q, extend_path(p, y), to_pos)

    # Path not found
    return closest

# This is faster than the line-of-sight for pathfinding, but it's
# not very accurate. Moving objects don't count.
def objs_not_in_tile(map_, pos, from_obj, to_obj):
    pos = (int(pos[0]), int(pos[1]))
    for obj in map_.objects:
        if obj == from_obj or obj == to_obj or obj.is_walkover() or obj.objtype == TYPE_ENEMY or obj.objtype == TYPE_CHARACTER:
            continue
        rect = obj.get_rect()
        if rect.left >= pos[0] + 1:
            continue
        if rect.right <= pos[0]:
            continue
        if rect.top >= pos[1] + 1:
            continue
        if rect.bottom <= pos[1]:
            continue
        return False
    return True

def extend_path(path, node):
    p = copy.copy(path)
    p.append(node)
    return p

def enqueue(q, path, to_pos):
    for i in range(len(q)):
        if path_len(q[i], to_pos) > path_len(path, to_pos):
            q.insert(i, path)
            return
    q.append(path)

def path_len(path, to_pos):
    return len(path) - 1 + heuristic(path[-1], to_pos)


def heuristic(from_pos, to_pos):
    diff = (to_pos[0] - from_pos[0], to_pos[1] - from_pos[1])
    return abs(diff[0]) + abs(diff[1])
    #return math.sqrt(diff[0]*diff[0] + diff[1]*diff[1])

# This isn't really pathfinding, but this is the "most"
# logical place to put this in.
def line_of_sight(map_, from_pos, to_pos, from_obj = None, to_obj = None):
    # This is just bogus, but will have to do for now
    dx = (to_pos[0] - from_pos[0])
    dy = (to_pos[1] - from_pos[1])
    #print "moi"
    if dx > 0:
        y = from_pos[1] + dy/dx * (math.ceil(from_pos[0]) - from_pos[0])
        x = math.ceil(from_pos[0])

        while x != math.ceil(to_pos[0]):
            wall = map_.get_tile((int(x), int(math.floor(y)))).leftwall
            if wall and wall != 7:
                return False
            x += 1.0
            y += dy/dx
    if dx < 0:
        y = from_pos[1] - dy/dx * (from_pos[0] - math.floor(from_pos[0]))
        x = math.floor(from_pos[0])

        while x != math.floor(to_pos[0]):
            wall = map_.get_tile((int(x), int(math.floor(y)))).leftwall
            if wall and wall != 7:
                return False
            x -= 1.0
            y -= dy/dx
    if dy > 0:
        x = from_pos[0] +  dx/dy * (math.ceil(from_pos[1]) - from_pos[1])
        y = math.ceil(from_pos[1])

        while y != math.ceil(to_pos[1]):
            wall = map_.get_tile((int(math.floor(x)), int(y))).rightwall
            if wall and wall != 7:
                return False
            x += dx/dy
            y += 1.0
    if dy < 0:
        x = from_pos[0] - dx/dy * (from_pos[1] - math.floor(from_pos[1]))
        y = math.floor(from_pos[1])

        while y != math.floor(to_pos[1]):
            wall = map_.get_tile((int(math.floor(x)), int(y))).rightwall
            if wall and wall != 7:
                return False
            x -= dx/dy
            y -= 1.0
# Then we need to check the objects, but this'll do for now

# This is a separate function because for optimisation's sake we'll
# want to use that part in the pathfinding too...
    return obj_line_of_sight(map_, from_pos, to_pos, from_obj, to_obj)

def obj_line_of_sight(map_, from_pos, to_pos, from_obj = None, to_obj = None):
    #return True
    dx = to_pos[0] - from_pos[0]
    dy = to_pos[1] - from_pos[1]

    dist = math.sqrt(dx*dx+dy*dy)
    sin = dx / dist
    cos = dy / dist
# half of the object's size
    hobs = (0.0, 0.0)
    if from_obj:
        hobs = (from_obj.size[0]/2, from_obj.size[1]/2)
    #Moving objects don't count. This also eases shooting at large crowds!    
    for obj in map_.objects:
        if obj != from_obj and obj != to_obj and (not obj.is_walkover()) and obj.objtype != TYPE_ENEMY and obj.objtype != TYPE_CHARACTER:
            flag = 0
            rect = obj.get_rect()
            rect.left -= hobs[0]
            rect.right += hobs[0]
            rect.top -= hobs[1]
            rect.bottom += hobs[1]
            for point in ((rect.left, rect.top), (rect.left, rect.bottom), \
                          (rect.right, rect.top), (rect.right, rect.bottom)):
                ddx = point[0] - from_pos[0]
                ddy = point[1] - from_pos[1]
                yy = ddy * cos + ddx * sin
                if yy > 0 and yy < dist:
                    xx = -ddy * sin + ddx * cos
                    if xx < 0:
                        if flag > 0:
                            return False
                        flag = -1
                    else:
                        if flag < 0:
                            return False
                        flag = 1

            continue

    return True

def single_obj_line_of_sight(from_pos, to_pos, from_obj, obj):
    dx = to_pos[0] - from_pos[0]
    dy = to_pos[1] - from_pos[1]
    dist = math.sqrt(dx*dx+dy*dy)
    sin = dx/dist
    cos = dy/dist
    hobs = (0.0, 0.0)
    if from_obj:
        hobs = (from_obj.size[0]/2, from_obj.size[1]/2)
    if obj != from_obj and obj != to_obj and not obj.iswalkover() and obj.objtype != TYPE_ENEMY and obj.objtype != TYPE_CHARACTER:
        flag = 0
        rect = obj.get_rect()
        rect.left -= hobs[0]
        rect.right += hobs[0]
        rect.top -= hobs[1]
        rect.bottom += hobs[1]
        for point in ((rect.left, rect.top), (rect.left, rect.bottom), \
                        (rect.right, rect.top), (rect.right, rect.bottom)):
            ddx = point[0] - from_pos[0]
            ddy = point[1] - from_pos[1]
            yy = ddy * cos + ddx * sin
            if yy > 0 and yy < dist:
                xx = -ddy * sin + ddx * cos
                if xx < 0:
                    if flag > 0:
                        return False
                    flag = -1
                else:
                    if flag < 0:
                        return False
                    flag = 1
    return True
