from PIL import Image
import numpy as np
from numpy import asarray
import math
import heapq


class Point:
    def __init__(self, x1: int, y1: int):
        self.x = x1
        self.y = y1

    def __eq__(self, o):
        return self.x == o.x and self.y == o.y

    def __ne__(self, o):
        return self.x != o.x or self.y != o.y

    def is_available(self):
        return self.x >= 0 and self.y >= 0 and self.x < 512 and self.y < 512


class Node:
    def __init__(self, pos: Point,
                 cost: float = 0,
                 path_length: float = 0):
        self.pos = pos
        self.cost = cost
        self.path_length = path_length

    def __gt__(self, o):
        return self.cost > o.cost

    def __lt__(self, o):
        return self.cost < o.cost

    def __str__(self):
        return str(str(self.pos.x) + " " + str(self.pos.y))


def manhattan(n1: Point, n2: Point):
    dx = abs(n1.x - n2.x)
    dy = abs(n1.y - n2.y)
    return dx + dy


def diagonal(n1: Point, n2: Point):
    dx = abs(n1.x - n2.x)
    dy = abs(n1.y - n2.y)
    return (dx + dy) + (math.sqrt(2) - 2) * min(dx, dy)


def euclidean(n1: Point, n2: Point):
    dx = abs(n1.x - n2.x)
    dy = abs(n1.y - n2.y)
    return math.sqrt(dx * dx + dy * dy)



class PrioritySet(object):
    def __init__(self):
        self.heap = []
        self.set = set()

    def add(self, d, pri):
        if not d in self.set:
            heapq.heappush(self.heap, (pri, d))
            self.set.add(d)

    def get(self):
        pri, d = heapq.heappop(self.heap)
        self.set.remove(d)
        return d

    def is_empty(self):
        return len(self.heap) == 0

class PriorityQueue(object):
    def __init__(self):
        self.queue = []

    # for checking if the queue is empty
    def is_empty(self):
        return len(self.queue) == 0

    # for inserting an element in the queue
    def insert(self, data):
        for i in range (len(self.queue)):
            if data.pos == self.queue[i].pos and data.cost<self.queue[i].cost:
                self.queue.append(data)
                del self.queue[i]    
                i-=1
        self.queue.append(data)

    # for popping an element based on Priority
    def delete(self):
        try:
            min_index = 0
            for k in range(len(self.queue)):
                if self.queue[k] < self.queue[min_index]:
                    min_index = k
            item = self.queue[min_index]
            del self.queue[min_index]
            return item
        except IndexError:
            print()
            exit()
            
    def delete_2(self, pos:Point):
        for i in range (len(self.queue)):
            if self.queue[i].pos == pos:
                del self.queue[i]
                break

    def top_min(self):
        try:
            min_index = 0
            for k in range(len(self.queue)):
                if self.queue[k] < self.queue[min_index]:
                    min_index = k
            item = self.queue[min_index]
            return item
        except IndexError:
            print()
            exit()

    def __getitem__(self, item):
        return self.queue[item]

    def checkExist(self, cur:Node):
        for i in range(len(self.queue)):
            if self.queue[i].pos == cur.pos and self.queue[i].cost < cur.cost:
                return True
        return False

def a_star_path(pix_array: np.array,
                start: Point,
                goal: Point,
                m: int):


    h, w = len(pix_array), len(pix_array[0])
    start_node = Node(start)
    nodes_to_visit = PrioritySet()
    nodes_to_visit.add(start_node,start_node.cost)
    # adj = np.full(8, -1, int)
    new_point = np.full(8,Point(0,0),Point)
    g_cost = np.full((h, w),float('inf'),float)
    g_cost[start.x][start.y] = 0
    path = np.full((h, w), Point(-1, -1), Point)
    explorer = np.full((h, w), False, bool)

    while not nodes_to_visit.is_empty():
        cur = nodes_to_visit.get()       

        if cur.pos == goal:
            total_distance = cur.path_length
            break

        x = cur.pos.x
        y = cur.pos.y

        #Explorer set
        if not explorer[x][y]:
            explorer[x][y] = True           
        else:
            continue

        print(cur.cost)

        new_point[0] = Point(x, y + 1)
        new_point[1] = Point(x + 1, y + 1)
        new_point[2] = Point(x + 1, y)
        new_point[3] = Point(x + 1, y - 1)
        new_point[4] = Point(x, y - 1)
        new_point[5] = Point(x - 1, y - 1)
        new_point[6] = Point(x - 1, y)
        new_point[7] = Point(x - 1, y + 1)


        for i in range(8):   
        
             if new_point[i].is_available():
                x2 = new_point[i].x
                y2 = new_point[i].y
                delta_alpha = pix_array[x][y] - pix_array[x2][y2]
            
                if delta_alpha <= m and not explorer[x2][y2]:                               
                    
                    
                    sgn = delta_alpha > 0 and 1 or -1
                    distance = math.sqrt((x2 - x)*(x2 - x) + (y2 - y)*(y2 - y)) + (1/2 * sgn + 1)*abs(delta_alpha)
                    # print("distance ", distance)
                    g_new = g_cost[x][y] + distance
                    
                    # print("heuristic: ", heuristic)

                    if g_new < g_cost[x2][y2]:
                        heuristic = diagonal(new_point[i], goal)               
                        g_cost[x2][y2] = g_new
                        path[x2][y2] = cur.pos
                        f_cost = g_new + heuristic     
                        # if nodes_to_visit.checkExist(Node(new_point[i], f_cost , cur.path_length + distance)):
                        #     continue              
                        nodes_to_visit.add(Node(new_point[i], f_cost , cur.path_length + distance), f_cost)
                        print("new cost: ", g_cost[x2][y2])         
            
    path2D = []
    i = goal
    while i != start:
        path2D.append(i)
        i = path[i.x][i.y]

    path2D.append(start)

    total_distance = cur.path_length

    return path2D, total_distance


im = Image.open("map.bmp")
pix = im.load()
row, column = im.size
pix_array = np.zeros((row, column))
a = asarray(im)  # convert pixel to array

for i in range(row):
    for j in range(column):
        pix_array[i][j] = a[i][j].item(0)  # only get 1 value because 3 value same as each other

start = Point(500, 5)
goal = Point(505, 50)
m = 21

# print(m)
path, total_distance = a_star_path(pix_array, start, goal, m)

print("path length: ",total_distance," n point: ", len(path))

print(path)
for i in range(len(path)):
    x = int(path[i].x)
    y = int(path[i].y)
    r, g, b, p = pix[x, y]
    pix[x, y] = 255, g, b, p

im.show()