summaryrefslogtreecommitdiffhomepage
path: root/digital/beacon/src/recovery.c
blob: 891e35ed89233d87a02a304cdf583cf466d88859 (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
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
/* recovery.c */
/* Beacon recovery mode. {{{
 *
 * Copyright (C) 2012 Florent Duchon
 *
 * 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 "position.h"
#include "recovery.h"
#include "trust.h"
#include "debug_simu.h"

/* This function is used to calculate all obstacle positions from sractch */
TRecoveryStatus recovery(coord_s * new_point,opponent_s opp[MAX_OBSTACLE])
{
	/* Declare variables */
	int i = 0;
	int j = 0;
	int dx = 0;
	int dy = 0;

	static int counter = 0;								/* Loop counter */
	static int recovery_number = 0;						/* Potential valid positions Number */
	static int best_occurence = 0;							/* Maximum occurence amoung Potential valid Position */
	static recovery_s recovery_tab[MAX_POINT_RECOVERY];	/* Potention valid position Table containing (x;y) and occurence for all positions found */
	
	int best_position = 0;								/* Best Position variable found into recovery tab */

	
	/* In recovery mode, trust must be lowest possible till real position is not found. */
	for(j = 1 ; j <= MAX_OBSTACLE ; j++)
	{
		opp[j].trust=TRUST_MIN;
	}

	/* If we didn't reach the recovery threshold we continue to feed the recovery tab */	
	if(counter < MAX_POINT_RECOVERY)
	{	
		/* Search into already known positions */
		for(i = 0; i < recovery_number ; i ++)
		{
			dx = recovery_tab[i].x - new_point->x;
			dy = recovery_tab[i].y - new_point->y;
			
			if(dx * dx + dy * dy < OBSTACLE_RADIUS * OBSTACLE_RADIUS)
			{
				/* Seems be this position : update structure*/
				recovery_tab[i].occurence++;
				recovery_tab[i].x = new_point->x;
				recovery_tab[i].y = new_point->y;
				counter++;
				
				/* Check if the new occurence is the best one amout all other positions */
				if(recovery_tab[i].occurence > best_occurence)
				{
					best_occurence = recovery_tab[i].occurence;
					/* !! Tricky operation  !! : If counter reaches a defined threshold and if we just found the best occurence (ie best accuracy) we invalidate the current angle in order to ignore it next time.  */
					/* Doing that increase a lot the global computation accuracy by  ignoring unwanted phantom position */
					if(counter > RECOVERY_TRICKY_THRESHOLD)
					{ 				
						return RECOVERY_IGNORE_ANGLE_NEXT_TIME;
					}
				}
				/* Value found, counter and structures updated. Return NO_ERROR */
				return RECOVERY_IN_PROGRESS;
				
			}
		}
		/* Position was not found in the tab, we have to add it */
		recovery_tab[recovery_number].x = new_point->x;
		recovery_tab[recovery_number].y = new_point->y;
		recovery_tab[recovery_number].occurence++;
		recovery_number++;
		counter++;
		return RECOVERY_IN_PROGRESS;
	}
	else /* We have sufficient values to find absolute position of all obstacles */
	{
		
		for(j = 0 ; j < recovery_number ; j++)
		{
 			DEBUG_RECOVERY("[%d] (%d ; %d)\n",recovery_tab[j].occurence,recovery_tab[j].x,recovery_tab[j].y);
		}
		
		for(i = 1 ; i <= MAX_OBSTACLE ; i++)
		{
			/* Search for the position with best occurence */
			for(j = 0 ; j < recovery_number ; j++)
			{
				if(recovery_tab[j].occurence >= best_occurence)
				{
					best_position = j;
					best_occurence = recovery_tab[j].occurence;
				}
			}
			
			/* Update the obstacle structure with update position */
			recovery_tab[best_position].occurence = 0;
			opp[i].x = recovery_tab[best_position].x;
			opp[i].y = recovery_tab[best_position].y;
			opp[i].trust = TRUST_MAX;
			
			/* Reset variable for next obstacle */
			best_position = 0;
			best_occurence = 0;
			DEBUG_RECOVERY("Opponent %d ( %d ; %d ) trust = %d\n",i,opp[i].x,opp[i].y,opp[i].trust);
		}
		
		/* Reset couner variable for next recovery */
		counter = 0;
		recovery_number = 0;
		best_occurence = 0;
		for(j = 0 ; j < MAX_POINT_RECOVERY ; j++)
		{
			recovery_tab[j].occurence=0;
			recovery_tab[j].x=0;
			recovery_tab[j].y=0;
		}
		return RECOVERY_FINISHED;
	}
}