import numpy as np
import pandas as pd
import math

class Edge:
    def __init__(self, targetNode, cost):
        self.node = targetNode
        self.cost = cost

class Node:
    def __init__(self, value, position):
        self.value = value
        self.position = position
        self.parent = 0
        self.isVisited = False
        self.gScore = 0
        self.edgeList = []

    def Connect(self, targetNode, cost):
        newEdge = Edge(targetNode, cost)
        self.edgeList.append(newEdge)

class Graph:
    def __init__(self, nodes):
        self.nodeList = nodes

    def ConnectNodes(self, startNode, endNode, cost):
        startNode.Connect(endNode, cost)

    def DijkstraSearch(self, startNode, endNode):
        for i in self.nodeList:
          i.parent = 0
          i.gScore = math.inf
          i.isvisited = False

        startNode.gScore = 0

        queueList = self.nodeList

        while len(queueList) != 0:
          currentVertex = 0
          minimum = 9999999
          for i in queueList:
            if i.gScore < minimum:
              currentVertex = i
              minimum = i.gScore
          currentVertex.isVisited = True

          if currentVertex == endNode:
            break

          for e in currentVertex.edgeList:
            if e.node.isVisited == False:
              newCost = currentVertex.gScore + e.cost
              if newCost < e.node.gScore:
                e.node.gScore = newCost
                e.node.parent = currentVertex
                parent = currentVertex
          queueList.remove(currentVertex)
        pathList = []

        currentPathNode = endNode
        while currentPathNode != 0:
          pathList.append(currentPathNode)
          currentPathNode = currentPathNode.parent

        return pathList

def main():
    print("pathfinding in python")
    # list of items
    a = [['A','B','C','D','E'],['F','G','H','I','J'],['K','L','M','N','O']]

    # empty list
    lNodes = []

    # iterate through the number of items in 'a' list
    for i in range(len(a)):
        # iterates the number of items in each sublist
        for j in range(len(a[i])):
            # create a node for the position
            lNodes.append(Node(a[i][j], [i, j]))
            # print the position of the new node
            print(i,j)

    # create new graph with the nodes
    g = Graph(lNodes)

    # iterates through the number of items in the nodeList
    for i in range(len(g.nodeList)):
        # iterates throught the number of items in the nodeList
        for j in range(len(g.nodeList)):
            # check its not the same nodes
            if g.nodeList[i] != g.nodeList[j]:
                # calc x dist
                distX = g.nodeList[j].position[0] - g.nodeList[i].position[0]
                #calc y disst
                distY = g.nodeList[j].position[1] - g.nodeList[i].position[1]
                # x ^ 2 + y ^ 2
                distance = distX**2 + distY**2
                # if its the next node
                if distance <= 1:
                    # connect the nodes
                    g.ConnectNodes(g.nodeList[i], g.nodeList[j], distance)

    # print edge list
    for i in range(len(g.nodeList)):
        for j in g.nodeList[i].edgeList:
            print(j.node.value)
        print("\n")

    startNode = g.nodeList[0]
    endNode = g.nodeList[11]

    path = g.DijkstraSearch(startNode, endNode)

    for i in path:
      print(i.value)


if __name__ == "__main__":
    main()