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)

[((130, 196), 'argparse.ArgumentParser', 'argparse.ArgumentParser', ([], {'description': '"""Path Planning Visualisation"""'}), "(description='Path Planning Visualisation')\n", (153, 196), False, 'import argparse\n'), ((1313, 1346), 'numpy.zeros', 'np.zeros', (['(512, 512, 3)', 'np.uint8'], {}), '((512, 512, 3), np.uint8)\n', (1321, 1346), True, 'import numpy as np\n'), ((1382, 1422), 'cv2.namedWindow', 'cv.namedWindow', (['"""Draw the Occupancy Map"""'], {}), "('Draw the Occupancy Map')\n", (1396, 1422), True, 'import cv2 as cv\n'), ((1423, 1474), 'cv2.setMouseCallback', 'cv.setMouseCallback', (['"""Draw the Occupancy Map"""', 'draw'], {}), "('Draw the Occupancy Map', draw)\n", (1442, 1474), True, 'import cv2 as cv\n'), ((1588, 1610), 'cv2.destroyAllWindows', 'cv.destroyAllWindows', ([], {}), '()\n', (1608, 1610), True, 'import cv2 as cv\n'), ((1636, 1676), 'cv2.namedWindow', 'cv.namedWindow', (['"""Set the Starting Point"""'], {}), "('Set the Starting Point')\n", (1650, 1676), True, 'import cv2 as cv\n'), ((1677, 1728), 'cv2.setMouseCallback', 'cv.setMouseCallback', (['"""Set the Starting Point"""', 'draw'], {}), "('Set the Starting Point', draw)\n", (1696, 1728), True, 'import cv2 as cv\n'), ((1863, 1885), 'cv2.destroyAllWindows', 'cv.destroyAllWindows', ([], {}), '()\n', (1883, 1885), True, 'import cv2 as cv\n'), ((1926, 1945), 'cv2.namedWindow', 'cv.namedWindow', (['end'], {}), '(end)\n', (1940, 1945), True, 'import cv2 as cv\n'), ((1946, 1976), 'cv2.setMouseCallback', 'cv.setMouseCallback', (['end', 'draw'], {}), '(end, draw)\n', (1965, 1976), True, 'import cv2 as cv\n'), ((2101, 2123), 'cv2.destroyAllWindows', 'cv.destroyAllWindows', ([], {}), '()\n', (2121, 2123), True, 'import cv2 as cv\n'), ((2131, 2185), 'cv2.resize', 'cv.resize', (['img_', '(50, 50)'], {'interpolation': 'cv.INTER_AREA'}), '(img_, (50, 50), interpolation=cv.INTER_AREA)\n', (2140, 2185), True, 'import cv2 as cv\n'), ((2196, 2214), 'numpy.ones', 'np.ones', (['img.shape'], {}), '(img.shape)\n', (2203, 2214), True, 'import numpy as np\n'), ((2261, 2286), 'matplotlib.pyplot.imshow', 'plt.imshow', (['(inv_img - img)'], {}), '(inv_img - img)\n', (2271, 2286), True, 'import matplotlib.pyplot as plt\n'), ((2365, 2386), 'planning_framework.path', 'path', (['start', 'end', 'N_H'], {}), '(start, end, N_H)\n', (2369, 2386), False, 'from planning_framework import path\n'), ((1356, 1374), 'numpy.ones', 'np.ones', (['img.shape'], {}), '(img.shape)\n', (1363, 1374), True, 'import numpy as np\n'), ((1488, 1537), 'cv2.imshow', 'cv.imshow', (['"""Draw the Occupancy Map"""', '(inv_im - img)'], {}), "('Draw the Occupancy Map', inv_im - img)\n", (1497, 1537), True, 'import cv2 as cv\n'), ((1742, 1791), 'cv2.imshow', 'cv.imshow', (['"""Set the Starting Point"""', '(inv_im - img)'], {}), "('Set the Starting Point', inv_im - img)\n", (1751, 1791), True, 'import cv2 as cv\n'), ((1983, 2011), 'cv2.getWindowProperty', 'cv.getWindowProperty', (['end', '(0)'], {}), '(end, 0)\n', (2003, 2011), True, 'import cv2 as cv\n'), ((2022, 2050), 'cv2.imshow', 'cv.imshow', (['end', '(inv_im - img)'], {}), '(end, inv_im - img)\n', (2031, 2050), True, 'import cv2 as cv\n'), ((2237, 2261), 'numpy.array', 'np.array', (['img[:, :, (0)]'], {}), '(img[:, :, (0)])\n', (2245, 2261), True, 'import numpy as np\n'), ((2296, 2314), 'numpy.array', 'np.array', (['[sx, sy]'], {}), '([sx, sy])\n', (2304, 2314), True, 'import numpy as np\n'), ((2333, 2351), 'numpy.array', 'np.array', (['[dx, dy]'], {}), '([dx, dy])\n', (2341, 2351), True, 'import numpy as np\n'), ((1545, 1559), 'cv2.waitKey', 'cv.waitKey', (['(20)'], {}), '(20)\n', (1555, 1559), True, 'import cv2 as cv\n'), ((1799, 1813), 'cv2.waitKey', 'cv.waitKey', (['(20)'], {}), '(20)\n', (1809, 1813), True, 'import cv2 as cv\n'), ((2058, 2072), 'cv2.waitKey', 'cv.waitKey', (['(20)'], {}), '(20)\n', (2068, 2072), True, 'import cv2 as cv\n'), ((844, 914), 'cv2.rectangle', 'cv.rectangle', (['img', '(x - 5, y - 5)', '(x + 5, y + 5)', '(255, 255, 255)', '(-1)'], {}), '(img, (x - 5, y - 5), (x + 5, y + 5), (255, 255, 255), -1)\n', (856, 914), True, 'import cv2 as cv\n'), ((1016, 1086), 'cv2.rectangle', 'cv.rectangle', (['img', '(x - 5, y - 5)', '(x + 5, y + 5)', '(255, 255, 255)', '(-1)'], {}), '(img, (x - 5, y - 5), (x + 5, y + 5), (255, 255, 255), -1)\n', (1028, 1086), True, 'import cv2 as cv\n'), ((1127, 1169), 'cv2.circle', 'cv.circle', (['img', '(x, y)', '(5)', '(255, 0, 0)', '(-1)'], {}), '(img, (x, y), 5, (255, 0, 0), -1)\n', (1136, 1169), True, 'import cv2 as cv\n'), ((1236, 1278), 'cv2.circle', 'cv.circle', (['img', '(x, y)', '(5)', '(0, 255, 0)', '(-1)'], {}), '(img, (x, y), 5, (0, 255, 0), -1)\n', (1245, 1278), True, 'import cv2 as cv\n')]