#!/usr/bin/env python
# coding: utf-8
'''

'''
import math
import numpy as np
import cv2
import time as t

class Node:
    def __init__(self, x, y):
        self.x = x
        self.y = y
        self.cost = math.inf
        self.parent = None

class Robot:
    def __init__(self, radius, clearance, start, goal):
        self.radius = radius
        self.clearance = clearance
        self.start = start
        self.goal = goal
        
def get_min_node(queue):
    min_node = 0
    for node in range(len(queue)):
        if queue[node].cost < queue[min_node].cost:
            min_node = node
    return queue.pop(min_node)

def node_exists(x,y, queue):
    for node in queue:
        if node.x == x and node.y == y:
            return queue.index(node)
        else:
            return None
        
def try_move(move, current_point, radius, clearance):
    if move == 'move_up':
        return move_up(current_point, radius, clearance)
    if move == 'move_down':
        return move_down(current_point, radius, clearance)
    if move == 'move_left':
        return move_left(current_point, radius, clearance)
    if move == 'move_right':
        return move_right(current_point, radius, clearance)
    if move == 'move_up_right':
        return move_up_right(current_point, radius, clearance)
    if move == 'move_up_left':
        return move_up_left(current_point, radius, clearance)
    if move == 'move_down_right':
        return move_down_right(current_point, radius, clearance)
    if move == 'move_down_left':
        return move_down_left(current_point, radius, clearance)

def ways_in(x,y): # a pixel with no obstacles or edges nearby can be achieved from 8 moves
    count = 0
    if y > 0: #from top
        count+=1
    if y < 200: #from bottom
        count+=1
    if x > 0: #from left
        count+=1
    if x < 200: #from right
        count+=1
    if x < 300 and y < 200: #bottom right
        count+=1
    if x < 300 and y > 0: #top left
        count+=1
    if x > 0 and y > 0: #top left
        count+=1
    if x > 0 and y < 200: #bottom right
        count+=1
    return count

def fill_pixel(img,x,y): #fill visited pixes
    img[y,x] = [255,0,0]
    return img

def backtrack(node): #create list of parent node locations
    parentList = list()
    parent = node.parent
    while parent is not None:
        parentList.append(parent)
        parent = parent.parent
    return parentList


def check_viableX(point):
    if point >= 0 and point < 300:
        return True
    else:
        print("Invalid")
        print()
        return False
    
def check_viableY(point):
    if point > 0 and point < 200:
        return True
    else:
        print("Invalid")
        print()
        return False

def check_oval(x,y):
    center = [150, 100]
    rx = 40 + radius+clearance
    ry = 20 + radius+clearance
    dist = (((x - center[0]) ** 2) / (rx ** 2)) + (((y - center[1]) ** 2) / (ry ** 2))
    if dist <= 1:
        print("Don't go in the oval!")
        return False
    else:
        return True
    
    
def check_circle(x,y):
    y = 200 - y
    center = [225, 150]
    dist = np.sqrt((x - center[0]) ** 2 + (y - center[1]) ** 2)
    
    if dist <= 25+radius+clearance:
        print("Don't go in the circle!")
        return False
    else:
        return True
    
def check_rectangle(x,y):
    y = 200-y
    one = [95,200-170]
    two = [30.5,200-132.5]
    three = [35.5,200-123.9]
    four = [100,200-161.4]
    
    m_bottom = (two[1]-one[1])/(two[0]-one[0])
    m_top = (three[1]-four[1])/(three[0]-four[0])
    m_right = (one[1]-four[1])/(one[0]-four[0])
    m_left = (two[1]-three[1])/(two[0]-three[0])
    
    #solve for y intercept  y=mx+b or b = y-mx
    b_bottom = one[1]-m_bottom*one[0]
    b_top = three[1]-m_top*three[0]
    b_left = two[1]-m_left*two[0]
    b_right = four[1]-m_right*four[0]
    
    # 0 = y -mx -b
    eq_top = y - m_top*x-(b_top+radius+clearance+1)
    eq_bottom = y - m_bottom*x -(b_bottom-radius-clearance)
    eq_left = y -m_left*x-(b_left+radius+clearance+2) # +2 is from rounding
    eq_right = y - m_right*x -(b_right-radius-clearance)
    
    if eq_top <= 0 and eq_bottom >= 0 and eq_left <= 0 and eq_right >= 0:
        print("Don't go in the rectangle!")
        return False
    else:
        return True

