+ if (found == -1)
+ DEBUG(E_USER_STRAT, "no circuit found");
+ else
+ DEBUG(E_USER_STRAT, "circuit found: %s, %s",
+ (*selected_circuit)->name,
+ (*selected_face) ? "faceA":"faceB");
+ return found;
+}
+
+static void test_all_circuits(void)
+{
+ const struct circuit **circuit;
+ const struct wp_coord *cur;
+ const struct wp_coord *start;
+ const struct wp_coord *end;
+ uint8_t prev_i, prev_j, i, j, dir;
+
+ for (circuit = &circuits[0]; *circuit; circuit++) {
+ start = &(*circuit)->path[0];
+ end = start + (*circuit)->len - 1;
+
+ prev_i = start->i;
+ prev_j = start->j;
+ start ++;
+
+ for (cur = start; cur != end;
+ cur ++, prev_i = i, prev_j = j) {
+
+ i = cur->i;
+ j = cur->j;
+
+ dir = get_dir(prev_i, prev_j, i, j);
+ if (dir == 0xFF)
+ printf_P("Bad circuit %s %d %d\r\n", (*circuit)->name, i, j);
+ }
+ }
+}
+
+
+static void dump_circuit_wp(struct wp_line *circuit_wpline, int8_t len)
+{
+ int8_t i;
+ if (len <= 0)
+ return;
+ for (i = 0; i < len; i ++) {
+ DEBUG(E_USER_STRAT, "linenum %d dir %d",
+ circuit_wpline[i].line_num,
+ circuit_wpline[i].dir);
+ }
+
+}
+
+/* choose a circuit, then harvest on this circuit */
+uint8_t strat_harvest_circuit(void)
+{
+ const struct circuit *selected_circuit;
+ int8_t selected_face;
+ struct wp_line circuit_wpline[MAX_CIRCUIT_WPLINE];
+ int8_t len;
+ uint8_t i, j, idx;
+ int16_t x, y;
+ uint8_t linenum, prev_linenum;
+ uint8_t dir, prev_dir;
+ uint8_t err;
+
+ strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW);
+
+ x = position_get_x_s16(&mainboard.pos);
+ y = position_get_y_s16(&mainboard.pos);
+
+ if (xycoord_to_ijcoord(&x, &y, &i, &j) < 0) {
+ DEBUG(E_USER_STRAT, "%s(): cannot find waypoint at %d,%d",
+ __FUNCTION__, x, y);
+ return END_ERROR;
+ }
+
+ if (find_best_circuit(i, j, &selected_circuit, &selected_face) < 0) {
+ DEBUG(E_USER_STRAT, "%s(): cannot find a good circuit",
+ __FUNCTION__);
+ return END_ERROR;
+ }
+
+ len = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
+ if (len < 0) {
+ DEBUG(E_USER_STRAT, "%s(): cannot find a path",
+ __FUNCTION__);
+ return END_ERROR;
+ }
+
+ dump_circuit_wp(circuit_wpline, len);
+
+ prev_linenum = circuit_wpline[0].line_num;
+ prev_dir = circuit_wpline[0].dir;
+
+ /* do all lines of circuit */
+ for (idx = 1; idx < len; idx ++) {
+ linenum = circuit_wpline[idx].line_num;
+ dir = circuit_wpline[idx].dir;
+
+ DEBUG(E_USER_STRAT, "%s(): line %d dir %d -> line %d dir %d",
+ __FUNCTION__, prev_linenum, prev_dir, linenum, dir);
+ err = line2line(prev_linenum, prev_dir, linenum, dir,
+ TRAJ_FLAGS_NO_NEAR);
+ if (!TRAJ_SUCCESS(err))
+ return err;
+
+ prev_linenum = linenum;
+ prev_dir = dir;
+ }
+
+ return END_TRAJ;
+}
+
+/* try to unblock in any situation */
+uint8_t strat_unblock(void)
+{
+ int16_t x, y;
+ uint8_t i, j;
+ uint16_t old_dspeed, old_aspeed;
+ uint8_t err;
+
+ DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
+
+ strat_want_pack = 1;
+ strat_get_speed(&old_dspeed, &old_aspeed);
+
+ strat_hardstop();
+ strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
+ x = position_get_x_s16(&mainboard.pos);
+ y = position_get_y_s16(&mainboard.pos);
+ if (xycoord_to_ijcoord(&x, &y, &i, &j) < 0) {
+ /* aie... go to center... but it's really a bad
+ * idea */
+ x = CENTER_X;
+ y = CENTER_Y;
+ }
+
+ /* XXX if opponent is too close, go back, or wait ? */
+
+ /* go to nearest waypoint */
+ trajectory_goto_xy_abs(&mainboard.traj, x, y);
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ if (err == END_TIMER)
+ return err;
+
+ if (!TRAJ_SUCCESS(err))
+ return err;
+
+ strat_set_speed(old_dspeed, old_aspeed);
+ strat_want_pack = 0;
+ return END_TRAJ;
+}
+
+void test_strat_avoid(void)
+{
+ test_all_circuits();
+
+#ifdef TEST_STRAT_AVOID
+ uint8_t i, j;
+ const struct circuit *selected_circuit;
+ int8_t selected_face;
+ struct wp_line circuit_wpline[MAX_CIRCUIT_WPLINE];
+ int8_t ret;
+
+ i = 1; j = 1;
+ printf_P(PSTR("========= i=%d, j=%d\r\n"), i, j);
+
+ ts = 0; bc = 0; cc = 0;
+ printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
+ find_best_circuit(i, j, &selected_circuit, &selected_face);
+ ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
+ dump_circuit_wp(circuit_wpline, ret);
+
+ ts = 0; bc = 3; cc = 0;
+ printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
+ find_best_circuit(i, j, &selected_circuit, &selected_face);
+ ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
+ dump_circuit_wp(circuit_wpline, ret);
+
+ ts = 0; bc = 4; cc = 0;
+ printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
+ find_best_circuit(i, j, &selected_circuit, &selected_face);
+ ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
+ dump_circuit_wp(circuit_wpline, ret);
+
+ ts = 0; bc = 3; cc = 5;
+ printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
+ find_best_circuit(i, j, &selected_circuit, &selected_face);
+ ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
+ dump_circuit_wp(circuit_wpline, ret);
+
+ ts = 0; bc = 4; cc = 5;
+ printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
+ find_best_circuit(i, j, &selected_circuit, &selected_face);
+ ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
+ dump_circuit_wp(circuit_wpline, ret);
+
+ ts = 80; bc = 0; cc = 0;
+ printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
+ find_best_circuit(i, j, &selected_circuit, &selected_face);
+ ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
+ dump_circuit_wp(circuit_wpline, ret);
+
+ i = 4; j = 3;
+ printf_P(PSTR("========= i=%d, j=%d\r\n"), i, j);
+
+ ts = 0; bc = 0; cc = 0;
+ printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
+ find_best_circuit(i, j, &selected_circuit, &selected_face);
+ ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
+ dump_circuit_wp(circuit_wpline, ret);
+
+ ts = 0; bc = 3; cc = 0;
+ printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
+ find_best_circuit(i, j, &selected_circuit, &selected_face);
+ ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
+ dump_circuit_wp(circuit_wpline, ret);
+
+ ts = 80; bc = 0; cc = 0;
+ printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
+ find_best_circuit(i, j, &selected_circuit, &selected_face);
+ ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
+ dump_circuit_wp(circuit_wpline, ret);
+
+ i = 11; j = 6;
+ printf_P(PSTR("========= i=%d, j=%d\r\n"), i, j);
+
+ ts = 0; bc = 0; cc = 0;
+ printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
+ find_best_circuit(i, j, &selected_circuit, &selected_face);
+ ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
+ dump_circuit_wp(circuit_wpline, ret);
+
+ ts = 0; bc = 3; cc = 0;
+ printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
+ find_best_circuit(i, j, &selected_circuit, &selected_face);
+ ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
+ dump_circuit_wp(circuit_wpline, ret);
+
+ ts = 80; bc = 0; cc = 0;
+ printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
+ find_best_circuit(i, j, &selected_circuit, &selected_face);
+ ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
+ dump_circuit_wp(circuit_wpline, ret);
+#endif