package main

import (
  "encoding/csv"
  "fmt"
  "os"
  "log"
  "strconv"
  "strings"
  "errors"
) 

type Edge struct {
  dst, value int
}

type Node struct {
  edges []Edge 
  id int
  value int
}

type DijkstraNode struct {
  node *Node // A pointor to the node
  dist int   // Dist to that node
  path []int // Node's id list
}

type DijkstraPQueue struct {
  array []DijkstraNode
  ref []int
}

func (q *DijkstraPQueue) isEmpty() bool {
  return q.array == nil || len(q.array) < 2
}

func (q *DijkstraPQueue) push( n DijkstraNode) {
  if q.array == nil {
    q.array = make([]DijkstraNode, 1)
  }
  q.array = append(q.array, n)
  q.ref[n.node.id] = len(q.array) - 1

  for i := len(q.array) - 1 ; i > 1 ; i = i >> 1 {
    if q.array[i].dist < q.array[i>>1].dist {
      q.array[i], q.array[i>>1] = q.array[i>>1], q.array[i]
      q.ref[q.array[i].node.id], q.ref[q.array[i>>1].node.id] = i, i>>1
    } else {
      break
    }
  }

  return
}

func (q *DijkstraPQueue) pop() (DijkstraNode, error) {
  if q.isEmpty() {
    return DijkstraNode{}, errors.New("PQeueue empty")
  }

  // The node to return, it's the root of the tree
  v := q.array[1]
  q.ref[v.node.id] = -2

  // We overwrite him by the last node
  q.array[1] = q.array[len(q.array)-1]
  q.ref[q.array[1].node.id] = 1

  q.array = q.array[:len(q.array)-1]


  ind := 2
  for i := 1 ; i*2 < len(q.array) ; i = ind {
    // Choose if we should deal with the Right or the Left next node.
    // Select the lower
    if i << 1 + 1 < len(q.array) {
      if q.array[i<<1+1].dist < q.array[i<<1].dist {
        ind = i<<1+1
      } else {
        ind = i<<1
      }
    }

    // And then we deal with that node.
    if q.array[ind].dist < q.array[i].dist {
      q.array[ind], q.array[i] = q.array[i], q.array[ind]
      q.ref[q.array[ind].node.id], q.ref[q.array[i].node.id] = ind, i
    } else {
      break
    }
  }

  return v,nil
}

func (q *DijkstraPQueue) update( idNode, dist int, path []int ) {
  // We find the node
  // It the only purpose of the ref array. So sad.
  id := q.ref[idNode]

  if id == -2 {
    return
  } else if id == -1 {
    log.Fatal("Can't update")
  }

  if dist < q.array[id].dist {
    // The update function update the distance
    q.array[id].dist = dist
    q.array[id].path = path
  } else {
    return
  }


  // And because the distance modify on the position
  // in the binary tree, we have to update it too.
  // Because the new distance if lower than the
  // old one, we have to push id down to the leafs
  for i := id ; i > 1 ; i = i >> 1 {
    if q.array[i].dist < q.array[i>>1].dist {
      q.array[i], q.array[i>>1] = q.array[i>>1], q.array[i]

      q.ref[q.array[i].node.id], q.ref[q.array[i>>1].node.id] = i, i>>1
    } else {
      break
    }
  }
}

func dijkstra( nodes []Node, src, dst int) (dist int, path []int) {
  // Initialisation of the Dijkstra's priority queue
  priority := DijkstraPQueue{}

  priority.ref = make([]int, len(nodes))
  for i := range priority.ref {
    priority.ref[i] = -1
  }

  priority.push( DijkstraNode{ node: &nodes[src], dist: 0, path: make([]int, 1) } )



  // Main algorithm
  var node DijkstraNode
  var err error
  for node,err = priority.pop() ; err == nil && node.node.id != dst ; node,err = priority.pop() {
    for _,edge := range node.node.edges {
      if priority.ref[edge.dst] != -1 {
        tmp := make([]int, len(node.path))
        copy(tmp, node.path)
        priority.update( edge.dst, node.dist + edge.value, append(tmp, edge.dst))
      } else {
        tmp := make([]int, len(node.path))
        copy(tmp, node.path)
        priority.push( DijkstraNode{ node: &nodes[ edge.dst ], dist: node.dist + edge.value, path: append(tmp, edge.dst) } )
      }
    }
    node.path = []int{}
  }
  if node.node.id == dst {
    v := 0
    for _,e := range nodes[1].edges {
      if e.dst == 0 {
        v = e.value
      }
    }
    return node.dist + v, node.path
  } else {
    return -1, nil
  }
 
}

func extractRecord( record []string ) []int { 
  row := make([]int, len(record))

  for i := range record {
    if v, err := strconv.Atoi(strings.TrimSpace(record[i])) ; err == nil {
      row[i] = int(v)
    } else {
      log.Fatalln(err)
    }
  }

  return row
}


func main() {
  file, err := os.Open("p083_matrix.txt")
  
  if err != nil {
    log.Fatal(err)
  }

  array := make([][]int, 0)
  reader := csv.NewReader(file)

  line,err := reader.Read()


  for ; err == nil ; line,err = reader.Read()  {
    array = append( array, extractRecord(line) )
  }

  id := func( x, y int) int {
    return x * len(array[0]) + y
  }

  nodes := make([]Node, 0)

  for i,r := range array {
    for j,v := range r {

      edges := []Edge{}

      if i > 0 {
        edges = append( edges, Edge{ dst: id(i-1,j), value: array[i-1][j]} )
      }
      if j > 0 {
        edges = append( edges, Edge{ dst: id(i,j-1), value: array[i][j-1]} )
      }
      if i < len(array)-1 {
        edges = append( edges, Edge{ dst: id(i+1,j), value: array[i+1][j]} )
      }
      if j < len(r)-1 {
        edges = append( edges, Edge{ dst: id(i,j+1), value: array[i][j+1]} )
      }
      nodes = append(nodes, Node{ id: id(i,j), edges: edges, value: v } )
    }
  }
  res,path := dijkstra(nodes, 0, len(array)*len(array[0])-1)
  fmt.Println(res)
  fmt.Println(path)
  fmt.Println("Resum")
  sum := 0
  for _,id := range path {
    sum += nodes[id].value
  }
  fmt.Println(sum)
}