def check_diamond(x,y):
    y = 200-y
    one = [225,200-190]
    two = [200,200-175]
    three = [225,200-160]
    four = [250,200-175]
    
    m_bottom = (two[1]-one[1])/(two[0]-one[0])
    m_top = (three[1]-four[1])/(three[0]-four[0])
    m_right = (one[1]-four[1])/(one[0]-four[0])
    m_left = (two[1]-three[1])/(two[0]-three[0])
    
    #solve for y intercept  y=mx+b or b = y-mx
    b_bottom = one[1]-m_bottom*one[0]
    b_top = three[1]-m_top*three[0]
    b_left = two[1]-m_left*two[0]
    b_right = four[1]-m_right*four[0]
    
    # 0 = y -mx -b
    eq_top = y - m_top*x-(b_top+radius+clearance)
    eq_bottom = y - m_bottom*x -(b_bottom-radius-clearance)
    eq_left = y -m_left*x-(b_left+radius+clearance)
    eq_right = y - m_right*x -(b_right-radius-clearance)
    
    if eq_top <= 0 and eq_bottom >= 0 and eq_left <= 0 and eq_right >= 0:
        print("Don't go in the diamond!")
        return False
    else:
        return True
    

def check_polygon(x,y):
    y = 200 - y
    one = [20,200-80]
    two = [25,200-15]
    three = [75,200-15]
    four = [100,200-50]
    five = [75,200-80]
    six = [50,200-50] 
    
    
    m_one = (one[1]-two[1])/(one[0]-two[0])
    m_two = (two[1]-three[1])/(two[0]-three[0])
    m_three = (three[1]-four[1])/(three[0]-four[0])
    m_four = (five[1]-four[1])/(five[0]-four[0])
    m_five = (six[1]-five[1])/(six[0]-five[0])
    m_six = (one[1]-six[1])/(one[0]-six[0])
    m_seven = (six[1]-three[1])/(six[0]-three[0])
    
    #solve for y intercept  y=mx+b or b = y-mx
    b_one = one[1]-m_one*one[0]
    b_two = two[1]-m_two*two[0]
    b_three = three[1]-m_three*three[0]
    b_four = four[1]-m_four*four[0]
    b_five = five[1]-m_five*five[0]
    b_six = six[1]-m_six*six[0]
    b_seven = six[1]-m_seven*six[0]
    
    # 0 = y -mx -b
    
    eq_one = y - m_one*(x+clearance+radius)-(b_one)
    eq_two = (y) - m_two*x -(b_two+clearance+radius) 
    eq_three = y -m_three*(x)-(b_three+clearance+radius)
    eq_four = y - m_four*x -(b_four-clearance-radius)
    eq_five = y - m_five*x - (b_five-clearance-radius)
    eq_six = y - (m_six*x) - (b_six-clearance-radius)
    eq_seven = y - m_seven*x-(b_seven) #interior line segment
    

    if eq_one <= 0 and eq_two <= 0 and eq_six>=0 and eq_seven >=0:
        print("Don't go in the polygon1!")
        return False
    
    if eq_three <= 0 and eq_four >= 0 and eq_five >= 0 and eq_seven <= 0:
        print("Don't go in the polygon2!")
        return False
    
    else:
        return True

