summaryrefslogtreecommitdiff
path: root/digital/avr/modules/path/test/test_path.c
blob: 218a31838027efc4624359502acf63ee84735126 (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
/* test_path.c */
/* avr.path - Path finding module. {{{
 *
 * Copyright (C) 2008 Nicolas Schodet
 *
 * APBTeam:
 *        Web: http://apbteam.org/
 *      Email: team AT apbteam DOT org
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
 *
 * }}} */
#include "common.h"
#include "modules/path/path.h"

#include <stdio.h>
#include <stdlib.h>

void
syntax (void)
{
    fprintf (stderr,
	     "test_path borders source destination escape_factor"
	     " [obstacles (0-%d)]\n"
	     "  borders: xmin,ymin,xmax,ymax\n"
	     "  source, destination: x,y\n"
	     "  obstacles: x,y,r,f\n"
	     "example: test_path 0,0,1500,1500 300,750 1200,750 0"
	     " 600,680,100,0"
	     " 900,820,100,10\n",
	     AC_PATH_OBSTACLES_NB);
    exit (1);
}

/** Read a comma separated list of N integers into TAB from S.  Exit on
 * error. */
void
read_tab (const char *s, int *tab, int n)
{
    assert (n > 0);
    while (n)
      {
	char *sp;
	int v;
	v = strtol (s, &sp, 10);
	if (sp == s)
	    syntax ();
	if ((n > 1 && *sp != ',')
	    || (n == 1 && *sp != '\0'))
	    syntax ();
	s = sp + 1;
	*tab++ = v;
	n--;
      }
}

int
main (int argc, char **argv)
{
    if (argc < 5 || argc > 5 + AC_PATH_OBSTACLES_NB)
	syntax ();
    int tab[4];
    read_tab (argv[1], tab, 4);
    path_init (tab[0], tab[1], tab[2], tab[3]);
    read_tab (argv[2], tab, 2);
    read_tab (argv[3], tab + 2, 2);
    path_endpoints (tab[0], tab[1], tab[2], tab[3]);
    read_tab (argv[4], tab, 1);
    if (tab[0])
	path_escape (tab[0]);
    int i;
    for (i = 0; i + 5 < argc; i++)
      {
	read_tab (argv[5 + i], tab, 4);
	path_obstacle (i, tab[0], tab[1], tab[2], tab[3], 1);
      }
    path_update ();
    path_print_graph ();
    uint16_t x, y;
    if (path_get_next (&x, &y))
	printf ("// Next point: %d, %d\n", x, y);
    else
	printf ("// Failure\n");
    return 0;
}

void
path_report (uint16_t *points, uint8_t len,
	     struct path_obstacle_t *obstacles, uint8_t obstacles_nb)
{
}