Modified from TSP_SA