from planning_framework import path
import cv2 as cv
import numpy as np
import argparse
import matplotlib.pyplot as plt

parser = argparse.ArgumentParser(description="Path Planning Visualisation")

parser.add_argument(
    "-n",
    "--n_heuristic",
    default=2,
    help="Heuristic for A* Algorithm (default = 2). 0 for Dijkstra's Algorithm",
)
args = parser.parse_args()
N_H = int(args.n_heuristic)


drawing = False  # true if mouse is pressed
mode = "obs"  # if True, draw rectangle. Press 'm' to toggle to curve
ix, iy = -1, -1
sx, sy = 0, 0
dx, dy = 50, 50
# mouse callback function
def draw(event, x, y, flags, param):
    global mode, sx, sy, dx, dy, drawing

    if event == cv.EVENT_LBUTTONDOWN:
        drawing = True

    elif event == cv.EVENT_MOUSEMOVE:
        if drawing == True:
            if mode == "obs":
                cv.rectangle(img, (x - 5, y - 5), (x + 5, y + 5), (255, 255, 255), -1)

    elif event == cv.EVENT_LBUTTONUP:
        drawing = False
        if mode == "obs":
            cv.rectangle(img, (x - 5, y - 5), (x + 5, y + 5), (255, 255, 255), -1)
        elif mode == "src":
            cv.circle(img, (x, y), 5, (255, 0, 0), -1)
            sx, sy = x, y
        elif mode == "dst":
            cv.circle(img, (x, y), 5, (0, 255, 0), -1)
            dx, dy = x, y


img = np.zeros((512, 512, 3), np.uint8)
inv_im = np.ones(img.shape) * 255

cv.namedWindow("Draw the Occupancy Map")
cv.setMouseCallback("Draw the Occupancy Map", draw)
while 1:
    cv.imshow("Draw the Occupancy Map", inv_im - img)
    if cv.waitKey(20) & 0xFF == 27:
        break
cv.destroyAllWindows()
mode = "src"
img_ = img

cv.namedWindow("Set the Starting Point")
cv.setMouseCallback("Set the Starting Point", draw)
while 1:
    cv.imshow("Set the Starting Point", inv_im - img)
    if cv.waitKey(20) & 0xFF == 27:
        break
    # cv.waitKey(20)
cv.destroyAllWindows()
mode = "dst"

end = "Set the End Point"
cv.namedWindow(end)
cv.setMouseCallback(end, draw)
while cv.getWindowProperty(end, 0) >= 0:
    cv.imshow(end, inv_im - img)
    if cv.waitKey(20) & 0xFF == 27:
        break
cv.destroyAllWindows()

img = cv.resize(img_, (50, 50), interpolation=cv.INTER_AREA)
inv_img = np.ones(img.shape)
np.savetxt("map.txt", np.array(img[:, :, 0]))
plt.imshow(inv_img - img)

start = np.array([sx, sy]) * 50 // 512
end = np.array([dx, dy]) * 50 // 512

path(start, end, N_H)

[(7, 'argparse.ArgumentParser', 'argparse.ArgumentParser', ([], {'description': '"""Path Planning Visualisation"""'}), False, 'import argparse\n'), (48, 'numpy.zeros', 'np.zeros', (['(512, 512, 3)', 'np.uint8'], {}), True, 'import numpy as np\n'), (51, 'cv2.namedWindow', 'cv.namedWindow', (['"""Draw the Occupancy Map"""'], {}), True, 'import cv2 as cv\n'), (52, 'cv2.setMouseCallback', 'cv.setMouseCallback', (['"""Draw the Occupancy Map"""', 'draw'], {}), True, 'import cv2 as cv\n'), (57, 'cv2.destroyAllWindows', 'cv.destroyAllWindows', ([], {}), True, 'import cv2 as cv\n'), (61, 'cv2.namedWindow', 'cv.namedWindow', (['"""Set the Starting Point"""'], {}), True, 'import cv2 as cv\n'), (62, 'cv2.setMouseCallback', 'cv.setMouseCallback', (['"""Set the Starting Point"""', 'draw'], {}), True, 'import cv2 as cv\n'), (68, 'cv2.destroyAllWindows', 'cv.destroyAllWindows', ([], {}), True, 'import cv2 as cv\n'), (72, 'cv2.namedWindow', 'cv.namedWindow', (['end'], {}), True, 'import cv2 as cv\n'), (73, 'cv2.setMouseCallback', 'cv.setMouseCallback', (['end', 'draw'], {}), True, 'import cv2 as cv\n'), (78, 'cv2.destroyAllWindows', 'cv.destroyAllWindows', ([], {}), True, 'import cv2 as cv\n'), (80, 'cv2.resize', 'cv.resize', (['img_', '(50, 50)'], {'interpolation': 'cv.INTER_AREA'}), True, 'import cv2 as cv\n'), (81, 'numpy.ones', 'np.ones', (['img.shape'], {}), True, 'import numpy as np\n'), (83, 'matplotlib.pyplot.imshow', 'plt.imshow', (['(inv_img - img)'], {}), True, 'import matplotlib.pyplot as plt\n'), (88, 'planning_framework.path', 'path', (['start', 'end', 'N_H'], {}), False, 'from planning_framework import path\n'), (49, 'numpy.ones', 'np.ones', (['img.shape'], {}), True, 'import numpy as np\n'), (54, 'cv2.imshow', 'cv.imshow', (['"""Draw the Occupancy Map"""', '(inv_im - img)'], {}), True, 'import cv2 as cv\n'), (64, 'cv2.imshow', 'cv.imshow', (['"""Set the Starting Point"""', '(inv_im - img)'], {}), True, 'import cv2 as cv\n'), (74, 'cv2.getWindowProperty', 'cv.getWindowProperty', (['end', '(0)'], {}), True, 'import cv2 as cv\n'), (75, 'cv2.imshow', 'cv.imshow', (['end', '(inv_im - img)'], {}), True, 'import cv2 as cv\n'), (82, 'numpy.array', 'np.array', (['img[:, :, (0)]'], {}), True, 'import numpy as np\n'), (85, 'numpy.array', 'np.array', (['[sx, sy]'], {}), True, 'import numpy as np\n'), (86, 'numpy.array', 'np.array', (['[dx, dy]'], {}), True, 'import numpy as np\n'), (55, 'cv2.waitKey', 'cv.waitKey', (['(20)'], {}), True, 'import cv2 as cv\n'), (65, 'cv2.waitKey', 'cv.waitKey', (['(20)'], {}), True, 'import cv2 as cv\n'), (76, 'cv2.waitKey', 'cv.waitKey', (['(20)'], {}), True, 'import cv2 as cv\n'), (34, 'cv2.rectangle', 'cv.rectangle', (['img', '(x - 5, y - 5)', '(x + 5, y + 5)', '(255, 255, 255)', '(-1)'], {}), True, 'import cv2 as cv\n'), (39, 'cv2.rectangle', 'cv.rectangle', (['img', '(x - 5, y - 5)', '(x + 5, y + 5)', '(255, 255, 255)', '(-1)'], {}), True, 'import cv2 as cv\n'), (41, 'cv2.circle', 'cv.circle', (['img', '(x, y)', '(5)', '(255, 0, 0)', '(-1)'], {}), True, 'import cv2 as cv\n'), (44, 'cv2.circle', 'cv.circle', (['img', '(x, y)', '(5)', '(0, 255, 0)', '(-1)'], {}), True, 'import cv2 as cv\n')]