import ika
import time

class Node:

  def __init__(self,x,y,parent=None,g=0,h=0):
    self.x = x
    self.y = y
    self.parent = parent
    self.g = g
    self.h = h

  def cost(self):
    return self.g + self.h

  def equal(self,node):
    if self.x == node.x and self.y == node.y:
      return True
    else:
      return False

class Emerald_Pathfinder:

  def __init__(self):
    pass

  def setup(self,start,goal):
    self.start = start
    self.goal = goal
    self.openlist = [None,start]    # Implemented as binary heap
    self.closedlist = {}            # Implemented as hash
    self.onopenlist = {}            # Hash, for searching the openlist
    self.found = False
    self.current = None
    self.iterations = 0

  def lowest_cost(self):
    pass

  def add_nodes(self,current):
    nodes = []
    x = current.x
    y = current.y
    self.add_node(x+1,y,current,10,nodes)
    self.add_node(x-1,y,current,10,nodes)
    self.add_node(x,y+1,current,10,nodes)
    self.add_node(x,y-1,current,10,nodes)
    # Dont cut across corners
    up = map.is_obstacle((x,y-1),x,y-1)
    down = map.is_obstacle((x,y+1),x,y+1)
    left = map.is_obstacle((x-1,y),x-1,y)
    right = map.is_obstacle((x+1,y),x+1,y)
    if right == False and down == False:
      self.add_node(x+1,y+1,current,14,nodes)
    if left == False and up == False:
      self.add_node(x-1,y-1,current,14,nodes)
    if right == False and up == False:
      self.add_node(x+1,y-1,current,14,nodes)
    if left == False and down == False:
      self.add_node(x-1,y+1,current,14,nodes)
    return nodes

  def heuristic(self,x1,y1,x2,y2):
    return (abs(x1-x2)+abs(y1-y2))*10

  def add_node(self,x,y,parent,cost,list):
    # If not obstructed
    if map.is_obstacle((x,y),x,y) == False:
      g = parent.g + cost
      h = self.heuristic(x,y,self.goal.x,self.goal.y)
      node = Node(x,y,parent,g,h)
      list.append(node)

  def ignore(self,node,current):
    # If its on the closed list, or open list, ignore
    try:
      if self.closedlist[(node.x,node.y)] == True:
        return True
    except:
      pass
    # If the node is on the openlist, do the following
    try:
      # If its on the open list
      if self.onopenlist[(node.x,node.y)] != None:
        # Get the id number of the item on the real open list
        index = self.openlist.index(self.onopenlist[(node.x,node.y)])
        # If one of the coordinates equal, its not diagonal.
        if node.x == current.x or node.y == current.y:
          cost = 10
        else:
          cost = 14
        # Check, is this items G cost is higher, than the current G + cost
        if self.openlist[index].g > (current.g + cost):
          # If so, then, make the list items parent, the current node.
          self.openlist[index].g = current.g + cost
          self.openlist[index].parent = current
          # Now resort the binary heap, in the right order.
          self.resort_binary_heap(index)
        # And ignore the node
        return True
    except:
      pass
    return False

  def resort_binary_heap(self,index):
    m = index
    while m > 1:
      if self.openlist[m/2].cost() > self.openlist[m].cost():
        temp = self.openlist[m/2]
        self.openlist[m/2] = self.openlist[m]
        self.openlist[m] = temp
        m = m / 2
      else:
        break

  def heap_add(self,node):
    self.openlist.append(node)
    # Add item to the onopenlist.
    self.onopenlist[(node.x,node.y)] = node
    m = len(self.openlist)-1
    while m > 1:
      if self.openlist[m/2].cost() > self.openlist[m].cost():
        temp = self.openlist[m/2]
        self.openlist[m/2] = self.openlist[m]
        self.openlist[m] = temp
        m = m / 2
      else:
        break

  def heap_remove(self):
    if len(self.openlist) == 1:
      return
    first = self.openlist[1]
    # Remove the first item from the onopenlist
    self.onopenlist[(self.openlist[1].x,self.openlist[1].y)] = None
    last = self.openlist.pop(len(self.openlist)-1)
    if len(self.openlist) == 1:
      return last
    else:
      self.openlist[1] = last
    v = 1
    while True:
      u = v
      # If there is two children
      if (2*u)+1 < len(self.openlist):
        if self.openlist[2*u].cost() <= self.openlist[u].cost():
          v = 2*u
        if self.openlist[(2*u)+1].cost() <= self.openlist[v].cost():
          v = (2*u)+1
      # If there is only one children
      elif 2*u < len(self.openlist):
        if self.openlist[2*u].cost() <= self.openlist[u].cost():
          v = 2*u
      # If at least one child is smaller, than parent, swap them
      if u != v:
        temp = self.openlist[u]
        self.openlist[u] = self.openlist[v]
        self.openlist[v] = temp
      else:
        break
    return first

  def iterate(self):
    # If the open list is empty, exit the game
    if len(self.openlist) == 1:
      ika.Exit("no path found")
    # Expand iteration by one
    self.iterations += 1
    # Make the current node the lowest cost
    self.current = self.heap_remove()
    # Add it to the closed list
    self.closedlist[(self.current.x,self.current.y)] = True
    # Are we there yet?
    if self.current.equal(self.goal) == True:
      # Target reached
      self.goal = self.current
      self.found = True
      print self.iterations
    else:
      # Add the adjacent nodes, and check them
      nodes_around = self.add_nodes(self.current)
      for na in nodes_around:
        if self.ignore(na,self.current) == False:
          self.heap_add(na)

  def iterateloop(self):
    time1 = time.clock()
    while 1:
      # If the open list is empty, exit the game
      if len(self.openlist) == 1:
        ika.Exit("no path found")
      # Expand iteration by one
      self.iterations += 1
      # Make the current node the lowest cost
      self.current = self.heap_remove()
      # Add it to the closed list
      self.closedlist[(self.current.x,self.current.y)] = True
      # Are we there yet?
      if self.current.equal(self.goal) == True:
        # Target reached
        self.goal = self.current
        self.found = True
        print "Number of iterations"
        print self.iterations
        break
      else:
        # Add the adjacent nodes, and check them
        nodes_around = self.add_nodes(self.current)
        for na in nodes_around:
          if self.ignore(na,self.current) == False:
            self.heap_add(na)
    time2 = time.clock()
    time3 = time2-time1
    print "Seconds to find path:"
    print time3

