/home/slemaign/softs-local/BioMove3D-git/include/roadmap.h

#ifndef _ROADMAP_H
#define _ROADMAP_H

#include "stat.h" // Statistic module; Commit Jim; date: 01/10/2008
#include <vector>

typedef struct node {
  int         type; /* type du noeud (isole,...) */
  int         num; /* numero du noeud dans le graphe */
  int         numcomp; /* numero de la composante connexe du noeud */
  struct compco *comp;
  configPt q; /* la configuration du robot */
  double* coldeg_q; /*configuration in the space of the collective degrees */ // Modif Juan
  double      dq; /* le rayon de la boule */
  int *iksol;  /* index of the solution generated by the cntrt (modif Juan) */
  double dqmin;
  int nneighb;    /* nombre de voisins dans le graphe */
  struct list_node *neighb; /* voisins du noeud dans le graphe */
  struct list_node *last_neighb;
  int nedge;    /* nombre d'arete partant du noeud dans le graphe */
  struct list_edge *edges; /* aretes reliant le noeud a ses voisins */
  struct list_edge *last_edge; /* aretes reliant le noeud a ses voisins */
  double dist_Nnew; /* distance au noeud courant cree */
  /* for graph exploration Astar*/
  double f;                /*g+h*/
  double g;                /*Real cost from the init node*/
  double h;                /*heuristic cost to goal node*/
  short opened;            /* whether node is open in A* search */
  short closed;            /* whether node is closed in A* search */
  struct node *search_from; /* parent node in A* search */
  struct node *search_to;  /* child node in A* search */
  struct edge *edge_from;  /* edge connecting to node search from */
  int n_fail_extend;       // modif Juan (test)
  int n_extend;            // modif Juan (test)
  double weight;           // modif Juan (test)
  /* for graph search DFS*/
  int discovered;
  int processed;

  /* cost of a node according to the cost function in a space */
  double cost;

  /* total cost from the root node to the current node in the tree
     graph (for diffusion techniques) */
  double sumCost;

  /* Position of the node in the tree starting from the root node. The
     goal nodes have a rank of 1 and all the sons of a node of rank n
     have a rank of n+1 */
  int rankFromRoot;

  /* Flag TRUE if a node is discarded for the selection as nearest
     neighbor*/
  int IsDiscarded;

 /* Used in cost space problems. Current node temperature. */
  double temp;

  /*Used in cost space problems. Number of consecutive times the node
    failed to be extended due to the temperature in the cost test */
  int nbFailedTemp;

  /* Used in cost space problems. Number of consecutive times the node
     create a son with a cost inferior than its own cost.
     Warning: this variable is currently no more used*/
  // int NbDown;

  /* Used in the p3d_OrderingGraphSearch algorithm,
     variant of the astar algorithm that is used if the flag
     GlobalOrdering is set to TRUE*/
  dbl_list* orderCostListEdge;


  double avr_dist;         // modif Noureddine
  double min_dist;         // modif Noureddine
  double E;                // modif Noureddine
  struct node *parent;     // modif Juan (test)
  p3d_matrix4 RelMobFrame;
  double radius; /* modif Leonard */
  int pinpointed;          // flag to indicate pinpointed node (for graph displaying)
  int boundary; /* say if the node is a boundary node or not (Leonard) */

  int is_dist_sc_ligand_checked;
  int* list_closed_flex_sc;   //table of the size of nb flexible sc.
                              // list_closed_flex_sc[i] = 1 if the ligand and sc are closed,
                              // 0 otherwise
  int isSingularity;
  //start path deform
  int only_for_cycle; /*flag for nodes used in cycling graphs */
  int visible;
  //end path deform

  p3d_vector3 g3d_position; // 3d position of the node when drawing the graph
  bool g3d_position_flag;
#ifdef MULTIGRAPH
  int mergeState; //The merge state of the node: 0 None, 1 trajectory, 2 All
  int needMgCycle;
#endif

} p3d_node, *pp3d_node;

typedef struct list_node{
  p3d_node *N;
  struct list_node *next;
  struct list_node *prev;
} p3d_list_node;


/* Structure d'arete du graphe */
typedef struct edge {
  p3d_node  *Ni, *Nf;
  p3d_localpath *path;
  p3d_localpath_type planner;
  double     longueur;
  double cost;
  int sens_edge;
  //start path deform
  int visible;
  int unvalid;
  int for_cycle;
  //end path deform
} p3d_edge;

typedef struct list_edge{
  p3d_edge *E;
  struct list_edge *next;
  struct list_edge *prev;
} p3d_list_edge;

/* Structure de composante connexe */
typedef struct compco {
  int num; /* numero de la composante connexe */
  int nnode; /* nombre de noeuds dans la composante connexe */
  p3d_list_node *nodes;  /* noeuds faisant partie de la composante connexe */
  p3d_list_node *dist_nodes;/* noeuds de la composante connexe pour le tri par distance */
  p3d_list_node *last_node; /* dernier noeud de la composante */
  struct compco *suiv;
  struct compco *prec;
  int ncanreach;
  int nbRefinNodes;
  double temperature;
  struct list_compco * canreach;
  struct list_compco * last_canreach;
  configPt box_env_small[2];
  double minCost;   /*Used in cost space problems. Minimal
                      cost actually found in the connect componant*/
  double maxCost;   /*Used in cost space problems. Maximal
                      cost actually found in the connect componant*/
  double maxUrmsonCost; /*Used in cost space problems (Urmson variant). Maximal
                       sum cost actually found in the connect componant*/
  int* AnaSuccessTab;
  int nbTests;
} p3d_compco;

typedef struct list_compco{
  struct compco *comp;
  struct list_compco *next;
  struct list_compco *prev;
} p3d_list_compco;


