Hh6chen dijkstras with reverse

user/pathfinding.c

@@ -199,12 +199,13 @@ uint8_t first_reverse_on_path_or_dest(uint8_t dest,
 									  uint8_t parking_spot,
 									  uint32_t curtime,
 									  uint8_t* reverse_train) {
+	ASSERT(*reverse_train == 0);
 	// only give reverse node if we can stop there, otherwise give first sensor
 	struct track_path* path_ptr = &paths[dest];
 	uint8_t cur_node = 0;
 	uint8_t prev_node = 0;
 	uint8_t ret_node = dest;
-	char select_first_sensor = 0;
+	uint8_t select_first_sensor = 0;
 	while (path_ptr != NULL) {
 		cur_node = path_ptr->cur->idx;
 		if (path_ptr->prev != NULL) {
@@ -213,15 +214,16 @@ uint8_t first_reverse_on_path_or_dest(uint8_t dest,
 		if (track_nodes[cur_node].type == NODE_SENSOR && prev_node == reverse_node(cur_node)) {
 			if (!parking_spot_is_blocked(prev_node, blocked, parking_spot, curtime)) {
 				ret_node = prev_node;
-				select_first_sensor = 0;
 				*reverse_train = 1;
+				select_first_sensor = 0;
 			} else {
 				select_first_sensor = 1;
 				*reverse_train = 0;
 			}
 		}
 
-		if (select_first_sensor && path_ptr->prev == NULL) {
+		if ((select_first_sensor == 1) && path_ptr->prev == NULL) {
+			*reverse_train = 0;
 			ret_node = cur_node;
 		}
 		path_ptr = path_ptr->prev;
@@ -239,7 +241,9 @@ uint8_t determine_stopping_node(uint8_t dest,
 								uint8_t parking_spot,
 								uint32_t curtime,
 								uint8_t* reverse_train) {
+	ASSERT(*reverse_train == 0);
 	if (paths[dest].dist != UINT16_MAX) {
+		// if unreachable destination, go on shortest path
 		return first_reverse_on_path_or_dest(dest, paths, blocked, parking_spot, curtime, reverse_train);
 	}
 
@@ -296,7 +300,7 @@ uint8_t dijkstra_tc2(uint8_t src,
 					 uint8_t* reverse_train) {
 	struct Ring_Buffer path_buf_internal = create_buffer(CHAR);
 	dijkstra(src, dest, &path_buf_internal);
-	print_path("shortest path: ", &path_buf_internal);
+	//print_path("shortest path: ", &path_buf_internal);
 	if (path_buf_internal.cbuffer[path_buf_internal.r - 1] != dest) {
 		debug_prints("PATH IS NOT REACHABLE\r\n");
 		return 0xFF;
@@ -392,6 +396,8 @@ uint8_t dijkstra_tc2(uint8_t src,
 		dest, paths, &path_buf_internal, blocked, parking_spot, curtime, reverse_train)];
 	uint8_t potentially_new_dest = path.cur->idx;
 
+	debug_printi("debug_stopping node pick node: ", potentially_new_dest, "\r\n");
+
 	uint8_t tmp[50];
 	uint8_t count = 0;
 	while (path.prev) {
@@ -403,15 +409,14 @@ uint8_t dijkstra_tc2(uint8_t src,
 	tmp[count] = path.cur->idx;
 	count++;
 
-	if (*reverse_train && count == 1) {
-		ring_buffer_append(path_buf, tmp[0]);
-		debug_prints("dijkstra_tc2 chose to reverse train in place\r\n");
-		return potentially_new_dest;
-	}
-
-	if (count <= 1) {
-		// don't reserve nodes if no path, this check may need to be updated
-		return potentially_new_dest;
+	if (*reverse_train == 1) {
+		if (count == 1){
+			debug_prints("dijkstra_tc2 chose to reverse train in place\r\n");
+			ring_buffer_append(path_buf, tmp[0]);
+			return potentially_new_dest;
+		} else {
+			debug_prints("dijkstra_tc2 chose to reverse train\r\n");
+		}
 	}
 
 	if (potentially_new_dest != dest) {
@@ -422,6 +427,12 @@ uint8_t dijkstra_tc2(uint8_t src,
 		str_cat(debug_msg, "\r\n");
 		debug_prints(debug_msg);
 	}
+
+	if (count <= 1) {
+		// don't reserve nodes if no path, this check may need to be updated
+		return potentially_new_dest;
+	}
+
 	// on a path A->B->C->D
 	// want to reserve A for time from getting to A to time getting to B
 	// want to reserve B for time getting from B to C, add buffer time