00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include "include/lqm.h"
00038 #include "include/global.h"
00039 #include "include/dijkstra.h"
00040 #include "include/wmp_utils.h"
00041 #include "include/nstat.h"
00042
00043 static char ** lqm, ** lqm_dist, ** lqm_copy, ** lqm_pruned;
00044 static int ** A0, ** A1, ** Next;
00045 static int size, net_connected = 0, informed = 0;
00046 static THREAD_SEM_T isconn;
00047
00048 static char (*fp) (char);
00049
00050 char f_lqm(char val) {
00051
00052 if (val == 0){
00053 return 0;
00054 } else if (val < status.w100)
00055 return 100;
00056 else if (val >= status.w100 && val < status.w3)
00057 return 3;
00058 else if (val >= status.w2 && val < status.w1)
00059 return 2;
00060 else
00061 return 1;
00062 }
00063
00064
00065 void init_lqm(int m_size){
00066 int i;
00067 size=m_size;
00068 lqm=(char **) MALLOC(size*sizeof(char*));
00069 for (i=0;i<size;i++){
00070 lqm[i]=(char *) MALLOC(size*sizeof(char));
00071 }
00072
00073
00074 lqm_dist=(char**) MALLOC(size*sizeof(char*));
00075 for (i=0;i<size;i++){
00076 lqm_dist[i]=(char *) MALLOC(size*sizeof(char));
00077 memset(lqm_dist[i],-1,size*sizeof(char));
00078 }
00079
00080
00081 lqm_copy=(char**) MALLOC(size*sizeof(char*));
00082 for (i=0;i<size;i++){
00083 lqm_copy[i]=(char *) MALLOC(size*sizeof(char));
00084 }
00085 lqm_pruned=(char**) MALLOC(size*sizeof(char*));
00086 for (i=0;i<size;i++){
00087 lqm_pruned[i]=(char *) MALLOC(size*sizeof(char));
00088 }
00089
00090
00091 A0=(int**) MALLOC(size*sizeof(int*));
00092 for (i=0;i<size;i++){
00093 A0[i]=(int *) MALLOC(size*sizeof(int));
00094 }
00095 A1=(int**) MALLOC(size*sizeof(int*));
00096 for (i=0;i<size;i++){
00097 A1[i]=(int *) MALLOC(size*sizeof(int));
00098 }
00099
00100 Next=(int**) MALLOC(size*sizeof(int*));
00101 for (i=0;i<size;i++){
00102 Next[i]=(int *) MALLOC(size*sizeof(int));
00103 }
00104
00105 fp = f_lqm;
00106
00107 THREAD_SEM_INIT_LOCKED(&isconn);
00108 }
00109
00110 void free_lqm() {
00111 int i;
00112
00113 for (i = 0; i < size; i++) {
00114 FREE((char*)lqm[i]);
00115 }
00116 FREE((char*)lqm);
00117
00118 for (i = 0; i < size; i++) {
00119 FREE((char*)lqm_dist[i]);
00120 }
00121 FREE((char*)lqm_dist);
00122
00123 for (i = 0; i < size; i++) {
00124 FREE((char*)lqm_copy[i]);
00125 }
00126 FREE((char*)lqm_copy);
00127
00128 for (i = 0; i < size; i++) {
00129 FREE((char*)lqm_pruned[i]);
00130 }
00131 FREE((char*)lqm_pruned);
00132
00133 for (i = 0; i < size; i++) {
00134 FREE((int*)A0[i]);
00135 }
00136 FREE((int*)A0);
00137
00138 for (i = 0; i < size; i++) {
00139 FREE((int*)A1[i]);
00140 }
00141 FREE((int*)A1);
00142 for (i = 0; i < size; i++) {
00143 FREE((int*)Next[i]);
00144 }
00145 FREE((int*)Next);
00146
00147 }
00148
00149 void fill_lqm(char val){
00150 int i,j;
00151 for (i=0;i<size;i++){
00152 for (j=0;j<size;j++){
00153 if (i!=j) lqm[i][j]=val;
00154 else lqm[i][j]=0;
00155 }
00156 }
00157 }
00158
00159 char lqm_get_val(int i, int j){
00160 return lqm[i][j];
00161 }
00162
00163 int lqm_get_num_hops(int i, int j){
00164 if (i<0 || j<0 || i>=size || j>=size){
00165 return -1;
00166 }else{
00167 return lqm_dist[i][j];
00168 }
00169 }
00170
00171 void lqm_set_val(int i, int j, char val){
00172 lqm[i][j]=val;
00173 }
00174
00175 char** lqm_get_ptr(void){
00176 return lqm;
00177 }
00178
00179 int wmpIsNetworkConnected(void){
00180 return net_connected;
00181 }
00182
00183 int wmpIsNetworkConnectedBlocking(int timeout_ms){
00184 int ret;
00185 informed = 0;
00186 if (timeout_ms > 0){
00187 ret = ( THREAD_SEM_WAIT_TIMED(isconn, timeout_ms) == 0 );
00188 } else{
00189 THREAD_SEM_WAIT(&isconn);
00190 ret = 1;
00191 }
00192 return ret;
00193 }
00194
00195 void lqm_calculate_distances(void){
00196 int i, j;
00197 net_connected = 1;
00198
00199
00200 dij_put_matrix(lqm);
00201
00202 for (i = 0; i < size; i++) {
00203
00204 for (j = 0; j < i; j++) {
00205 if (i != j) {
00206 int len = dijkstra_path_len(0, i, j);
00207
00208 if (nstat_isLost(i) || nstat_isLost(j)){
00209 len = -1;
00210 }
00211
00212 lqm_dist[i][j] = len;
00213 lqm_dist[j][i] = len;
00214 net_connected = (net_connected && (len > 0));
00215 } else {
00216 lqm_dist[i][j] = 0;
00217 }
00218
00219 }
00220 }
00221 if (net_connected && !informed) {
00222 THREAD_SEM_SIGNAL(&isconn);
00223 informed = 1;
00224 }else{
00225 informed = 0;
00226 }
00227 }
00228
00229 int lqm_is_leaf(char id){
00230 int i, sum=0;
00231 for (i=0;i<size;i++){
00232 if (lqm[(int)id][i]>0) sum+=1;
00233 }
00234 if (sum > 1) return 0;
00235 else if (sum ==1) return 1;
00236 else return -1;
00237 }
00238 static int copied=0;
00239
00240 void lqm_backup(){
00241 int i,j;
00242 for (i=0;i<size;i++){
00243 for (j=0;j<size;j++){
00244 lqm_copy[i][j]=lqm[i][j];
00245 }
00246 }
00247 copied = 1;
00248 }
00249
00250 void lqm_restore(){
00251 if (copied) {
00252 int i,j;
00253 for (i=0;i<size;i++){
00254 for (j=0;j<size;j++){
00255 lqm[i][j]=lqm_copy[i][j];
00256 }
00257 }
00258 }
00259 copied=0;
00260 }
00261
00262 int lqm_get_distance(char i, char j){
00263 return lqm_dist[(int)i][(int)j];
00264 }
00265
00266
00267 void print_lqm3(char* txt, char **lqm){
00268 char q[1020];
00269 lqmToString(lqm,q,status.N_NODES);
00270 WMP_ERROR(stderr,"@%s@\n%s#\n",txt,q);
00271 }
00272
00273 char ** lqm_prune(char ** mlqm) {
00274 int i, j, val_ij, val_ji, found;
00275
00276 if (!isConnected(lqm)) {
00277 return lqm;
00278 }
00279
00280 for (i = 0; i < size; i++) {
00281 for (j = 0; j < size; j++) {
00282 lqm_pruned[i][j] = mlqm[i][j];
00283 }
00284 }
00285
00286
00287
00288
00289 found = 1;
00290 while (found) {
00291 int umbral = 15, sel_i = 0, sel_j = 0;
00292 found = 0;
00293 for (i = 0; i < size; i++) {
00294 for (j = 0; j < size; j++) {
00295 if (i == j){
00296 continue;
00297 }
00298 if (lqm_pruned[i][j] > 0 && lqm_pruned[i][j] < umbral) {
00299 sel_i = i;
00300 sel_j = j;
00301 found = 1;
00302 umbral = lqm_pruned[i][j];
00303 }
00304 }
00305 }
00306
00307 if (found){
00308
00309
00310 val_ij = lqm_pruned[sel_i][sel_j];
00311 val_ji = lqm_pruned[sel_j][sel_i];
00312
00313 lqm_pruned[sel_i][sel_j] = 0;
00314 lqm_pruned[sel_j][sel_i] = 0;
00315
00316 if (!isConnected(lqm_pruned)) {
00317
00318 lqm_pruned[sel_i][sel_j] = -val_ij;
00319 lqm_pruned[sel_j][sel_i] = -val_ji;
00320 }
00321 }
00322
00323 }
00324 for (i = 0; i < size; i++) {
00325 for (j = 0; j < size; j++) {
00326 if (i==j) {
00327 continue;
00328 }
00329 lqm_pruned[i][j] = abs(lqm_pruned[i][j]);
00330 }
00331 }
00332
00333
00334
00335 return lqm_pruned;
00336 }
00337
00338 void lqm_copy_to(char ** dest, char ** src) {
00339 int i,j;
00340 for (i = 0; i < size; i++) {
00341 for (j = 0; j < size; j++) {
00342 if (i != j) {
00343 dest[i][j] = src[i][j];
00344 }
00345 }
00346 }
00347 }
00348
00349
00350 char (*lqm_get_f())(char){
00351 return fp;
00352 }
00353
00354
00355 void lqm_set_f( char (*f) (char)){
00356 fp = f;
00357 }
00358
00359 void lqm_compute_prob(char ** lqm) {
00360 int i,j,k;
00361 for (i=0; i< size; i++){
00362 for (j=0; j< size; j++){
00363 Next[i][j]=j;
00364 if (i==j){
00365 A0[i][j] = 1000;
00366 }else{
00367 A0[i][j] = lqm[i][j]*10;
00368 }
00369 }
00370 }
00371
00372 for (k = 0; k < size; k++) {
00373 for (i = 0; i < size; i++) {
00374 for (j = 0; j < size; j++) {
00375 if (A0[i][k] * A0[k][k] * A0[k][j] / 1000000 > A0[i][j]) {
00376 Next[i][j] = Next[i][k];
00377 A1[i][j] = A0[i][k] * A0[k][k] * A0[k][j] / 1000000;
00378
00379 } else {
00380 A1[i][j] = A0[i][j];
00381 }
00382 }
00383 }
00384 memcpy(A0, A1, sizeof(A0));
00385 }
00386 }
00387
00388
00389 int lqm_prob_get_path(int src, int dest, char * path) {
00390 int res = src, exres = 0, idx = 0;
00391 while (res != dest) {
00392
00393 res = Next[res][dest];
00394
00395 if (path != 0){
00396 path[idx] = res;
00397 idx++;
00398 }
00399
00400
00401
00402 exres = res;
00403 }
00404 return path[0];
00405 }
00406
00407 int lqm_prob_get_val(int src, int dest) {
00408 return A0[src][dest];
00409 }