import Main.{allNodes, board, width, world}

import scala.collection.mutable
import scala.io.Source


object Main extends App {

  val filename = "C:\\Users\\Sheenah\\IdeaProjects\\untitled3\\TrainingSets\\6.txt"
  val lines = Source.fromFile(filename).getLines.toList
  val height = lines(0).trim().toInt
  val width = lines(1).trim().toInt
  val board = lines.drop(2).flatten.filterNot((x: Char) => x.isWhitespace)

  println(board)
  val allNodes = for {node <- board.zipWithIndex } yield
    if (node._1 == 'x') WorldNode(node._2%width, node._2/width.floor.toInt, true)
    else WorldNode(node._2%width, node._2/width.floor.toInt, false)
  println(allNodes)

  val world = new World(width, height, board)

//  allNodes.foreach(x => println(x.x.toString + x.y.toString + x.isWall.toString))


  println("START: " + world.start)
  doEverything(new mutable.PriorityQueue[WorldNode]() ++= world.getNeighbours(world.start), Set(world.start), 1, Map.empty)
//  def worldNodePriority(node: WorldNode) = -node.totalCost
//  def Astar(world: World): Unit = {
//
//
//  }
  def doEverything(openList: mutable.PriorityQueue[WorldNode], closedList: Set[WorldNode], currentGScore: Int, path: Map[WorldNode, WorldNode]): Unit = {
    println("*****************")
    println("*****************")
    println("EXPANSION NUMBER: " + currentGScore)
//    println("OPEN LIST: " + openList)
    openList.foreach(_.update(currentGScore))
  println("OPEN LIST START")
    openList.foreach(x => println(x + " " + x.distanceToGoal.toString + " " + x.distanceFromStart.toString + " " + x.totalCost.toString))
  println(openList.clone().dequeueAll)
  println("OPEN LIST END")
  println("OPEN LIST MAX")
  println(openList.max)
//    println("CLOSED LIST: " + closedList)
    if(openList.isEmpty) {
      println("NO PATH TO GOAL")
      return
    }
    val currentNode = openList.dequeue()
    println("CURRENT NODE:" + currentNode)
    println("Distance to goal: " + currentNode.distanceToGoal.toString)
    println("Distance from start: " + currentNode.distanceFromStart.toString)
    println("Total cost: " + currentNode.totalCost.toString)
    if(currentNode == world.goal) println("SUCCESS")
    else {
      val newElements = world.getNeighbours(currentNode) diff closedList
      doEverything(new mutable.PriorityQueue[WorldNode]() ++= (openList.toSet ++ newElements),
        closedList + currentNode,
        currentGScore+1,
        path)
    }
  }
//  val foo = WorldNode(1,0,false)
//  foo.totalCost = 10
//  val bar = WorldNode(0,1 ,false)
//  bar.totalCost = 20
//  println("foo(10) compare bar(20)")
//  println(foo compare bar)
//  println("bar(20) compare foo(10)")
//  println(bar compare foo)
//    println("TEST: " + test)
//    else if (openList.isEmpty) println("NO PATH TO GOAL")
//    else {
//    }

//  openList.foreach(println)
//  openList.foreach(x => println(x.distanceToGoal))
//  println(openList.head)
//
//  val openList2 = world.getNeighbours(openList.head) ::: openList
//
//  for (node <- openList2) {
//    node.distanceToGoal = Math.abs(world.goal.x - node.x) + Math.abs(world.goal.y - node.y)
//  }
//  val heap = new mutable.PriorityQueue[WorldNode]()
//
//  for(node <- openList2) {
//    heap.enqueue(node)
//  }
//  for (node <- heap) {
//    println(node + node.distanceToGoal.toString)
//  }
//  val dxstart = start.x - goal.x
//  val dystart = start.y - goal.y
//
//  println(start)
//  val dx1 = foo1.x - goal.x
//  val dy1 = foo1.y - goal.y
//  val cross1 = Math.abs(dx1*dystart - dxstart*dy1)
//  println(cross1)
 //  val dx2 = foo2.x - goal.x
//  val dy2 = foo2.y - goal.y
//  val cross2 = Math.abs(dx1*dystart - dxstart*dy1)
//  println(cross2)
}


class World(width: Int, height: Int, board: List[Char]) {

  val start: WorldNode = WorldNode(board.indexOf('s')%width, board.indexOf('s')/width.floor.toInt, false)
  val goal: WorldNode = WorldNode(board.indexOf('g')%width, board.indexOf('g')/width.floor.toInt, false)

  val nodes: Set[WorldNode] = (for {node <- board.zipWithIndex } yield
    if (node._1 == 'x') WorldNode(node._2%width, node._2/width.floor.toInt, true)
    else WorldNode(node._2%width, node._2/width.floor.toInt, false)).toSet

  for (node <- nodes) {
    node.distanceToGoal = Math.abs(goal.x - node.x) + Math.abs(goal.y - node.y)
  }

  def getNeighbours(node: WorldNode): Set[WorldNode] = {
    nodes.intersect(Set(WorldNode(node.x-1,node.y, false),
      WorldNode(node.x,node.y-1, false),
      WorldNode(node.x+1,node.y, false),
      WorldNode(node.x,node.y+1, false)))
  }

  def heuristic(): Unit ={

  }

}

case class WorldNode(x: Int, y: Int, isWall: Boolean) extends Ordered[WorldNode] {
  var distanceFromStart: Int = 0 //g
  var distanceToGoal: Int = 0 //h

  var totalCost = 0 //f

  def compare(that: WorldNode):Int = this.totalCost compare that.totalCost

  def update(newDistanceFromStart: Int): Unit = {
    if(distanceFromStart == 0) distanceFromStart = newDistanceFromStart
    totalCost = distanceFromStart + distanceToGoal
  }

//  def heuristic(): Int = {
//    distanceToGoal +
//  }
//  def heuristic()
//  def calculateEuclideanDistance(target: WorldNode): Unit = {
//    val dx1 = this.x - target.x
//    val dy1 = this.y - target.y
//
//  }

}