projects/microb2010/mainboard/strat_avoid.c

   1 /*
   2  *  Copyright Droids, Microb Technology (2010)
   3  *
   4  *  This program is free software; you can redistribute it and/or modify
   5  *  it under the terms of the GNU General Public License as published by
   6  *  the Free Software Foundation; either version 2 of the License, or
   7  *  (at your option) any later version.
   8  *
   9  *  This program is distributed in the hope that it will be useful,
  10  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  *  GNU General Public License for more details.
  13  *
  14  *  You should have received a copy of the GNU General Public License
  15  *  along with this program; if not, write to the Free Software
  16  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  17  *
  18  *  Revision : $Id: strat.c,v 1.6 2009-11-08 17:24:33 zer0 Exp $
  19  *
  20  *  Olivier MATZ <zer0@droids-corp.org>
  21  */
  22
  23
  24 #include <stdio.h>
  25 #include <stdlib.h>
  26 #include <stdint.h>
  27 #include <math.h>
  28
  29 #include <aversive.h>
  30 #include <aversive/pgmspace.h>
  31 #include <aversive/error.h>
  32
  33 #include <ax12.h>
  34 #include <uart.h>
  35 #include <pwm_ng.h>
  36 #include <clock_time.h>
  37 #include <spi.h>
  38
  39 #include <pid.h>
  40 #include <quadramp.h>
  41 #include <control_system_manager.h>
  42 #include <trajectory_manager.h>
  43 #include <trajectory_manager_utils.h>
  44 #include <trajectory_manager_core.h>
  45 #include <vect_base.h>
  46 #include <lines.h>
  47 #include <polygon.h>
  48 #include <obstacle_avoidance.h>
  49 #include <blocking_detection_manager.h>
  50 #include <robot_system.h>
  51 #include <position_manager.h>
  52
  53 #include <diagnostic.h>
  54
  55 #include <rdline.h>
  56 #include <parse.h>
  57
  58 #include "../common/i2c_commands.h"
  59 #include "i2c_protocol.h"
  60 #include "main.h"
  61 #include "strat.h"
  62 #include "strat_db.h"
  63 #include "strat_base.h"
  64 #include "strat_corn.h"
  65 #include "strat_avoid.h"
  66 #include "strat_utils.h"
  67 #include "sensor.h"
  68 #include "actuator.h"
  69
  70 //#define VERBOSE
  71
  72 #ifdef VERBOSE
  73 #define DPR(fmt, ...) printf_P(PSTR(fmt), ##__VA_ARGS__)
  74 #else
  75 #define DPR(args...) do {} while (0)
  76 #endif
  77
  78 struct circuit {
  79         const char *name;
  80         uint8_t len;
  81         const struct wp_coord *path;
  82 };
  83
  84 const struct wp_coord butterfly_tab[] = {
  85         { .i = 11, .j = 6, },
  86         { .i = 10, .j = 6, },
  87         { .i = 9, .j = 5, },
  88         { .i = 8, .j = 5, },
  89         { .i = 7, .j = 4, },
  90         { .i = 6, .j = 4, },
  91         { .i = 5, .j = 3, },
  92         { .i = 4, .j = 4, },
  93         { .i = 3, .j = 2, },
  94         { .i = 2, .j = 2, },
  95         { .i = 1, .j = 1, },
  96         { .i = 1, .j = 2, },
  97         { .i = 1, .j = 3, },
  98         { .i = 1, .j = 4, },
  99         { .i = 1, .j = 5, },
 100         { .i = 1, .j = 6, },
 101         { .i = 2, .j = 6, },
 102         { .i = 3, .j = 5, },
 103         { .i = 4, .j = 5, },
 104         { .i = 5, .j = 4, },
 105         { .i = 6, .j = 4, },
 106         { .i = 7, .j = 3, },
 107         { .i = 8, .j = 3, },
 108         { .i = 9, .j = 2, },
 109         { .i = 10, .j = 2, },
 110         { .i = 11, .j = 1, },
 111         { .i = 11, .j = 2, },
 112         { .i = 11, .j = 3, },
 113         { .i = 11, .j = 4, },
 114         { .i = 11, .j = 5, },
 115         { .i = 11, .j = 6, },
 116 };
 117
 118 const struct circuit butterfly_circuit = {
 119         .name = "butterfly",
 120         .len = sizeof(butterfly_tab)/sizeof(struct wp_coord),
 121         .path = butterfly_tab,
 122 };
 123
 124 const struct wp_coord losange_tab[] = {
 125         { .i = 11, .j = 6, },
 126         { .i = 10, .j = 6, },
 127         { .i = 9, .j = 5, },
 128         { .i = 9, .j = 4, },
 129         { .i = 9, .j = 3, },
 130         { .i = 10, .j = 4, },
 131         { .i = 11, .j = 4, },
 132         { .i = 11, .j = 5, },
 133         { .i = 11, .j = 6, },
 134 };
 135
 136 const struct circuit losange_circuit = {
 137         .name = "losange",
 138         .len = sizeof(losange_tab)/sizeof(struct wp_coord),
 139         .path = losange_tab,
 140 };
 141
 142 const struct wp_coord triangle_tab[] = {
 143         { .i = 11, .j = 6, },
 144         { .i = 10, .j = 6, },
 145         { .i = 9, .j = 5, },
 146         { .i = 8, .j = 5, },
 147         { .i = 7, .j = 4, },
 148         { .i = 6, .j = 4, },
 149         { .i = 7, .j = 3, },
 150         { .i = 8, .j = 3, },
 151         { .i = 9, .j = 2, },
 152         { .i = 10, .j = 2, },
 153         { .i = 11, .j = 1, },
 154         { .i = 11, .j = 2, },
 155         { .i = 11, .j = 3, },
 156         { .i = 11, .j = 4, },
 157         { .i = 11, .j = 5, },
 158         { .i = 11, .j = 6, },
 159 };
 160
 161 const struct circuit triangle_circuit = {
 162         .name = "triangle",
 163         .len = sizeof(triangle_tab)/sizeof(struct wp_coord),
 164         .path = triangle_tab,
 165 };
 166
 167 const struct wp_coord answer_d_tab[] = {
 168         { .i = 11, .j = 6, },
 169         { .i = 11, .j = 5, },
 170         { .i = 11, .j = 4, },
 171         { .i = 11, .j = 3, },
 172         { .i = 11, .j = 2, },
 173         { .i = 11, .j = 1, },
 174         { .i = 10, .j = 2, },
 175         { .i = 9, .j = 2, },
 176         { .i = 8, .j = 3, },
 177         { .i = 9, .j = 3, },
 178         { .i = 10, .j = 4, },
 179         { .i = 11, .j = 4, },
 180         { .i = 11, .j = 5, },
 181         { .i = 11, .j = 6, },
 182 };
 183
 184 const struct circuit answer_d_circuit = {
 185         .name = "answer_d",
 186         .len = sizeof(answer_d_tab)/sizeof(struct wp_coord),
 187         .path = answer_d_tab,
 188 };
 189
 190 const struct wp_coord h_lambda_tab[] = {
 191         { .i = 11, .j = 6, },
 192         { .i = 10, .j = 6, },
 193         { .i = 9, .j = 5, },
 194         { .i = 8, .j = 5, },
 195         { .i = 7, .j = 4, },
 196         { .i = 6, .j = 4, },
 197         { .i = 5, .j = 3, },
 198         { .i = 5, .j = 4, },
 199         { .i = 5, .j = 5, },
 200         { .i = 5, .j = 6, },
 201         { .i = 6, .j = 6, },
 202         { .i = 7, .j = 5, },
 203         { .i = 8, .j = 5, },
 204         { .i = 9, .j = 5, },
 205         { .i = 10, .j = 6, },
 206         { .i = 11, .j = 6, },
 207 };
 208
 209 const struct circuit h_lambda_circuit = {
 210         .name = "h_lambda",
 211         .len = sizeof(h_lambda_tab)/sizeof(struct wp_coord),
 212         .path = h_lambda_tab,
 213 };
 214
 215 const struct wp_coord asym_butterfly_tab[] = {
 216         { .i = 11, .j = 6, },
 217         { .i = 10, .j = 6, },
 218         { .i = 9, .j = 5, },
 219         { .i = 8, .j = 5, },
 220         { .i = 7, .j = 4, },
 221         { .i = 6, .j = 4, },
 222         { .i = 5, .j = 3, },
 223         { .i = 4, .j = 3, },
 224         { .i = 3, .j = 2, },
 225         { .i = 3, .j = 3, },
 226         { .i = 3, .j = 4, },
 227         { .i = 3, .j = 5, },
 228         { .i = 4, .j = 5, },
 229         { .i = 5, .j = 4, },
 230         { .i = 6, .j = 4, },
 231         { .i = 7, .j = 3, },
 232         { .i = 8, .j = 3, },
 233         { .i = 9, .j = 2, },
 234         { .i = 10, .j = 2, },
 235         { .i = 11, .j = 1, },
 236         { .i = 11, .j = 2, },
 237         { .i = 11, .j = 3, },
 238         { .i = 11, .j = 4, },
 239         { .i = 11, .j = 5, },
 240         { .i = 11, .j = 6, },
 241 };
 242
 243 const struct circuit asym_butterfly_circuit = {
 244         .name = "asym_butterfly",
 245         .len = sizeof(asym_butterfly_tab)/sizeof(struct wp_coord),
 246         .path = asym_butterfly_tab,
 247 };
 248
 249 const struct wp_coord big_h_lambda_tab[] = {
 250         { .i = 11, .j = 6, },
 251         { .i = 10, .j = 6, },
 252         { .i = 9, .j = 5, },
 253         { .i = 8, .j = 5, },
 254         { .i = 7, .j = 4, },
 255         { .i = 6, .j = 4, },
 256         { .i = 5, .j = 4, },
 257         { .i = 4, .j = 5, },
 258         { .i = 3, .j = 5, },
 259         { .i = 2, .j = 6, },
 260         { .i = 1, .j = 6, },
 261         { .i = 1, .j = 5, },
 262         { .i = 1, .j = 4, },
 263         { .i = 1, .j = 3, },
 264         { .i = 1, .j = 2, },
 265         { .i = 1, .j = 1, },
 266         { .i = 2, .j = 2, },
 267         { .i = 3, .j = 2, },
 268         { .i = 4, .j = 3, },
 269         { .i = 5, .j = 3, },
 270         { .i = 6, .j = 4, },
 271         { .i = 7, .j = 4, },
 272         { .i = 8, .j = 5, },
 273         { .i = 9, .j = 5, },
 274         { .i = 10, .j = 6, },
 275         { .i = 11, .j = 6, },
 276 };
 277
 278 const struct circuit big_h_lambda_circuit = {
 279         .name = "big_h_lambda",
 280         .len = sizeof(big_h_lambda_tab)/sizeof(struct wp_coord),
 281         .path = big_h_lambda_tab,
 282 };
 283
 284 /* list of all possible circuits */
 285 const struct circuit *circuits[] = {
 286         &butterfly_circuit,
 287         &losange_circuit,
 288         &triangle_circuit,
 289         &answer_d_circuit,
 290         &h_lambda_circuit,
 291         &asym_butterfly_circuit,
 292         &big_h_lambda_circuit,
 293         NULL,
 294 };
 295
 296 /* symetric neighbor position */
 297 static inline uint8_t opposite_position(uint8_t pos)
 298 {
 299         pos += 3;
 300         if (pos > LINE_L_UP)
 301                 pos -= 6;
 302         return pos;
 303 }
 304
 305 #ifdef HOST_VERSION
 306 //#define TEST_STRAT_AVOID
 307 #endif
 308
 309 #ifdef TEST_STRAT_AVOID
 310 static uint8_t cc;
 311 uint8_t xget_cob_count(void)
 312 {
 313         return cc;
 314 }
 315
 316 static uint8_t bc;
 317 uint8_t xget_ball_count(void)
 318 {
 319         return bc;
 320 }
 321
 322 static uint32_t ts;
 323 uint8_t xtime_get_s(void)
 324 {
 325         return ts;
 326 }
 327 #else
 328 #define xget_cob_count() get_cob_count()
 329 #define xget_ball_count() get_ball_count()
 330 #define xtime_get_s() time_get_s()
 331 #endif
 332
 333 /* return true if turn is at 60 deg */
 334 uint8_t is_60deg(uint8_t dir1, uint8_t dir2)
 335 {
 336         int8_t turn;
 337
 338         turn = dir2-dir1;
 339         if (turn < 0)
 340                 turn += 6;
 341         if (turn == 1)
 342                 return 1;
 343         if (turn == 5)
 344                 return 1;
 345         return 0;
 346 }
 347
 348 /* return true if turn is at 60 deg */
 349 uint8_t is_120deg(uint8_t dir1, uint8_t dir2)
 350 {
 351         int8_t turn;
 352
 353         turn = dir2-dir1;
 354         if (turn < 0)
 355                 turn += 6;
 356         if (turn == 2)
 357                 return 1;
 358         if (turn == 4)
 359                 return 1;
 360         return 0;
 361 }
 362
 363 /* get the neighbour of the point at specified dir, return -1 if
 364  * there is no neighbor */
 365 int8_t wp_get_neigh(uint8_t i, uint8_t j, uint8_t *ni, uint8_t *nj,
 366                  uint8_t dir)
 367 {
 368         switch (dir) {
 369         case LINE_UP:
 370                 j++;
 371                 break;
 372         case LINE_R_UP:
 373                 if ((i & 1)) j++;
 374                 i++;
 375                 break;
 376         case LINE_R_DOWN:
 377                 if (!(i & 1)) j--;
 378                 i++;
 379                 break;
 380         case LINE_DOWN:
 381                 j--;
 382                 break;
 383         case LINE_L_DOWN:
 384                 if (!(i & 1)) j--;
 385                 i--;
 386                 break;
 387         case LINE_L_UP:
 388                 if ((i & 1)) j++;
 389                 i--;
 390                 break;
 391         default:
 392                 return -1;
 393         }
 394         if (i >= WAYPOINTS_NBX || j >= WAYPOINTS_NBY)
 395                 return -1;
 396
 397         *ni = i;
 398         *nj = j;
 399         return 0;
 400 }
 401
 402 static uint8_t get_line_num(int8_t i, int8_t j, uint8_t dir)
 403 {
 404         switch (dir) {
 405         case LINE_UP:
 406         case LINE_DOWN:
 407                 return i/2;
 408         case LINE_R_UP:
 409         case LINE_L_DOWN:
 410                 i &= 0xfe;
 411                 j -= i/2;
 412                 return (5-j)/2;
 413         case LINE_R_DOWN:
 414         case LINE_L_UP:
 415                 i &= 0xfe;
 416                 j += i/2;
 417                 return (11-j)/2;
 418         default:
 419                 return -1;
 420         }
 421 }
 422
 423 static uint8_t get_dir(uint8_t prev_i, uint8_t prev_j,
 424                        uint8_t i, uint8_t j)
 425 {
 426         int8_t diff_i, diff_j;
 427
 428         diff_i = i - prev_i;
 429         diff_j = j - prev_j;
 430
 431         if (diff_i == 0 && diff_j == 1)
 432                 return LINE_UP;
 433         if (diff_i == 0 && diff_j == -1)
 434                 return LINE_DOWN;
 435
 436         if ((prev_i & 1) == 0) {
 437                 if (diff_i == 1 && diff_j == 0)
 438                         return LINE_R_UP;
 439                 if (diff_i == 1 && diff_j == -1)
 440                         return LINE_R_DOWN;
 441                 if (diff_i == -1 && diff_j == 0)
 442                         return LINE_L_UP;
 443                 if (diff_i == -1 && diff_j == -1)
 444                         return LINE_L_DOWN;
 445         }
 446         else {
 447                 if (diff_i == 1 && diff_j == 1)
 448                         return LINE_R_UP;
 449                 if (diff_i == 1 && diff_j == 0)
 450                         return LINE_R_DOWN;
 451                 if (diff_i == -1 && diff_j == 1)
 452                         return LINE_L_UP;
 453                 if (diff_i == -1 && diff_j == 0)
 454                         return LINE_L_DOWN;
 455         }
 456
 457         /* invalid value */
 458         return 0xFF;
 459 }
 460
 461 /* return true if a waypoint belongs to a line */
 462 uint8_t wp_belongs_to_line(uint8_t i, uint8_t j, uint8_t linenum, uint8_t dir)
 463 {
 464         uint8_t ln;
 465         ln = get_line_num(i, j, dir);
 466         if (ln == linenum)
 467                 return 1;
 468         return 0;
 469 }
 470
 471 /* count the number of non-black corns which are neighbors of
 472  * specified cob */
 473 uint8_t corn_count_neigh(uint8_t i, uint8_t j)
 474 {
 475         uint8_t dir, n = 0;
 476         uint8_t ni, nj;
 477
 478         for (dir = LINE_UP; dir <= LINE_R_UP; dir++) {
 479                 if (wp_get_neigh(i, j, &ni, &nj, dir) < 0)
 480                         continue;
 481
 482                 /* is there a corn cob ? */
 483                 if (strat_db.wp_table[ni][nj].type == WP_TYPE_CORN &&
 484                     strat_db.wp_table[ni][nj].present &&
 485                     strat_db.wp_table[ni][nj].corn.color != I2C_COB_BLACK)
 486                         n ++;
 487         }
 488
 489         return n;
 490 }
 491
 492
 493 /* fill circuit_wpline table with waypoints from circuit starting at
 494  * i,j and using selected face */
 495 static int8_t get_path(const struct circuit *circuit,
 496                        uint8_t starti, uint8_t startj, uint8_t faceA,
 497                        struct wp_line *circuit_wpline)
 498 {
 499         const struct wp_coord *curcircuit;
 500         const struct wp_coord *start;
 501         const struct wp_coord *end;
 502         uint8_t prev_i, prev_j;
 503         uint8_t dir, prev_dir = 0xFF;
 504         uint8_t found = 0, i = 0, j = 0;
 505         uint8_t linenum;
 506         int8_t step = faceA ? 1 : -1;
 507         int8_t path_len = 0;
 508
 509         /* start and end of circuit */
 510         if (faceA) {
 511                 start = &circuit->path[0];
 512                 end = start + circuit->len - 1;
 513         }
 514         else {
 515                 end = &circuit->path[0];
 516                 start = end + circuit->len - 1;
 517         }
 518
 519         DPR("face: %s %d\r\n", circuit->name, faceA);
 520
 521         /* check that the point is present in the circuit */
 522         for (curcircuit = start; curcircuit != end; curcircuit += step) {
 523                 if (curcircuit->i == starti && curcircuit->j == startj) {
 524                         found = 1;
 525                         break;
 526                 }
 527         }
 528         if (found == 0)
 529                 return -1;
 530
 531
 532         /* browse the circuit from starti, startj in the specified
 533          * direction, and fill the table when direction changes */
 534         prev_i = starti;
 535         prev_j = startj;
 536         for ( ; curcircuit != end;
 537               curcircuit += step, prev_dir = dir, prev_i = i, prev_j = j) {
 538
 539                 i = curcircuit->i;
 540                 j = curcircuit->j;
 541
 542                 dir = get_dir(prev_i, prev_j, i, j);
 543
 544                 if (prev_dir != dir) {
 545                         linenum = get_line_num(prev_i, prev_j, dir);
 546                         circuit_wpline[path_len].line_num = linenum;
 547                         circuit_wpline[path_len].dir = dir;
 548                         path_len++;
 549                 }
 550         }
 551
 552         return path_len;
 553 }
 554
 555 /* process score from retrieved objects number, and circuit len */
 556 static int16_t get_score(uint32_t wcorn_retrieved,
 557                          uint32_t ucorn_retrieved,
 558                          uint16_t tomato_retrieved,
 559                          uint8_t len, uint8_t opp_on_path)
 560 {
 561         int16_t score = 0;
 562         uint8_t i;
 563         uint32_t mask = 1;
 564         uint8_t n;
 565
 566         /* score with corn */
 567         n = xget_cob_count() * 2;
 568         for (i = 0; i<CORN_NB; i++) {
 569                 if (n >= 10)
 570                         break;
 571                 if (wcorn_retrieved & mask) {
 572                         score += 250;
 573                         n += 2;
 574                 }
 575                 if (n >= 10)
 576                         break;
 577                 if (ucorn_retrieved & mask) {
 578                         score += 125;
 579                         n += 1;
 580                 }
 581                 mask <<= 1UL;
 582         }
 583
 584         DPR("get score: cob %d (->%d)\r\n", n, n/2);
 585
 586         /* score with tomato */
 587         n = xget_ball_count();
 588         mask = 1;
 589         for (i = 0; i<TOMATO_NB; i++) {
 590                 if (n >= 4)
 591                         break;
 592                 if (tomato_retrieved & mask) {
 593                         score += 150;
 594                         n += 1;
 595                 }
 596                 mask <<= 1UL;
 597         }
 598
 599         DPR("get score: ball %d\r\n", n);
 600
 601         /* malus for long circuits */
 602         score -= (len * 20);
 603         DPR("malus for length: %d\r\n", len * 20);
 604
 605         /* double malus for long circuits if we don't have much
 606          * time */
 607 #define WP_SPEED 1
 608         if (len * WP_SPEED > (MATCH_TIME - xtime_get_s())) {
 609                 int32_t extra;
 610                 extra = (len * WP_SPEED) - (MATCH_TIME - xtime_get_s());
 611                 extra = (200 * extra);
 612                 if (extra < 0) /* should not happen */
 613                         extra = 0;
 614                 if (extra > 10000)
 615                         extra = 10000;
 616                 score -= extra;
 617                 DPR("malus for length + time: %d\r\n", extra);
 618         }
 619
 620         /* malus if there is opponent on the path */
 621         if (opp_on_path) {
 622                 DPR("malus for opponent: 1000\r\n");
 623                 score -= 2000;
 624         }
 625
 626         return score;
 627 }
 628
 629 /* return the corn type of specified coords: I2C_COB_WHITE,
 630  * I2C_COB_UNKNOWN, or I2C_COB_NONE if it is black or not present */
 631 static uint8_t get_corn_type(uint8_t i, uint8_t j)
 632 {
 633         uint8_t color;
 634         /* is there a corn cob ? */
 635         if (strat_db.wp_table[i][j].type == WP_TYPE_CORN &&
 636             strat_db.wp_table[i][j].present) {
 637                 color = strat_db.wp_table[i][j].corn.color;
 638                 if (color == I2C_COB_WHITE)
 639                         return I2C_COB_WHITE;
 640                 else if (color == I2C_COB_UNKNOWN)
 641                         return I2C_COB_UNKNOWN;
 642         }
 643         return I2C_COB_NONE;
 644 }
 645
 646 /* i,j: starting position */
 647 static int8_t evaluate_one_face(const struct circuit *circuit,
 648                                 uint8_t starti, uint8_t startj,
 649                                 uint8_t faceA, int16_t *score)
 650 {
 651         const struct wp_coord *curcircuit;
 652         const struct wp_coord *start;
 653         const struct wp_coord *end;
 654         uint32_t wcorn_retrieved = 0; /* bit mask */
 655         uint32_t ucorn_retrieved = 0; /* bit mask */
 656         uint16_t tomato_retrieved = 0; /* bit mask */
 657         uint8_t opponent_on_path = 0;
 658         uint8_t len = 0, found = 0;
 659         uint8_t i, j, prev_i, prev_j;
 660         uint8_t ni = 0, nj = 0;
 661         uint8_t dir, color, idx;
 662         int8_t step = faceA ? 1 : -1;
 663         int16_t x, y, d;
 664         int16_t oppx, oppy;
 665
 666         *score = 0x8000; /* -int_max */
 667
 668         /* start and end of circuit */
 669         if (faceA) {
 670                 start = &circuit->path[0];
 671                 end = start + circuit->len - 1;
 672         }
 673         else {
 674                 end = &circuit->path[0];
 675                 start = end + circuit->len - 1;
 676         }
 677
 678         DPR("%s() face: %s %d\r\n", __FUNCTION__, circuit->name, faceA);
 679
 680         /* check that the point is present in the circuit */
 681         for (curcircuit = start; curcircuit != end; curcircuit += step) {
 682                 if (curcircuit->i == starti && curcircuit->j == startj) {
 683                         found = 1;
 684                         break;
 685                 }
 686         }
 687         if (found == 0)
 688                 return -1;
 689
 690         /* get opponent coords */
 691         if (get_opponent_xy(&oppx, &oppy) < 0)
 692                 oppx = I2C_OPPONENT_NOT_THERE;
 693
 694         /* silent the compiler */
 695         prev_i = 0xff;
 696         prev_j = 0xff;
 697
 698         /* browse all points and calculate the score */
 699         for (curcircuit = start;
 700              curcircuit != end;
 701              curcircuit += step, len ++, prev_i = i, prev_j = j) {
 702                 i = curcircuit->i;
 703                 j = curcircuit->j;
 704
 705                 /* is opponent near the point ? */
 706                 ijcoord_to_xycoord(i, j, &x, &y);
 707                 if (oppx != I2C_OPPONENT_NOT_THERE) {
 708                         d = distance_between(oppx, oppy, x, y);
 709                         if (d < 600)
 710                                 opponent_on_path = 1;
 711                 }
 712
 713                 /* don't try to look cobs/tomato for first point */
 714                 if (curcircuit == start)
 715                         continue;
 716
 717                 /* get current direction, we wil check cobs behind us
 718                  * on left and right */
 719                 dir = get_dir(prev_i, prev_j, i, j);
 720
 721                 DPR("%d %d -> %d %d  (%d)\n", prev_i, prev_j, i, j, dir);
 722
 723                 /* is there a tomato ? */
 724                 if (strat_db.wp_table[i][j].type == WP_TYPE_TOMATO &&
 725                     strat_db.wp_table[i][j].present) {
 726                         DPR("  TOMATO\n");
 727                         tomato_retrieved |= (1UL << strat_db.wp_table[i][j].tomato.idx);
 728                 }
 729
 730                 /* behind left */
 731                 if (wp_get_neigh(i, j, &ni, &nj, (dir + 2) % 6) == 0) {
 732                         color = get_corn_type(ni, nj);
 733                         idx = strat_db.wp_table[ni][nj].corn.idx;
 734                         if (color == I2C_COB_WHITE) {
 735                                 DPR("  LEFT WCORN (%d)\n", idx);
 736                                 wcorn_retrieved |= (1UL << idx);
 737                         }
 738                         else if (color == I2C_COB_UNKNOWN) {
 739                                 DPR("  LEFT UCORN (%d)\n", idx);
 740                                 ucorn_retrieved |= (1UL << idx);
 741                         }
 742                 }
 743
 744                 /* behind right */
 745                 if (wp_get_neigh(i, j, &ni, &nj, (dir + 4) % 6) == 0) {
 746                         color = get_corn_type(ni, nj);
 747                         idx = strat_db.wp_table[ni][nj].corn.idx;
 748                         if (color == I2C_COB_WHITE) {
 749                                 DPR("  RIGHT WCORN (%d)\n", idx);
 750                                 wcorn_retrieved |= (1UL << idx);
 751                         }
 752                         else if (color == I2C_COB_UNKNOWN) {
 753                                 DPR("  RIGHT UCORN (%d)\n", idx);
 754                                 ucorn_retrieved |= (1UL << idx);
 755                         }
 756                 }
 757
 758                 /* prev_i, prev_j, len and curcircuit are updated in
 759                  * for (;;) */
 760         }
 761
 762         /* write score and exit */
 763         *score = get_score(wcorn_retrieved, ucorn_retrieved,
 764                            tomato_retrieved, len, opponent_on_path);
 765         return 0;
 766 }
 767
 768 /* i,j: starting position */
 769 static int8_t evaluate_one_circuit(const struct circuit *circuit,
 770                                    uint8_t starti, uint8_t startj,
 771                                    int16_t *scoreA, int16_t *scoreB)
 772 {
 773         if (evaluate_one_face(circuit, starti, startj, 1, scoreA) < 0)
 774                 return -1;
 775
 776         /* we are on eject point, scoreB is the same */
 777         if (starti == 11 && startj == 6) {
 778                 *scoreB = *scoreA;
 779                 return 0;
 780         }
 781
 782         if (evaluate_one_face(circuit, starti, startj, 0, scoreB) < 0)
 783                 return -1;
 784         return 0;
 785 }
 786
 787 /* i,j starting position */
 788 int8_t find_best_circuit(uint8_t i, uint8_t j,
 789                        const struct circuit **selected_circuit,
 790                        int8_t *selected_face)
 791 {
 792         const struct circuit **circuit;
 793         int16_t scoreA, scoreB;
 794         int16_t selected_score = 0x8000; /* ~-int_max */
 795         int8_t found = -1;
 796
 797         *selected_face = 0;
 798         *selected_circuit = circuits[0] ;
 799         for (circuit = &circuits[0]; *circuit; circuit++) {
 800                 if (evaluate_one_circuit(*circuit, i, j, &scoreA, &scoreB) < 0)
 801                         continue;
 802                 found = 0;
 803                 DEBUG(E_USER_STRAT, "Scores for %s are: faceA=%d, faceB=%d",
 804                       (*circuit)->name, scoreA, scoreB);
 805                 if (scoreA > selected_score) {
 806                         *selected_circuit = *circuit;
 807                         selected_score = scoreA;
 808                         *selected_face = 0;
 809                 }
 810                 if (scoreB > selected_score) {
 811                         *selected_circuit = *circuit;
 812                         selected_score = scoreB;
 813                         *selected_face = 1;
 814                 }
 815         }
 816
 817         if (found == -1)
 818                 DEBUG(E_USER_STRAT, "no circuit found");
 819         else
 820                 DEBUG(E_USER_STRAT, "circuit found: %s, %s",
 821                       (*selected_circuit)->name,
 822                       (*selected_face) ? "faceA":"faceB");
 823         return found;
 824 }
 825
 826 static void dump_circuit_wp(struct wp_line *circuit_wpline, int8_t len)
 827 {
 828         int8_t i;
 829         if (len <= 0)
 830                 return;
 831         for (i = 0; i < len; i ++) {
 832                 DEBUG(E_USER_STRAT, "linenum %d dir %d",
 833                       circuit_wpline[i].line_num,
 834                        circuit_wpline[i].dir);
 835         }
 836
 837 }
 838
 839 /* choose a circuit, then harvest on this circuit */
 840 uint8_t strat_harvest_circuit(void)
 841 {
 842         const struct circuit *selected_circuit;
 843         int8_t selected_face;
 844         struct wp_line circuit_wpline[MAX_CIRCUIT_WPLINE];
 845         int8_t len;
 846         uint8_t i, j, idx;
 847         int16_t x, y;
 848         uint8_t linenum, prev_linenum;
 849         uint8_t dir, prev_dir;
 850         uint8_t err;
 851
 852         x = position_get_x_s16(&mainboard.pos);
 853         y = position_get_y_s16(&mainboard.pos);
 854
 855         if (xycoord_to_ijcoord(&x, &y, &i, &j) < 0) {
 856                 DEBUG(E_USER_STRAT, "%s(): cannot find waypoint at %d,%d",
 857                       __FUNCTION__, x, y);
 858                 return END_ERROR;
 859         }
 860
 861         if (find_best_circuit(i, j, &selected_circuit, &selected_face) < 0) {
 862                 DEBUG(E_USER_STRAT, "%s(): cannot find a good circuit",
 863                       __FUNCTION__);
 864                 return END_ERROR;
 865         }
 866
 867         len = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
 868         if (len < 0) {
 869                 DEBUG(E_USER_STRAT, "%s(): cannot find a path",
 870                       __FUNCTION__);
 871                 return END_ERROR;
 872         }
 873
 874         dump_circuit_wp(circuit_wpline, len);
 875
 876         prev_linenum = circuit_wpline[0].line_num;
 877         prev_dir = circuit_wpline[0].dir;
 878         for (idx = 1; idx < len; idx ++) {
 879         retry:
 880                 linenum = circuit_wpline[idx].line_num;
 881                 dir = circuit_wpline[idx].dir;
 882
 883                 /* XXX basic opponent management */
 884                 DEBUG(E_USER_STRAT, "%s(): line %d dir %d -> line %d dir %d",
 885                       __FUNCTION__, prev_linenum, prev_dir, linenum, dir);
 886                 err = line2line(prev_linenum, prev_dir, linenum, dir,
 887                                 TRAJ_FLAGS_NO_NEAR);
 888                 if (!TRAJ_SUCCESS(err)) {
 889                         strat_hardstop();
 890                         time_wait_ms(2000);
 891                         goto retry;
 892                 }
 893
 894                 prev_linenum = linenum;
 895                 prev_dir = dir;
 896         }
 897
 898         return END_TRAJ; // XXX
 899 }
 900
 901 uint8_t strat_unblock(void)
 902 {
 903         return END_TRAJ;
 904 }
 905
 906 void test_strat_avoid(void)
 907 {
 908 #ifdef TEST_STRAT_AVOID
 909         uint8_t i, j;
 910         const struct circuit *selected_circuit;
 911         int8_t selected_face;
 912         struct wp_line circuit_wpline[MAX_CIRCUIT_WPLINE];
 913         int8_t ret;
 914
 915         i = 1; j = 1;
 916         printf_P(PSTR("========= i=%d, j=%d\r\n"), i, j);
 917
 918         ts = 0; bc = 0; cc = 0;
 919         printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
 920         find_best_circuit(i, j, &selected_circuit, &selected_face);
 921         ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
 922         dump_circuit_wp(circuit_wpline, ret);
 923
 924         ts = 0; bc = 3; cc = 0;
 925         printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
 926         find_best_circuit(i, j, &selected_circuit, &selected_face);
 927         ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
 928         dump_circuit_wp(circuit_wpline, ret);
 929
 930         ts = 0; bc = 4; cc = 0;
 931         printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
 932         find_best_circuit(i, j, &selected_circuit, &selected_face);
 933         ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
 934         dump_circuit_wp(circuit_wpline, ret);
 935
 936         ts = 0; bc = 3; cc = 5;
 937         printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
 938         find_best_circuit(i, j, &selected_circuit, &selected_face);
 939         ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
 940         dump_circuit_wp(circuit_wpline, ret);
 941
 942         ts = 0; bc = 4; cc = 5;
 943         printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
 944         find_best_circuit(i, j, &selected_circuit, &selected_face);
 945         ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
 946         dump_circuit_wp(circuit_wpline, ret);
 947
 948         ts = 80; bc = 0; cc = 0;
 949         printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
 950         find_best_circuit(i, j, &selected_circuit, &selected_face);
 951         ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
 952         dump_circuit_wp(circuit_wpline, ret);
 953
 954         i = 4; j = 3;
 955         printf_P(PSTR("========= i=%d, j=%d\r\n"), i, j);
 956
 957         ts = 0; bc = 0; cc = 0;
 958         printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
 959         find_best_circuit(i, j, &selected_circuit, &selected_face);
 960         ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
 961         dump_circuit_wp(circuit_wpline, ret);
 962
 963         ts = 0; bc = 3; cc = 0;
 964         printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
 965         find_best_circuit(i, j, &selected_circuit, &selected_face);
 966         ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
 967         dump_circuit_wp(circuit_wpline, ret);
 968
 969         ts = 80; bc = 0; cc = 0;
 970         printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
 971         find_best_circuit(i, j, &selected_circuit, &selected_face);
 972         ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
 973         dump_circuit_wp(circuit_wpline, ret);
 974
 975         i = 11; j = 6;
 976         printf_P(PSTR("========= i=%d, j=%d\r\n"), i, j);
 977
 978         ts = 0; bc = 0; cc = 0;
 979         printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
 980         find_best_circuit(i, j, &selected_circuit, &selected_face);
 981         ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
 982         dump_circuit_wp(circuit_wpline, ret);
 983
 984         ts = 0; bc = 3; cc = 0;
 985         printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
 986         find_best_circuit(i, j, &selected_circuit, &selected_face);
 987         ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
 988         dump_circuit_wp(circuit_wpline, ret);
 989
 990         ts = 80; bc = 0; cc = 0;
 991         printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
 992         find_best_circuit(i, j, &selected_circuit, &selected_face);
 993         ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
 994         dump_circuit_wp(circuit_wpline, ret);
 995 #endif
 996 }