class Map:

  def __init__(self):
    self.map_size_x = 20
    self.map_size_y = 15
    self.obstructed = {} # Library, containing x,y couples
    self.start = [2*40,3*40]
    self.unit = [16*40,8*40]

  def is_obstacle(self,couple,x,y):
    if (x >= self.map_size_x or x < 0) or (y >= self.map_size_y or y < 0):
      return True
    try:
      if self.obstructed[(couple)] != None:
        return True
    except:
      return False

def render_screen():
  # Draw the Character
  ika.Video.DrawRect(map.start[0],map.start[1],map.start[0]+40,map.start[1]+40,ika.RGB(40,200,10),1)
  # Draw walls
  for x in range(0,map.map_size_x):
    for y in range(0,map.map_size_y):
      if map.is_obstacle((x,y),x,y) == True:
        ika.Video.DrawRect(x*40,y*40,(x*40)+40,(y*40)+40,ika.RGB(168,44,0),1)
  # Draw openlist items
  for node in path.openlist:
    if node == None:
      continue
    x = node.x
    y = node.y
    ika.Video.DrawRect(x*40,y*40,(x*40)+40,(y*40)+40,ika.RGB(100,100,100,50),1)
  # Draw closedlist items
  for x in range(0,map.map_size_x):
    for y in range(0,map.map_size_y):
      try:
        if path.closedlist[(x,y)] == True:
          ika.Video.DrawRect(x*40,y*40,(x*40)+20,(y*40)+20,ika.RGB(0,0,255))
      except:
        pass
  # Draw the current square
  try:
    ika.Video.DrawRect(path.current.x*40,path.current.y*40,(path.current.x*40)+40,(path.current.y*40)+40,ika.RGB(128,128,128), 1)
  except:
    pass
  ika.Video.DrawRect(mouse_x.Position(),mouse_y.Position(),mouse_x.Position()+8,mouse_y.Position()+8,ika.RGB(128,128,128), 1)
  # Draw the path, if reached
  if path.found == True:
    node = path.goal
    while node.parent:
      ika.Video.DrawRect(node.x*40,node.y*40,(node.x*40)+40,(node.y*40)+40,ika.RGB(40,200,200),1)
      node = node.parent
  # Draw the Target
  ika.Video.DrawRect(map.unit[0],map.unit[1],map.unit[0]+40,map.unit[1]+40,ika.RGB(128,40,200),1)

def mainloop():
  while 1:
    render_screen()
    if mouse_middle.Pressed():
      # Iterate pathfinder
      if path.found == False:
        path.iterateloop()
    elif mouse_right.Pressed():
      # Iterate pathfinder by one
      if path.found == False:
        path.iterate()
    elif ika.Input.keyboard["A"].Pressed():
      # Iterate pathfinder
      if path.found == False:
        path.iterateloop()
    elif ika.Input.keyboard["S"].Pressed():
      # Iterate pathfinder by one
      if path.found == False:
        path.iterate()
    elif mouse_left.Position():
      # Add a square to the map, to be obstructed
      if path.iterations == 0:
        x = mouse_x.Position()
        y = mouse_y.Position()
        map.obstructed[(int(x/40),int(y/40))] = True
    # Mouse preview
    x = mouse_x.Position()
    y = mouse_y.Position()
    mx = int(x/40)*40
    my = int(y/40)*40
    ika.Video.DrawRect(mx,my,mx+40,my+40,ika.RGB(150,150,150,70),1)
    ika.Video.ShowPage()
    ika.Input.Update()

map = Map()
path = Emerald_Pathfinder()
path.setup(Node(map.start[0]/40,map.start[1]/40),Node(map.unit[0]/40,map.unit[1]/40))
mouse_middle = ika.Input.mouse.middle
mouse_right = ika.Input.mouse.right
mouse_left = ika.Input.mouse.left
mouse_x = ika.Input.mouse.x
mouse_y = ika.Input.mouse.y
# Initialize loop
mainloop()