/* Structure d'exterma pour classer les paves selon les ddl */
typedef struct ddlbox{
  int num; /* numero du noeud auquel ce ddl se rattache */
  char   min_max; /* 0 si c'est un min, 1 si c'est un max */
  double val; /* valeur du min ou du max */
  struct ddlbox *next;
  struct ddlbox *prev;
} p3d_ddlbox;

/* Structure de graphe */
typedef struct graph {
  p3d_env *env; /* environnement auquel le graphe se rattache */
  p3d_rob *rob; /* robot auquel le graphe se rattache */
  char * file; /* nom du fichier de sauvegarde du graphe */
  int oriented;

  int nnode; /* nombre de noeuds dans le graphe */
  p3d_list_node *nodes; /* noeuds du graphe */
  p3d_list_node *last_node; /* dernier noeud du graphe */
  int nedge; /*nombre d'aretes */
  p3d_list_edge *edges; /* aretes */
  p3d_list_edge *last_edge;
  int ncomp; /* nombre de composantes connexes */
  p3d_compco *comp; /*liste des composantes connexes */
  p3d_compco *last_comp;
  p3d_ddlbox **ddlbox; /* liste des intervalles des paves sur les ddl */
  unsigned long int hhCount; /* number of the next point of the Halton Set */
  int nb_standart_nodes; /*Number of nodes built by standart-RRT*/
  int nb_DD_nodes;       /*Number of nodes built by DD-RRT*/
  int nboundary; /* number of boundary nodes */
  int n_call_nearest; /*number  of call to the nearest neighbour function */

  int IsCurrentNodeBias; /* Flag to tell if the current
                            direction of expansion is given by a node
                            (used to avoid the superposition of 2 nodes) */
  p3d_node*  NodeBiasPt;
  int        arc_type;
  int        arc_value;
  p3d_node   *search_start;
  p3d_node   *search_goal;
  p3d_node   *search_goal_sol;
  int        search_done;
  int        search_path;
  int        search_numdev;
  double     search_cost;
  int        search_path_length;

  double time;
#ifdef MULTIGRAPH
  double mgTime;
#endif
#ifdef DPG
  class DpgGrid * dpgGrid;
#endif

        // Planning on graph stat
        double totTime;
        double optTime;
  double rrtTime;
  double rrtCost1;
  double rrtCost2;

  int nb_test_BB;
  int nb_test_coll;
  int nb_test_energy;  //mod noureddine
  int nb_local_call;
  int nb_q;
  int nb_q_free;
  int nb_bkb_q_free;
  int nb_q_closed;

  int ** usedIkSols; //List of iksol in the graph
  int nbUsedIkSols; //The number of item in the list
  //start path deform
  p3d_list_node *dist_nodes ;/* noeuds du graph pour un tri global par distance */
  p3d_node* start_nodePt;
  p3d_node* last_nodePt;
  p3d_node* prev_nodePt;
  p3d_traj* traj1Pt;
  p3d_traj* traj2Pt;
  //end path deform

  double critic_cur_pb_level;
  int n_consec_fail_pb_level;
  int n_consec_pb_level;
  double CurPbLevel;
  p3d_vector3 g3d_position; // 3d position of the node when drawing the graph
  bool g3d_position_flag;
  p3d_jnt* g3d_drawnjnt; // used when drawing the graph

  struct p3d_statistics * stat; /* Statistic data structure of the algorithm
                                                  associated with the graph; Commit Jim; date: 01/10/2008 */

} p3d_graph, *pp3d_graph;


//start path deform
typedef struct project_point {
  int i;
  int j;
  int in_test;
  int is_valid;
  double cost;
}p3d_project_point;

typedef struct path_nodes {
  dbl_list* list_node; // liste de noeuds
  /*p3d_node*/void* N; // Le noeud
  double f;
  double g;
  double h;
  int opened;
  int closed;
  int nnode;// nombre de noeuds dans list_node
}p3d_path_nodes;
//end path deform

#ifdef MULTIGRAPH

typedef struct fsgListNode{
  struct flatSuperGraphNode * node;
  struct fsgListNode * prev;
  struct fsgListNode * next;
}p3d_fsgListNode;

typedef struct fsgListEdge{
  struct flatSuperGraphEdge * edge;
  struct fsgListEdge * prev;
  struct fsgListEdge * next;
}p3d_fsgListEdge;

typedef struct flatSuperGraphNode{
  int num;
  int nNodes;
  configPt q;
  p3d_node * mergedNode;
  p3d_node ** nodes;
  int nEdges;
  p3d_fsgListEdge * fsgEdges;

  /************* Astar Variables****************/
  double h;
  double g;
  double f;
  int opened;
  int closed;
  struct flatSuperGraphNode *search_from;
  struct flatSuperGraphNode *search_to;
  struct flatSuperGraphEdge *edge_from;
  dbl_list * orderCostListEdge;
}p3d_flatSuperGraphNode;

typedef struct flatSuperGraphEdge{
  struct flatSuperGraphNode * node1;
  struct flatSuperGraphNode * node2;
  /************* Astar Variables****************/
  double cost;
}p3d_flatSuperGraphEdge;

typedef struct flatSuperGraph{
  int nNodes;
  p3d_fsgListNode * nodes;
  int nEdges;
  p3d_fsgListEdge * edges;
  std::vector<p3d_flatSuperGraphEdge *> autoColNodes;

  /************* Astar Variables****************/
  struct flatSuperGraphNode * search_start;
  struct flatSuperGraphNode   *search_goal;
  int search_done;
  int search_numdev;
  int search_path;
  struct flatSuperGraphNode *search_goal_sol;
  double search_cost;
  int search_path_length;
}p3d_flatSuperGraph;
#endif //MULTIGRAPH

#endif