algorithms.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 *
00003 * Software License Agreement (BSD License)
00004 *
00005 *  Copyright (c) 2014, ISR University of Coimbra.
00006 *  All rights reserved.
00007 *
00008 *  Redistribution and use in source and binary forms, with or without
00009 *  modification, are permitted provided that the following conditions
00010 *  are met:
00011 *
00012 *   * Redistributions of source code must retain the above copyright
00013 *     notice, this list of conditions and the following disclaimer.
00014 *   * Redistributions in binary form must reproduce the above
00015 *     copyright notice, this list of conditions and the following
00016 *     disclaimer in the documentation and/or other materials provided
00017 *     with the distribution.
00018 *   * Neither the name of the ISR University of Coimbra nor the names of its
00019 *     contributors may be used to endorse or promote products derived
00020 *     from this software without specific prior written permission.
00021 *
00022 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 *  POSSIBILITY OF SUCH DAMAGE.
00034 *
00035 * Author: David Portugal (2011-2014)
00036 *********************************************************************/
00037 
00038 #include <ctime>
00039 #include <climits>
00040 #include <cmath>
00041 #include <ros/ros.h>
00042 
00043 #include "getgraph.h"
00044 #include "algorithms.h"
00045 
00046 //#define M_LOG2E 1.44269504088896340736 //log2(e)
00047 uint reward_count = 0;
00048 
00049 using namespace std;
00050 
00051 
00052 
00053 inline long double log2(const long double x){
00054     return  log(x) * M_LOG2E;
00055 }
00056 
00057 
00058 uint random (uint current_vertex, vertex *vertex_web){
00059 
00060   //number of neighbors of current vertex (number of existing possibilites)
00061   uint num_neighs = vertex_web[current_vertex].num_neigh;
00062   uint next_vertex;
00063   
00064   srand ( time(NULL) );
00065   int i = rand() % num_neighs;
00066   next_vertex = vertex_web[current_vertex].id_neigh[i];
00067   
00068   return next_vertex;
00069 }
00070 
00071 
00072 uint conscientious_reactive (uint current_vertex, vertex *vertex_web, double *instantaneous_idleness){
00073 
00074   //number of neighbors of current vertex (number of existing possibilites)
00075   uint num_neighs = vertex_web[current_vertex].num_neigh;
00076   uint next_vertex;
00077   
00078   if (num_neighs > 1){
00079     
00080     double decision_table [num_neighs];
00081     uint neighbors [num_neighs];
00082     uint possibilities[num_neighs];
00083      
00084     uint i, hits=0;
00085     double max_idleness= -1;
00086     
00087     for (i=0; i<num_neighs; i++){
00088       neighbors[i] = vertex_web[current_vertex].id_neigh[i];            //neighbors table
00089       decision_table[i] = instantaneous_idleness [ neighbors[i] ];      //corresponding idleness table
00090       
00091       //choose the one with maximum idleness:
00092       if (decision_table[i] > max_idleness){
00093         max_idleness = decision_table[i];               //maximum idleness
00094 
00095         hits=0;
00096         possibilities[hits] = neighbors[i];
00097         
00098       }else if(decision_table[i] == max_idleness){
00099         hits ++;
00100         possibilities[hits] = neighbors[i];
00101       }
00102     }      
00103       
00104     if(hits>0){ //more than one possibility (choose at random)
00105       srand ( time(NULL) );
00106       i = rand() % (hits+1) + 0;        //0, ... ,hits
00107         
00108       //printf("rand integer = %d\n", i);
00109       next_vertex = possibilities [i];          // random vertex with higher idleness
00110         
00111       }else{
00112         next_vertex = possibilities[hits];      //vertex with higher idleness
00113       }
00114     
00115   }else{
00116     next_vertex = vertex_web[current_vertex].id_neigh[0]; //only one possibility
00117   }
00118   
00119   return next_vertex;
00120 
00121 }
00122 
00123 
00124 uint heuristic_conscientious_reactive (uint current_vertex, vertex *vertex_web, double *instantaneous_idleness){
00125 
00126   //number of neighbors of current vertex (number of existing possibilites)
00127   uint num_neighs = vertex_web[current_vertex].num_neigh;
00128   uint next_vertex;
00129   
00130   if (num_neighs > 1){
00131     
00132     //obtain max_idleness between neighbors
00133     //obtain max_distance between neighbors
00134     //build table with decision values (and neighbor index)
00135     
00136     uint i, max_distance=0;
00137     double max_idleness=-1;
00138     uint neighbors [num_neighs];    
00139     
00140     //printf("\n");
00141     
00142     //obtain max
00143     for (i=0; i<num_neighs; i++){      
00144       neighbors[i] = vertex_web[current_vertex].id_neigh[i];            //neighbors table
00145 
00146       if (instantaneous_idleness [neighbors[i]] > max_idleness){
00147         max_idleness = instantaneous_idleness [neighbors[i]];
00148       }
00149       
00150       if (vertex_web[current_vertex].cost[i] > max_distance){
00151         max_distance = vertex_web[current_vertex].cost[i];
00152       }
00153       
00154       //printf ("idleness[%u] = %f\n",neighbors[i],instantaneous_idleness [neighbors[i]]);
00155       //printf ("distance[%u] = %u\n",neighbors[i],vertex_web[current_vertex].cost[i]);
00156     }
00157     
00158     //printf ("\nmax idleness = %f\n", max_idleness);
00159     //printf ("max_distance = %u\n\n", max_distance);
00160     
00161     
00162     double normalized_idleness, normalized_distance, distance, idleness;
00163     double decision_table [num_neighs];
00164     
00165     //calculate decision values and build table
00166     for (i=0; i<num_neighs; i++){    
00167       
00168       distance = vertex_web[current_vertex].cost[i];
00169       idleness = instantaneous_idleness [ neighbors[i] ];
00170       
00171       normalized_distance = (double) (max_distance - distance) / (double) (max_distance); // [0,1]
00172       if (max_idleness == 0.0){
00173         normalized_idleness = 0.0;
00174       }else{
00175         normalized_idleness = idleness / max_idleness; // [0,1]
00176       }
00177      
00178       //printf ("normalized_distance[%u] = %f\n",neighbors[i],normalized_distance);
00179       //printf ("normalized_idleness[%u] = %f\n",neighbors[i],normalized_idleness);
00180       
00181       decision_table[i] = normalized_distance + normalized_idleness;     
00182       //printf("decision_table[%u] = %f\n\n",neighbors[i],decision_table[i]);
00183     }
00184       
00185     double max_decision=-1;
00186     uint hits = 0;
00187     uint possibilities[num_neighs];
00188     
00189     //verify if there is only one choice, if there are more - choose at random
00190     for (i=0; i<num_neighs; i++){  
00191       
00192       if (decision_table[i]>max_decision){
00193           max_decision = decision_table[i];
00194           hits = 0;
00195           possibilities[hits] = neighbors[i];
00196       }else if (decision_table[i]==max_decision){
00197           hits ++;
00198           possibilities[hits] = neighbors[i];
00199       }      
00200     }
00201     
00202     if(hits>0){ //more than one possibility (choose at random)
00203       //printf("MORE THAN ONE POSSIBILITY, CHOOSE RANDOMLY\n");
00204       srand ( time(NULL) );
00205       i = rand() % (hits+1) + 0;        //0, ... ,hits
00206         
00207       //printf("rand integer = %d\n", i);
00208       next_vertex = possibilities [i];          // random vertex with higher idleness
00209       //printf("next_vertex = %u\n",next_vertex);
00210         
00211     }else{
00212         next_vertex = possibilities[hits];      //vertex with higher idleness
00213         //printf("next_vertex = %u\n",next_vertex);
00214     } 
00215   
00216   
00217   }else{ //num_neighs == 1 -> only one possible choice
00218     next_vertex = vertex_web[current_vertex].id_neigh[0]; //only one possibility
00219     //printf("Only 1 neighbor: next_vertex = %u\n",next_vertex);
00220   }
00221   
00222   return next_vertex;
00223   
00224 }
00225 
00226 uint greedy_bayesian_strategy (uint current_vertex, vertex *vertex_web, double *instantaneous_idleness, double G1, double G2, double edge_min){
00227 
00228   //number of neighbors of current vertex (number of existing possibilites)
00229   uint num_neighs = vertex_web[current_vertex].num_neigh;
00230   uint next_vertex;
00231   
00232   if (num_neighs > 1){
00233     
00234     double posterior_probability [num_neighs];
00235     uint neighbors [num_neighs];
00236     uint possibilities[num_neighs];
00237      
00238     uint i, hits=0;
00239     double max_pp= -1;
00240     double gain, exp_param, edge_weight;
00241     double log_result = log (1.0/G1);
00242     
00243     for (i=0; i<num_neighs; i++){
00244       neighbors[i] = vertex_web[current_vertex].id_neigh[i];            //neighbors table
00245      
00246       edge_weight = (double) vertex_web[current_vertex].cost[i];
00247       if (edge_weight<edge_min) {edge_weight = edge_min;}       
00248       
00249       //printf("Edge cost = %d\n",vertex_web[current_vertex].cost[i]);    
00250       gain = (instantaneous_idleness [ neighbors[i] ] / edge_weight);           //corresponding gain
00251       //printf("Gain = Inst Idleness / Edge Cost = %f / %f = %f\n", instantaneous_idleness [neighbors[i]],(double)vertex_web[current_vertex].cost[i],gain);
00252       
00253       if (gain < G2){
00254       
00255         //printf("Log_result = ln(1/G1) = 1.0 / %f = %f\n", G1, log_result);
00256         
00257         exp_param = (log_result/G2)*gain;
00258         //printf("exp_param = exp(log_param/G2 * gain) = ( %f / %f ) * %f = %f\n", log_param, (double)G2, gain, exp_param);
00259         
00260         posterior_probability[i] = G1 * exp (exp_param);        //P(move)=0.5 anula com P(gain) = 0.5 [factor de escala]
00261         //printf("posterior_probability = G1 * exp_param = %f  * %f = %f\n", G1, exp_param,  posterior_probability[i]);      
00262         
00263       }else{
00264         posterior_probability[i] = 1.0;
00265       }
00266       
00267       //printf("Vertex [%d]; PP = %f\n", vertex_web[current_vertex].id_neigh[i], posterior_probability[i]);
00268       
00269       //choose the one with maximum posterior_probability:
00270       if (posterior_probability[i] > max_pp){
00271         max_pp = posterior_probability[i];              //maximum idleness
00272 
00273         hits=0;
00274         possibilities[hits] = neighbors[i];
00275         
00276       }else if (posterior_probability[i] == max_pp){
00277         hits ++;
00278         possibilities[hits] = neighbors[i];
00279       }
00280     }      
00281       
00282     if(hits>0){ //more than one possibility (choose at random)
00283       srand ( time(NULL) );
00284       i = rand() % (hits+1) + 0;        //0, ... ,hits
00285         
00286       //printf("rand integer = %d\n", i);
00287       next_vertex = possibilities [i];          // random vertex with higher idleness
00288         
00289       }else{
00290         next_vertex = possibilities[hits];      //vertex with higher idleness
00291       }
00292     
00293   }else{
00294     next_vertex = vertex_web[current_vertex].id_neigh[0]; //only one possibility
00295   }
00296   
00297   return next_vertex;
00298 
00299 }
00300 
00301 int count_intention (uint vertex, int *tab_intention, int nr_robots){
00302  
00303   int count = 0;
00304   
00305   for (int i = 0; i<nr_robots; i++){   
00306     if(tab_intention[i]==vertex){
00307       count++;      
00308     }    
00309   }
00310   return count;  
00311 }
00312 
00313 int count_intention_cbls (uint vertex, int *tab_intention, int nr_robots, int id_robot){
00314  
00315   int count = 0;
00316   
00317   for (int i = 0; i<nr_robots; i++){   
00318     if(tab_intention[i]==vertex && i!=id_robot){ //ignore own intention
00319       count++;      
00320     }     
00321   }
00322   return count;  
00323 }
00324 
00325 uint state_exchange_bayesian_strategy (uint current_vertex, vertex *vertex_web, double *instantaneous_idleness, int *tab_intention, int nr_robots, double G1, double G2, double edge_min){
00326 
00327   //number of neighbors of current vertex (number of existing possibilites)
00328   uint num_neighs = vertex_web[current_vertex].num_neigh;
00329   uint next_vertex;
00330   
00331   if (num_neighs > 1){
00332     
00333     double posterior_probability [num_neighs];
00334     uint neighbors [num_neighs];
00335     uint possibilities[num_neighs];
00336      
00337     uint i, hits=0;
00338     double max_pp= -1;
00339     double gain, exp_param, edge_weight;
00340     double log_result = log (1.0/G1);
00341     
00342     for (i=0; i<num_neighs; i++){
00343       neighbors[i] = vertex_web[current_vertex].id_neigh[i];            //neighbors table
00344      
00345       edge_weight = (double) vertex_web[current_vertex].cost[i];
00346       if (edge_weight<edge_min) {edge_weight = edge_min;}
00347       
00348       //printf("Edge cost = %d\n",vertex_web[current_vertex].cost[i]);    
00349       gain = (instantaneous_idleness [ neighbors[i] ] / edge_weight);           //corresponding gain
00350       //printf("Gain = Inst Idleness / Edge Cost = %f / %f = %f\n", instantaneous_idleness [neighbors[i]],(double)vertex_web[current_vertex].cost[i],gain);
00351       
00352       if (gain < G2){
00353       
00354         //printf("Log_result = ln(1/G1) = 1.0 / %f = %f\n", G1, log_result);
00355         
00356         exp_param = (log_result/G2)*gain;
00357         //printf("exp_param = exp(log_param/G2 * gain) = ( %f / %f ) * %f = %f\n", log_param, (double)G2, gain, exp_param);
00358         
00359         posterior_probability[i] = G1 * exp (exp_param);        //P(move)=0.5 anula com P(gain) = 0.5 [factor de escala]
00360         //printf("posterior_probability = G1 * exp_param = %f  * %f = %f\n", G1, exp_param,  posterior_probability[i]);      
00361         
00362       }else{
00363         posterior_probability[i] = 1.0;
00364       }
00365       
00366       //NESTE PONTO TEMOS A PP considerando so o GANHO, agora vamos considerar tb o ESTADO ("intenções") do sistema:
00367       
00368       //contar ocorrencias do vertice na tab_intention:
00369       int count = count_intention (neighbors[i], tab_intention, nr_robots);
00370       
00371       if (count>0){     //There is a robot who intends to go to this vertex! -> Update State
00372         
00373         //printf("COUNT = %d\n",count);
00374         //printf("Vertex [%d]; PP (without state exchange) = %f\n", vertex_web[current_vertex].id_neigh[i], posterior_probability[i]);
00375         double P_gain_state = ( pow(2,nr_robots-count) ) / ( pow(2,nr_robots) - 1.0);   
00376         posterior_probability[i] *= P_gain_state;
00377         //printf("Vertex [%d]; PP (depois) = %f\n", vertex_web[current_vertex].id_neigh[i], posterior_probability[i]);
00378         
00379       }
00380 
00381       //printf("Vertex [%d]; PP = %f\n", vertex_web[current_vertex].id_neigh[i], posterior_probability[i]);
00382       
00383       //choose the one with maximum posterior_probability:
00384       if (posterior_probability[i] > max_pp){
00385         max_pp = posterior_probability[i];              //maximum idleness
00386 
00387         hits=0;
00388         possibilities[hits] = neighbors[i];
00389         
00390       }else if (posterior_probability[i] == max_pp){
00391         hits ++;
00392         possibilities[hits] = neighbors[i];
00393       }
00394     }      
00395       
00396     if(hits>0){ //more than one possibility (choose at random)
00397       srand ( time(NULL) );
00398       i = rand() % (hits+1) + 0;        //0, ... ,hits
00399         
00400       //printf("rand integer = %d\n", i);
00401       next_vertex = possibilities [i];          // random vertex with higher idleness
00402         
00403       }else{
00404         next_vertex = possibilities[hits];      //vertex with higher idleness
00405       }
00406     
00407   }else{
00408     next_vertex = vertex_web[current_vertex].id_neigh[0]; //only one possibility
00409   }
00410   
00411   return next_vertex;
00412 
00413 }
00414 
00415 void dijkstra( uint source, uint destination, int *shortest_path, uint &elem_s_path, vertex *vertex_web, uint dimension){
00416   
00417   uint i,j,k,x;
00418   int id_next_vertex=-1;
00419   
00420   s_path *tab_dijkstra = new s_path[dimension];
00421   
00422   //Initialization:
00423   for(i=0; i<dimension; i++){
00424         
00425         tab_dijkstra[i].id = vertex_web[i].id;
00426         tab_dijkstra[i].elem_path = 0;
00427         tab_dijkstra[i].visit = false;
00428         
00429         if (vertex_web[i].id == source) {
00430           tab_dijkstra[i].dist = 0;
00431           tab_dijkstra[i].path[0] = source;
00432           tab_dijkstra[i].elem_path++;
00433           id_next_vertex = i;
00434         }else{
00435           tab_dijkstra[i].dist = INT_MAX;
00436         }
00437 
00438   }  
00439   
00440   int next_vertex = source;
00441   int minim_dist;
00442   bool cont;
00443   
00444   while(true){  
00445         
00446 //      printf("next_vertex = %i\n", next_vertex);
00447         
00448         if(next_vertex == destination){
00449          break; 
00450         }
00451         
00452         tab_dijkstra[id_next_vertex].visit = true;
00453         
00454         /* id_next_vertex has INDEX of next_vertex in tab_dijkstra */
00455         /* j has INDEX of the neighbor in tab_disjkstra */
00456         /* k has index of the neighbor in the neighbor table of next_vertex */
00457         
00458         //Go to neihgobors;     
00459         for(k=0; k<vertex_web[next_vertex].num_neigh; k++){
00460           
00461                 cont = false;
00462                 
00463                 //condition - cannot have been visited:
00464                 for(j=0; j<dimension; j++){
00465                   if(tab_dijkstra[j].id == vertex_web[next_vertex].id_neigh[k] && tab_dijkstra[j].visit == false){
00466                         cont = true;
00467                         break;
00468                   }               
00469                 }
00470                 
00471                 if(cont){
00472                   
00473                   //calculate distance from this neighbor:
00474                   if( tab_dijkstra[id_next_vertex].dist + vertex_web[next_vertex].cost[k] < tab_dijkstra[j].dist){
00475                         
00476                         //update distance to this vertex:
00477                         tab_dijkstra[j].dist = tab_dijkstra[id_next_vertex].dist + vertex_web[next_vertex].cost[k];
00478                         
00479                         //update path (previous path + this one):
00480                         for (x=0; x<tab_dijkstra[id_next_vertex].elem_path; x++){                        
00481                           tab_dijkstra[j].path[x] = tab_dijkstra[id_next_vertex].path[x];                         
00482                         }
00483                         
00484                         tab_dijkstra[j].path[tab_dijkstra[id_next_vertex].elem_path] = tab_dijkstra[j].id;
00485                         tab_dijkstra[j].elem_path = tab_dijkstra[id_next_vertex].elem_path+1;
00486 
00487                   }     
00488                   
00489                 }
00490                 
00491 
00492  
00493         }
00494         
00495         minim_dist = INT_MAX;
00496         
00497         //decide next_vertex:
00498         for(i=0; i<dimension; i++){       
00499           
00500           if(tab_dijkstra[i].dist < minim_dist && tab_dijkstra[i].visit == false){
00501                 minim_dist = tab_dijkstra[i].dist;
00502                 next_vertex = tab_dijkstra[i].id;
00503                 id_next_vertex = i;
00504           }      
00505           
00506         }
00507         
00508   }
00509   
00510   //Save shortest_path & delete tab_dijkstra... 
00511   elem_s_path = tab_dijkstra[id_next_vertex].elem_path; //id_next_vertex has the ID of the destination in tab_dijkstra
00512 
00513   for(i=0; i<elem_s_path; i++){ 
00514         shortest_path[i] = tab_dijkstra[id_next_vertex].path[i];        
00515   }
00516   
00517   delete [] tab_dijkstra;
00518   
00519 }
00520 
00521 
00522 int is_neigh(uint vertex1, uint vertex2, vertex *vertex_web, uint dimension){
00523   int i;
00524   
00525   for (i=0; i<vertex_web[vertex1].num_neigh; i++){
00526    if(vertex2 == vertex_web[vertex1].id_neigh[i]){
00527      return i; //neighbor_id (vertex1 in respect to vertex2)
00528    }
00529   }
00530   
00531   return -1; //not neighbor
00532 }
00533 
00534 
00535 void dijkstra_mcost( uint source, uint destination, int *shortest_path, uint &elem_s_path, vertex *vertex_web, double new_costs[][8], uint dimension){
00536   
00537   uint i,j,k,x;
00538   int id_next_vertex=-1;
00539   
00540   s_path_mcost *tab_dijkstra = new s_path_mcost[dimension];
00541   
00542   //Initialization:
00543   for(i=0; i<dimension; i++){
00544         
00545         tab_dijkstra[i].id = vertex_web[i].id;
00546         tab_dijkstra[i].elem_path = 0;
00547         tab_dijkstra[i].visit = false;
00548         
00549         if (vertex_web[i].id == source) {
00550           tab_dijkstra[i].dist = 0.0;
00551           tab_dijkstra[i].path[0] = source;
00552           tab_dijkstra[i].elem_path++;
00553           id_next_vertex = i;
00554         }else{
00555           tab_dijkstra[i].dist = INFINITY;
00556         }
00557 
00558   }  
00559   
00560   int next_vertex = source;
00561   double minim_dist;
00562   bool cont;
00563   
00564   while(true){  
00565         
00566 //      printf("next_vertex = %i\n", next_vertex);
00567         
00568         if(next_vertex == destination){
00569          break; 
00570         }
00571         
00572         tab_dijkstra[id_next_vertex].visit = true;
00573         
00574         /* id_next_vertex has INDEX of next_vertex in tab_dijkstra */
00575         /* j has INDEX of the neighbor in tab_disjkstra */
00576         /* k has index of the neighbor in the neighbor table of next_vertex */
00577         
00578         //Go to neihgobors;     
00579         for(k=0; k<vertex_web[next_vertex].num_neigh; k++){
00580           
00581                 cont = false;
00582                 
00583                 //condition - cannot have been visited:
00584                 for(j=0; j<dimension; j++){
00585                   if(tab_dijkstra[j].id == vertex_web[next_vertex].id_neigh[k] && tab_dijkstra[j].visit == false){
00586                         cont = true;
00587                         break;
00588                   }               
00589                 }
00590                 
00591                 if(cont){
00592                   
00593                   //calculate distance from this neighbor:
00594                   if( tab_dijkstra[id_next_vertex].dist + new_costs[next_vertex][k] < tab_dijkstra[j].dist){
00595                         
00596                         //update distance to this vertex:
00597                         tab_dijkstra[j].dist = tab_dijkstra[id_next_vertex].dist + new_costs[next_vertex][k];
00598                         
00599                         //update path (previous path + this one):
00600                         for (x=0; x<tab_dijkstra[id_next_vertex].elem_path; x++){                        
00601                           tab_dijkstra[j].path[x] = tab_dijkstra[id_next_vertex].path[x];                         
00602                         }
00603                         
00604                         tab_dijkstra[j].path[tab_dijkstra[id_next_vertex].elem_path] = tab_dijkstra[j].id;
00605                         tab_dijkstra[j].elem_path = tab_dijkstra[id_next_vertex].elem_path+1;
00606 
00607                   }     
00608                   
00609                 }
00610                 
00611 
00612  
00613         }
00614         
00615         minim_dist = INFINITY;
00616         
00617         //decide next_vertex:
00618         for(i=0; i<dimension; i++){       
00619           
00620           if(tab_dijkstra[i].dist < minim_dist && tab_dijkstra[i].visit == false){
00621                 minim_dist = tab_dijkstra[i].dist;
00622                 next_vertex = tab_dijkstra[i].id;
00623                 id_next_vertex = i;
00624           }      
00625           
00626         }
00627         
00628   }
00629   
00630   //Save shortest_path & delete tab_dijkstra... 
00631   elem_s_path = tab_dijkstra[id_next_vertex].elem_path; //id_next_vertex has the ID of the destination in tab_dijkstra
00632 
00633   for(i=0; i<elem_s_path; i++){ 
00634         shortest_path[i] = tab_dijkstra[id_next_vertex].path[i];        
00635   }
00636   
00637   delete [] tab_dijkstra;
00638   
00639 }
00640 
00641 
00642 uint heuristic_pathfinder_conscientious_cognitive (uint current_vertex, vertex *vertex_web, double *instantaneous_idleness, uint dimension, uint *path){
00643   
00644           //Heuristic Decision (Considering ALL vertices of the graph and shortest path to them)
00645           uint distance[dimension];                     //distance from current_vertex to all vertices    
00646           uint i,j, elem_s_path, max_distance=0, max_cost=0, min_cost=INT_MAX;
00647           double max_idleness=-1;
00648           
00649           
00650           for (i=0; i<dimension; i++){
00651             
00652             //max_idleness
00653             if (instantaneous_idleness [i] > max_idleness){
00654               max_idleness = instantaneous_idleness [i];
00655             }      
00656            
00657             if (i==current_vertex){           
00658               distance[i] = 0;
00659 //            printf("distance[%u]=%u\n\n",i,distance[i]);
00660               
00661             }else{
00662               
00663               int id_neigh = is_neigh(current_vertex, i, vertex_web, dimension);
00664               
00665               if (id_neigh>=0){ //neighbors             
00666                 distance[i] = vertex_web[current_vertex].cost[id_neigh];
00667 //              printf("distance[%u]=%u\n\n",i,distance[i]);
00668                 
00669               }else{    //not neighbors
00670                 
00671                 int *shortest_path = new int[dimension]; 
00672                 uint j;
00673         
00674                 //Call with normal costs:
00675                 dijkstra( current_vertex, i, shortest_path, elem_s_path, vertex_web, dimension); //structure with normal costs
00676                 distance[i] = 0;
00677                 
00678                 for(j=0; j<elem_s_path; j++){
00679 //                printf("caminho[%u] = %d\n",j,shortest_path[j]);
00680                   
00681                   if (j<elem_s_path-1){
00682                     id_neigh = is_neigh(shortest_path[j], shortest_path[j+1], vertex_web, dimension);
00683                     distance[i] += vertex_web[shortest_path[j]].cost[id_neigh];
00684                   }               
00685                 }
00686 //              printf("distance[%u]=%u\n\n",i,distance[i]);
00687                 delete [] shortest_path;
00688               }       
00689             }
00690             
00691             //max and min costs:
00692             for(j=0; j<vertex_web[i].num_neigh; j++){
00693               if (vertex_web[i].cost[j] > max_cost){
00694                 max_cost = vertex_web[i].cost[j];
00695               }     
00696               if (vertex_web[i].cost[j] < min_cost){
00697                 min_cost = vertex_web[i].cost[j];
00698               }
00699             }    
00700             
00701             //max distance:
00702             if (distance[i] > max_distance){
00703               max_distance = distance[i];
00704             }       
00705           }
00706           
00707 //        printf("max_distance = %u\n\n",max_distance);
00708 //        printf("max_cost = %u\n\n",max_cost);
00709 //        printf("min_cost = %u\n\n",min_cost);
00710           
00711           double normalized_idleness, normalized_distance;
00712           double decision_table [dimension];
00713           
00714           //obtain max_decision having max_distance & max_idleness
00715           for (i=0; i<dimension; i++){    
00716             
00717             normalized_distance = (double) (max_distance - distance[i]) / (double) (max_distance); // [0,1]
00718             
00719             if (max_idleness == 0.0){
00720               normalized_idleness = 0.0;
00721             }else{
00722               normalized_idleness = instantaneous_idleness[i] / max_idleness; // [0,1]
00723             }
00724                     
00725             if (i!=current_vertex){
00726               decision_table[i] = normalized_distance + normalized_idleness;     
00727             }else{
00728               decision_table[i] = 0.0;
00729             }
00730           }  
00731           
00732 //        printf("max_idleness = %f\n\n",max_idleness);
00733           
00734           //next_vertex is the one with max_decision
00735 //        double max_decision = 0.0;
00736           uint next_vertex;
00737           
00738           //Pathfinder -> Convert the weights (edge costs) of the graph to a new scale and find shortest path to the next_vertex
00739           double new_costs [dimension][8];
00740                   
00741           for (i=0; i<dimension; i++){
00742             
00743             //Get Next Vertex:
00744 //          printf("decision_table[%u] = %f\n",i,decision_table[i]);
00745             
00746            /* if (decision_table[i]>max_decision){
00747              next_vertex = i;
00748              max_decision = decision_table[i]; // GUARDAR PARA TABELA DE POSSIBILIDADES E ESCOLHER ALEATORIAMENTE
00749             }*/
00750             
00751             //Get New Edge Costs:
00752             for(j=0; j<vertex_web[i].num_neigh; j++){
00753                   
00754                   //the same edge will have 2 different costs depending on the direction of travelling:
00755                   if (max_idleness==0.0){
00756                     normalized_idleness = 0.0;
00757                   }else{
00758                     normalized_idleness = (max_idleness - instantaneous_idleness[ vertex_web[i].id_neigh[j] ]) / max_idleness; 
00759                   }
00760                   
00761                   if (max_cost==min_cost){
00762                     normalized_distance = 0.0;
00763                   }else{
00764                     normalized_distance = (double)(vertex_web[i].cost[j] - min_cost) / (double)(max_cost-min_cost);
00765                   }
00766                   
00767 //                printf("normalized_idleness = %f\n", normalized_idleness);
00768 //                printf("normalized_distance = %f\n", normalized_distance);
00769                   
00770                   new_costs[i][j] = normalized_idleness + normalized_distance;  
00771                   
00772 //                printf("new_costs[%d][%d] = %f\n\n", i,j,new_costs[i][j]);
00773             }
00774             
00775           }
00776           
00777           
00778         double max_decision=-1;
00779         uint hits = 0;
00780         uint possibilities[dimension];
00781         
00782         //verify if there is only one choice, if there are more - choose at random
00783         for (i=0; i<dimension; i++){  
00784                 
00785                 if (decision_table[i]>max_decision){
00786                         max_decision = decision_table[i];
00787                         hits = 0;
00788                         possibilities[hits] = i;
00789                 }else if (decision_table[i]==max_decision){
00790                         hits ++;
00791                         possibilities[hits] = i;
00792                 }      
00793         }
00794         
00795         if(hits>0){     //more than one possibility (choose at random)
00796                 //printf("MORE THAN ONE POSSIBILITY, CHOOSE RANDOMLY\n");
00797                 srand ( time(NULL) );
00798                 i = rand() % (hits+1) + 0;      //0, ... ,hits
00799                         
00800                 //printf("rand integer = %d\n", i);
00801                 next_vertex = possibilities [i];                // random vertex with higher idleness
00802                 //printf("next_vertex = %u\n",next_vertex);
00803                         
00804         }else{
00805                 next_vertex = possibilities[hits];      //vertex with higher idleness
00806                 //printf("next_vertex = %u\n",next_vertex);
00807         }         
00808           
00809 //        printf("max_decision=%f\n\n",max_decision);
00810 //        printf("next_vertex=%u\n",next_vertex);
00811           
00812           int *shortest_path = new int[dimension]; 
00813   
00814           //Disjkstra function with modified costs:
00815           dijkstra_mcost( current_vertex, next_vertex, shortest_path, elem_s_path, vertex_web, new_costs, dimension);
00816           
00817           //int caminho_final[elem_s_path];
00818           
00819 //        printf("\n");
00820           for(i=0; i<elem_s_path; i++){
00821             path[i] = shortest_path[i];
00822 //          printf("path [%d] = %d\n",i,path[i]);
00823           }
00824           
00825           delete [] shortest_path;
00826           
00827           return elem_s_path;
00828   
00829 }
00830 
00831 //Check if an element belongs to a table
00832 bool pertence (int elemento, int *tab, int tam_tab){
00833   
00834   for (int i=0; i<tam_tab; i++){
00835         
00836         if( tab[i] == elemento) {
00837           return true;
00838         }
00839         
00840   }
00841   
00842   return false;  
00843 }
00844 
00845 int pertence_idx (int elemento, int *tab, int tam_tab){
00846   
00847   for (int i=0; i<tam_tab; i++){
00848         
00849         if( tab[i] == elemento) {
00850           return i;
00851         }
00852         
00853   }
00854   
00855   return -1;  
00856 }
00857 
00858 bool UHC (vertex *vertex_web, int v1, int *caminho_principal, uint dimension) {
00859   
00860 //  printf("\n\nORIGEM_da_PROCURA=%i\n",v1);
00861 //  printf("1o.Vertice = %i\n", v1);
00862   
00863   int prox_vertice = v1, elem_cp = 0, v, i, i_viz;
00864   int caminho_parcial [dimension];
00865   int *viz_pvert = new int[8];
00866   srand(1000);
00867   
00868   caminho_parcial[elem_cp] = prox_vertice;
00869   elem_cp++;
00870 //  printf("adicionei prox_vertice=%i ao caminho parcial\n", prox_vertice);
00871   
00872   
00873   while( elem_cp < dimension ){
00874         
00875         i_viz=0;
00876         
00877         //construir tab de vizinhos do prox_vertice:
00878         for(i=0; i<vertex_web[prox_vertice].num_neigh; i++){
00879                 
00880           if ( !pertence (vertex_web[prox_vertice].id_neigh[i], caminho_parcial, elem_cp) ){
00881             viz_pvert[i_viz] = vertex_web[prox_vertice].id_neigh[i];
00882             i_viz++;
00883           }
00884 
00885           
00886         }       
00887         
00888         if(i_viz>0){
00889           i = rand() % i_viz;
00890           v = viz_pvert[i]; //v = vizinho aleatorio do prox_vertice
00891         }else{
00892 //        printf("Todos os vizinhos ja pertencem ao caminho parcial\n");
00893           return false;   
00894         }
00895         
00896 //      printf("v = vizinho aleatorio do prox_vertice = %i\n",v);
00897 //      printf("elem_cp = %i\t dimension = %i\n", elem_cp, dimension);
00898         
00899         
00900         if( !pertence(v, caminho_parcial, elem_cp) ) {          //adicionar v ao caminho parcial:
00901 //              printf("adicionar v = %i ao caminho parcial\n",v);
00902                 caminho_parcial [elem_cp] = v;
00903                 elem_cp++;
00904 //              printf("prox_vertice = %i\n", v);
00905                 prox_vertice = v;
00906         }
00907         
00908 /*      printf("\nCAMINHO PARCIAL ATE AO MOMENTO:\n");
00909         
00910         for(i=0; i<elem_cp; i++){
00911           printf("caminho_parcial[%i]=%i\n", i, caminho_parcial[i]); 
00912         }
00913 */      
00914   }
00915   
00916 //  printf("ENCONTREI C. HAMILTON!\n");
00917   
00918   delete [] viz_pvert;
00919   
00920   //atribuir caminho principal:
00921   for(i=0; i<elem_cp; i++){
00922         caminho_principal[i] = caminho_parcial[i];
00923   }
00924   
00925   return true;  
00926   
00927 }
00928 
00929 void clear_visited (vertex *vertex_web, uint dimension){ //LIMPA TODA A REDE DE NOS (NAO SO DA REGIAO)
00930   
00931   int i,j;
00932   
00933   for (i=0; i<dimension; i++){
00934         for(j=0; j<vertex_web[i].num_neigh; j++){
00935           //inicializar a false:
00936           vertex_web[i].visited[j] = false;
00937         }
00938   }
00939   
00940 }
00941 
00942 bool procurar_ciclo (vertex *vertex_web, uint dimension, int *caminho_principal, int &elem_caminho, int &custo_max, int seed) {
00943   
00944   int i, k=0, n, rec, i_list=0, origem, custo, ult_elemento, elem_cp=0, caminho_parcial [dimension];
00945   bool encontra_destino, escolha_aleatoria;
00946   int *lista_v2v = new int[1000];
00947   
00948   clear_visited(vertex_web, dimension);
00949 
00950   for(i=0; i<dimension; i++){
00951         
00952         if( vertex_web[i].num_neigh > 1 ) { //tem + d 1 vizinho: criar tabela com estes.
00953           
00954           lista_v2v[i_list] = vertex_web[i].id;
00955           i_list++; 
00956           
00957         }
00958   }
00959 
00960 
00961 
00962   bool encontrou_ciclo=false;
00963   int count=0;
00964   custo_max = 0;
00965 
00966   for (n=0; n<i_list+1; n++){
00967         
00968         if(encontrou_ciclo && count<3){ //numero de tentativas total apos encontrar o 1� ciclo = 3
00969                 
00970                 count++;
00971                 seed+=1000;
00972                 n--; //volta a repetir o mm 'n' com outra semente
00973 
00974         }else{
00975           count = 0;
00976           seed = 1000;
00977           if(n==i_list){break;}
00978         }
00979         
00980         encontrou_ciclo = false;        
00981         srand(seed);
00982 
00983         
00984         //limpar tudo:
00985         clear_visited(vertex_web, dimension);
00986         
00987         
00988         //colocar nos com 1 so vizinho a TRUE [e reciprocos!!]:
00989         for(i=0; i<dimension; i++){
00990           
00991           if( vertex_web[i].num_neigh == 1 ) { //so tem 1 vizinho
00992                 
00993                 vertex_web[i].visited[0] = true;
00994                 //        printf(" Arco de (%i para %i).visitado = true\n", elem_reg[i], vertex_web[elem_reg[i]].id_neigh[0] );
00995                 
00996                 rec = vertex_web[i].id_neigh[0];
00997                 
00998                 for(k=0; k<vertex_web[rec].num_neigh; k++){
00999                   if (vertex_web[rec].id_neigh[k] ==  vertex_web[i].id){
01000                         vertex_web[rec].visited[k]=true;
01001                         //                printf(" Arco de (%i para %i).visitado = true\n", rec, vertex_web[rec].id_neigh[k] );
01002                         break;
01003                   }
01004                 }
01005                 
01006           }
01007         }
01008   
01009         
01010 //      printf("\n\n========================== NO %i ============================\n\n", lista_v2v[n]);
01011         
01012         custo = 0;
01013         origem = lista_v2v[n];
01014         elem_cp = 0;    
01015         encontra_destino = false; //numa primeira fase queremos adiar a chegada ao destino.
01016         int *viz = new int[8];
01017         ult_elemento=-1;
01018         int aux, conta, viz_c_4_arcos;
01019         
01020         //caminho:
01021         caminho_parcial [ elem_cp ] = origem;
01022         elem_cp++;
01023         
01024         while( ult_elemento!=origem || elem_cp>1 ){ //acaba quando voltar a origem.
01025           
01026           escolha_aleatoria = false;
01027           
01028           ult_elemento = caminho_parcial [elem_cp-1];
01029           
01030 //        printf("---------------------------------------\n");
01031 //        printf("Ultimo Elemento = %i\n", ult_elemento);
01032 //        printf("num. de elementos no cp= %i\n", elem_cp);
01033 //        printf("origem= %i\n", origem);
01034           
01035           //verificar estado dos vizinhos do elemento actual:   
01036           k=0;
01037           
01038           //construir tab de vizinhos do prox_vertice:
01039           for(i=0; i<vertex_web[ult_elemento].num_neigh; i++){
01040                 
01041                 //pertence a mm regiao:
01042                 //if (vertex_web[vertex_web[ult_elemento].id_neigh[i]].regiao == ID_REG || (vertex_web[ult_elemento].regiao == ID_REG && vertex_web[vertex_web[ult_elemento].id_neigh[i]].regiao != ID_REG && vertex_web[ult_elemento].arco_front[i] == true) ){
01043                 //if( pertence( vertex_web[ult_elemento].id_neigh[i], elem_reg, dimension) ){
01044                   
01045                   //nunca foi visitado e nao pertence ao caminho parcial:
01046                   //if ( vertex_web[ult_elemento].visitado[i] == false && ( !pertence(vertex_web[ult_elemento].id_neigh[i], caminho_parcial, elem_cp) || vertex_web[ult_elemento].id_neigh[i] == origem) ){
01047                   
01048                   /*pode repetir vertices: na pratica so o vai fazer s este tiver 4 ou + vizinhos*/
01049                   if ( vertex_web[ult_elemento].visited[i] == false){ 
01050                         viz[k] = vertex_web[ult_elemento].id_neigh[i];
01051                         k++;
01052                   }
01053                 //}
01054                 
01055           }
01056           
01057           
01058           if (k==0){ //ja nao ha vizinhos disponiveis -- voltar para tras.
01059                 
01060 //              printf("k=0\n");
01061                 encontra_destino = true;
01062                 
01063                 if(elem_cp>1){
01064                   //voltar para o anterior e passar visitados a false (menos o q liga este vizinho ao anterior no caminho parcial.
01065 //                printf("percorrer vizinhos de %i\n", vertex_web[ult_elemento].id);
01066                  
01067                   for(i=0; i<vertex_web[ult_elemento].num_neigh; i++){
01068                         
01069 //                      printf("vizinho %i\n", vertex_web[ult_elemento].id_neigh[i]);
01070                         
01071                         //if (vertex_web[vertex_web[ult_elemento].id_neigh[i]].regiao == ID_REG || (vertex_web[ult_elemento].regiao == ID_REG && vertex_web[vertex_web[ult_elemento].id_neigh[i]].regiao != ID_REG && vertex_web[ult_elemento].arco_front[i] == true) ){
01072                         //if( pertence( vertex_web[ult_elemento].id_neigh[i], elem_reg, dimension) ){   
01073                           
01074 //                        printf("%i pertence a mesma regiao\n", vertex_web[ult_elemento].id_neigh[i]);
01075                           
01076                           //vizinhos com + d 1vizinho:
01077                           if ( vertex_web[ vertex_web[ult_elemento].id_neigh[i] ].num_neigh > 1 ){
01078                                 
01079 //                              printf("%i tem + de 1 vizinho\n", vertex_web[ult_elemento].id_neigh[i]);
01080 
01081                                 //NAO ESTA NO CAMINHO PARCIAL:
01082                                 if( !pertence(vertex_web[ult_elemento].id_neigh[i],caminho_parcial,elem_cp)){
01083    
01084 //                                printf("%i e diferente do ult. vertice do caminho parcial %i\n", vertex_web[ult_elemento].id_neigh[i], vertex_web[caminho_parcial[elem_cp-2]].id);
01085    
01086                                   //passar para false:
01087                                   vertex_web[ult_elemento].visited[i] = false;   
01088 //                                printf("ID = %i, Vizinho = %i -> disponivel\n", vertex_web[ult_elemento].id, vertex_web[ult_elemento].id_neigh[i]);
01089    
01090                                 /*Reciproco: */
01091                                 
01092                                 for(k=0; k<vertex_web[vertex_web[ult_elemento].id_neigh[i]].num_neigh; k++){
01093                                   if( vertex_web[vertex_web[ult_elemento].id_neigh[i]].id_neigh[k] ==  vertex_web[ult_elemento].id){
01094                                         vertex_web[vertex_web[ult_elemento].id_neigh[i]].visited[k] = false;
01095 //                                      printf("Reciproco: ID = %i, Vizinho = %i -> descartado\n", vertex_web[vertex_web[ult_elemento].id_neigh[i]].id, vertex_web[vertex_web[ult_elemento].id_neigh[i]].id_neigh[k]);
01096                                         break;
01097                                   }
01098                                 }
01099                                 
01100                           }
01101                         }
01102                   //}             
01103                 }
01104                 
01105                   elem_cp--; //retirar o ult.elemento (nao volta a ser escolhido pq o camiho ta true para ele).
01106                   
01107                 }else{
01108                   //1 ou 0 e n tem vizinhos disponiveis
01109 //                printf("so tem 1 elemento no caminho parcial e nao ha caminho pro destino. \n");
01110                   break;
01111                 }
01112                 
01113           }else{
01114                 
01115 //              printf("k>0\n");
01116                 
01117                 //se tem a origem como vizinho;
01118                 if(pertence(origem, viz, k)){
01119                   
01120 //               printf("tem a origem como vizinho!\n");
01121                   
01122                   //encontrei origem!!!!
01123                   if(encontra_destino || k==1){
01124                         
01125                           /* FINAL- encontrei origem! */
01126                         caminho_parcial[elem_cp] = origem;
01127                         elem_cp++;
01128 //                      printf("acabei com sucesso (nao sobram + vizinhos para alem da origem)\n");
01129                         
01130 /*                      printf("caminho_parcial: ");
01131                         for(i=0; i<elem_cp; i++){
01132                           if(i==elem_cp-1){printf("%i\n", caminho_parcial[i]);}else{printf("%i, ", caminho_parcial[i]);}
01133                         }
01134                         printf("elementos = %i\n----------------------\n", elem_cp);
01135 */
01136 
01137                                 
01138                         /* calcular custo do caminho parcial: */
01139                         //elementos em caminho parcial:
01140                         
01141                         for (i=0; i<elem_cp-1; i++){                    
01142                           for(k=0; k<vertex_web[ caminho_parcial[i] ].num_neigh; k++){                    
01143                                 if( vertex_web[ caminho_parcial[i] ].id_neigh[k] == vertex_web[ caminho_parcial[i+1] ].id ){                            
01144                                   custo += vertex_web[ caminho_parcial[i] ].cost[k];
01145                                   break;
01146                                 }                               
01147                           }                     
01148                         }
01149                         
01150 //                      printf("custo = %i\n", custo);
01151                         
01152                         if(custo>custo_max){
01153                           custo_max=custo;
01154                           
01155                           /* IR GUARDANDO O MELHOR...*/
01156                           for(i=0; i<elem_cp; i++){
01157                                 caminho_principal[i] = caminho_parcial[i];
01158                           }
01159                           
01160                           elem_caminho = elem_cp;
01161                         }
01162                         
01163                         
01164                         //return true;
01165                         encontrou_ciclo = true;
01166                         break;
01167                   
01168                   }else{ //retirar o destino das hipoteses (k>1)
01169                   
01170 //                      printf("retirar a origem da lista de vizinhos!\n");
01171                         
01172                         for(i=0; i<k; i++){
01173                           
01174                           if(viz[i] == origem){
01175                                 
01176                                 if(i<k-1){
01177                                   for(rec=i; rec<k-1; rec++){
01178                                         viz[rec] = viz[rec+1];                            
01179                                   }
01180                                 }
01181                                 
01182                                 //diminuir elementos:
01183                                 k--;
01184                                 
01185                           }                     
01186                         }
01187                         
01188 /*                      printf("lista de vizinhos apos retirar:\n");
01189                         for(i=0; i<k; i++){
01190                           printf("viz[%i]=%i\n",i,viz[i]);
01191                         }
01192 */                                
01193                         /* escolher aleatoriamente proximo elemento: */          
01194                         escolha_aleatoria = true;
01195                   }
01196                   
01197                 }else{
01198                   /* escolher aleatoriamente proximo elemento: */                
01199                   escolha_aleatoria = true;
01200                 }
01201                 
01202                 if(escolha_aleatoria == true){
01203                   
01204                   if(encontra_destino){/*printf("passei encontra destino a false\n");*/encontra_destino=false;}
01205                   
01206                   /* escolher aleatoriamente proximo elemento:
01207                   
01208                   A NAO SER QUE UM DOS VIZINHOS TENHA PELO MENOS 4 ARCOS 
01209                   LIGADO A ELE QUE AINDA NAO TENHAM SIDO VISITADOS -> nesse caso escolher esse, 
01210                   caso contrario: ALEATORIO. */
01211                   
01212                   //variavel inteira com vizinho nessa situaçao (inicializada a -1):
01213                   viz_c_4_arcos = -1;
01214                   
01215                   for(i=0; i<k; i++){
01216                         
01217                         conta=0;
01218                         
01219 //                      printf("verificar arcos ligados a %i\n", viz[i]);
01220                         
01221                         for(aux=0; aux<vertex_web[viz[i]].num_neigh; aux++){
01222                           
01223 //                        printf("arco q liga a %i\n", vertex_web[viz[i]].id_neigh[aux]);                         
01224                           
01225                          // if (vertex_web[vertex_web[viz[i]].id_neigh[aux]].regiao == ID_REG || (vertex_web[viz[i]].regiao == ID_REG && vertex_web[vertex_web[viz[i]].id_neigh[aux]].regiao != ID_REG && vertex_web[viz[i]].arco_front[aux] == true) ){
01226                                 
01227 //                              printf("ta na mm regiao\n");
01228                                 
01229                                 if(vertex_web[viz[i]].visited[aux] == false){
01230                                   conta++;
01231 //                                printf("conta = %i\n",conta);
01232                                 }
01233                                 
01234                                 if(conta>=4){
01235                                   viz_c_4_arcos = viz[i];
01236 //                                printf("encontrei vizinho com 4 arcos livres: %i\n", viz[i]);
01237                                   break;
01238                                 }
01239                                 
01240                           //}
01241                           
01242                         }
01243                   }
01244                   
01245                   
01246                   //CASO +COMUM: ESCOLHER ALEATORIAMENTE;
01247                   if (viz_c_4_arcos==-1){
01248                         i = rand() % k;
01249                         caminho_parcial [elem_cp] = viz[i];
01250                         
01251                         //CASO EM Q VIZINHO TEM 4 ARCOS ADJACENTES LIVRES - escolhe-lo:
01252                   }else{
01253                         caminho_parcial [elem_cp] = viz_c_4_arcos;
01254                   }
01255                   
01256                   
01257 /*                printf("escolhas possiveis: ");
01258                   for(i=0; i<k; i++){printf("%i ",viz[i]);}
01259                   printf("\nproximo vertice escolhido aleatoriamente = %i\n", caminho_parcial [elem_cp]);
01260 */                
01261                   
01262                   //passar o arco entre eles para true.
01263                   for(rec=0; rec<vertex_web[ult_elemento].num_neigh; rec++){
01264                         if( is_neigh ( vertex_web[ult_elemento].id, vertex_web[ult_elemento].id_neigh[rec], vertex_web, dimension ) > -1){                        
01265                           
01266                           if (vertex_web[ult_elemento].id_neigh[rec] == caminho_parcial [elem_cp]){
01267 //                              printf("arco de %i para %i passou a fazer parte do caminho\n", vertex_web[ult_elemento].id, vertex_web[ult_elemento].id_neigh[rec]);
01268                                 vertex_web[ult_elemento].visited[rec] = true;                           
01269                                 
01270                                 
01271                                 //vertex_web de ID=caminho_parcial [elem_cp]
01272                                 //passe a ter vizinho = vertex_web[ult_elemento].id com visitado a true.
01273                                 
01274                                 for(k=0; k<vertex_web[caminho_parcial [elem_cp]].num_neigh; k++){
01275                                   if (vertex_web[caminho_parcial [elem_cp]].id_neigh[k] == vertex_web[ult_elemento].id){
01276 //                                      printf("RECIPROCO: arco de %i para %i passou a fazer parte do caminho\n", vertex_web[caminho_parcial [elem_cp]].id, vertex_web[caminho_parcial [elem_cp]].id_neigh[k]);
01277                                         vertex_web[caminho_parcial [elem_cp]].visited[k]=true;
01278                                         break;
01279                                   }
01280                                 }        
01281                                 
01282                           }     
01283                           
01284                         }
01285                   }
01286                   
01287                   elem_cp++;
01288                   
01289           }
01290           
01291         }
01292           
01293           
01294           
01295         
01296         
01297         }//fechar WHILE
01298         
01299         delete [] viz;
01300 //      printf("---------------------------------------\n");
01301         
01302   }//for
01303   
01304 //  if(custo_max>0){printf("custo maximo do ciclo = %i\n",custo_max);
01305 //  }else{printf("nao possui ciclo.\n");}
01306   return true;
01307   
01308 }
01309 
01310 int devolve_viz_unicos (vertex *vertex_web, int vertice){
01311   
01312   if (vertex_web[vertice].num_neigh == 1){
01313         return vertex_web[vertice].id_neigh[0];
01314         
01315   }else{
01316         return -1;
01317   }
01318   
01319 }
01320 
01321 int longest_path (vertex *vertex_web, int origem, int destino, int *lista_v1v, int i_list, int dimension, int seed, int *caminho_parcial, int &elem_cp) {
01322   
01323 //  printf("ORIGEM - No %i\tDESTINO - No %i\n", origem, destino);
01324   
01325   //PENSAR SEMPRE NO RECIPROCO!!!!!!!!!!!
01326   bool debug = false; //DEBUGGING PURPOSES
01327   
01328   int i, k;
01329   int rec;
01330   //int caminho_parcial [num_nos_regiao];
01331   /*int*/ elem_cp = 0;
01332   int custo = 0;
01333   
01334   //limpar tudo
01335   clear_visited(vertex_web, dimension);
01336 
01337   
01338   for(i=0; i<i_list; i++){
01339         if(!(lista_v1v[i]== origem || lista_v1v[i]==destino)){
01340           
01341           vertex_web[ lista_v1v[i] ].visited[0] = true;
01342           if(debug){printf("ID = %i, Vizinho = %i -> descartado\n", lista_v1v[i],vertex_web[ lista_v1v[i] ].id_neigh[0]);}
01343           
01344           rec = vertex_web[ lista_v1v[i] ].id_neigh[0];
01345           
01346           for(k=0; k<vertex_web[rec].num_neigh; k++){
01347                 if (vertex_web[rec].id_neigh[k] == vertex_web[lista_v1v[i]].id){
01348                   vertex_web[rec].visited[k]=true;
01349                   break;
01350                   if(debug){printf("ID = %i, Vizinho = %i -> descartado\n", rec,vertex_web[ rec ].id_neigh[k]);}
01351                 }
01352           }
01353         
01354         }else{ //apenas para garantir q a origem e o destino estao a FALSE:
01355           
01356           vertex_web[ lista_v1v[i] ].visited[0] = false;
01357           
01358           rec = vertex_web[ lista_v1v[i] ].id_neigh[0];
01359           
01360           for(k=0; k<vertex_web[rec].num_neigh; k++){
01361                 if (vertex_web[rec].id_neigh[k] == vertex_web[lista_v1v[i]].id){
01362                   vertex_web[rec].visited[k]=false;
01363                   break;
01364                 }
01365           }
01366           
01367         }
01368   }
01369   
01370   if(debug){printf("\n");}
01371   
01372   
01373   
01374   bool encontra_destino = false; //numa primeira fase queremos adiar a chegada ao destino.
01375   bool escolha_aleatoria;
01376   int *viz = new int[8];
01377   int ult_elemento=-1;
01378   int aux, conta, viz_c_4_arcos;
01379   //caminho:
01380   caminho_parcial [ elem_cp ] = origem;
01381   elem_cp++;
01382   
01383   srand(seed);
01384 
01385   
01386   while( ult_elemento!=destino ){ //acaba quando chegar ao destino.
01387         
01388         escolha_aleatoria = false;
01389 
01390         ult_elemento = caminho_parcial [elem_cp-1];
01391 
01392         if(debug){printf("---------------------------------------\n");
01393         printf("Ultimo Elemento = %i\n", ult_elemento);
01394         printf("elementos no cp= %i\n", elem_cp);}
01395         
01396         //verificar estado dos vizinhos do elemento actual:     
01397         k=0;
01398         
01399         //construir tab de vizinhos do prox_vertice:
01400         for(i=0; i<vertex_web[ult_elemento].num_neigh; i++){
01401           
01402           //if (vertex_web[vertex_web[ult_elemento].id_neigh[i]].regiao == ID_REG || (vertex_web[ult_elemento].regiao == ID_REG && vertex_web[vertex_web[ult_elemento].id_neigh[i]].regiao != ID_REG && vertex_web[ult_elemento].arco_front[i] == true) ){
01403           //if( pertence( vertex_web[ult_elemento].id_neigh[i], elem_reg, num_nos_regiao) ){
01404                 
01405                 /* basta nao ter sido visited (para cobrir o caso de ter 4 ou + vizinhos */
01406                 if ( vertex_web[ult_elemento].visited[i] == false /*&& !pertence(vertex_web[ult_elemento].id_neigh[i], caminho_parcial, elem_cp)*/){
01407                   viz[k] = vertex_web[ult_elemento].id_neigh[i];
01408                   k++;
01409                 }
01410           //}
01411           
01412         }
01413         
01414         if(debug){printf("k=%i\n",k);}
01415         
01416 
01417         if (k==0){ //ja nao ha vizinhos disponiveis -- voltar para tras.
01418           encontra_destino = true;
01419           
01420           if(elem_cp>1){
01421                 //voltar para o anterior e passar visiteds a false (menos o q liga este vizinho ao anterior no caminho parcial.
01422                 if(debug){printf("percorrer vizinhos de %i\n", vertex_web[ult_elemento].id);}
01423                 for(i=0; i<vertex_web[ult_elemento].num_neigh; i++){
01424                   
01425                   if(debug){printf("vizinho %i\n", vertex_web[ult_elemento].id_neigh[i]);}
01426                   
01427                   //if (vertex_web[vertex_web[ult_elemento].id_neigh[i]].regiao == ID_REG || (vertex_web[ult_elemento].regiao == ID_REG && vertex_web[vertex_web[ult_elemento].id_neigh[i]].regiao != ID_REG && vertex_web[ult_elemento].arco_front[i] == true) ){
01428                   //if( pertence( vertex_web[ult_elemento].id_neigh[i], elem_reg, num_nos_regiao) ){     
01429                         
01430 //                      printf("%i pertence a mesma regiao\n", vertex_web[ult_elemento].id_neigh[i]);
01431                         
01432                         //vizinhos com + d 1vizinho:
01433                         if ( vertex_web[ vertex_web[ult_elemento].id_neigh[i] ].num_neigh  > 1 ){
01434                           
01435                          if(debug){ printf("%i tem + de 1 vizinho\n", vertex_web[ult_elemento].id_neigh[i]); }
01436 
01437                           //diferentes do ultimo arco do caminho parcial:
01438                           //if( vertex_web[ult_elemento].id_neigh[i] != vertex_web[caminho_parcial[elem_cp-2]].id){
01439                           //NAO ESTA NO CAMINHO PARCIAL:
01440                           if( !pertence(vertex_web[ult_elemento].id_neigh[i],caminho_parcial,elem_cp)) {
01441                                 
01442                                 if(debug){ printf("%i e diferente do ult. vertice do caminho parcial %i\n", vertex_web[ult_elemento].id_neigh[i], vertex_web[caminho_parcial[elem_cp-2]].id); }
01443                                 
01444                                 //passar para false:
01445                                 vertex_web[ult_elemento].visited[i] = false;            
01446                                 if(debug){ printf("ID = %i, Vizinho = %i -> disponivel\n", vertex_web[ult_elemento].id, vertex_web[ult_elemento].id_neigh[i]); }
01447                           
01448                                 /*Reciproco: */
01449                                 //ID = vertex_web[ult_elemento].id_neigh[i]
01450                                 //ID_VIZ = vertex_web[ult_elemento].id
01451                                 
01452                                 for(k=0; k<vertex_web[vertex_web[ult_elemento].id_neigh[i]].num_neigh; k++){
01453                                   if( vertex_web[vertex_web[ult_elemento].id_neigh[i]].id_neigh[k] ==  vertex_web[ult_elemento].id){
01454                                         vertex_web[vertex_web[ult_elemento].id_neigh[i]].visited[k] = false;
01455                                         if(debug){ printf("Reciproco: ID = %i, Vizinho = %i -> descartado\n", vertex_web[vertex_web[ult_elemento].id_neigh[i]].id, vertex_web[vertex_web[ult_elemento].id_neigh[i]].id_neigh[k]);}
01456                                         break;
01457                                   }
01458                                 }
01459   
01460                           }
01461                         }
01462                   //}             
01463                 }
01464                 elem_cp--; //retirar o ult.elemento (nao volta a ser escolhido pq o camiho ta true para ele).
01465           }else{
01466                 //1 ou 0 e n tem vizinhos disponiveis
01467 //              printf("so tem 1 elemento no caminho parcial e nao ha caminho pro destino (Erro?) \n");
01468                 return 0;
01469           }
01470           
01471         }else{
01472           
01473           //se tem o destino como vizinho;
01474           if(pertence(destino, viz, k)){
01475                 
01476 //              printf("tem o destino como vizinho!\n");
01477                 
01478                 //encontrei destino!!!!
01479                 if(encontra_destino || k==1){
01480                   
01481                   /* FINAL- encontrei destino! */
01482                   caminho_parcial[elem_cp] = destino;
01483                   elem_cp++;
01484                 if(debug){  printf("acabei com sucesso (nao sobram + vizinhos para alem do destino)\n");
01485                   printf("caminho_parcial: ");
01486                   for(i=0; i<elem_cp; i++){
01487                         if(i==elem_cp-1){printf("%i\n", caminho_parcial[i]);}else{printf("%i, ", caminho_parcial[i]);}
01488                   }
01489                   printf("elementos = %i\n----------------------\n", elem_cp); }
01490                   
01491                   /* calcular custo do caminho parcial: */
01492                   //elementos em caminho parcial:
01493                   
01494                   for (i=0; i<elem_cp-1; i++){                  
01495                         for(k=0; k<vertex_web[ caminho_parcial[i] ].num_neigh; k++){                      
01496                           if( vertex_web[ caminho_parcial[i] ].id_neigh[k] == vertex_web[ caminho_parcial[i+1] ].id ){                          
01497                                 custo += vertex_web[ caminho_parcial[i] ].cost[k];
01498                                 break;
01499                           }                             
01500                         }                       
01501                   }
01502                   
01503 //                printf("custo = %i\n", custo);
01504 
01505                   delete [] viz;
01506                   return custo;
01507                 
01508                 }else{ //retirar o destino das hipoteses (k>1)
01509                 
01510                 if(debug){printf("retirar o destino da lista de vizinhos!\n");}
01511                   
01512                   for(i=0; i<k; i++){
01513                         
01514                         if(viz[i] == destino){
01515                           
01516                           if(i<k-1){
01517                                 for(rec=i; rec<k-1; rec++){
01518                                   viz[rec] = viz[rec+1];                                  
01519                                 }
01520                           }
01521                           
01522                           //diminuir elementos:
01523                           k--;
01524                           
01525                         }                       
01526                   }
01527                   
01528                   if(debug){printf("lista de vizinhos apos retirar:\n");
01529                   for(i=0; i<k; i++){printf("viz[%i]=%i\n",i,viz[i]); }
01530                   }
01531                   
01532                   /* escolher aleatoriamente proximo elemento: */                
01533                   escolha_aleatoria = true;
01534                 }
01535                 
01536           }else{
01537                 /* escolher aleatoriamente proximo elemento: */          
01538                 escolha_aleatoria = true;
01539           }
01540           
01541           if(escolha_aleatoria == true){
01542           
01543                 if(encontra_destino){/*printf("passei encontra destino a false\n");*/encontra_destino=false;}
01544                 
01545                   /* escolher aleatoriamente proximo elemento:
01546                   
01547                   A NAO SER QUE UM DOS VIZINHOS TENHA PELO MENOS 4 ARCOS 
01548                   LIGADO A ELE QUE AINDA NAO TENHAM SIDO VISITADOS -> nesse caso escolher esse, 
01549                   caso contrario: ALEATORIO. */
01550                   
01551                   //variavel inteira com vizinho nessa situaçao (inicializada a -1):
01552                   viz_c_4_arcos = -1;
01553                   
01554                   for(i=0; i<k; i++){
01555                         
01556                         conta=0;
01557                         
01558                         if(debug){printf("verificar arcos ligados a %i\n", viz[i]);}
01559                         
01560                         for(aux=0; aux<vertex_web[viz[i]].num_neigh; aux++){
01561                           
01562                           if(debug){printf("arco q liga a %i\n", vertex_web[viz[i]].id_neigh[aux]);}                      
01563                           
01564                           //if (vertex_web[vertex_web[viz[i]].id_neigh[aux]].regiao == ID_REG || (vertex_web[viz[i]].regiao == ID_REG && vertex_web[vertex_web[viz[i]].id_neigh[aux]].regiao != ID_REG && vertex_web[viz[i]].arco_front[aux] == true) ){
01565                                 
01566 //                              printf("ta na mm regiao\n");
01567                                 
01568                                 if(vertex_web[viz[i]].visited[aux] == false){
01569                                   conta++;
01570                                   if(debug){printf("conta = %i\n",conta);}
01571                                 }
01572                                 
01573                                 if(conta>=4){
01574                                   viz_c_4_arcos = viz[i];
01575                                   if(debug){printf("encontrei vizinho com 4 arcos livres: %i\n", viz[i]);}
01576                                   break;
01577                                 }
01578                                 
01579                           //}
01580                           
01581                         }
01582                   }
01583                   
01584                   
01585                   //CASO +COMUM: ESCOLHER ALEATORIAMENTE;
01586                   if (viz_c_4_arcos==-1){
01587                         i = rand() % k;
01588                         caminho_parcial [elem_cp] = viz[i];
01589                         
01590                   //CASO EM Q VIZINHO TEM 4 ARCOS ADJACENTES LIVRES - escolhe-lo:
01591                   }else{
01592                         caminho_parcial [elem_cp] = viz_c_4_arcos;
01593                   }
01594                   
01595                   
01596                  if(debug){ printf("escolhas possiveis: ");
01597                   for(i=0; i<k; i++){printf("%i ",viz[i]);}
01598                  printf("\nproximo vertice escolhido aleatoriamente = %i\n", caminho_parcial [elem_cp]); }
01599                   
01600                   //passar o arco entre eles para true.
01601                   for(rec=0; rec<vertex_web[ult_elemento].num_neigh; rec++){
01602                         if( is_neigh ( vertex_web[ult_elemento].id, vertex_web[ult_elemento].id_neigh[rec], vertex_web, dimension ) > -1){                        
01603                           
01604                           if (vertex_web[ult_elemento].id_neigh[rec] == caminho_parcial [elem_cp]){
01605                                 if(debug){ printf("arco de %i para %i passou a fazer parte do caminho\n", vertex_web[ult_elemento].id, vertex_web[ult_elemento].id_neigh[rec]);}
01606                                 vertex_web[ult_elemento].visited[rec] = true;                           
01607                                 
01608                           
01609                                 //vertex_web de ID=caminho_parcial [elem_cp]
01610                                 //passe a ter vizinho = vertex_web[ult_elemento].id com visited a true.
01611                                 
01612                                 for(k=0; k<vertex_web[caminho_parcial [elem_cp]].num_neigh; k++){
01613                                   if (vertex_web[caminho_parcial [elem_cp]].id_neigh[k] == vertex_web[ult_elemento].id){
01614                                         if(debug){ printf("RECIPROCO: arco de %i para %i passou a fazer parte do caminho\n", vertex_web[caminho_parcial [elem_cp]].id, vertex_web[caminho_parcial [elem_cp]].id_neigh[k]);}
01615                                         vertex_web[caminho_parcial [elem_cp]].visited[k]=true;
01616                                         break;
01617                                   }
01618                                 }        
01619                           
01620                           }     
01621                           
01622                         }
01623                   }
01624                   
01625                   elem_cp++;
01626 
01627           }
01628 
01629         }
01630         
01631   }
01632   
01633   delete [] viz;
01634   return 0;
01635   
01636 }
01637 
01638 bool caminho_apartir_vizinhos_unicos (vertex *vertex_web, int dimension, int *caminho_principal, int &elem_max, int &custo_max, int seed){
01639   
01640   int i,j,i_uc = 0;
01641   
01642   int *lista_v1v = new int[1000];
01643   int i_list=0;
01644   
01645 
01646         /* CRIAR LISTA DOS VIZINHOS UNICOS: */
01647         int valor = -1;
01648 //      int pertence_ = -1;
01649         
01650         for(i=0; i<dimension; i++){
01651           
01652           valor = devolve_viz_unicos (vertex_web, vertex_web[i].id);
01653           
01654           if (valor > -1){      //no so tem 1 vizinho
01655                   
01656                   lista_v1v[i_list] = vertex_web[i].id;
01657 //                printf("LISTA_vizinhos_unicos [%i] = %i\n", i_list, lista_v1v[i_list]);
01658                   i_list++;
01659 
01660           }
01661           
01662         }
01663 
01664         
01665         //se mesmo assim i_list <= 1 -> e pq todos os nos tem 2 ou + vizs...
01666         if (i_list<=1){
01667           ROS_WARN("Impossible to compute longest path.\n");
01668 //        printf("Nao existem nos so com 1 vizinho. Impossivel computar caminho +longo\n");
01669           return false;
01670         }
01671         
01672 //      delete [] tab_viz_unic;
01673 //      delete [] tab_viz_comuns;
01674         
01675         
01676         /* PERCORRER TODOS OS NOS ENTRE SI PARA ENCONTRAR MAIOR CAMINHO */
01677         custo_max = 0; 
01678         int retorno=0, origem_caminho=-1, destino_caminho=-1, elem_caminho = 0;
01679         int caminho_parcial[dimension];
01680         
01681         for(i=0; i<i_list; i++){
01682           for(j=0; j<i_list; j++){
01683                 if(i<j){
01684                         //encontrar longest_path entre vizinhos unicos...
01685                         retorno = longest_path(vertex_web, lista_v1v[i], lista_v1v[j], lista_v1v, i_list, dimension, seed, caminho_parcial, elem_caminho);
01686                         
01687                         if(retorno>custo_max){
01688                                 custo_max=retorno;
01689                                 
01690                                 for(i_uc = 0; i_uc<elem_caminho; i_uc++){
01691                                   caminho_principal[i_uc] = caminho_parcial[i_uc];
01692                                 }
01693                                 
01694                                 elem_max = elem_caminho;
01695                                 
01696                                 origem_caminho=lista_v1v[i];
01697                                 destino_caminho=lista_v1v[j]; 
01698                           }
01699                 }
01700           }
01701         }
01702         
01703 //      printf("custo maximo do caminho = %i\n",custo_max);
01704 //      printf("origem = %i\tdestino = %i\n", origem_caminho, destino_caminho);
01705         delete [] lista_v1v;
01706         return true;
01707   
01708 }
01709 
01710 bool verificar_arco_cp (int no_1, int no_2, int *caminho_principal, int elem_cp){
01711   
01712   int i;
01713   for(i=0; i<elem_cp; i++){
01714         
01715         if(caminho_principal[i] == no_1){
01716          
01717           if(i>0){
01718                 if(caminho_principal[i-1] == no_2){
01719                  return true; 
01720                 }
01721           }
01722           
01723           if(i<elem_cp-1){
01724                 if(caminho_principal[i+1] == no_2){
01725                   return true; 
01726                 }
01727           }
01728           
01729         }       
01730   }
01731   
01732   return false;
01733   
01734 }
01735 
01736 bool check_visited (vertex *vertex_web, int dimension){
01737  
01738   int i, j;
01739   
01740   for(i=0; i<dimension; i++){
01741         
01742         for(j=0; j<vertex_web[i].num_neigh; j++){
01743          
01744                 if( vertex_web[i].visited[j] == false ){                  
01745 
01746                   return false;
01747                   
01748                 }  
01749         }       
01750   }
01751   
01752   return true;
01753   
01754 }
01755 
01756 
01757 int devolve_vizinhos_nao_visitados (vertex *vertex_web, int dimension, int vertice){
01758   
01759   int i_viz=0;
01760   int i;
01761   
01762   //contar vizinhos do vertice da mesma regiao:
01763   for(i=0; i<vertex_web[vertice].num_neigh; i++){
01764         
01765 //      if (vertex_web[vertex_web[vertice].id_viz[i]].regiao == ID_REG || (vertex_web[vertice].regiao == ID_REG && vertex_web[tab_nos[vertice].id_viz[i]].regiao != ID_REG && vertex_web[vertice].arco_front[i] == true) ){
01766           
01767           if(vertex_web[vertice].visited[i] == false){
01768                 i_viz++;
01769           }
01770         //}
01771  
01772   }
01773         
01774         return i_viz;
01775         
01776   }
01777 
01778 bool computar_caminho_de_ida (vertex *vertex_web, int dimension, int *caminho_de_ida, int &elem_c_ida, int *caminho_principal, int elem_cp, int num_arcos){
01779   
01780   int i,j;
01781   int rec, idx_viz_cp=-1;
01782 //  int num_arcos=49;                           //nº de arcos q pdemos visitar no caminho secundario
01783   int arcos_ja_visitados;
01784 //  int elem_c_ida=0;                           //guarda registo do nº de elementos do caminho de ida
01785   int desvio [dimension*5];                     //para ir mantendo nos a percorrer qd entramos no caminho secundario
01786   int regresso [dimension];
01787   int i_desvio=0, i_reg, idx_reg_directo=-1;
01788   bool sai_logo;
01789   int no_ant, prox_no, ult_no_cp, viz_escolhido, idx_cp=0;
01790   bool sai, tem_reg_directo, pertence_CP;
01791   int lista_viz[8];
01792   int i_list, num_neigh;
01793   
01794   caminho_de_ida[0] = caminho_principal[idx_cp];
01795 //  printf("caminho de ida [%i] = %i\n", elem_c_ida, caminho_de_ida[elem_c_ida]);
01796   elem_c_ida++;
01797   
01798   while(idx_cp<elem_cp){
01799         
01800         
01801         //inicializa-lo ao proximo elemento do caminho principal
01802         prox_no = caminho_principal[idx_cp+1];
01803         ult_no_cp = caminho_principal[idx_cp];
01804         
01805         //proteccao:
01806         if(vertex_web[ult_no_cp].id != ult_no_cp){vertex_web[ult_no_cp].id = ult_no_cp;} 
01807         
01808         //decidir qual e o prox no:
01809         for(j=0; j<vertex_web[ult_no_cp].num_neigh; j++){
01810            //sao vizinhos e na mm regiao:
01811            if( is_neigh (ult_no_cp, vertex_web[ult_no_cp].id_neigh[j],vertex_web,dimension) > -1 //verificar se sao vizinhos
01812                  && vertex_web[ult_no_cp].visited[j] == false                                                   //ainda nao foram visitados
01813                  && !verificar_arco_cp (ult_no_cp, vertex_web[ult_no_cp].id_neigh[j], caminho_principal, elem_cp) ){ //o arco nao pertence ao caminho principal
01814                   
01815                   prox_no = vertex_web[ult_no_cp].id_neigh[j];
01816 //                printf("vou-me desviar em %i\n", prox_no);
01817                   break;
01818                 }
01819                 
01820                 //se for igual ao proximo no no caminho - guardar o index
01821                 if (vertex_web[ult_no_cp].id_neigh[j]== caminho_principal[idx_cp+1]){
01822                   idx_viz_cp = j;
01823                 }
01824                 
01825          }
01826          
01827 //       printf("prox_no = %i\n", prox_no);
01828 //       printf("idx_viz_cp = %i (tabela de %i p viz. no c.principal)\n", idx_viz_cp, ult_no_cp);
01829          
01830          //mantem-se no caminho principal:
01831          if(prox_no == caminho_principal[idx_cp+1]){
01832            
01833                 //ASSINALAR ARCO VISITADO:
01834                 for(i=0; i<vertex_web[ult_no_cp].num_neigh; i++){
01835                   if(vertex_web[ult_no_cp].id_neigh[i] == vertex_web[ult_no_cp].id_neigh[idx_viz_cp] && vertex_web[ult_no_cp].visited[i] == false) {
01836                         vertex_web[ult_no_cp].visited[i] = true;                
01837                   }
01838                 }
01839                 
01840                 rec = vertex_web[ult_no_cp].id_neigh[idx_viz_cp];
01841                 //reciproco:
01842                 for(j=0; j<vertex_web[rec].num_neigh; j++){
01843                   if(vertex_web[rec].id_neigh[j] == ult_no_cp && vertex_web[rec].visited[j]==false){
01844                         vertex_web[rec].visited[j] = true;
01845                   }
01846                 }       
01847                 
01848                 //adicionar proximo no do caminho principal ao caminho de ida (arcos a TRUE - visitados)
01849                 caminho_de_ida[elem_c_ida] = prox_no;
01850 //              printf("caminho de ida [%i] = %i\n", elem_c_ida, caminho_de_ida[elem_c_ida]);
01851                 elem_c_ida++;
01852                 idx_cp++;
01853                 
01854                 if(prox_no == caminho_principal[elem_cp-1]){
01855 //                printf("TERMINAMOS!\n");
01856                   break;
01857                 }
01858            
01859          }else{ //desvio apartir do prox_no -> caminho secundario:
01860                 /* encontrar desvio completo - assinalar arcos - passar para caminho de ida e voltar */
01861 
01862            //index j -> tem o index do vizinho na tab do no anterior.
01863 //         printf("desvio apartir de %i\n", vertex_web[ult_no_cp].id_neigh[j] );           
01864 //         printf("ultimo no do cp: %i\n", ult_no_cp);
01865            
01866            /*      
01867            de acordo com o valor de num_arcos, construir a tabela desvio q começa e acaba em ult_no_cp  
01868            dar preferencia a nos com vizinhos unicos
01869            */
01870 
01871           arcos_ja_visitados = 0;
01872           no_ant = ult_no_cp;
01873           //prox_no = vertex_web[ult_no_cp].id_neigh[j];
01874           //j guarda o index.
01875           //desvio [num_nos_regiao];    //para ir mantendo nos a percorrer qd entramos no caminho secundario
01876           //regresso [num_nos_regiao];
01877           i_desvio=0; i_reg=0;
01878           desvio[i_desvio] = ult_no_cp;
01879           i_desvio++;
01880           bool assinala;
01881           sai = false;
01882           sai_logo = false;
01883           pertence_CP = false; //GARANTIR REGRESSO AO NO ANTERIOR QUANDO E TRUE.
01884           
01885           
01886           if( pertence (vertex_web[ult_no_cp].id_neigh[j], caminho_principal, elem_cp) ){
01887 //              printf("ESTE PERTENCE JA AO CAMINHO PRINCIPAL!\n");
01888                 pertence_CP = true;
01889                 sai = true;
01890           }
01891           
01892           while (arcos_ja_visitados<num_arcos){ //desvio IDA -> construir tabela de desvio e regresso
01893                 
01894                 tem_reg_directo = false;
01895                 assinala = false;
01896                 
01897                 //assinala no:
01898                 //vertex_web[no_ant].visited[j] = true;
01899                 
01900                 /* j tem o index do prox_no na tabela de vizinhos de no_ant - so e assinalado qd for necessario */
01901                 
01902                 //assinala reciproco:
01903                 for(i=0; i<vertex_web[prox_no].num_neigh; i++){
01904                   if(vertex_web[prox_no].id_neigh[i] == no_ant && vertex_web[prox_no].visited[i] == false){ //protecçao contra loopings
01905                         vertex_web[prox_no].visited[i] = true;
01906                         assinala = true;
01907 //                      printf("assinalei arco %i - %i\n", prox_no, vertex_web[prox_no].id_neigh[i]);
01908                   }
01909                   
01910                   //verficar se tem vizinho directo para ult_no_cp.
01911                   if (vertex_web[prox_no].id_neigh[i] == ult_no_cp){
01912                         tem_reg_directo = true;
01913                         idx_reg_directo = i;
01914                   }
01915                 }
01916                   
01917                 //so assinala se forem, de facto, vizinhos:
01918                 if(assinala){
01919                         
01920                   //protecçao contra loopings:
01921                   for(i=0; i<vertex_web[no_ant].num_neigh; i++){                          
01922                         if(vertex_web[no_ant].id_neigh[i] == vertex_web[no_ant].id_neigh[j] && vertex_web[no_ant].visited[i] == false){
01923                           vertex_web[no_ant].visited[i] = true;                   
01924 //                        printf("assinalei arco %i - %i\n", no_ant, vertex_web[no_ant].id_neigh[i]);
01925                         }
01926                   }
01927                 }
01928                   
01929 
01930                 
01931                 desvio[i_desvio] = prox_no;
01932 
01933                 //no que ja existe no desvio - cortar caminho ja existente de regresso:
01934                 viz_escolhido = pertence_idx (desvio[i_desvio], desvio, i_desvio-1);
01935                 
01936                 if( viz_escolhido > -1){ 
01937 //                printf("no %i ja pertence ao desvio - nao adicionar no anterior ao regresso\n", desvio[i_desvio]);
01938 
01939                   //pode ser um ciclo - neste caso:
01940                   //no anterior so se adicionar ao regresso se o ultimo elemento de regresso nao for vizinho do prox_no.
01941                   
01942                   if ( is_neigh (prox_no, regresso[i_reg-1], vertex_web, dimension) == -1 ){ //não são vizinhos
01943                         
01944                         regresso[i_reg] = no_ant;
01945                         i_reg++;
01946                         
01947                   }
01948                   
01949                 }else{//no novo no desvio - caminho de regresso para ele apartir do ult no:  
01950                   
01951                   regresso[i_reg] = no_ant;
01952                   i_reg++;                
01953                   
01954                   //so conta para arco visitado quando o no e novo:
01955                   arcos_ja_visitados++;
01956                 }
01957                                 
01958                 //incrementar desvio (foi adicionado no):
01959                 i_desvio++;
01960                 
01961                 //ultimo no pertence ao caminho principal - vamos forçar o regresso ao no anterior:
01962                 if(pertence_CP){
01963                   desvio[i_desvio] = regresso[i_reg-1];
01964                   i_desvio++;
01965                   i_reg--;
01966                   
01967                   //trocar prox_no com no_ant:
01968                   rec = no_ant;
01969                   no_ant = prox_no;
01970                   prox_no = rec;                  
01971                 }
01972                 
01973                 
01974 /*              printf("TABELA DE DESVIO:\n");
01975                 for(i=0; i<i_desvio; i++){
01976                   printf("desvio[%i]=%i:\n",i,desvio[i]);
01977                 }       
01978                 
01979                 printf("TABELA DE REGRESSO:\n");
01980                 for(i=0; i<i_reg; i++){
01981                   printf("regresso[%i]=%i:\n",i,regresso[i]);
01982                 }
01983 */              
01984 
01985         
01986                 //inicializar:
01987                 pertence_CP = false;
01988                 
01989                 viz_escolhido = -1;
01990                 
01991                 if(arcos_ja_visitados>=num_arcos){ //tomar regresso ou ver s ha no directo deste para ult_no_cp
01992                   
01993 //                printf("arcos_ja_visitados>=num_arcos -> vamos regressar ou tomar arco directo\n");
01994                   
01995                   if (tem_reg_directo && i_reg>0){ //tem regresso directo:
01996                         
01997 //                      printf("%i tem regresso directo por %i\n", prox_no, vertex_web[prox_no].id_neigh[idx_reg_directo]);
01998                         
01999                         //assinalar arco:
02000                         vertex_web[prox_no].visited[idx_reg_directo] = true;
02001                         
02002                         //assinalar reciproco:
02003                         for(i=0; i<vertex_web[ult_no_cp].num_neigh; i++){
02004                           if(vertex_web[ult_no_cp].id_neigh[i] == prox_no){
02005                                 vertex_web[ult_no_cp].visited[i] = true;
02006                           }
02007                         }
02008 
02009                         desvio[i_desvio] = ult_no_cp;
02010                         i_desvio++;
02011                         
02012 //                      printf("TABELA FINAL DE DESVIO:\n");
02013 //                      for(i=0; i<i_desvio; i++){
02014 //                        printf("desvio[%i]=%i:\n",i,desvio[i]);
02015 //                      }
02016                         
02017                         break; //sai do while - desvio completo.
02018                         
02019                   }else{ //acrescentar o regresso:
02020 
02021                         sai=true;
02022                   }
02023                   
02024                 }else{ //ainda posso continuar (se puder)
02025                 
02026 //                printf("arcos_ja_visitados < num_arcos -> vamos continuar a procura de arcos novos\n");
02027                 
02028                   //criar lista de vizinhos.
02029                   i_list=0;
02030                   pertence_CP = false;
02031                                   
02032                   for(i=0; i<vertex_web[prox_no].num_neigh; i++){
02033                         
02034                         if (is_neigh (prox_no, vertex_web[prox_no].id_neigh[i], vertex_web, dimension)>-1  //verificar se sao vizinhos da mm regiao
02035                           && vertex_web[prox_no].visited[i] == false && !verificar_arco_cp(prox_no, vertex_web[prox_no].id_neigh[i], caminho_principal, elem_cp) ){ //arco ainda nao foi visitado e o ARCO nao pertence ao CP
02036                           
02037                           lista_viz[i_list] = vertex_web[prox_no].id_neigh[i];
02038                           i_list++;
02039                           
02040                         }
02041                   }
02042                   
02043 //                printf("lista de vizinhos de %i:", prox_no);
02044 //                for(i=0; i<i_list; i++){
02045 //                      printf(" %i",lista_viz[i]);
02046 //                }
02047                   
02048                   i=0;
02049                   //se so tiver 1 nao entra.
02050                   while(i_list>1){
02051                         
02052                         if( pertence(lista_viz[i], caminho_principal, elem_cp)){                          
02053                           //retirar vizinho da lista:
02054                           if(i<i_list-1){
02055                                 for(j=i; j<i_list-1; j++){
02056                                   lista_viz[j] = lista_viz[j+1];                                  
02057                                 }
02058                           }                               
02059                           //diminuir elementos:
02060                           i_list--;
02061                         }
02062                         
02063                         i++;
02064                         
02065                         if(i == i_list){ //ha mais de 1 vizinho e nenhum pertence ao CP (ou so sobrou 1 viz):
02066                           
02067 //                        printf("\nlista apos retirar arco(s) q pertence(m) ao CP:");
02068 //                        for(i=0; i<i_list; i++){
02069 //                              printf(" %i",lista_viz[i]);
02070 //                        }                       
02071                           
02072                           break;
02073                         }
02074                         
02075                   }
02076                   
02077                   if(i_list==1){
02078 //                      printf("\nlista so tem 1 vizinho\n");
02079                         if(pertence(lista_viz[0], caminho_principal, elem_cp)){
02080                          pertence_CP = true; 
02081 //                       printf("no vizinho pertence ao caminho principal - temos d garantir q regressamos na prox iteracao\n");
02082                         }
02083                   }
02084                   
02085                   if(i_list <= 0){ //nao tem vizinhos - volta para tras
02086                         
02087 //                      printf("\nlista de vizinhos vazia - %i n tem vizinhos validos... voltar para tras\n", prox_no);
02088                         
02089                         //voltar para tras ate encontrar elemento q tenha vizinhos (ou xegar ao fim)
02090                         for(i=i_reg-1; i>=0; i--){                       
02091                           
02092                           j = devolve_vizinhos_nao_visitados(vertex_web, dimension, regresso[i]);
02093                           
02094                           //verificar se ha vizinhos livres ou se se continua.
02095                           if( j > 0) {
02096 //                              if(i!=0){printf("%i (membro de regresso) ainda tem vizinhos livres\n", regresso[i]); }
02097                                 //ha vizinhos livres - paro neste no.
02098                                 break;
02099                           
02100                           }else{
02101                                 //adicionar ao desvio (retirar do regresso):
02102                                 desvio[i_desvio] = regresso [i];
02103                                 i_desvio++;
02104                                 i_reg--;
02105                           }
02106                           
02107                         }
02108                         
02109                         if(i<=0){ //regressamos ao ult_no_cp
02110 //                          printf("nenhum membro de regresso tem vizinhos livres...regressamos ao ultimo elemento no CP\n");
02111                           //equivalente a adicionar regresso (i_reg inda tem o valor certo)
02112                           
02113                           sai = true;
02114                           
02115                         }else{//tamos agora noutro no.
02116                           
02117                           //encontrar equivalente a "i" (regresso) no desvio.
02118                           for(j=0; j<i_desvio;j++){       
02119                                 if (desvio[j] == regresso[i]){
02120                                   prox_no = desvio[j];
02121                                   no_ant = desvio[j+1];
02122 //                                printf("prox_no = %i\tno_ant = %i\n", prox_no, no_ant);
02123                                   break;
02124                                 }
02125                                 
02126                                 if(j==i_desvio-1){
02127 //                                printf("ERRO: nao houve atribuicao de prox_no e no_ant\n");
02128                                   ROS_WARN("Error: There was no atribution of previous and next vertex.\n");
02129                                 }
02130                           }                       
02131                           
02132                           //decrementar elementos de regresso:
02133 //                        printf("decrementar i_reg de %i para %i\n", i_reg, i_reg-(i_reg-i) );
02134                           i_reg-=(i_reg-i);
02135                           
02136 
02137 //                        printf("TABELA DE REGRESSO:\n");
02138 //                        for(i=0; i<i_reg; i++){
02139 //                              printf("regresso[%i]=%i:\n",i,regresso[i]);
02140 //                        }  
02141                           
02142                           
02143                         }
02144                         
02145                         
02146                   }else{ //decidir viz_escolhido
02147                         
02148 //                      printf("\nvamos decidir qual e o proximo vizinho\n");
02149                         
02150                         if(i_list==1){
02151                          viz_escolhido = lista_viz[0]; 
02152 //                       printf("viz_escolhido = %i\n", viz_escolhido);
02153                          
02154                         }else{
02155                           //escolher o no q tem menos vizinhos.
02156                           
02157 //                        printf("escolher no q tem menos vizinhos\n");
02158                           
02159                           num_neigh=INT_MAX;
02160                           
02161                           for(i=0; i<i_list; i++){
02162                                 
02163                                 j=devolve_vizinhos_nao_visitados(vertex_web, dimension, lista_viz[i]);
02164 //                              printf("{no %i} - vizinhos nao visitados= %i\n", lista_viz[i], j);
02165                                 
02166                                 if(j<num_neigh){
02167                                   viz_escolhido = lista_viz[i];
02168                                   num_neigh = j;
02169                                 }
02170                                 
02171                                 /* mandar backup fora */
02172                                 /*int k,l;
02173                                 for(k=0; k<num_nos_total; k++){
02174                                  printf("-------\nno %i:\n", k);
02175                                  for(l=0; l<vertex_web[k].num_neigh; l++){
02176                                    printf("viz %i, visitado = %i\n", vertex_web[k].id_neigh[l], vertex_web[k].visited[l]);
02177                                  }
02178                                 }*/
02179                                 /*
02180                                 if(j==0){
02181                                   printf("Attention! All of Vertex %i neighbours are already visited. There might be a problem with the edge atribution\n",lista_viz[i]);
02182                                 }*/
02183                                 
02184                           }
02185                           
02186                         }
02187                         
02188 //                      printf("viz_escolhido: %i\n", viz_escolhido);                                           
02189                         
02190                         
02191                         /* TIRAR o index 'j' para o inicio */
02192                         for(j=0; j<vertex_web[prox_no].num_neigh; j++){
02193                           if(vertex_web[prox_no].id_neigh[j] == viz_escolhido && vertex_web[prox_no].visited[j]==false){
02194                                 break;
02195                           }
02196                         }
02197                         
02198                         no_ant = prox_no;
02199                         prox_no = viz_escolhido;
02200                         
02201                   }
02202                   
02203                   
02204                 }
02205                 
02206                 if(sai){ //acrescentar o regresso ao desvio:
02207                   
02208 //                printf("acrescentar o regresso ao desvio\n");
02209                   for(i=i_reg-1; i>=0; i--){                     
02210                         desvio[i_desvio] =  regresso[i];
02211                         i_desvio++;
02212                   }
02213                   
02214                   //desvio terminou e para sair do while, independentemente de termos atingido o num_arcos.               
02215                   break; //sai do while           
02216 
02217                 }
02218                 
02219           } //else (arcos_ja_visitados < num_arcos - decidir no proximo no)
02220                   
02221 
02222 //       for(i=0; i<i_desvio; i++){
02223 //         printf("desvio[%i] = %i\n", i, desvio[i]);
02224 //       }
02225          
02226          /*ADICIONAR DESVIO AO CAMINHO DE IDA AQUI (actualizar num. elementos) */
02227 
02228           for(i=1; i<i_desvio; i++){
02229 //              printf("desvio[%i] = %i\n", i, desvio[i]);
02230                 caminho_de_ida[elem_c_ida] = desvio[i];
02231 //              printf("caminho de ida [%i] = %i\n", elem_c_ida, caminho_de_ida[elem_c_ida]);
02232                 elem_c_ida++;
02233           }
02234                    
02235          }      //while (estamos no desvio)
02236          
02237 
02238          
02239   }//while (estamos no CP)
02240   
02241   //mostrar_estado_nos(num_nos_total);
02242   
02243   sai = check_visited (vertex_web, dimension);
02244   return sai;
02245   
02246 }
02247 
02248 int computar_custo_caminho_final (vertex *vertex_web, int *caminho_final, int elem_caminho_final){
02249   
02250   int i,j;
02251   int custo_final = 0;
02252   int anterior, proximo;
02253   
02254   for(i=1; i<elem_caminho_final; i++){
02255         
02256         anterior = caminho_final[i-1];
02257         proximo = caminho_final[i];
02258         
02259         for(j=0; j<vertex_web[anterior].num_neigh; j++){
02260          
02261           if(vertex_web[anterior].id_neigh[j] == proximo){
02262                 custo_final += vertex_web[anterior].cost[j];
02263                 break;
02264           }
02265           
02266         }
02267         
02268   }
02269   
02270   return custo_final;
02271   
02272 }
02273 
02274 int cyclic (uint dimension, vertex *vertex_web, int *caminho_final) {
02275   
02276   int caminho_principal [dimension+1]; //pode ser apenas parcial como pode ser circuito de hamilton
02277   int elem_caminho;
02278   int custo_caminho;
02279   
02280   bool testa = false;
02281   bool hamilton_path = false;
02282   bool cycle = false;
02283   int i;
02284   uint j;
02285   
02286   clear_visited(vertex_web,dimension);
02287 
02288   for(i=0; i<dimension; i++){
02289         
02290         testa = UHC ( vertex_web, vertex_web[i].id, caminho_principal, dimension);
02291           
02292           if(testa){
02293                 
02294                 //verificar caminho_principal:
02295 /*               for(i=0; i<dimension; i++){
02296                         printf("caminho_principal[%i] = %i\n", i, caminho_principal[i]); 
02297                   } 
02298 */
02299                 int hcycle;
02300                 
02301                 //printf("Hamilton Path found.\n");
02302                 hamilton_path = true;
02303                 
02304                 //verificar se e d facto um caminho ou um circuito (1º elemento e ultimo sao vizinhos?):
02305                 hcycle = is_neigh ( caminho_principal[0], caminho_principal[dimension-1], vertex_web, dimension);                       
02306                 
02307                 //e um ciclo de hamilton - podes sair:
02308                 if(hcycle>-1){
02309                   //printf("Hamilton circuit found.\n");
02310                   caminho_principal [dimension] = caminho_principal [0]; //repetir o primeiro.
02311                   cycle = true;
02312                   break;
02313                 }
02314                 
02315           } 
02316   }
02317   
02318   // nesta fase: se hamilton_path e false - longest path... 
02319   // se for true - ver s hamilton_cycle tb e true...
02320 
02321   if(!hamilton_path){
02322         
02323         //printf("No Hamilton Circuit/Path found.\n\n");
02324         
02325         int *ciclo_principal = new int[dimension]; 
02326         int elem_ciclo = 0, custo_ciclo = 0;
02327         int seed = 1000;
02328 
02329         //PROCURAR CICLO:
02330         procurar_ciclo(vertex_web, dimension, ciclo_principal, elem_ciclo, custo_ciclo, seed);
02331         
02332         clear_visited(vertex_web, dimension);           
02333         
02334         //LONGEST PATH APARTIR DOS VIZINHOS UNICOS:
02335         caminho_apartir_vizinhos_unicos(vertex_web, dimension, caminho_principal, elem_caminho, custo_caminho, seed);
02336         
02337         
02338         /* NESTA FASE: DECIDIR ENTRE CICLO E LONGEST PATH [VERIFICAR QUAL O MELHOR METODO DE DECISAO]: */
02339         /* caso haja CICLO */
02340         if(elem_ciclo>=(dimension/2)){ //caminho principal passa a ser o ciclo.
02341           
02342           custo_caminho = custo_ciclo;
02343           elem_caminho=elem_ciclo;
02344           
02345           for(i=0; i<elem_caminho; i++){
02346                 caminho_principal [i] = ciclo_principal[i];
02347           }
02348           
02349           cycle = true;   
02350   
02351         }//nao precisa de else - caminho principal ja e o longest path.
02352         
02353         
02354         delete [] ciclo_principal;
02355         
02356         
02357 /*      if(cycle){printf("\nMain path (cycle): ");}else{printf("Main path (longest path): ");}
02358         for(i=0; i<elem_caminho; i++){
02359           if(i==elem_caminho-1){printf("%i\n", caminho_principal[i]);}else{printf("%i, ", caminho_principal[i]);}
02360         }
02361         printf("Number of elements = %i\nCost = %i\n", elem_caminho, custo_caminho);
02362 */      
02363         
02364   }else{
02365         
02366         if(cycle){ //HAMILTON CIRCUIT (CYCLE)
02367         
02368           //ult.elemento = primeiro:
02369           elem_caminho = dimension+1; 
02370           
02371         }else{ //HAMILTON PATH
02372           
02373            elem_caminho = dimension; 
02374          }
02375         
02376         //verificar caminho_principal:
02377         printf("Hamilton Path/Circuit: ");      
02378         custo_caminho=0;
02379         
02380         for(i=0; i<elem_caminho; i++){
02381           
02382           if(i==elem_caminho-1){
02383                 printf("%i\n", caminho_principal[i]);
02384                 printf("cost = %i\n", custo_caminho);
02385                 
02386           }else{
02387                 printf("%i, ", caminho_principal[i]);
02388                 
02389                 /*COMPUTAR O CUSTO DO CAMINHO DE HAMILTON: */
02390                 for(j=0; j<vertex_web[ caminho_principal[i] ].num_neigh; j++){                    
02391                   if( vertex_web[ caminho_principal[i] ].id_neigh[j] == caminho_principal[i+1] ){                               
02392                         custo_caminho += vertex_web[ caminho_principal[i] ].cost[j];
02393                         break;
02394                   }                             
02395                 }       
02396                 
02397           }
02398         }       
02399   }
02400   
02401   int *caminho_de_ida = new int[3*dimension];  //e suficiente?
02402   int elem_c_ida;
02403   
02404   if (!hamilton_path){
02405   
02406    /* Já temos caminho principal...agora desvios... */
02407    
02408   int num_max = elem_caminho;
02409   int num_min = 1;
02410   bool resultado = false, sai = false;
02411   double tentativa = 1;
02412   int num_arcos = num_min;
02413   
02414   //garantir que num_min e minorante:
02415   clear_visited(vertex_web, dimension); 
02416   elem_c_ida = 0;
02417   resultado = computar_caminho_de_ida (vertex_web, dimension, caminho_de_ida, elem_c_ida, caminho_principal, elem_caminho, num_min);
02418 //  printf("Minorante testado\n");
02419   if(resultado == true){ /*printf("num_arcos e 1!! Sai logo!!\n");*/ sai=true; } //se der so com 1 arco -> ja encontramos o num_arcos!
02420 //  printf("\n");
02421   
02422   resultado = false;
02423   
02424   int prob_reg = 0;
02425   bool haprob = false;
02426   
02427         if(!sai){
02428           while (!resultado){ //garantir que num_max e majorante.
02429                 
02430                 clear_visited(vertex_web, dimension); 
02431                 elem_c_ida = 0;
02432                 resultado = computar_caminho_de_ida (vertex_web, dimension, caminho_de_ida, elem_c_ida, caminho_principal, elem_caminho, num_max);
02433 //              printf("Majorante testado\n");
02434 //              printf("num_max = %i\n\n", num_max);
02435                 
02436                 tentativa+=0.5;
02437                 
02438                 if(!resultado) {
02439                   num_min = num_max;
02440                   num_max = tentativa*elem_caminho;
02441                   prob_reg++;
02442                 }
02443                 
02444                 //MECANISMO DE PROTECCAO:
02445                 if(prob_reg == 100){
02446                   resultado = true;
02447                   num_max = elem_caminho;
02448                   haprob = true;
02449                 }
02450                 
02451           } 
02452 
02453         num_arcos = (num_min + num_max)/2;  
02454         if ( (num_min + num_max)%2 == 1 ){num_arcos++;}
02455 
02456         tentativa = 0;
02457   
02458   }
02459   
02460   while(sai==false && haprob==false){
02461         
02462         tentativa++;
02463         
02464         clear_visited(vertex_web, dimension);
02465         elem_c_ida = 0;
02466         resultado = computar_caminho_de_ida (vertex_web, dimension, caminho_de_ida, elem_c_ida, caminho_principal, elem_caminho, num_arcos); 
02467         
02468         
02469 //      printf("tentativa %i (num_arcos = %i)\n", (int)tentativa,num_arcos);
02470         
02471         if(resultado){
02472           
02473           num_max = num_arcos;
02474           
02475           if( num_max - 1 == num_min) {
02476                 sai=true;
02477           
02478           }else{          
02479                 num_arcos = (num_min + num_max)/2;      
02480                 if ( (num_min + num_max)%2 == 1 ){num_arcos++;}
02481           }       
02482           
02483         }else{
02484           
02485           num_min = num_arcos;
02486           
02487           if( num_max - 1 == num_min) {
02488                 num_arcos++;
02489           
02490           }else{  
02491                 
02492                 num_arcos = (num_min + num_max)/2;
02493                 if ( (num_min + num_max)%2 == 1 ){num_arcos++;}
02494                 
02495           }
02496           
02497         }
02498         
02499   }
02500   
02501   /* nesta fase tenho caminho_de_ida computado. */
02502 
02503 /* printf("caminho de ida:\n");
02504  for(i=0; i<elem_c_ida; i++){
02505         if(i==elem_c_ida-1){ printf("%i\n", caminho_de_ida[i]); }else{ printf("%i, ", caminho_de_ida[i]); }
02506  }
02507  printf("elementos_c_ida = %i\n", elem_c_ida);   */
02508 
02509 
02510   }else{        //hamilton cycle or path
02511           //caminho_de_ida = caminho_principal
02512                   
02513           for (i=0; i<elem_caminho; i++){
02514                 caminho_de_ida [i] = caminho_principal[i];
02515           }
02516           elem_c_ida = elem_caminho;
02517   }
02518   
02519   
02520   /* computar custo do caminho de ida (+ volta) */
02521   
02522   int elem_caminho_final = 2*elem_c_ida-1;
02523   //int caminho_final [elem_caminho_final];
02524   
02525   //1ª parte do caminho final e igual ao caminho de ida para todos os casos:
02526   for(i=0; i<elem_c_ida; i++){
02527         caminho_final [i] = caminho_de_ida[i];                    
02528   }
02529 
02530 
02531   
02532   if(!cycle){ //add inverse_path to (Hamilton/Longest) Path
02533     
02534           for(i=elem_c_ida-2; i>=0; i--){
02535                 caminho_final [2*(elem_c_ida-1) - i] = caminho_de_ida[i];
02536           }
02537           
02538   }else{ //ciclico: caminho de ida = caminho final.             
02539         elem_caminho_final = elem_c_ida;                  
02540   }
02541 
02542   
02543   delete [] caminho_de_ida; 
02544   
02545   
02546   /*printf("Final Path: ");
02547   for(i=0; i<elem_caminho_final; i++){
02548         if(i==elem_caminho_final-1){ printf("%i\n", caminho_final[i]); }else{ printf("%i, ", caminho_final[i]); }
02549   }
02550   printf("Number of elements = %i\n", elem_caminho_final);*/
02551   
02552   i = computar_custo_caminho_final (vertex_web, caminho_final, elem_caminho_final);
02553   //printf("Cost = %i\n\n", i);
02554   
02555   //mecanismo contra loops infinitos:
02556   //if(haprob){ printf("Warning: Couldn't access all nodes.\n"); }
02557   
02558   clear_visited(vertex_web, dimension);    
02559   
02560   return elem_caminho_final;
02561   
02562 } //fim da função
02563 
02564 void shift_cyclic_path (uint start_vertex, int *caminho_final, int elem_caminho_final){
02565   
02566   uint posshift = 0;
02567   int i;
02568   int temp_table[elem_caminho_final];
02569   
02570   for (i=0; i<elem_caminho_final; i++){
02571     if (caminho_final[i] == start_vertex){
02572      posshift = i;
02573      break;
02574     }
02575   }
02576   
02577   uint aux;
02578   i=0;
02579   
02580   for(aux=posshift; aux<elem_caminho_final-1; aux++){
02581    temp_table[i] = caminho_final[aux];
02582    i++;
02583   }
02584   
02585   for(aux=0; aux<=posshift; aux++){
02586    temp_table[i]=caminho_final[aux];
02587    i++;
02588   }
02589   
02590   for(i=0; i<elem_caminho_final;i++){
02591    caminho_final[i] = temp_table[i]; 
02592   }
02593   
02594 }
02595 
02596 uint get_MSP_dimension (const char* msp_file) {
02597         
02598         FILE *file;
02599         file = fopen (msp_file,"r");
02600         uint dimension;
02601         
02602         if(file == NULL){
02603                 ROS_INFO("Can not open filename %s", msp_file);
02604                 ROS_BREAK();    
02605         }else{
02606                 ROS_INFO("MSP Route File Opened. Reading Dimensions.\n");
02607                 fscanf (file, "%u", &dimension);
02608         }
02609         fclose(file);
02610         return dimension;
02611 }
02612 
02613 void get_MSP_route (uint *route, uint dimension, const char* msp_file) {
02614         
02615    FILE *file;
02616    file = fopen (msp_file,"r");
02617    
02618    if(file == NULL){
02619       ROS_INFO("Can not open filename %s", msp_file);
02620       ROS_BREAK();      
02621    }else{
02622       ROS_INFO("MSP Route File Opened. Getting MSP Route.\n");
02623       
02624       uint i;
02625       float temp;
02626       
02627       //Start Reading the File from the second line On:
02628       //Start Reading the File from FIRST_VID On:
02629         fscanf (file, "%f", &temp);   
02630 
02631       
02632       for (i=0;i<dimension;i++){        
02633         fscanf (file, "%u", &route[i]); 
02634       }     
02635       
02636    }
02637         
02638     //printf ("[v=10], x = %f (meters)\n",vertex_web[10].x); 
02639 
02640    fclose(file);
02641 }
02642 
02643 void create_source_and_dest_tables(vertex *vertex_web, uint *source, uint *destination, uint dimension){
02644 
02645   uint idx=0;
02646   uint i, j;
02647   
02648   for (i=0; i<dimension; i++){    
02649     for (j=0; j<vertex_web[i].num_neigh; j++){      
02650       source[idx] = vertex_web[i].id;
02651       destination[idx] = vertex_web[i].id_neigh[j];
02652       idx++;      
02653     }    
02654   }
02655     
02656 }
02657 
02658 void get_hist_sort(vertex *vertex_web, double *hist_sort, uint dimension){
02659   
02660   uint idx=0;
02661   
02662   for (uint i=0; i<dimension; i++){
02663     for (uint j=0; j<vertex_web[i].num_neigh; j++){      
02664       if (vertex_web[i].id < vertex_web[i].id_neigh[j] /*&& vertex_web[i].num_neigh > 1*/){
02665 
02666           hist_sort[idx] = vertex_web[i].cost_m[j];
02667           idx++;
02668           
02669       }      
02670     }    
02671   }
02672   
02673   //int elements = sizeof(edge_costs) / sizeof(edge_costs[0]); 
02674   std::sort(hist_sort, hist_sort + idx); 
02675 }
02676 
02677 int get_hist_idx_from_edge_cost (double *hist_sort, uint size, double edge_cost){
02678   
02679   for (uint i=0; i<size;i++){
02680    
02681     //tou a comparar doubles, o q pode ser perigoso.
02682    if ( hist_sort[i]>=edge_cost - 0.001 && hist_sort[i]<=edge_cost + 0.001){ 
02683     return i; 
02684    }
02685    
02686   }
02687   
02688   return -1;  
02689 }
02690 
02691 int get_hist_idx (uint *source, uint *destination, uint source_vertex, uint dest_vertex, uint hist_dimension){
02692   
02693   for (uint i=0; i<hist_dimension;i++){    
02694     if (source[i] == source_vertex && destination[i] == dest_vertex){      
02695       return i;      
02696     }
02697   }
02698   
02699   return -1;
02700   
02701 }
02702 
02703 double get_edge_cost_between (vertex *vertex_web, uint vertex_A, uint vertex_B){
02704   
02705   for (uint i=0; i<vertex_web[vertex_A].num_neigh; i++){
02706    
02707     if ( vertex_web[vertex_A].id_neigh[i] == vertex_B){
02708         return vertex_web[vertex_A].cost_m[i];
02709     }    
02710   }
02711   
02712   return -1.0;
02713   
02714 }
02715 
02716 void load_real_histogram(double *real_histogram, uint size_hist, char* filename){
02717  
02718     FILE *file;
02719     file = fopen (filename,"r"); 
02720     
02721     float b,c;
02722     int a,e,d,f,i=0;
02723     char inicio[128];
02724     
02725     if (file!=NULL){ 
02726      
02727        //ignore first three lines:
02728         fgets (inicio , 128 , file);
02729         fgets (inicio , 128 , file);      
02730         fgets (inicio , 128 , file);
02731       
02732       for (i=0; i<size_hist; i++){              //"%f(%d)-%f(%d)\t%f\t%f"       
02733         //fscanf(file, "%f\t", &a);      
02734         fscanf(file, "%d(%d)-%d(%d)", &a, &d, &e, &f);
02735         fscanf(file, "%f\t", &b);
02736         fscanf(file, "%f", &c);
02737         real_histogram[i] = c;
02738       }
02739       
02740       fclose(file);
02741     }
02742   
02743 }
02744 
02745 void normalize_histogram(double *real_histogram, double *histogram, uint size_hist){
02746   
02747   double sum_real = 0.0;
02748   int i;
02749   
02750   for (i=0; i<size_hist; i++){
02751     sum_real += real_histogram[i];
02752   }
02753   
02754   for (i=0; i<size_hist; i++){
02755     histogram[i] = real_histogram[i]/sum_real;
02756   }  
02757   
02758 }
02759 
02760 int pertence_uint_idx (uint elemento, uint *tab, uint tam_tab){
02761   
02762   for (int i=0; i<tam_tab; i++){
02763         
02764         if( tab[i] == elemento) {
02765           return i;
02766         }
02767         
02768   }
02769   
02770   return -1;  
02771 }
02772 
02773 int get_min(uint *tab, uint tam_tab){
02774   
02775   int min_elem = INT_MAX;
02776   
02777   for (int i=0; i<tam_tab; i++){
02778         
02779         if( tab[i] < min_elem) {
02780           min_elem = tab[i];
02781         }       
02782   }  
02783   return min_elem;   
02784 }
02785 
02786 double get_min_dbl(double *tab, uint tam_tab){
02787   
02788   double min_elem = tab[0];
02789   
02790   for (int i=0; i<tam_tab; i++){
02791         
02792         if( tab[i] < min_elem) {
02793           min_elem = tab[i];
02794         }       
02795   }  
02796   return min_elem;   
02797 }
02798 
02799 int get_max(uint *tab, uint tam_tab){
02800   int max_elem = 0;
02801   
02802   for (int i=0; i<tam_tab; i++){
02803         
02804         if( tab[i] > max_elem) {
02805           max_elem = tab[i];
02806         }       
02807   }  
02808   return max_elem;  
02809   
02810 }
02811 
02812 double get_max_dbl(double *tab, uint tam_tab){
02813   double max_elem = 0.0;
02814   
02815   for (int i=0; i<tam_tab; i++){
02816         
02817         if( tab[i] > max_elem) {
02818           max_elem = tab[i];
02819         }       
02820   }  
02821   return max_elem;  
02822   
02823 }
02824 
02825 void write_histogram_to_file (vertex *vertex_web, double *real_histogram, double *histogram, uint *source, uint *destination, uint hist_dimension, uint number, uint robotid){
02826   
02827   FILE *file;
02828   char filename[128];
02829   char number_str[20];
02830   strcpy(filename,"results/histogram_r");       //e.g.: #histogram_r1_d1000
02831   itoa(robotid, number_str, 10);
02832   strcat(filename,number_str);
02833   strcat(filename,"_d");
02834   itoa(number, number_str, 10);  
02835   strcat(filename,number_str);
02836   strcat(filename,".txt");
02837   
02838   file = fopen (filename,"w"); 
02839   
02840   if (file!=NULL){ 
02841     
02842     fprintf(file, "\n[x] Source(deg)-Dest(deg):\t[y] Norm_hist:\tReal_hist:\n\n");
02843     
02844     for (int i=0; i<hist_dimension; i++){
02845       fprintf(file, "%d(%d)-%d(%d)\t%f\t%f\n", source[i], vertex_web[source[i]].num_neigh, destination[i], vertex_web[destination[i]].num_neigh, histogram[i], real_histogram[i]);
02846       //fprintf(file, "%f\t%f\t%f\n", hist_sort[i], histogram[i], real_histogram[i]);
02847     }   
02848     fclose(file);
02849   }
02850   
02851 }
02852 
02853 void write_reward_evolution(double reward, uint robotid){ //"results/reward_evolution.txt"
02854   
02855   FILE *file;
02856   char filename[128];
02857   char number_str[20];
02858   strcpy(filename,"results/reward_evolution_r");
02859   itoa(robotid, number_str, 10);
02860   strcat(filename,number_str);
02861   strcat(filename,".txt");
02862   
02863   file = fopen (filename,"a");
02864   
02865     if (file!=NULL){ 
02866       fprintf(file, "%d\t%f\n", reward_count,reward);
02867       fclose(file);
02868     }
02869     reward_count++;  
02870 }
02871 
02872 void update_likelihood_old (reinforcement_learning RL, double *real_histogram, double *hist_sort, uint size_hist, vertex *vertex_web){ //actualizar idleness_new: campo que falta preencher na struct
02873 
02874   //só se:   RL.num_possible_neighs > 1 :: 1ª verificação para evitar analisar lixo
02875   if (RL.num_possible_neighs <= 1){
02876    return; 
02877   }
02878  
02879   int id_next_vertex = pertence_uint_idx (RL.next_vertex, RL.id_neighbors, RL.num_possible_neighs);
02880   
02881   //ROS_INFO("id_next_vertex = %d", id_next_vertex);
02882   
02883   if (id_next_vertex<0){
02884     return;
02885   }
02886   
02887   int SIGN = 1;
02888   uint i = 0;
02889   
02890   
02891   uint node_count = RL.node_count[id_next_vertex];
02892   //ROS_INFO("node_count = %d", node_count);
02893   
02894   uint node_max = get_max(RL.node_count, RL.num_possible_neighs);
02895   //ROS_INFO("node_max = %d", node_max);
02896   
02897   uint node_min = get_min(RL.node_count, RL.num_possible_neighs);
02898   //ROS_INFO("node_min = %d", node_min);
02899   
02900   if (node_count == node_max && node_max > node_min){
02901    SIGN = -1; 
02902   }  
02903   
02905   if (node_min == node_max){ //check idleness effect:
02906     
02907     double idleness_new [RL.num_possible_neighs];
02908     double add_time = ros::Time::now().toSec() - (RL.time_then);
02909     
02910     //double idleness_old_current = 0.0;
02911     //double idleness_new_current = add_time;
02912     
02913     //isto é so dos vizinhos
02914     for(i=0; i<RL.num_possible_neighs; i++){
02915       idleness_new[i] = RL.idleness_old[i] + add_time;
02916     }
02917     
02918     idleness_new[id_next_vertex] = 0.0;
02919     
02920     //calculate sums:
02921     double sum_idle_old = 0.0;
02922     double sum_idle_new = 0.0;
02923     
02924     for(i=0; i<RL.num_possible_neighs; i++){
02925       sum_idle_old += RL.idleness_old[i];
02926       sum_idle_new += idleness_new[i];      
02927     }    
02928     
02929     sum_idle_new += add_time; //add current_vertex idlenss (in idle_old was zero)
02930 
02931     //ROS_INFO("sum_idle_old: %f", sum_idle_old);
02932     //ROS_INFO("sum_idle_new: %f", sum_idle_new);
02933     
02934     if (sum_idle_old < sum_idle_new){
02935       SIGN = -1;
02936     }
02937     
02938   } 
02939   
02940   //Now we have the sign.
02941   //ROS_INFO("Sign = %d", SIGN);
02942   
02943   
02946   double edge_cost = get_edge_cost_between (vertex_web, RL.current_vertex, RL.next_vertex);
02947   //ROS_INFO("edge_cost = %f",edge_cost);
02948   
02949   int hist_idx_next_vertex = get_hist_idx_from_edge_cost (hist_sort, size_hist, edge_cost);
02950   //ROS_INFO("hist_idx_next_vertex = %d",hist_idx_next_vertex);
02951   
02952   ROS_INFO("Punish/Reward: %f", ((double)SIGN)*(1.0 - RL.entropy) );
02953   
02954   //ROS_INFO("Before: real_histogram [hist_idx_next_vertex] = %f", real_histogram [hist_idx_next_vertex]);
02955   
02956   //Punish or Reward:
02957   real_histogram [hist_idx_next_vertex] += ((double)SIGN)*(1.0 - RL.entropy);
02958   
02959   //minimum normalized histogram value = 1/nedges*2.5 (4)
02960   //initial histogram value = 1/nedges (10)
02961   //maximum normalized histogram value = 4/nedges (40)
02962   
02963   //minimum real histogram value = 4.0:
02964   if (real_histogram [hist_idx_next_vertex] < 4.0){
02965     real_histogram [hist_idx_next_vertex] = 4.0;
02966   }
02967   
02968   //maximum real histogram value = 40.0:
02969   if (real_histogram [hist_idx_next_vertex] > 40.0){
02970     real_histogram [hist_idx_next_vertex] = 40.0;
02971   }  
02972   
02973   //ROS_INFO("After: real_histogram [hist_idx_next_vertex] = %f", real_histogram [hist_idx_next_vertex]);
02974     
02975 }
02976 
02977 void update_likelihood (reinforcement_learning RL, double *real_histogram, uint *source, uint *destination, uint hist_dimension, vertex *vertex_web, int minimum_global_node_count, uint robotid){ //actualizar idleness_new: campo que falta preencher na struct
02978 
02979   //só se:   RL.num_possible_neighs > 1 :: 1ª verificação para evitar analisar lixo
02980   if (RL.num_possible_neighs <= 1){
02981    //write_reward_evolution(0.0, robotid);
02982    return;
02983   }
02984   
02985  
02986   int id_next_vertex = pertence_uint_idx (RL.next_vertex, RL.id_neighbors, RL.num_possible_neighs);
02987   
02988   ROS_INFO("id_next_vertex = %d", id_next_vertex);
02989   
02990   if (id_next_vertex<0){
02991     ROS_WARN("Couldn't find id_next_vertex in update_likelihood()");
02992     return;
02993   }
02994   
02995   int SIGN = 0;
02996   double strong_reward = 0.0;
02997   uint i = 0;
02998   
02999   double node_count_tab[RL.num_possible_neighs];
03000   
03002   double node_count;
03003   
03004   //if (vertex_web[RL.next_vertex].num_neigh <= 1){
03005     node_count = (double) RL.node_count[id_next_vertex] / (double) vertex_web[RL.next_vertex].num_neigh;
03006   //}else{
03007   //  node_count = (double) RL.node_count[id_next_vertex] / (double) 2*vertex_web[RL.next_vertex].num_neigh;
03008   //}
03009   
03010   
03011   for (i=0; i<RL.num_possible_neighs; i++){
03012     
03013     //if (vertex_web[ RL.id_neighbors[i] ].num_neigh <= 1){
03014       node_count_tab[i] = (double) RL.node_count[i] / (double) vertex_web[ RL.id_neighbors[i] ].num_neigh;
03015     //}else{
03016     //  node_count_tab[i] = (double) RL.node_count[i] / (double) 2*vertex_web[ RL.id_neighbors[i] ].num_neigh;
03017     //}
03018     
03019     ROS_INFO("node_count (%d) = %d", RL.id_neighbors[i], RL.node_count[i] );
03020     ROS_INFO("degree (%d) = %d", RL.id_neighbors[i], vertex_web[RL.id_neighbors[i]].num_neigh);
03021     ROS_INFO("node_count_norm (%d) = %f",RL.id_neighbors[i], node_count_tab[i]);    
03022     
03023   }
03024   
03025   //uint node_max = get_max(RL.node_count, RL.num_possible_neighs);
03026   double node_max = get_max_dbl(node_count_tab, RL.num_possible_neighs);
03027   ROS_INFO("node_max_norm = %f",node_max); 
03028 
03029   //ROS_INFO("node_max = %d", node_max);
03030   
03031   double node_min = get_min_dbl(node_count_tab, RL.num_possible_neighs);
03032   ROS_INFO("node_min_norm = %f",node_min); 
03033   //ROS_INFO("node_min = %d", node_min);
03034   
03035   /*if (node_count == node_max && node_max > node_min){
03036    SIGN = -1; 
03037   }*/
03038   
03039   if (node_count == node_max){ 
03040    SIGN = -5;
03041    strong_reward = -1.0;
03042    ROS_INFO("STRONG PUNISHMENT!!! node_count = node_max, SIGN = %d",SIGN);
03043   }
03044   
03045   if (node_count == node_min){
03046    SIGN = 1;
03047    ROS_INFO("node_count = node_min, SIGN = %d",SIGN); 
03048   }  
03049   
03050   if (node_min == node_max){
03051    SIGN = 0;   
03052    ROS_INFO("node_max = node_min, SIGN = %d",SIGN);
03053    
03054   }else{
03055     
03057    if (RL.node_count[id_next_vertex] > 0 && RL.node_count[id_next_vertex] == minimum_global_node_count){ 
03058      SIGN = 5;  
03059      strong_reward = 1.0;
03060      ROS_INFO("node_count = minimum_global_node_count, SIGN = %d",SIGN);
03061      ROS_WARN("STRONG REWARD!!!!!!!!!!! Vertex with minimum global node count"); 
03062    }   
03063     
03064    if(SIGN==0){
03065      ROS_INFO("node_max > node_count >= node_min, SIGN = %d",SIGN);
03066    }
03067   }
03068   
03069   //if(SIGN==5){
03070   //  ROS_WARN("STRONG REWARD!!!!!!!!!!! Vertex with minimum global node count"); 
03071   //}
03072   
03073   if (SIGN==0){ //not the node with minimum node count: but it may have high instantaneous idleness
03074     
03075     
03076     double max_idleness = 0.0;
03077     
03078     for (i=0; i<RL.num_possible_neighs; i++){
03079       if (RL.idleness_old[i] > max_idleness){
03080         max_idleness = RL.idleness_old[i];
03081       }
03082     }
03083     
03084     ROS_INFO("max_idleness = %f", max_idleness);
03085     ROS_INFO("idleness_old(v=%d) = %f", RL.next_vertex, RL.idleness_old[id_next_vertex]);
03086     
03087     //doesn't have the highest idleness - bad decision: negative reward
03088     if (max_idleness > RL.idleness_old[id_next_vertex]){
03089      SIGN = -1; 
03090      ROS_INFO("V=%d Doesn't have the highest idleness - bad decision, SIGN = %d",RL.next_vertex, SIGN);
03091      
03092     }/*else{ //se idleness do seleccionado for 2X maior q idleness do 2º maior (Reward+)
03093 
03094       bool reward = true;
03095       
03096       for (i=0; i<RL.num_possible_neighs; i++){
03097         if (max_idleness <= 2.0*RL.idleness_old[i]+0.001 ){ //tamos a comparar doubles...
03098           reward = false;
03099         }
03100         ROS_INFO("idleness_old(v=%d) = %f", RL.id_neighbors[i], RL.idleness_old[i]);
03101       }
03102       
03103       if (reward){
03104         SIGN=1;
03105         ROS_INFO("V=%d Has the highest idleness: Twice the 2nd one, SIGN = %d",RL.next_vertex,SIGN);
03106       }else{
03107         ROS_INFO("V=%d Has the highest idleness: But NOT twice the 2nd one, SIGN = %d",RL.next_vertex,SIGN);
03108       }
03109       
03110     }*/
03111     
03112   }
03113   
03114   if (SIGN==0){
03115     ROS_WARN("Punish/Reward: 0.0");
03116     //write_reward_evolution(0.0, robotid);
03117     return;
03118   }
03119   
03121    /*if (node_min == node_max){ //check idleness effect:
03122     
03123     double idleness_new [RL.num_possible_neighs];
03124     double add_time = ros::Time::now().toSec() - (RL.time_then);
03125     
03126     //double idleness_old_current = 0.0;
03127     //double idleness_new_current = add_time;
03128     
03129     //isto é so dos vizinhos
03130     for(i=0; i<RL.num_possible_neighs; i++){
03131       idleness_new[i] = RL.idleness_old[i] + add_time;
03132     }
03133     
03134     idleness_new[id_next_vertex] = 0.0;
03135     
03136     //calculate sums:
03137     double sum_idle_old = 0.0;
03138     double sum_idle_new = 0.0;
03139     
03140     for(i=0; i<RL.num_possible_neighs; i++){
03141       sum_idle_old += RL.idleness_old[i];
03142       sum_idle_new += idleness_new[i];      
03143     }    
03144     
03145     sum_idle_new += add_time; //add current_vertex idlenss (in idle_old was zero)
03146 
03147     //ROS_INFO("sum_idle_old: %f", sum_idle_old);
03148     //ROS_INFO("sum_idle_new: %f", sum_idle_new);
03149     
03150     if (sum_idle_old < sum_idle_new){
03151       SIGN = -1;
03152     }
03153     
03154   }*/
03155   
03156   //Now we have the sign.
03157   //ROS_INFO("Sign = %d", SIGN);
03158   
03159   
03162   //double edge_cost = get_edge_cost_between (vertex_web, RL.current_vertex, RL.next_vertex);
03163   //ROS_INFO("edge_cost = %f",edge_cost);
03164   
03165   
03166   //int hist_idx_next_vertex = get_hist_idx_from_edge_cost (hist_sort, size_hist, edge_cost);
03167   
03169   int hist_idx_next_vertex = get_hist_idx (source, destination, RL.current_vertex, RL.next_vertex, hist_dimension);
03170   
03171   //ROS_INFO("hist_idx_next_vertex = %d",hist_idx_next_vertex);
03172   
03173  
03174   double reward_inc = ((double)SIGN)*(1.0 - RL.entropy);
03175   
03176   if (abs(SIGN)>1){ //strong reward...
03177     reward_inc = strong_reward;
03178   }
03179   
03180   //double reward_inc = ((double)SIGN*SCALE_FACTOR)*(RL.entropy);
03181   ROS_WARN("Punish/Reward: %f", reward_inc );  
03182   ROS_INFO("Before Update: real_histogram [hist_idx_next_vertex] = %f", real_histogram [hist_idx_next_vertex]);
03183   
03184   //Update histogram:
03185   real_histogram [hist_idx_next_vertex] += reward_inc;  
03186   //write_reward_evolution(reward_inc, robotid);
03187   
03188   //minimum normalized histogram value = 1/nedges*2.5 (4)
03189   //initial histogram value = 1/nedges (10)
03190   //maximum normalized histogram value = 4/nedges (40)
03191   
03192   //minimum real histogram value = 5.0:
03193   if (real_histogram [hist_idx_next_vertex] < 5.0){ //tava 4
03194     real_histogram [hist_idx_next_vertex] = 5.0;
03195   }
03196   
03197   //maximum real histogram value = 20.0:
03198   if (real_histogram [hist_idx_next_vertex] > 20.0){ //tava 40
03199     real_histogram [hist_idx_next_vertex] = 20.0;
03200   }  
03201   
03202   ROS_INFO("After Update (and truncation): real_histogram [hist_idx_next_vertex] = %f", real_histogram [hist_idx_next_vertex]);
03203     
03204 }
03205 
03206 void update_likelihood_new (reinforcement_learning RL, uint *node_count_table, double *inst_idleness, uint dimension, double *real_histogram, uint *source, uint *destination, uint hist_dimension, vertex *vertex_web, uint robotid){ //actualizar idleness_new: campo que falta preencher na struct
03207 
03208   //só se:   RL.num_possible_neighs > 1 :: 1ª verificação para evitar analisar lixo
03209   if (RL.num_possible_neighs <= 1){
03210    //write_reward_evolution(0.0, robotid);
03211    return;
03212   }
03213   
03214   int SIGN_tab [RL.num_possible_neighs];
03215   
03216   //preencher isto:
03217   //int idx_vertex[RL.num_possible_neighs]; //se for preciso - preencher ja no prox for
03218   double node_norm[RL.num_possible_neighs]; //
03219   double node_min, node_max; //
03220   int idx_min, idx_max; //
03221   uint i; //
03222     
03223   for (i=0; i<RL.num_possible_neighs; i++){
03224     
03225     //if (vertex_web[ RL.id_neighbors[i] ].num_neigh <= 1){
03226     SIGN_tab[i] = 0;
03227     
03228     node_norm[i] = (double) node_count_table[ RL.id_neighbors[i] ] / (double) vertex_web[ RL.id_neighbors[i] ].num_neigh;
03229     //node_norm[i] = (double) node_count_table[ RL.id_neighbors[i] ];
03230     
03231     //}else{
03232     //  node_count_tab[i] = (double) RL.node_count[i] / (double) 2*vertex_web[ RL.id_neighbors[i] ].num_neigh;
03233     //}
03234     
03235     if (i==0){
03236       //initializations
03237      node_min = node_norm[i];
03238      node_max = 0.0;
03239      idx_min = 0;
03240      idx_max = -1;
03241     }
03242     
03243     //updates
03244     if (node_norm[i] <= node_min){
03245       idx_min=i;
03246       node_min = node_norm[i];
03247     }
03248     
03249     if (node_norm[i] >= node_max){
03250       idx_max=i;
03251       node_max = node_norm[i];
03252     }
03253     
03254     //ROS_INFO("node_count (%d) = %d", RL.id_neighbors[i], node_count_table[ RL.id_neighbors[i] ] );
03255     //ROS_INFO("degree (%d) = %d", RL.id_neighbors[i], vertex_web[RL.id_neighbors[i]].num_neigh);
03256     //ROS_INFO("node_count_norm (%d) = %f",RL.id_neighbors[i], node_norm[i]);
03257     //ROS_INFO("inst_idl[%d] = %f", RL.id_neighbors[i], inst_idleness[RL.id_neighbors[i]] );
03258   }
03259   
03260   
03261   
03262   
03263   int ocurr_min = 0;
03264   int ocurr_max = 0;
03265   int tab_idx_min [RL.num_possible_neighs];
03266   int tab_idx_max [RL.num_possible_neighs];
03267   
03268   for (i=0; i<RL.num_possible_neighs; i++){
03269       
03270     if(node_min <= node_norm[i]+ 0.001 ||  node_min >= node_norm[i] -0.001) { //double comparation
03271       tab_idx_min[ocurr_min] = i;
03272       ocurr_min++;
03273     }
03274     
03275     if(node_max <= node_norm[i] + 0.001 ||  node_max >= node_norm[i] - 0.001 ) {//double comparation
03276       tab_idx_max[ocurr_max] = i;
03277       ocurr_max++;      
03278     }
03279   }
03280   
03281   
03282   
03283   
03284   //ROS_INFO("ocurr_min = %d", ocurr_min);
03285   
03286   /*for (i=0; i<ocurr_min; i++){
03287     ROS_INFO("tab_idx_min[%d] = %d", i,tab_idx_min[i]);
03288   }*/
03289   
03290   //ROS_INFO("ocurr_max = %d", ocurr_max);
03291   
03292   /*for (i=0; i<ocurr_max; i++){
03293     ROS_INFO("tab_idx_max[%d] = %d", i,tab_idx_max[i]);
03294   } */
03295   
03296    
03297     
03298     if (ocurr_min == 1){ // 1 choice:
03299       SIGN_tab [idx_min] = 1;
03300       //ROS_INFO("SIGN(%d)[v=%d] = %d", idx_min, RL.id_neighbors[idx_min], SIGN_tab [idx_min]);
03301     }
03302     
03303     if (ocurr_max == 1){ //1 choice:
03304       SIGN_tab [idx_max] = -1;
03305       //ROS_INFO("SIGN(%d)[v=%d] = %d", idx_max, RL.id_neighbors[idx_max], SIGN_tab [idx_max]);
03306     }
03307     
03308     if (ocurr_min > 1){ //more than 1 choice:
03309       
03310       //ESCOLHER O(S) QUE TEM IDLENESS MAIOR
03311       double max_idl = 0.0;
03312       int vertex_index;
03313       
03314       //get max_idl:
03315       for (i=0; i<ocurr_min; i++){
03316         
03317         vertex_index = RL.id_neighbors [ tab_idx_min [i] ];
03318         
03319         if (inst_idleness [vertex_index] > max_idl){
03320           max_idl = inst_idleness [vertex_index];
03321         }
03322       }
03323       
03324       //tnh max_idl mas n sei num d ocurr
03325       int ocurr_idl_max = 0;
03326       int tab_idl_max [ocurr_min];
03327       
03328       //get num ocurr of max_idl:
03329       for (i=0; i<ocurr_min; i++){
03330         vertex_index = RL.id_neighbors [ tab_idx_min [i] ]; //guardar idx da vertex_web / inst_idleness
03331         
03332         if (inst_idleness [vertex_index] == max_idl){
03333           tab_idl_max[ocurr_idl_max] = tab_idx_min [i]; // guardar idx da RL.id_neighbors table
03334           ocurr_idl_max++;
03335         }
03336       }
03337       
03338       //SIGNs:
03339       for (i=0; i<ocurr_idl_max; i++){
03340         
03341         vertex_index = RL.id_neighbors [ tab_idl_max [i] ];
03342         SIGN_tab [ tab_idl_max[i] ] = 1;
03343         
03344         //ROS_INFO("SIGN(%d)[v=%d] = %d", tab_idl_max[i], vertex_index, SIGN_tab [ tab_idl_max[i] ]);
03345       }
03346       
03347       
03348       
03349     }
03350     
03351     if (ocurr_max > 1) {
03352       //ESCOLHER O(S) QUE TEM IDLENESS MENOR
03353       double min_idl;
03354       int vertex_index;
03355       
03356       //get min_idl:
03357       for (i=0; i<ocurr_max; i++){
03358         
03359         vertex_index = RL.id_neighbors [ tab_idx_max [i] ];     
03360         if (i==0){min_idl = inst_idleness [vertex_index];} //initialization
03361         
03362         if (inst_idleness [vertex_index] < min_idl){
03363           min_idl = inst_idleness [vertex_index];
03364         }
03365       }
03366       
03367       //tnh min_idl mas n sei num d ocurr
03368       int ocurr_idl_min = 0;
03369       int tab_idl_min [ocurr_max];
03370       
03371       //get num ocurr of min_idl:
03372       for (i=0; i<ocurr_max; i++){
03373         vertex_index = RL.id_neighbors [ tab_idx_max [i] ]; //guardar idx da vertex_web / inst_idleness
03374         
03375         if (inst_idleness [vertex_index] == min_idl){
03376           tab_idl_min[ocurr_idl_min] = tab_idx_max [i]; // guardar idx da RL.id_neighbors table
03377           ocurr_idl_min++;
03378         }
03379       }
03380       
03381       //SIGNs:
03382       for (i=0; i<ocurr_idl_min; i++){
03383         
03384         vertex_index = RL.id_neighbors [ tab_idl_min [i] ];
03385         SIGN_tab [ tab_idl_min[i] ] = -1;  
03387         //ROS_INFO("SIGN(%d)[v=%d] = %d", tab_idl_min[i], vertex_index, SIGN_tab [ tab_idl_min[i] ]);
03388       }      
03389       
03390       
03391       
03392     }
03393     
03394   
03395   /*if (node_min == node_max){
03396     //TODOS SAO IGUAIS É PRECISO VER IDLENESS MAX E MIN
03397     ROS_WARN ("node_min = node_max");
03398   }*/
03399   
03400   int hist_idx_next_vertex, vertex_index;
03401   
03402   double reward_inc = (1.0 - RL.entropy);
03403   //write_reward_evolution(reward_inc, robotid); //apenas para monitorizar a dimensão do valor
03404   
03405   for (i=0; i<RL.num_possible_neighs; i++){
03406     
03407     vertex_index = RL.id_neighbors[i];   
03408     
03409     //ROS_INFO("SIGN(%d)[v=%d] = %d", i, vertex_index, SIGN_tab[i]);   
03410 
03411     hist_idx_next_vertex = get_hist_idx (source, destination, RL.current_vertex, vertex_index, hist_dimension);
03412     
03413     reward_inc = ((double)SIGN_tab[i])*(1.0 - RL.entropy);
03414     
03415     //ROS_WARN("real_histogram [e(%d,%d)] = (%f) + (%f) = %f", RL.current_vertex, vertex_index, real_histogram [hist_idx_next_vertex], reward_inc, real_histogram [hist_idx_next_vertex]+reward_inc);
03416    
03417     //Update histogram:
03418     real_histogram [hist_idx_next_vertex] += reward_inc;  
03419     
03420     //Truncation:
03421     if (real_histogram [hist_idx_next_vertex] < 0.5){
03422       real_histogram [hist_idx_next_vertex] = 0.5;
03423     }
03424     
03425     //maximum real histogram value = 20.0:
03426     if (real_histogram [hist_idx_next_vertex] > 2.0){
03427       real_histogram [hist_idx_next_vertex] = 2.0;
03428     }      
03429     
03430   } 
03431   
03432   
03433   //int minimum_global_node_count = get_min(node_count_table, dimension);
03434     
03436    /*if (RL.node_count[id_next_vertex] > 0 && RL.node_count[id_next_vertex] == minimum_global_node_count){ 
03437      SIGN = 5;  //Dimension this as a parameter (alfa) in the reward funcion
03438      strong_reward = 1.0;
03439      ROS_INFO("node_count = minimum_global_node_count, SIGN = %d",SIGN);
03440      ROS_WARN("STRONG REWARD!!!!!!!!!!! Vertex with minimum global node count"); 
03441    } */  
03442 
03443 }
03444 
03445 int learning_algorithm(uint current_vertex, vertex *vertex_web, double *instantaneous_idleness, double *avg_idleness, int *tab_intention, double *histogram, uint *source, uint *destination, uint hist_dimension, int nr_robots, int id_robot, uint *node_count, reinforcement_learning &RL){
03446   
03447     RL.current_vertex = current_vertex;
03448     int next_vertex; //result of the decision 
03449     
03450     //ROS_INFO("current_vertex: %d", current_vertex);
03451 
03452     // FIRST STEP: FILTER POSSIBLE NEIGHBORS - REMOVE THOSE UNDER INTENTION OF OTHER ROBOTS 
03453     uint num_neighs = vertex_web[current_vertex].num_neigh;
03454     uint neighbors [num_neighs];    
03455     uint num_possible_neighs = 0;
03456     int id_neigh, count;
03457     uint i;
03458     bool all_neigh_occupied = false;
03459 
03460      for (i=0; i<num_neighs; i++){
03461           
03462       id_neigh = vertex_web[current_vertex].id_neigh[i];
03463       //ROS_INFO("Possible Neighbor: %d", id_neigh);
03464       count = count_intention_cbls (id_neigh, tab_intention, nr_robots, id_robot);
03465           
03466       if (count==0){
03467           //save possible neighbor:
03468           neighbors [num_possible_neighs] =  id_neigh;
03469           RL.id_neighbors [num_possible_neighs] = id_neigh;
03470           num_possible_neighs++;
03471            
03472       } //else{ignore neighbor: some robot is going there}
03473       
03474      }
03475      
03476      RL.num_possible_neighs = num_possible_neighs;
03477      
03478      
03479     // SECOND STEP: CALCULATE POSTERIOR PROBABILITY OF NEIHBORS ACCORDING TO PRIOR AND LIKELIHOOD DIST 
03480     
03481     if (num_possible_neighs == 0){
03482       //OLD BEHAVIOR: next_vertex = current_vertex (No decision: Wait)
03483       
03484       //NEW BEHAVIOR:
03485       //All neighbors are occupied so choose 1 between all of them:
03486       //fill "neighbors" table and "num_possible_neighs" again:
03487       
03488       
03489       for (i=0; i<num_neighs; i++){
03490         neighbors[i] = vertex_web[current_vertex].id_neigh[i];
03491         RL.id_neighbors [i] = neighbors[i];
03492       }
03493       
03494       num_possible_neighs = num_neighs;
03495       RL.num_possible_neighs = num_possible_neighs;
03496       all_neigh_occupied = true; 
03497       
03498     }
03499     
03500     if (num_possible_neighs == 1){ //Only one possible decision      
03501       next_vertex = neighbors [0];
03502       
03503     }else if (num_possible_neighs > 1){ //In this case there are several possible decisions: Calculate Posterior & then Punish / Reward
03504 
03505       //Calculate Common Prior Denominator:
03506       double prior_denominator = 0.0;
03507       
03508       //For Lookahead prior we need:
03509       double w_first_layer = 0.75;
03510       double max_idl_scnd_layer [num_possible_neighs];
03511       double prior_tab [num_possible_neighs];      
03512       
03513       bool prior_with_lookahead = true;
03514       
03515       if (!prior_with_lookahead){  
03516         
03518         for (i=0; i<num_possible_neighs; i++){
03519           //prior_denominator += instantaneous_idleness [ neighbors[i] ];
03520           prior_denominator += avg_idleness [ neighbors[i] ];
03521         }       
03522         //ROS_INFO("prior_denominator: %f", prior_denominator);  
03523         
03524       }else{    
03525         int j;
03526         //save each prior in a table "prior_tab"
03527         for (i=0; i<num_possible_neighs; i++){    
03528           int id_v = neighbors[i];
03529           max_idl_scnd_layer[i] = 0.0;
03530           
03531           for (j=0; j<vertex_web[id_v].num_neigh; j++) { //2nd layer (lookahead)
03532 
03533             if (vertex_web[id_v].num_neigh==1){
03534               prior_tab [i] = avg_idleness[id_v];
03535               
03536             }else{
03537             
03538               int id_neigh_scnd_layer = vertex_web[id_v].id_neigh[j];
03539               count = count_intention_cbls (id_neigh_scnd_layer, tab_intention, nr_robots, id_robot);
03540               
03541               //ignore those intended by other robots
03542               if ( all_neigh_occupied == false && count == 0 ){
03543                 
03544                 //ROS_INFO("avg_idleness[v=%d(viz=%i)] = %f", id_neigh_scnd_layer, id_v, avg_idleness [id_neigh_scnd_layer]);
03545 
03546                 //save max avg idl among neighbors
03547                 if (avg_idleness [id_neigh_scnd_layer] > max_idl_scnd_layer [i]){
03548                   max_idl_scnd_layer [i] = avg_idleness [id_neigh_scnd_layer];          
03549                 }             
03550               }
03551             
03552             }
03553           } //end for
03554           
03555           if (max_idl_scnd_layer[i]<=0.001){  //many robots may be near...
03556            prior_tab[i] = avg_idleness[id_v];
03557            //ROS_INFO("prior_tab [%d] = %f", i, prior_tab[i]);
03558             
03559           }else{            
03560             //ROS_INFO("avg_idleness[v=%d] = %f", id_v, avg_idleness[id_v]);
03561             //ROS_INFO("max_idl_scnd_layer = %f", max_idl_scnd_layer[i]);
03562             prior_tab [i] = (w_first_layer*avg_idleness[id_v]) + ((1.0-w_first_layer)*max_idl_scnd_layer[i]);
03563             //ROS_INFO("prior_tab [%d] = %f", i, prior_tab[i]);
03564           }
03565           //calcular o denominador 
03566           prior_denominator += prior_tab [i];
03567         }
03568         //ROS_INFO("prior_denominator = %f", prior_denominator);
03569         
03570         
03571       } //else lookahead prior
03572       
03573       //calculate Bayes rule (Prior * Likelihood):
03574       double posterior_probability [num_possible_neighs];
03575       double norm_posterior_probability [num_possible_neighs];
03576       double max_pp= -1;
03577       uint possibilities[num_neighs];     
03578       uint hits=0; 
03579       double posterior_sum = 0.0;
03580       double entropy = 0.0;
03581       
03582       for (i=0; i<num_possible_neighs; i++){    
03583         //prior:
03584         double prior; 
03585         
03586         if (prior_denominator <= 0.001){ //in the beginning all idlenesses are zero.
03587           prior = 1.0 / (double) num_possible_neighs;
03588           
03589         }else{
03590           //prior = instantaneous_idleness [ neighbors[i] ] / prior_denominator;  // [0-1]
03591           
03592           if (!prior_with_lookahead){  
03593             prior = avg_idleness [ neighbors[i] ] / prior_denominator;  // [0-1]
03594             
03595           }else{        
03596             prior = prior_tab[i] / prior_denominator;     
03597           }
03598         }
03599         
03600         //ROS_INFO("Prior(v=%d) = %f", neighbors[i], prior);
03601         
03602         
03603         //likelihood:
03604         
03605         /*double edge_cost = get_edge_cost_between (vertex_web, current_vertex, neighbors[i]);
03606         if (edge_cost < 0.0){
03607           ROS_ERROR("learning_algorithm(): edge_cost = -1");
03608           return -1;
03609         }
03610         
03611         int idx_edge = get_hist_idx_from_edge_cost (hist_sort, number_of_edges, edge_cost);
03612         if (idx_edge < 0) {
03613           ROS_ERROR("learning_algorithm(): idx_edge = -1");
03614           return -1;
03615         }*/
03616         
03617         int idx_edge = get_hist_idx (source, destination, current_vertex, neighbors[i], hist_dimension);
03618         if (idx_edge < 0) {
03619           ROS_ERROR("learning_algorithm(): idx_edge = -1");
03620           return -1;
03621         }
03622         
03623         double likelihood = histogram[idx_edge];  // [0-1]
03624         
03625         //ROS_INFO("Edge (%d - %d) has idx %d", current_vertex, neighbors[i], idx_edge);
03626         //ROS_INFO("Likelihood(v=%d, edge=%d) = %f", neighbors[i], idx_edge, likelihood);
03627         
03628         //posterior:
03629         posterior_probability[i] = prior * likelihood;
03630         posterior_sum += posterior_probability[i]; //get normalizing factor
03631         
03632         RL.node_count[i] = node_count [ neighbors[i] ];
03633         RL.idleness_old[i] = instantaneous_idleness [ neighbors[i] ];
03634         
03635         //choose the one with maximum posterior_probability:
03636         if (posterior_probability[i] > max_pp){
03637           max_pp = posterior_probability[i];
03638           
03639           hits=0;
03640           possibilities[hits] = neighbors[i];
03641           
03642         }else if (posterior_probability[i] == max_pp){
03643           hits ++;
03644           possibilities[hits] = neighbors[i];
03645         } 
03646         
03647       }
03648       
03649       // Normalize Posterior:      
03650       for (i=0; i<num_possible_neighs; i++){    
03651         norm_posterior_probability[i] = posterior_probability[i] / posterior_sum;       
03652         //ROS_INFO("PP(v=%d) = %f", neighbors[i], norm_posterior_probability[i]);
03653         entropy += norm_posterior_probability[i] * log2 (norm_posterior_probability[i]);        
03654       }
03655       
03657       entropy = -entropy;
03658       //ROS_INFO("Decision Entropy = %f", entropy);
03659       
03661       double equiv_prob = 1.0/(double)num_possible_neighs;
03662       //ROS_INFO("equiv_prob = %f", equiv_prob);
03663       double entropy_max = -((double)num_possible_neighs)*( equiv_prob * log2(equiv_prob) );      
03664       //ROS_INFO("Max Entropy (%d decisions) = %f", num_possible_neighs, entropy_max);
03665       
03666       RL.entropy = entropy / entropy_max;
03667       
03668       if (RL.entropy > 1.0) {RL.entropy = 1.0;}
03669       //ROS_INFO("Normalized Entropy = %f", RL.entropy);
03670       
03671       //there may be more than one "correct" decision: choose closer vertex
03672       if(hits>0){
03673 
03674         //ROS_INFO("More than one possibility: Choose closer vertex");
03675         int possible_vertex = possibilities[0];
03676         int id_neigh_possible_vertex = -1;      
03677               
03678         for (i=0; i<num_neighs; i++){      
03679           if (vertex_web[current_vertex].id_neigh[i] == possible_vertex){
03680             id_neigh_possible_vertex = i;
03681           }
03682         }       
03683               
03684         int min_edge = vertex_web[current_vertex].cost[id_neigh_possible_vertex];
03685         int chosen=0;
03686               
03687         //printf("hits: %d\n", hits);   
03688         //printf("Edge (%d): %d\n", possible_vertex, min_edge);
03689               
03690         int j;
03691               
03692         for (i=1; i<=hits; i++){          
03693           possible_vertex=possibilities[i];
03694                 
03695           id_neigh_possible_vertex = -1;        
03696                         
03697                 for (j=0; j<num_neighs; j++){      
03698                         if (vertex_web[current_vertex].id_neigh[j] == possible_vertex){
03699                             id_neigh_possible_vertex = j;
03700                         }
03701                 }         
03702                 
03703                 //printf("Edge (%d): %d\n", possible_vertex, vertex_web[current_vertex].cost[id_neigh_possible_vertex]);
03704                 
03705                 if (vertex_web[current_vertex].cost[id_neigh_possible_vertex] <= min_edge){
03706                   min_edge = vertex_web[current_vertex].cost[id_neigh_possible_vertex];
03707                   chosen = i;
03708                 }
03709                 
03710          }      
03711                 
03712          //printf("rand integer = %d\n", i);
03713          next_vertex = possibilities [chosen];          // closest vertex 
03714       
03715         
03716       }else{
03717         next_vertex = possibilities[0];
03718       }
03719 
03720       
03721     }
03722     
03723     RL.time_then = ros::Time::now().toSec();
03724     RL.next_vertex = next_vertex;    
03725     return next_vertex;
03726 
03727 }


patrolling_sim
Author(s):
autogenerated on Thu Jun 6 2019 20:27:09