#!/usr/bin/python3

from itertools import permutations
import numpy as np
import matplotlib.pyplot as plt
from numpy import random
from PIL import Image
from matplotlib.animation import FuncAnimation #Creating frame with data
from matplotlib.animation import PillowWriter #creating and saving animation
from numpy import asarray
import os

def getconnectedDirection(mat,i,j,trackmat):

  mystr = "";

  if i != 0: #only when it is not the 0th row
    x0 = i-1
    y0 = j

  x1 = i
  y1 = j+1

  x2 = i+1
  y2 = j

  if j != 0: #only when it is not the 0th cols
    x3 = i
    y3 = j-1

  if i != 0:
    if mat[x0,y0] == 0:
      #print(str(x0),str(y0))
      if trackmat[x0,y0] != 1:
        mystr = mystr+"U"

  if mat[x1,y1] == 0:
    #print(str(x1),str(y1))
    if trackmat[x1,y1] != 1:
      mystr = mystr+"R"

  if mat[x2,y2] == 0:
    #print(str(x2),str(y2))
    if trackmat[x2,y2] != 1:
      mystr = mystr+"D"

  if j != 0:
    if mat[x3,y3] == 0:
      #print(str(x3),str(y3))
      if trackmat[x3,y3] != 1:
         mystr = mystr+"L"

  return mystr

def countX(lst, x):
  count = 0
  for element in lst:
    if(element == x):
      count = count + 1
  return count

def atexitgate(mat, starti, startj, thepath):
  atexit = False

  size = mat.shape
  row = size[0]
  cols = size[1]
  length = len(thepath)
  #print("Length of path is = "+str(length))

  if length > 1:
    if starti == 0:
      if startj in range(1,cols):
        atexit = True
    elif startj == 0:
      if starti in range(1,row):
        atexit = True
    elif starti == row-1:
      if startj in range(1,cols):
        atexit = True
    elif startj == cols-1:
      if starti in range(1,row):
        atexit = True
    else:
      atexit = False

  return atexit

