# -*- coding: utf-8 -*-
"""Hw1FAI.ipynb

Automatically generated by Colaboratory.

Original file is located at
    https://colab.research.google.com/drive/1FY3COCvvvw4cuGsCW_bfnd-fzLIeCxZT

```
**
Take care of the following edge cases: When Start state is goal state
when there is no solution to reach the goal state.
Take care of maze dimensions eliminate nodes going out of bound check if inside maze
Take care of invalid actions maybe?
Take care of
Check if the Path is updated in UCS and Astar if lesser cost path is found to reach the node & Not to terminate when goal is reached!
**
```
"""

from collections import deque
import heapq
#import numpy as np
#import time
import math

#Defining all global variable heree
G = {} # Graph Variable
P = {} # Parent Variable
N = [] # Nodes Variable
Dim = () # Dimensions of the graph
Algo = str() # Stores which algortihm to perform
Start = () # Starting coordinates
End = () # End coordinates
#Avail_Actions = {} # Determines Available actions of Node
#Pairwise_Distance = [] # Helps in determing cost
Heuristic = {} #Used to store heuristics for A*

# Action Dictionary - Retrieves Action Tuple based on keys
Action_Dict = {
    '1' : (1,0,0),
    '2' : (-1,0,0),
    '3' : (0,1,0),
    '4' : (0,-1,0),
    '5' : (0,0,1),
    '6' : (0,0,-1),
    '7' : (1,1,0),
    '8' : (1,-1,0),
    '9' : (-1,1,0),
    '10' : (-1,-1,0),
    '11' : (1,0,1),
    '12' : (1,0,-1),
    '13' : (-1,0,1),
    '14' : (-1,0,-1),
    '15' : (0,1,1),
    '16' : (0,1,-1),
    '17' : (0,-1,1),
    '18' : (0,-1,-1)
}

#Read file
def read_file():
  global Algo, Dim, Start, End
  #with open("../resource/asnlib/public/sample/input8.txt","r") as f:
  with open("input.txt","r") as f:
  #with open("input1.txt","r") as f:
    #print(f.readline())
    first_line = str(f.readline()).strip("\n").strip()
    Algo = str(first_line)
    second_line = str(f.readline()).strip("\n").strip()
    Dim = tuple([int(x) -1 for x in second_line.split(" ")]) #Check Bounds to remove bounds thats why -1
    third_line = str(f.readline()).strip("\n").strip()
    Start = tuple([int(x) for x in third_line.split(" ")])
    fourth_line = str(f.readline()).strip("\n").strip()
    End = tuple([int(x) for x in fourth_line.split(" ")])
    fifth_line = int(str(str(f.readline()).strip(" ").strip("\n")))
    for _ in range(fifth_line):
      n_line = str(f.readline()).strip("\n").split(" ")
      #print(n_line)
      if int(n_line[0]) <= Dim[0] and int(n_line[1]) <= Dim[1] and int(n_line[2]) <= Dim[2] and int(n_line[0]) >= 0 and int(n_line[1]) >= 0 and int(n_line[2]) >= 0:
        N.append((int(n_line[0]), int(n_line[1]), int(n_line[2])))
        #Edit to make it quicker Graph can be created here itself
        #Avail_Actions[N[-1]] =  n_line[3:]
        create_graph(N[-1], n_line[3:])



#Graph is created here
def create_graph(node,Avail_Actions):
  #global Avail_Actions, Action_Dict
  #for actions in Avail_Actions:
    #print(node)
    #print(actions)
  G[node] = []
  P[node] = [None, 100]
  for action in Avail_Actions:
    temp_tuple = tuple(map(lambda a, b: a + b, node, Action_Dict[action]))
      #if temp_tuple in N:
    if temp_tuple[0] <= Dim[0] and temp_tuple[1] <= Dim[1] and temp_tuple[2] <= Dim[2] and temp_tuple[0] >= 0 and temp_tuple[1] >= 0 and temp_tuple[2] >= 0:
        G[node].append(temp_tuple)
    #print(time.time()-start)
    #print(G[node])
#create_graph()

#Helper function to find the path and cost and print the o/p file!
def find_path_and_create_op():
 with open("output.txt", "w") as f:
   try:
    if End not in P.keys() or P[End][0] is None:
        f.write("FAIL")
        return
   except KeyError:
    f.write("FAIL")
    return
   if Start == End:
        f.write(str(0)+"\n")
        f.write(str(1)+"\n")
        f.write(str(Start[0]) + " " + str(Start[1]) + " " + str(Start[2]) + " " + str(0))
        return
   temp_op_list = []
   curr_node = End
   steps, cost = 0, 0
   while True:
     temp_list = P[curr_node]
     #print(temp_list)
     if Algo == "BFS":
      temp_str = str(curr_node[0]) + " " + str(curr_node[1]) + " " + str(curr_node[2]) + " " + str(temp_list[1])
      temp_op_list = [temp_str] + temp_op_list
      cost += temp_list[1]
     else:
      calc_cost =  14 if find_cost(temp_list[0], curr_node) == 2 else 10
      temp_str = str(curr_node[0]) + " " + str(curr_node[1]) + " " + str(curr_node[2]) + " " + str(calc_cost)
      cost += calc_cost
      if curr_node == Start:
        cost -= calc_cost
        temp_str = str(curr_node[0]) + " " + str(curr_node[1]) + " " + str(curr_node[2]) + " " + str(0)
      temp_op_list = [temp_str] + temp_op_list

     steps += 1
     if curr_node == Start:
      break
     curr_node = temp_list[0]
   #temp_op_list = reversed(temp_op_list)
   #print(temp_op_list)
   f.write(str(cost)+"\n")
   f.write(str(steps)+"\n")
   f.write("\n".join(temp_op_list))

