AJUDA com Projeto (Inteligência Artificial)

Pessoal…

Estou postando aqui um projeto da minha facul… mas não por preguissa, é porque não consigo sair desse ponto, preciso de ajuda para resolver e a entrega é na Terça…

Segue em anexo o projeto atualmente…

Junto com o assunto do projeto…

Resumidamente: Tenho de fazer um robô (em um mapa qualquer) achar o melhor caminho pegando os tesouros até a posição final…

Estou com dificildades para implementar a heurística… pois o resto esta funcionando…

Alguem pode me dar uma luz?

Obrigado

E desculpem o transtorno…

abs

EDIT: Ta no NetBeans