def plot_workspace(x_start,y_start,x_goal,y_goal):
    img = 255 * np.ones((200, 300, 3), np.uint8)

    # Plot the diamond
    cords_square = np.array([[225 , 190 ],[200, 175 ], [225 , 160 ],[250 , 175]], dtype=np.int32)
    cv2.fillConvexPoly(img, cords_square, [0,0,0])
    
    # Plot the circle
    cv2.circle(img, (225, 50), 25, (0, 0, 0), -1)
    
    # Plot the oval
    cv2.ellipse(img, (150, 100), (40, 20), 0, 0, 360, 0, -1)
    
    # Plot the polygon
    poly1 = np.array([[20,80], [25,15],[75,15],[50,50]], dtype=np.int32)
    cv2.fillConvexPoly(img, poly1, [0,0,0])
    
    poly2 = np.array([[50,50], [75,15],[100,50],[75,80]], dtype=np.int32)
    cv2.fillConvexPoly(img, poly2, [0,0,0])
    
    # Plot rectangle
    poly3 = np.array([[95,170], [int(30.5),int(132.5)],[int(35.5),int(123.9)],[100,int(161.4)]], dtype=np.int32)
    cv2.fillConvexPoly(img, poly3, [0,0,0])

    return img

def move_up(point, radius, clearance):
    x = point[0]
    y = point[1]
    cost = 1
    if check_viableX(x) and check_viableY(y) and check_circle(x,y) and check_oval(x,y) and check_rectangle(x,y) and check_diamond(x,y)and check_polygon(x,y):
        new_point = [x, y - 1]
        return new_point, cost
    else:
        return None, None


def move_down(point, radius, clearance):
    x = point[0]
    y = point[1]
    cost = 1
    if check_viableX(x) and check_viableY(y) and check_circle(x,y) and check_oval(x,y) and check_rectangle(x,y)and check_diamond(x,y)and check_polygon(x,y):
        new_point = [x, y + 1]
        return new_point, cost
    else:
        return None, None


def move_left(point, radius, clearance):
    x = point[0]
    y = point[1]
    cost = 1
    if check_viableX(x) and check_viableY(y) and check_circle(x,y) and check_oval(x,y)and check_rectangle(x,y)and check_diamond(x,y)and check_polygon(x,y):
        new_point = [x - 1, y]
        return new_point, cost
    else:
        return None, None


def move_right(point, radius, clearance):
    x = point[0]
    y = point[1]
    cost = 1
    if check_viableX(x) and check_viableY(y) and check_circle(x,y) and check_oval(x,y)and check_rectangle(x,y)and check_diamond(x,y)and check_polygon(x,y):
        new_point = [x + 1, y]
        return new_point, cost
    else:
        return None, None


def move_up_right(point, radius, clearance):
    x = point[0]
    y = point[1]
    cost = np.sqrt(2)
    if check_viableX(x) and check_viableY(y) and check_circle(x,y) and check_oval(x,y)and check_rectangle(x,y)and check_diamond(x,y)and check_polygon(x,y):
        new_point = [x + 1, y - 1]
        return new_point, cost
    else:
        return None, None


def move_up_left(point, radius, clearance):
    x = point[0]
    y = point[1]
    cost = np.sqrt(2)
    if check_viableX(x) and check_viableY(y) and check_circle(x,y) and check_oval(x,y)and check_rectangle(x,y)and check_diamond(x,y)and check_polygon(x,y):
        new_point = [x - 1, y - 1]
        return new_point, cost
    else:
        return None, None


def move_down_right(point, radius, clearance):
    x = point[0]
    y = point[1]
    cost = np.sqrt(2)
    if check_viableX(x) and check_viableY(y) and check_circle(x,y) and check_oval(x,y)and check_rectangle(x,y)and check_diamond(x,y)and check_polygon(x,y):
        new_point = [x + 1, y + 1]
        return new_point, cost
    else:
        return None, None


def move_down_left(point, radius, clearance):
    x = point[0]
    y = point[1]
    cost = np.sqrt(2)
    if check_viableX(x) and check_viableY(y) and check_circle(x,y) and check_oval(x,y)and check_rectangle(x,y)and check_diamond(x,y) and check_polygon(x,y):
        new_point = [x - 1, y + 1]
        return new_point, cost
    else:
        return None, None