#find_path_and_create_op()

# Helper Function to retreive cost between two nodes
def find_cost(tuple1, tuple2):
  sum = 0
  for a,b in zip(tuple1,tuple2):
    sum += abs(a-b)
  return sum

def get_distance(point1, point2):
  dist = 0
  for a,b in zip(point1, point2):
    dist += (a-b)**2
  return (dist)**(0.5)#/(Dim[0]**2 + Dim[1]**2 + Dim[2] **2)**0.5

def create_heuristics():
  for node in N:
    Heuristic[node] = math.floor(10*get_distance(End, node))
  #print(Heuristic)

#BFS search occurs here
def do_BFS():
  queue = [Start]
  P[Start] = [Start,0]
  if Start == End:
    return
  while queue:
    #print(queue)
    curr_node = queue.pop(0)
    neighbors = G[curr_node]
    for neighbor in neighbors:
      #Just make sure there is no key error in the below statement!
      try:
          if P[neighbor][0] is None:
            P[neighbor] = [curr_node, 1]
            queue.append(neighbor)
      except KeyError:
        continue
      if neighbor == End:
        break
    #print(P)

#do_BFS()

def do_UCS():
  queue = []
  #create_graph()
  heapq.heappush(queue, (0,Start))
  P[Start] = [Start,0]
  if Start == End:
    return
  #print(queue)
  while queue:
    #print(queue)
    curr_cost, curr_node = heapq.heappop(queue)
    neighbors = G[curr_node]
    for neighbor in neighbors:
      try:
          calc_cost =  14 if find_cost(curr_node, neighbor) == 2 else 10
          combined_cost = calc_cost + curr_cost
          #Just make sure there is no key error in the below statement!
          if P[neighbor][0] is None:
            P[neighbor] = [curr_node,combined_cost]
            heapq.heappush(queue, (combined_cost, neighbor))
          #Add elif about cost if needed!
          elif P[neighbor][0] is not None and int(P[neighbor][1]) > combined_cost:
            P[neighbor] = [curr_node,combined_cost]
            heapq.heappush(queue, (combined_cost, neighbor))
      except KeyError:
          continue
      #Check algorithm for temination condition!
      #if neighbor == End:
        #break
  #print(P)
#do_UCS()

def do_Astar():
  queue = []
  #create_graph()
  heapq.heappush(queue, (0,Start,0))
  P[Start] = [Start,0]
  if Start == End:
    return
  #print(queue)
  #Node_data = P.keys()
  while queue:
    #print(queue)
    comb_cost, curr_node, curr_cost = heapq.heappop(queue)
    neighbors = G[curr_node]
    for neighbor in neighbors:
      #if neighbor not in  N:
        #continue
      try:
          calc_cost =  14 if find_cost(curr_node, neighbor) == 2 else 10
          combined_cost = calc_cost + curr_cost + Heuristic[neighbor]
      #try:
        #Just make sure there is no key error in the below statement!
          if P[neighbor][0] is None:
            P[neighbor] = [curr_node,combined_cost]
            heapq.heappush(queue, (combined_cost, neighbor, calc_cost+curr_cost))
          #Add elif about cost if needed!
          elif P[neighbor][0] is not None and float(P[neighbor][1]) > float(combined_cost):
            P[neighbor] = [curr_node,combined_cost]
            heapq.heappush(queue, (combined_cost, neighbor, calc_cost+curr_cost))
      except KeyError:
          continue
      #Check algorithm for temination condition!
      #if neighbor == End:
        #break
  #print(P)
#do_Astar()


#Driver Code goes here! & Check for exceptions too here dont forget!
#start=time.time()
read_file()
#print(len(G))
#print(time.time()-start)
#create_graph()
#print(time.time()-start)
#Algo = "UCS"
if Algo == "BFS":
  do_BFS()
elif Algo == "UCS":
  do_UCS()
else:
  create_heuristics()
  do_Astar()
#print(time.time()-start)
find_path_and_create_op()
#print(time.time()-start)


#read_file()
#print(Algo)
#print(Dim)
#print(Start)
#print(End)
"""
print(N)
print(Avail_Actions)"""