def getconnectedpath(mat,start_i,start_j,traversed_matrix):
  pathlist = []
  vlist = []
  uvlist = []
  uvdirlist = []

  last_i = start_i
  last_j = start_j

  size = mat.shape
  row = size[0]
  cols = size[1]

  vlist.append((start_i,start_j))

  while True:
    #print("Start point = ",start_i,start_j)
    if(atexitgate(mat,start_i,start_j,pathlist)):
      #print("Reached the exit gate")
      break
    direction = getconnectedDirection(mat,start_i,start_j,traversed_matrix)
    #print("From(",start_i,start_j,") To ",direction)
    if(len(direction) == 0):
      #print("Hit dead end")
      #print(traversed_matrix) 
      tmppt = vlist.pop()
      pathlist.pop()
      #print("remove the point from vlist :",tmppt[0],tmppt[1])

      #print(vlist)
      previouspt = vlist[-1]
      start_i = previouspt[0]
      start_j = previouspt[1]
      last_i = start_i
      last_j = start_j
      #print("Previous point is ",start_i,start_j)
      continue

      mypt = uvlist.pop()
      #print("Starting with : ",mypt)
      start_i = mypt[0]
      start_j = mypt[1]
      popdir = uvdirlist.pop()
      #print("popdir=",popdir)
      if popdir == "D":
        last_i = mypt[0]-1  
        last_j = mypt[1]
        #print("last point",last_i,last_j)
      if popdir == "R":
        last_i = mypt[0]  
        last_j = mypt[1]-1
        #print("last point",last_i,last_j)
      if popdir == "U":
        last_i = mypt[0]-1  
        last_j = mypt[1]
        #print("last point",last_i,last_j)
      if popdir == "L":
        last_i = mypt[0]  
        last_j = mypt[1]+1
        #print("last point",last_i,last_j)
      continue
    else:
      for dir in direction:
        if dir == "D":
          pt_i = last_i+1
          pt_j = last_j
          if(traversed_matrix[pt_i,pt_j] != 1):
            uvlist.append((pt_i,pt_j)) 
            uvdirlist.append("D")
        if dir == "R":
          pt_i = last_i
          pt_j = last_j+1
          if(traversed_matrix[pt_i,pt_j] != 1):
            uvlist.append((pt_i,pt_j)) 
            uvdirlist.append("R")
        if dir == "U":
          pt_i = last_i-1
          pt_j = last_j
          if(traversed_matrix[pt_i,pt_j] != 1):
            uvlist.append((pt_i,pt_j)) 
            uvdirlist.append("U")
        if dir == "L":
          pt_i = last_i
          pt_j = last_j-1
          if(traversed_matrix[pt_i,pt_j] != 1):
            uvlist.append((pt_i,pt_j)) 
            uvdirlist.append("L")
      
    #take top most element of uvdirlist stack
    #print("Unvisited point list before pop up : ",uvlist)
    #print("Unvisited dir list before pop up : ",uvdirlist)
    mypt = uvlist.pop()
    dir = uvdirlist.pop()
    #print("unvisited point list after pop up : ",uvlist)
    #print("unvisited direction list after pop up : ",uvdirlist)
    #print("processing for ",mypt," in direction : ",dir)
    if dir == "D":
      start_i = last_i+1
      start_j = last_j
      last_i = start_i
      #print("Current point = ",start_i,start_j)
      vlist.append((start_i,start_j))
      traversed_matrix[start_i,start_j] = 1
      pathlist.append(dir)
      #print("visited list : ",vlist)
      #print("path list : ", pathlist)
      #print("=======================")
    if dir == "R":
      start_i = last_i
      start_j = last_j+1
      last_j = start_j
      #print("Current point = ",start_i,start_j)
      vlist.append((start_i,start_j))
      traversed_matrix[start_i,start_j] = 1
      pathlist.append(dir)
      #print("visited list : ",vlist)
      #print("path list : ", pathlist)
      #print("=======================")
    if dir == "U":
      start_i = last_i-1
      start_j = last_j
      last_i = start_i
      #print("Current point = ",start_i,start_j)
      vlist.append((start_i,start_j))
      traversed_matrix[start_i,start_j] = 1
      pathlist.append(dir)
      #print("visited list : ",vlist)
      #print("path list : ", pathlist)
      #print("=======================")
    if dir == "L":
      start_i = last_i
      start_j = last_j-1
      last_j = start_j
      #print("Current point = ",start_i,start_j)
      vlist.append((start_i,start_j))
      traversed_matrix[start_i,start_j] = 1
      pathlist.append(dir)
      #print("visited list : ",vlist)
      #print("path list : ", pathlist)
      #print("=======================")

  #print(traversed_matrix)
  #print(pathlist)
  return vlist,pathlist

def check_for_wall_in_maze(matrix):
    size = matrix.shape
    rows = size[0]
    cols = size[1]

    #print(rows)
    #print(cols)

    #for i in range(rows):
    #    for j in range(cols):
    #        print(matrix[i][j])

    col_index = 0
    is_first_col_zero = np.all(matrix[:, col_index] == 0)
    #print("First col=",is_first_col_zero)

    col_index = cols-1
    is_last_col_zero = np.all(matrix[:, col_index] == 0)
    #print("Last col=",is_last_col_zero)

    row_index = 0
    is_first_row_zero = all(element == 0 for element in matrix[row_index])
    #print("First row=",is_first_row_zero)

    row_index = rows-1
    is_last_row_zero = all(element == 0 for element in matrix[row_index])
    #print("Last row=",is_first_col_zero)

    flipmatrix = is_first_col_zero or is_last_col_zero or is_first_row_zero or is_last_row_zero

    #print(flipmatrix)

    return flipmatrix

def create_newpoints_for_array(arr):
    i = 1
    nx = np.array([arr[0]])
    for k in arr[i:]:
        if(i < len(arr)-1):
            a = arr[i]
            b = arr[i+1]
            c = np.linspace(a,b, num=2)
            nx = np.append(nx,a)
            nx= np.append(nx,c[1:-1])
            nx = np.append(nx,b)
            i = i + 1

    return nx