def djikstra(image, robot):
    
    radius = robot.radius
    clearance = robot.clearance
    
    start_node_pos = robot.start
    goal_node_pos = robot.goal
    
    image[start_node_pos[1], start_node_pos[0]] = [0, 255, 0]
    image[goal_node_pos[1], goal_node_pos[0]] = [0, 0, 255]
    
    start_node = Node(start_node_pos[0],start_node_pos[1])
    start_node.cost = 0

    waysIn = ways_in(goal_node_pos[0],goal_node_pos[1])
    print("Ways in", waysIn)
    
    visitedNodes = list()
    queue = [start_node]
    
    moves = ["move_up", "move_down", "move_left", "move_right",
               "move_up_right", "move_down_right", "move_up_left", "move_down_left"]
    counter = 0
    frame = 0
    
    while queue:
        current_node = get_min_node(queue)
        current_point = [current_node.x,current_node.y]
        visitedNodes.append(str(current_point))

        for move in moves:
            new_point, cost = try_move(move, current_point, radius, clearance)
            frame +=1
            if new_point is not None:
                if new_point == goal_node_pos:
                    
                    if counter < waysIn:
                        counter += 1
                        print("Goal reached " +str(counter) + " times")

                new_node = Node(new_point[0],new_point[1])
                new_node.parent = current_node

                image = fill_pixel(image, current_node.x,current_node.y)
                image[start_node_pos[1], start_node_pos[0]] = [0, 255, 0]
                image[goal_node_pos[1], goal_node_pos[0]] = [0, 0, 255]

                # update display every 75 nodes explored
                if frame % 75 == 0:
                    cv2.imshow("Map", image)
                    cv2.waitKey(1)
                
                if str(new_point) not in visitedNodes:
                    new_node.cost = cost + new_node.parent.cost
                    visitedNodes.append(str(new_point))
                    queue.append(new_node)
                else:
                    node_exist_index = node_exists(new_point[0],new_point[0], queue)
                    if node_exist_index is not None:
                        temp_node = queue[node_exist_index]
                        if temp_node.cost > cost + new_node.parent.cost:
                            temp_node.cost = cost + new_node.parent.cost
                            temp_node.parent = current_node
            else:
                continue
        if counter == waysIn:
            return new_node.parent, image
    return None, None

#################################################
start = False
goal = False

#change these values for point/rigid robot
radius = 1
clearance = 5


while start == False:
    x_start = input("Enter robot x position : ")
    x_start = int(x_start)
    y_start = input("Enter robot y position : ") 
    y_start = int(y_start)
    start = check_viableY(y_start)
    if start == True:
        start = check_viableX(x_start)
        if start ==True:
            start = check_oval(x_start,y_start)
            if start == True:
                start = check_circle(x_start,y_start)
                if start == True:
                    start = check_rectangle(x_start,y_start)
                    if start == True:
                        start = check_diamond(x_start,y_start)
                        if start == True:
                            start = check_polygon(x_start,200-y_start)
    
while goal == False:
    x_goal = input("Enter goal x position : ") 
    x_goal = int(x_goal)
    y_goal = input("Enter goal y position : ") 
    y_goal = int(y_goal)
    goal = check_viableY(y_goal)
    if goal == True:
        goal = check_viableX(x_goal)
        if goal ==True:
            goal = check_oval(x_goal,y_goal)
            if goal == True:
                goal = check_circle(x_goal,y_goal)
                if goal == True:
                    goal = check_rectangle(x_goal,y_goal)
                    if goal == True:
                        goal = check_diamond(x_goal,y_goal)
                        if goal == True:
                            goal = check_polygon(x_goal,200-y_goal)




start = t.time()
y_start = 200 - y_start
y_goal = 200 - y_goal

start_node = [x_start,y_start]
goal_node = [x_goal,y_goal]



robot1 = Robot(radius, clearance, start_node, goal_node)

workspace = plot_workspace(x_start,y_start,x_goal,y_goal)


solution, image = djikstra(workspace, robot1)
print("Time to solve: " + str(t.time()-start) + " seconds")
if solution is not None:
    parent_list = backtrack(solution)
    for parent in parent_list:
        x = parent.x
        y = parent.y
        image[y, x] = [0, 255, 0]
        cv2.imshow("Map", image)
        cv2.waitKey(25)
    cv2.waitKey(0)
    cv2.destroyAllWindows()
else:
    print("No path to goal point")




