-/* XXX TODO
-static
-const
-change x,y -> i,j to avoid confusion with coords
-could be optimized in mem space: it is not needed to store the x,y coord,
- we can process it from idx. however it will be less optimized for speed
-
-*/
-
-#define OFFSET_CORN_X 150
-#define OFFSET_CORN_Y 222
-#define STEP_CORN_X 225
-#define STEP_CORN_Y 250
-
-#define CORN_NB 18
-
-#define WAYPOINTS_NBX 13
-#define WAYPOINTS_NBY 8
-
-/* enum is better */
-#define TYPE_WAYPOINT 0
-#define TYPE_DANGEROUS 1
-#define TYPE_WHITE_CORN 2
-#define TYPE_BLACK_CORN 3
-#define TYPE_OBSTACLE 4
-#define TYPE_UNKNOWN 5
-
-/* XXX enum possible ? else just rename */
-#define START 0
-#define UP 1
-#define UP_RIGHT 2
-#define DOWN_RIGHT 3
-#define DOWN 4
-#define DOWN_LEFT 5
-#define UP_LEFT 6
-#define END 7
-
-struct point {
- int32_t x;
- int32_t y;
-};
-
-struct djpoint {
- struct point pos;
- uint16_t weight;
- struct djpoint *parent;
-
- uint8_t type:3;
- uint8_t parent_pos:3;
- uint8_t updated:1;
- uint8_t todo:1;
-};
-
-uint8_t corn_table[CORN_NB];
-
-const uint8_t corn_coord_i[CORN_NB] = {
- 0, 0, 0, 2, 2, 2, 4, 4, 6,
- 6, 8, 8, 10, 10, 10, 12, 12, 12,
-};
-
-const uint8_t corn_coord_j[CORN_NB] = {
- 2, 4, 6, 3, 5, 7, 4, 6, 5,
- 7, 4, 6, 3, 5, 7, 2, 4, 6,
-};
-
-static struct djpoint djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY];
-
-/* table to find the symetric idx */
-uint8_t corn_sym[] = {
- 15, 16, 17, 12, 13, 14, 10, 11, 8, 9, 6, 7, 3, 4, 5, 0, 1, 2
-};
-
-uint8_t corn_side_confs[9][2] = {
- { 1, 4 },
- { 0, 4 },
- { 2, 4 },
- { 2, 3 },
- { 0, 3 },
- { 1, 3 },
- { 1, 6 },
- { 0, 6 },
- { 2, 6 },
-};
-uint8_t corn_center_confs[4][2] = {
- { 5, 8 },
- { 7, 8 },
- { 5, 9 },
- { 7, 8 },
-};
-
-
-/* return index from neigh pointer */
-#define PT2IDX(neigh) ( ((void *)(neigh)-(void *)(&djpoints)) / sizeof(*neigh) )
-
-void dump(void)
-{
- int8_t i, j;
- struct djpoint *pt;
-
- printf_P(PSTR(" "));
- for (i=0; i<WAYPOINTS_NBX; i++) {
- printf_P(PSTR(" %2d "), i);
- }
- printf_P(PSTR("\r\n"));
-
- for (j=WAYPOINTS_NBY*2-1; j>=0; j--) {
- printf_P(PSTR("%3d "), j/2);
-
- if ((j&1) == 0)
- printf_P(PSTR(" "));
-
- for (i=0; i<WAYPOINTS_NBX; i++) {
- pt = &djpoints[i][j/2];
-
- if (((i+j) & 1) == 0)
- continue;
-
- if (pt->type == TYPE_OBSTACLE)
- printf_P(PSTR(" X "));
- else if (pt->type == TYPE_DANGEROUS)
- printf_P(PSTR(" D "));
- else if (pt->type == TYPE_WHITE_CORN)
- printf_P(PSTR(" W "));
- else if (pt->type == TYPE_BLACK_CORN)
- printf_P(PSTR(" B "));
- else if (pt->type == TYPE_WAYPOINT)
- printf_P(PSTR(" %5d "), pt->weight);
- else
- printf_P(PSTR(" ? "));
- }
- printf_P(PSTR("\r\n"));
- }
-}
-
-static inline uint8_t opposite_position(uint8_t pos)
-{
- pos += 3;
- if (pos > UP_LEFT)
- pos -= 6;
- return pos;
-}
-
-/* return coord of the entry in the table from the pointer */
-static void djpoint2ij(struct djpoint *pt, int8_t *x, int8_t *y)
-{
- int8_t idx = PT2IDX(pt);
- *x = idx / WAYPOINTS_NBY;
- *y = idx % WAYPOINTS_NBY;
-}
-
-/* get the neighbour of the point at specified position */
-static struct djpoint *get_neigh(struct djpoint *pt,
- uint8_t position)