def getrobopathfromimage(imagefile):
  img = Image.open(imagefile)
  size = img.size
  width = size[0]
  height = size[1]
  #print(size[0],size[1])
  mat = asarray(img)
  #print("+++++++++++Image Matrix+++++++++++++++++++++++++")
  #print(mat)
  flip = check_for_wall_in_maze(mat)
  if flip:
    mat=mat ^ 1
    #print("+++++++++++Flipped Matrix+++++++++++++++++++++++++")
    #print(mat)

  #print("++++++++++++++++ "+ str(flip)+ "++++++++++++++++++++")

  size = mat.shape
  row = size[0]
  cols = size[1]
  traversed_matrix = random.randint(1, size=(row, cols))
  #print(row,cols)

  start_i = 0 
  start_j = 0 
  traversed_matrix[start_i,start_j] = 1
  directionlist = []

  coordinateslist,directionlist = getconnectedpath(mat,start_i,start_j,traversed_matrix)

  print("original matrix")
  print(mat)
  print("Traversal matrix")
  print(traversed_matrix)
 
  #create the image for the traversal path with backgroud blue (0,0,255)

  newimage = Image.new(mode = "RGB", size = (row, cols), color = (0, 0, 255))
  newimage = newimage.convert('RGB')

  #newimage.show()
  #Process every pixel
  for x in range(row):
    for y in range(cols):
       r, g, b = newimage.getpixel( (x,y) )
       if(x,y) in coordinateslist:
         #print("color change for ", x,y)
         new_color = (255,255,255) #define the color of interest here
         newimage.putpixel((y,x),new_color) #in images we consider (col,row) as coordinates
  #newimage.show()

  tmp = os.path.splitext(imagefile)
  newimgfile = tmp[0]+"_path"+tmp[1]
  giffile = tmp[0]+".gif";
  #print(newimgfile)
  #newimage.save(newimgfile)

  route = ""
  index = 0
  length = len(directionlist)

  for elem in directionlist:
    route = route+elem
    if index != length-1:
      route = route+"->"
    index += 1


  #print("Number of steps =",length)
  #print(coordinateslist)

  tmpx = coordinateslist[0][0]
  tmpy = coordinateslist[0][1]

  if mat[tmpx][tmpy] == 1:
      del coordinateslist[0]
      del directionlist[0]

  print("Number of steps =",length)
  print(coordinateslist)
  print(directionlist)

  coords_array = np.array(coordinateslist)

  #If no intermediate points are needed for animation
  newx = coords_array[:, 0]
  newy = coords_array[:, 1]

  #If additional imtermediate points are needed to be added for animation

  #x_coords = coords_array[:, 0]
  #y_coords = coords_array[:, 1]
  #newx = create_newpoints_for_array(x_coords)
  #newy = create_newpoints_for_array(y_coords)

  #print(newx)
  #print(newy)

  fig, ax = plt.subplots(1,1)
  ax.matshow(mat, cmap='Wistia')
  for (i, j), val in np.ndenumerate(mat):
     ax.text(j, i, val, ha='center', va='center', color='black')

  sinegraph, = ax.plot([],[],'r')

  def update_sine(i): #i stand for next value in the frame
    sinegraph.set_data(newy[:i+1],newx[:i+1])
    if i == len(newx)-1 and (newy[i],newx[i]) in coordinateslist:
      plt.plot(newy[i],newx[i])

    return sinegraph,

  ani = FuncAnimation(fig,
        update_sine,
        frames=len(newx),
        interval=20,
        repeat=True,
        blit=False)

  print("Creating GIF file for Maze Path ....")
  ani.save(giffile, dpi=100, writer=PillowWriter(fps=10))
  print("Created GIF for Maze Path successfully")

  plt.show()

  #print(route)
  #return route

filename = "maze2.png"
path = getrobopathfromimage(filename)
print(path)