solution.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import cv2\n",
    "import cv2.aruco as aruco\n",
    "import numpy as np\n",
    "import gym\n",
    "import math\n",
    "import vision_arena\n",
    "import pybullet as p\n",
    "import time"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Detecting and cropping the image"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Image(is_first = False):\n",
    "    global A1, A2, env\n",
    "    if is_first:\n",
    "        print(\"Instructions:\")\n",
    "        print(\"Crop The Image To Arena Size (Removing Additional Black Stripes).\")\n",
    "        img = env.camera_feed()\n",
    "        A1 = np.array(img.shape[:2])\n",
    "        \n",
    "        r = cv2.selectROI(img)\n",
    "        crop = img[int(r[1]):int(r[1]+r[3]),int(r[0]):int(r[0]+r[2])]\n",
    "        \n",
    "        A2 = np.array(crop.shape[:2])\n",
    "        A1 = (A1 - A2) / 2\n",
    "        \n",
    "        cv2.destroyAllWindows()\n",
    "    else:\n",
    "        img = env.camera_feed()\n",
    "        return img"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Required postion and vectors of bot"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Bot_Pos():\n",
    "    global A1, A2, Last, aruco_dict\n",
    "    while True:\n",
    "        img = Image()\n",
    "        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)\n",
    "        corners, ids, _ = aruco.detectMarkers(gray, aruco_dict, parameters=aruco.DetectorParameters_create())\n",
    "        try:\n",
    "            for i in range(len(ids)):\n",
    "                id = ids[i][0]\n",
    "                if id == 107:\n",
    "                    L = corners[i][0]\n",
    "                    X = int((L[0][0]+L[2][0])/2)\n",
    "                    Y = int((L[0][1]+L[2][1])/2)\n",
    "\n",
    "                    MX = int((Y-A1[1])/(A2[1]/9))\n",
    "                    MY = int((X-A1[0])/(A2[0]/9))\n",
    "\n",
    "                    X1 = int(((L[0][0]+L[1][0])/2))\n",
    "                    Y1 = int(((L[0][1]+L[1][1])/2))\n",
    "                    X2 = int(((L[2][0]+L[3][0])/2))\n",
    "                    Y2 = int(((L[2][1]+L[3][1])/2))\n",
    "                    return (X1,Y1),(X2,Y2),(X,Y),(MX,MY)\n",
    "        except:\n",
    "            if Last in [[0,0],[0,8],[8,8],[8,0]]:\n",
    "                Reverse()\n",
    "            else:\n",
    "                Forward()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Decides the end postion according to start position"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "def stop(start):\n",
    "    if start==(4,0):\n",
    "        return (4,3)\n",
    "    elif start==(4,8):\n",
    "        return (4,5)\n",
    "    elif start==(0,4):\n",
    "        return (3,4)\n",
    "    else:\n",
    "        return (5,4)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Array of arena using opencv color detection"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Detection():\n",
    "    global A, A1, A2, A3\n",
    "    Dict = {0:\"Red\",1:\"Yellow\"}\n",
    "    List = [[1,2,3],[4,5,6]]\n",
    "\n",
    "    print(\"Take Double Color Sample For Both Colours (Red, Yellow) One By One.\")\n",
    "    thresh = 15\n",
    "\n",
    "    for i in range(2):\n",
    "        print(\"Select\",Dict[i],\"Color\",end = \"\\t\")\n",
    "\n",
    "        img = Image()\n",
    "\n",
    "        r1 = cv2.selectROI(img)\n",
    "        crop1 = img[int(r1[1]):int(r1[1]+r1[3]),int(r1[0]):int(r1[0]+r1[2])]\n",
    "\n",
    "        lower1 = np.array([crop1[:,:,0].min()-thresh,crop1[:,:,1].min()-thresh,crop1[:,:,2].min()-thresh])\n",
    "        upper1 = np.array([crop1[:,:,0].max()+thresh,crop1[:,:,1].max()+thresh,crop1[:,:,2].max()+thresh])\n",
    "\n",
    "        r2 = cv2.selectROI(img)\n",
    "        crop2 = img[int(r2[1]):int(r2[1]+r2[3]),int(r2[0]):int(r2[0]+r2[2])]\n",
    "\n",
    "        lower2 = np.array([crop2[:,:,0].min()-thresh,crop2[:,:,1].min()-thresh,crop2[:,:,2].min()-thresh])\n",
    "        upper2 = np.array([crop2[:,:,0].max()+thresh,crop2[:,:,1].max()+thresh,crop2[:,:,2].max()+thresh])\n",
    "\n",
    "        print(Dict[i],\"Color Selected Sucessfully.\")\n",
    "\n",
    "        blur = cv2.bilateralFilter(img,9,75,75) \n",
    "        mask = cv2.bitwise_or(cv2.inRange(blur, lower1, upper1), cv2.inRange(blur, lower2, upper2))\n",
    "        mask = cv2.dilate(mask, np.ones((5,5), dtype = np.uint8), iterations = 1)\n",
    "        mask = cv2.erode(mask, np.ones((3,3), dtype = np.uint8), iterations = 1)\n",
    "\n",
    "        res = cv2.bitwise_and(img, img, mask = mask)\n",
    "\n",
    "        cv2.imshow(\"Detection\",res)\n",
    "        print(\"Press Any Key To Continue.\")\n",
    "        cv2.waitKey(0)\n",
    "        cv2.destroyAllWindows()\n",
    "\n",
    "        test = cv2.cvtColor(res, cv2.COLOR_BGR2GRAY)\n",
    "        _, test = cv2.threshold(test, 5, 95, cv2.THRESH_BINARY)\n",
    "        contours, _ = cv2.findContours(test, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)\n",
    "\n",
    "        for contour in contours:\n",
    "            area = cv2.contourArea(contour)\n",
    "            if area>100:\n",
    "                rect = cv2.minAreaRect(contour)\n",
    "                a=rect[1][0]*rect[1][1]\n",
    "                m = cv2.moments(contour)\n",
    "                Y = (m[\"m10\"]/m[\"m00\"])\n",
    "                X = (m[\"m01\"]/m[\"m00\"])\n",
    "\n",
    "                y = int((Y-A1[0])/(A2[0]/9))\n",
    "                x = int((X-A1[1])/(A2[1]/9))\n",
    "\n",
    "                A3[y][x][0] = X\n",
    "                A3[y][x][1] = Y\n",
    "\n",
    "                if (area/a) > 0.9:\n",
    "                    A[x][y] = List[i][0]\n",
    "                elif (area/a) <= 0.9 and (area/a) > 0.77:\n",
    "                    A[x][y] = List[i][1]\n",
    "                else:\n",
    "                    A[x][y] = List[i][2]\n",
    "\n",
    "        print(\"Shape Detection For Color\",Dict[i],\"Done Sucessfully.\")\n",
    "\n",
    "    A3[4][4][0] = A2[0]/2+A1[0]\n",
    "    A3[4][4][1] = A2[1]/2+A1[1]\n",
    "\n",
    "    A3[4][0][0] = A3[0][3][1]\n",
    "    A3[4][0][1] = A3[1][4][0]\n",
    "\n",
    "    A3[8][4][0] = A3[4][7][1]\n",
    "    A3[8][4][1] = A3[3][8][0]\n",
    "\n",
    "    A3[4][8][0] = A3[8][3][1]\n",
    "    A3[4][8][1] = A3[7][4][0]\n",
    "\n",
    "    A3[0][4][0] = A3[4][1][1]\n",
    "    A3[0][4][1] = A3[3][0][0]\n",
    "\n",
    "    cv2.destroyAllWindows()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Returns the correct neighbour of current position "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[(4, 5), (4, 7)]"
      ]
     },
     "execution_count": 6,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "def neighbours(t,rot):\n",
    "    lst=[]\n",
    "    i=t[0]\n",
    "    j=t[1]\n",
    "#     if j==0 :\n",
    "       \n",
    "#         if i==4 or i==0 :\n",
    "#             lst.append((i,j+1))\n",
    "        \n",
    "#         if i!=0:\n",
    "#             lst.append((i-1,j)\n",
    "    if j==0:\n",
    "        if i==4  or i==0:\n",
    "            lst.append((i,j+1))              \n",
    "        if i!=0 :\n",
    "            if i!=4:\n",
    "                lst.append((i-1,j))\n",
    "            elif i==4 and rot==False:\n",
    "                lst.append((i-1,j))\n",
    "    if j==8 :\n",
    "        if i==4 or i==8:\n",
    "            lst.append((i,j-1))\n",
    "        if i!=8:\n",
    "            if i!=4:\n",
    "                lst.append((i+1,j))\n",
    "            elif i==4 and rot==False:\n",
    "                lst.append((i+1,j))\n",
    "            \n",
    "\n",
    "    if j in range(1,8) and i<4:\n",
    "        if i==0:\n",
    "            if j==4 :\n",
    "                if rot==False:\n",
    "                    lst.append((i,j+1))\n",
    "                lst.append((i+1,j))\n",
    "            if j!=4:\n",
    "                lst.append((i,j+1))\n",
    "        if i==1:\n",
    "            lst.append((i+1,j))\n",
    "            lst.append((i-1,j))\n",
    "        if i==2:\n",
    "            if j==4 :\n",
    "                if rot==True:\n",
    "                    lst.append((i+1,j))\n",
    "                if rot==False:\n",
    "                    lst.append((i,j+1))\n",
    "                lst.append((i-1,j))\n",
    "                \n",
    "            if j==6:\n",
    "                lst.append((i+1,j))\n",
    "            elif j!=4:\n",
    "                lst.append((i,j+1))\n",
    "        if i==3:\n",
    "            if j==2:\n",
    "                lst.append((i-1,j))\n",
    "            if j==6:\n",
    "                lst.append((i+1,j))\n",
    "                \n",
    "            \n",
    "            \n",
    "    elif j in range(1,8) and i>4:\n",
    "        if i==8:\n",
    "            \n",
    "            if j==4 :\n",
    "                if rot==False:\n",
    "                    lst.append((i,j-1))\n",
    "                lst.append((i-1,j))\n",
    "            \n",
    "            if j!=4:\n",
    "                \n",
    "                lst.append((i,j-1))\n",
    "            \n",
    "            \n",
    "#             if j==4 :\n",
    "#                 lst.append((i-1,j))\n",
    "#             lst.append((i,j-1))\n",
    "        if i==7:\n",
    "            lst.append((i-1,j))\n",
    "            lst.append((i+1,j))\n",
    "        if i==6:\n",
    "            if j==4 :\n",
    "                if rot==True:\n",
    "                    lst.append((i-1,j))\n",
    "                if rot==False:\n",
    "                    lst.append((i,j-1))\n",
    "                lst.append((i+1,j))\n",
    "               \n",
    "#             if j==4 :\n",
    "#                 lst.append((i,j-1))\n",
    "#                 lst.append((i+1,j))\n",
    "            if j==2:\n",
    "                lst.append((i-1,j))\n",
    "            elif j!=4:\n",
    "                lst.append((i,j-1))\n",
    "        if i==5:\n",
    "            if j==2 :\n",
    "                lst.append((i-1,j))\n",
    "            if j==6:\n",
    "                lst.append((i+1,j))\n",
    "    if i==4 and j not in[0,8] :\n",
    "        if j ==7:\n",
    "            \n",
    "            lst.append((i,j-1))\n",
    "            lst.append((i,j+1))\n",
    "        elif j ==1:\n",
    "            \n",
    "            lst.append((i,j+1))\n",
    "            lst.append((i,j-1))\n",
    "        elif j == 6:\n",
    "            if rot==False:\n",
    "                lst.append((i+1,j))\n",
    "            if rot==True:\n",
    "                lst.append((i,j-1))\n",
    "            lst.append((i,j+1))\n",
    "#             lst.append((i+1,j))\n",
    "#             lst.append((i,j+1))\n",
    "        elif j == 2:\n",
    "            if rot==False:\n",
    "                lst.append((i-1,j))\n",
    "            if rot==True:\n",
    "                lst.append((i,j+1))\n",
    "            lst.append((i,j-1))\n",
    "            \n",
    "    if i==4 and ( j==3 or j==5):\n",
    "        lst.append((4,4))\n",
    "    if(i==4 and j==4):\n",
    "        return []\n",
    "        \n",
    "        \n",
    "    \n",
    "        \n",
    "    return lst\n",
    "neighbours((4,6),True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "def nodes(t):\n",
    "    return A[t[0]][t[1]]\n",
    "    \n",
    "            "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [],
   "source": [
    "def visited_in_path(path,node):\n",
    "    if node in path:\n",
    "        return False\n",
    "    return True"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# End position according to start "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [],
   "source": [
    "def End_list(start):\n",
    "    x=start[0]\n",
    "    y=start[1]\n",
    "    End=[]\n",
    "    if x==0 and y==4:\n",
    "        End.append((0,3))\n",
    "        End.append((2,3))\n",
    "    elif x==4 and y==0:\n",
    "        End.append((5,0))\n",
    "        End.append((5,2))\n",
    "    elif x==8 and y==4:\n",
    "        End.append((8,5))\n",
    "        End.append((6,5))\n",
    "    elif x==4 and y==8:\n",
    "        End.append((3,8))\n",
    "        End.append((3,6))\n",
    "    return End\n",
    "                "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# End Row according to start"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "def End_row(start):\n",
    "    x=start[0]\n",
    "    y=start[1]\n",
    "    End=[]\n",
    "    if x==0 and y==4:\n",
    "        End.append((0,3))\n",
    "        End.append((2,3))\n",
    "        End.append((0,4))\n",
    "        End.append((1,4))\n",
    "        End.append((2,4))\n",
    "    elif x==4 and y==0:\n",
    "        End.append((5,0))\n",
    "        End.append((5,2))\n",
    "        End.append((4,0))\n",
    "        End.append((4,1))\n",
    "        End.append((4,2))\n",
    "    elif x==8 and y==4:\n",
    "        End.append((8,5))\n",
    "        End.append((6,5))\n",
    "        End.append((6,4))\n",
    "        End.append((7,4))\n",
    "        End.append((8,4))\n",
    "    elif x==4 and y==8:\n",
    "        End.append((3,8))\n",
    "        End.append((3,6))\n",
    "        End.append((4,6))\n",
    "        End.append((4,7))\n",
    "        End.append((4,8))\n",
    "    return End"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [],
   "source": [
    "def init(is_first = False):\n",
    "    global Interpretation, A, A1, A2, A3, env, aruco_dict, Start, End, Last\n",
    "    \n",
    "    if is_first:\n",
    "        Interpretation = {\"Black\":0, \"SR\":1, \"CR\":2, \"TR\":3, \"SY\":4, \"CY\":5, \"TY\":6}\n",
    "        A = np.zeros([9,9], dtype = np.int32) # Arena\n",
    "        A1 = np.zeros([2], dtype = np.int32) # Black Stripes Thickness\n",
    "        A2 = np.zeros([2], dtype = np.int32) # Image Size\n",
    "        A3 = np.zeros([9,9,2], dtype = np.int32) # Node's Co-Orinates in Pixel Form\n",
    "\n",
    "        env = gym.make(\"vision_arena-v0\")\n",
    "        aruco_dict = aruco.Dictionary_get(aruco.DICT_ARUCO_ORIGINAL)\n",
    "\n",
    "        Image(True)\n",
    "        Detection()\n",
    "    else:\n",
    "        env.remove_car()\n",
    "        env.respawn_car()\n",
    "        _ = env.camera_feed()\n",
    "        \n",
    "    _, _, _, (X,Y) = Bot_Pos()\n",
    "    \n",
    "    Start = [X,Y]\n",
    "    Last = [X,Y]\n",
    "\n",
    "    if [X,Y] == [0,4]:\n",
    "        End = [[X,Y-1],[X+2,Y-1]]\n",
    "    elif [X,Y] == [4,8]:\n",
    "        End = [[X-1,Y],[X-1,Y-2]]\n",
    "    elif [X,Y] == [8,4]:\n",
    "        End = [[X,Y+1],[X-2,Y+1]]\n",
    "    elif [X,Y] == [4,0]:\n",
    "        End = [[X+1,Y],[X+1,Y+2]]\n",
    "\n",
    "    \n",
    "    print(\"Start\",Start)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Instructions:\n",
      "Crop The Image To Arena Size (Removing Additional Black Stripes).\n",
      "Take Double Color Sample For Both Colours (Red, Yellow) One By One.\n",
      "Select Red Color\tRed Color Selected Sucessfully.\n",
      "Press Any Key To Continue.\n",
      "Shape Detection For Color Red Done Sucessfully.\n",
      "Select Yellow Color\tYellow Color Selected Sucessfully.\n",
      "Press Any Key To Continue.\n",
      "Shape Detection For Color Yellow Done Sucessfully.\n",
      "Start [8, 4]\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "array([[5, 5, 4, 2, 7, 2, 1, 3, 4],\n",
       "       [4, 0, 0, 0, 6, 0, 0, 0, 4],\n",
       "       [3, 0, 5, 4, 1, 1, 6, 0, 2],\n",
       "       [3, 0, 4, 0, 3, 0, 5, 0, 1],\n",
       "       [7, 1, 5, 4, 8, 6, 1, 2, 7],\n",
       "       [3, 0, 1, 0, 6, 0, 3, 0, 6],\n",
       "       [4, 0, 2, 4, 6, 3, 2, 0, 2],\n",
       "       [4, 0, 0, 0, 1, 0, 0, 0, 3],\n",
       "       [6, 1, 4, 5, 7, 6, 5, 1, 1]])"
      ]
     },
     "execution_count": 12,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "init(True)\n",
    "A[4][0]=A[0][4]=A[8][4]=A[4][8]=7\n",
    "A[4][4]=8\n",
    "_, _, _,(X,Y) = Bot_Pos()\n",
    "start=(X,Y)\n",
    "sx=start[0]\n",
    "sy=start[1]\n",
    "st=stop(start)\n",
    "End=End_list(start)\n",
    "Endrow=End_row(start)\n",
    "rot=False\n",
    "\n",
    "A"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Algo for shortest path from intial to final position"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [],
   "source": [
    "def bfs(s,d):\n",
    "    result=list(list())\n",
    "    q=list(list())\n",
    "    path=list()\n",
    "    path.append(s)\n",
    "    q.append(path)\n",
    "    global rot\n",
    "    k=0\n",
    "    while len(q)!=0:\n",
    "        path=q[0]\n",
    "        q.pop(0)\n",
    "        last=path[-1]\n",
    "        if last in End:\n",
    "            rot=True\n",
    "        if nodes(last)==d and len(path)>1:\n",
    "            result.append(path)\n",
    "            k+=1\n",
    "            if k==1:\n",
    "                return result\n",
    "            \n",
    "        nbrs=neighbours(last,rot)\n",
    "        \n",
    "        for nbr in nbrs:\n",
    "            if(visited_in_path(path,nbr) and nodes(nbr)!=0):\n",
    "                newpath=[]\n",
    "                newpath.extend(path)\n",
    "                newpath.append(nbr)\n",
    "                q.append(newpath)\n",
    "                \n",
    "        "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Movement Functions"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Distance(a1,b1,a2,b2):\n",
    "    return ((b2-b1)**2 + (a2-a1)**2)**(1/2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Angle(a1,b1,a2,b2):\n",
    "    bot_vector = complex(a1,b1)\n",
    "    path_vector = complex(a2,b2)\n",
    "    angle = np.angle(path_vector/bot_vector)\n",
    "    angle = (angle*180)/3.14\n",
    "    return angle"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Forward(d=20):\n",
    "    global env\n",
    "    speed = min(10,max(d-12,5))\n",
    "    env.move_husky(speed,speed,speed,speed)\n",
    "    for _ in range(min(5,d-11)):\n",
    "        p.stepSimulation()\n",
    "    env.move_husky(0,0,0,0)\n",
    "    p.stepSimulation()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Reverse(d=20):\n",
    "    global env\n",
    "    speed = min(10,max(d-12,5))\n",
    "    env.move_husky(-speed,-speed,-speed,-speed)\n",
    "    for _ in range(min(5,d-11)):\n",
    "        p.stepSimulation()\n",
    "    env.move_husky(0,0,0,0)\n",
    "    p.stepSimulation()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Left(theta):\n",
    "    global env\n",
    "    speed = min(17,theta+2)\n",
    "    env.move_husky(-speed,speed,-speed,speed)\n",
    "    for _ in range(5):\n",
    "        p.stepSimulation()\n",
    "    env.move_husky(0,0,0,0)\n",
    "    p.stepSimulation()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Right(theta):\n",
    "    global env\n",
    "    speed = min(17,theta+2)\n",
    "    env.move_husky(speed,-speed,speed,-speed)\n",
    "    for _ in range(5):\n",
    "        p.stepSimulation()\n",
    "    env.move_husky(0,0,0,0)\n",
    "    p.stepSimulation()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Move(Best_Path):\n",
    "    global A3, Last, env\n",
    "    for i,j in Best_Path:\n",
    "        X,Y = A3[i][j]\n",
    "        while True:\n",
    "            (X1,Y1), (X2,Y2), (MX,MY), (x,y) = Bot_Pos()\n",
    "            Last = [x,y]\n",
    "            d = Distance(X,Y,MX,MY)\n",
    "            if d > 12:\n",
    "                theta = Angle((X1-X2),(Y1-Y2),(X - MX),(Y - MY))\n",
    "                img = Image()\n",
    "                if (theta <= 5 and theta >=-5):\n",
    "                    Forward(int(d))\n",
    "                elif d < 24 and theta >= 172 and theta <= -172:\n",
    "                     break\n",
    "                elif theta >= 130 or theta <= -130:\n",
    "                    if theta >= 175 or theta <= -175:\n",
    "                        Reverse(int(d))\n",
    "                    elif theta >= 130:\n",
    "                        Left(int(180-theta))\n",
    "                    elif theta <= -130:\n",
    "                        Right(int(180-abs(theta)))\n",
    "                elif theta < 0:\n",
    "                    Left(-int(theta))\n",
    "                elif theta > 0:\n",
    "                    Right(int(theta))\n",
    "            else:\n",
    "                break"
   ]
  },
  {
   "cell_type": "raw",
   "metadata": {},
   "source": [
    "def Run():\n",
    "    global Interpretation, env, Open, Closed, Paths\n",
    "    while True:\n",
    "        Node = Interpretation[env.roll_dice()]\n",
    "        _, _, _,(X,Y) = Bot_Pos()\n",
    "        if [X,Y] not in End:\n",
    "            Open = [[X,Y]]\n",
    "            Closed = []\n",
    "            Paths = [[[X,Y]]]\n",
    "            check, best_path = Path(Node)\n",
    "            if best_path != None and not check:\n",
    "                print(Node, best_path[1:])\n",
    "                Move(best_path[1:])\n",
    "            elif check:\n",
    "                print(Node, best_path[1:])\n",
    "                Move(best_path[1:])\n",
    "                if best_path[-1] == [4,4]:\n",
    "                    print(\"End\",[4,4])\n",
    "                    return None\n",
    "                End_Run()\n",
    "                print(\"End\",[4,4])\n",
    "                return None\n",
    "            else:\n",
    "                print(Node,None)\n",
    "        else:\n",
    "            best_path = End_Path(X,Y,Node)\n",
    "            if best_path!= None:\n",
    "                print(Node, best_path)\n",
    "                Move(best_path)\n",
    "                if best_path[-1] == [4,4]:\n",
    "                    print(\"End\",[4,4])\n",
    "                    return None\n",
    "                End_Run()\n",
    "                print(\"End\",[4,4])\n",
    "                return None\n",
    "            else:\n",
    "                print(Node,None)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {},
   "outputs": [],
   "source": [
    "bot=start"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Run():\n",
    "    global Interpretation,env,bot,rot\n",
    "    while bot!=st:\n",
    "        d=Interpretation[env.roll_dice()]\n",
    "        result=bfs(bot,d)\n",
    "        print(d)\n",
    "        print(result)\n",
    "        if result!=None:\n",
    "            Move(result[0][1:])\n",
    "        if  result!=None:\n",
    "            bot=result[-1][-1]\n",
    "        if result==None and bot not in Endrow:\n",
    "            rot=False\n",
    "    \n",
    "        \n",
    "        \n",
    "        "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "4\n",
      "[[(8, 4), (8, 3), (8, 2)]]\n",
      "3\n",
      "[[(8, 2), (8, 1), (8, 0), (7, 0), (6, 0), (5, 0)]]\n",
      "2\n",
      "[[(5, 0), (4, 0), (3, 0), (2, 0), (1, 0), (0, 0), (0, 1), (0, 2), (0, 3)]]\n",
      "2\n",
      "[[(0, 3), (0, 4), (0, 5)]]\n",
      "1\n",
      "[[(0, 5), (0, 6)]]\n",
      "1\n",
      "[[(0, 6), (0, 7), (0, 8), (1, 8), (2, 8), (3, 8)]]\n",
      "1\n",
      "[[(3, 8), (4, 8), (4, 7), (4, 6)]]\n",
      "1\n",
      "[[(4, 6), (5, 6), (6, 6), (6, 5), (6, 4), (7, 4)]]\n",
      "6\n",
      "[[(7, 4), (6, 4)]]\n",
      "1\n",
      "[[(6, 4), (7, 4)]]\n",
      "3\n",
      "None\n",
      "6\n",
      "[[(7, 4), (6, 4)]]\n",
      "6\n",
      "[[(6, 4), (5, 4)]]\n"
     ]
    }
   ],
   "source": [
    "if __name__ == \"__main__\":\n",
    "    Run()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[5, 5, 4, 2, 7, 2, 1, 3, 4],\n",
       "       [4, 0, 0, 0, 6, 0, 0, 0, 4],\n",
       "       [3, 0, 5, 4, 1, 1, 6, 0, 2],\n",
       "       [3, 0, 4, 0, 3, 0, 5, 0, 1],\n",
       "       [7, 1, 5, 4, 8, 6, 1, 2, 7],\n",
       "       [3, 0, 1, 0, 6, 0, 3, 0, 6],\n",
       "       [4, 0, 2, 4, 6, 3, 2, 0, 2],\n",
       "       [4, 0, 0, 0, 1, 0, 0, 0, 3],\n",
       "       [6, 1, 4, 5, 7, 6, 5, 1, 1]])"
      ]
     },
     "execution_count": 24,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "A"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}