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, 9, 7, 75), 'argparse.ArgumentParser', 'argparse.ArgumentParser', (), '', False, 'import argparse\n'), ((48, 6, 48, 39), 'numpy.zeros', 'np.zeros', ({(48, 15, 48, 28): '(512, 512, 3)', (48, 30, 48, 38): 'np.uint8'}, {}), '((512, 512, 3), np.uint8)', True, 'import numpy as np\n'), ((51, 0, 51, 40), 'cv2.namedWindow', 'cv.namedWindow', ({(51, 15, 51, 39): '"""Draw the Occupancy Map"""'}, {}), "('Draw the Occupancy Map')", True, 'import cv2 as cv\n'), ((52, 0, 52, 51), 'cv2.setMouseCallback', 'cv.setMouseCallback', ({(52, 20, 52, 44): '"""Draw the Occupancy Map"""', (52, 46, 52, 50): 'draw'}, {}), "('Draw the Occupancy Map', draw)", True, 'import cv2 as cv\n'), ((57, 0, 57, 22), 'cv2.destroyAllWindows', 'cv.destroyAllWindows', ({}, {}), '()', True, 'import cv2 as cv\n'), ((61, 0, 61, 40), 'cv2.namedWindow', 'cv.namedWindow', ({(61, 15, 61, 39): '"""Set the Starting Point"""'}, {}), "('Set the Starting Point')", True, 'import cv2 as cv\n'), ((62, 0, 62, 51), 'cv2.setMouseCallback', 'cv.setMouseCallback', ({(62, 20, 62, 44): '"""Set the Starting Point"""', (62, 46, 62, 50): 'draw'}, {}), "('Set the Starting Point', draw)", True, 'import cv2 as cv\n'), ((68, 0, 68, 22), 'cv2.destroyAllWindows', 'cv.destroyAllWindows', ({}, {}), '()', True, 'import cv2 as cv\n'), ((72, 0, 72, 19), 'cv2.namedWindow', 'cv.namedWindow', ({(72, 15, 72, 18): 'end'}, {}), '(end)', True, 'import cv2 as cv\n'), ((73, 0, 73, 30), 'cv2.setMouseCallback', 'cv.setMouseCallback', ({(73, 20, 73, 23): 'end', (73, 25, 73, 29): 'draw'}, {}), '(end, draw)', True, 'import cv2 as cv\n'), ((78, 0, 78, 22), 'cv2.destroyAllWindows', 'cv.destroyAllWindows', ({}, {}), '()', True, 'import cv2 as cv\n'), ((80, 6, 80, 60), 'cv2.resize', 'cv.resize', (), '', True, 'import cv2 as cv\n'), ((81, 10, 81, 28), 'numpy.ones', 'np.ones', ({(81, 18, 81, 27): 'img.shape'}, {}), '(img.shape)', True, 'import numpy as np\n'), ((83, 0, 83, 25), 'matplotlib.pyplot.imshow', 'plt.imshow', ({(83, 11, 83, 24): '(inv_img - img)'}, {}), '(inv_img - img)', True, 'import matplotlib.pyplot as plt\n'), ((88, 0, 88, 21), 'planning_framework.path', 'path', ({(88, 5, 88, 10): 'start', (88, 12, 88, 15): 'end', (88, 17, 88, 20): 'N_H'}, {}), '(start, end, N_H)', False, 'from planning_framework import path\n'), ((49, 9, 49, 27), 'numpy.ones', 'np.ones', ({(49, 17, 49, 26): 'img.shape'}, {}), '(img.shape)', True, 'import numpy as np\n'), ((54, 4, 54, 53), 'cv2.imshow', 'cv.imshow', ({(54, 14, 54, 38): '"""Draw the Occupancy Map"""', (54, 40, 54, 52): '(inv_im - img)'}, {}), "('Draw the Occupancy Map', inv_im - img)", True, 'import cv2 as cv\n'), ((64, 4, 64, 53), 'cv2.imshow', 'cv.imshow', ({(64, 14, 64, 38): '"""Set the Starting Point"""', (64, 40, 64, 52): '(inv_im - img)'}, {}), "('Set the Starting Point', inv_im - img)", True, 'import cv2 as cv\n'), ((74, 6, 74, 34), 'cv2.getWindowProperty', 'cv.getWindowProperty', ({(74, 27, 74, 30): 'end', (74, 32, 74, 33): '(0)'}, {}), '(end, 0)', True, 'import cv2 as cv\n'), ((75, 4, 75, 32), 'cv2.imshow', 'cv.imshow', ({(75, 14, 75, 17): 'end', (75, 19, 75, 31): '(inv_im - img)'}, {}), '(end, inv_im - img)', True, 'import cv2 as cv\n'), ((82, 22, 82, 44), 'numpy.array', 'np.array', ({(82, 31, 82, 43): 'img[:, :, (0)]'}, {}), '(img[:, :, (0)])', True, 'import numpy as np\n'), ((85, 8, 85, 26), 'numpy.array', 'np.array', ({(85, 17, 85, 25): '[sx, sy]'}, {}), '([sx, sy])', True, 'import numpy as np\n'), ((86, 6, 86, 24), 'numpy.array', 'np.array', ({(86, 15, 86, 23): '[dx, dy]'}, {}), '([dx, dy])', True, 'import numpy as np\n'), ((55, 7, 55, 21), 'cv2.waitKey', 'cv.waitKey', ({(55, 18, 55, 20): '(20)'}, {}), '(20)', True, 'import cv2 as cv\n'), ((65, 7, 65, 21), 'cv2.waitKey', 'cv.waitKey', ({(65, 18, 65, 20): '(20)'}, {}), '(20)', True, 'import cv2 as cv\n'), ((76, 7, 76, 21), 'cv2.waitKey', 'cv.waitKey', ({(76, 18, 76, 20): '(20)'}, {}), '(20)', True, 'import cv2 as cv\n'), ((34, 16, 34, 86), 'cv2.rectangle', 'cv.rectangle', ({(34, 29, 34, 32): 'img', (34, 34, 34, 48): '(x - 5, y - 5)', (34, 50, 34, 64): '(x + 5, y + 5)', (34, 66, 34, 81): '(255, 255, 255)', (34, 83, 34, 85): '(-1)'}, {}), '(img, (x - 5, y - 5), (x + 5, y + 5), (255, 255, 255), -1)', True, 'import cv2 as cv\n'), ((39, 12, 39, 82), 'cv2.rectangle', 'cv.rectangle', ({(39, 25, 39, 28): 'img', (39, 30, 39, 44): '(x - 5, y - 5)', (39, 46, 39, 60): '(x + 5, y + 5)', (39, 62, 39, 77): '(255, 255, 255)', (39, 79, 39, 81): '(-1)'}, {}), '(img, (x - 5, y - 5), (x + 5, y + 5), (255, 255, 255), -1)', True, 'import cv2 as cv\n'), ((41, 12, 41, 54), 'cv2.circle', 'cv.circle', ({(41, 22, 41, 25): 'img', (41, 27, 41, 33): '(x, y)', (41, 35, 41, 36): '(5)', (41, 38, 41, 49): '(255, 0, 0)', (41, 51, 41, 53): '(-1)'}, {}), '(img, (x, y), 5, (255, 0, 0), -1)', True, 'import cv2 as cv\n'), ((44, 12, 44, 54), 'cv2.circle', 'cv.circle', ({(44, 22, 44, 25): 'img', (44, 27, 44, 33): '(x, y)', (44, 35, 44, 36): '(5)', (44, 38, 44, 49): '(0, 255, 0)', (44, 51, 44, 53): '(-1)'}, {}), '(img, (x, y), 5, (0, 255, 0), -1)', True, 'import cv2 as cv\n')]