Olá Mateus,
É difícil dizer sem debugar o código, mas nós não costumamos debugar implementações pessoais. As partes mais evidentes (configuração da última camada, função de custo) me parecem estar OK. Eu te recomendo rodar seu código em uma IDE que te permita debugar, assim você executa linha por linha, isso ajuda a encontrar problemas.