Begin energy tsp logic
Added energy matrix to tsp logic