From fbb0f108b1ae0f2e33709d6553f49c25202c896b Mon Sep 17 00:00:00 2001
From: dave <david.st-onge@polymtl.ca>
Date: Tue, 4 Dec 2018 10:34:07 -0500
Subject: [PATCH] beautified and style fixes

---
 include/VoronoiDiagramGenerator.h |  290 +++---
 include/buzz_update.h             |  232 ++---
 include/buzz_utility.h            |    9 +-
 include/buzzuav_closures.h        |    4 +-
 include/roscontroller.h           |   14 +-
 readme.md                         |   99 +-
 src/VoronoiDiagramGenerator.cpp   | 1551 +++++++++++++++--------------
 src/buzz_update.cpp               | 1281 ++++++++++++------------
 src/buzz_utility.cpp              |   14 +-
 src/buzzuav_closures.cpp          |  691 +++++++------
 src/roscontroller.cpp             |  198 ++--
 11 files changed, 2239 insertions(+), 2144 deletions(-)

diff --git a/include/VoronoiDiagramGenerator.h b/include/VoronoiDiagramGenerator.h
index 4491a42..9876acf 100644
--- a/include/VoronoiDiagramGenerator.h
+++ b/include/VoronoiDiagramGenerator.h
@@ -12,9 +12,9 @@
 * OF THIS SOFTWARE OR ITS FITNESS FOR ANY PARTICULAR PURPOSE.
 */
 
-/* 
-* This code was originally written by Stephan Fortune in C code.  I, Shane O'Sullivan, 
-* have since modified it, encapsulating it in a C++ class and, fixing memory leaks and 
+/*
+* This code was originally written by Stephan Fortune in C code.  I, Shane O'Sullivan,
+* have since modified it, encapsulating it in a C++ class and, fixing memory leaks and
 * adding accessors to the Voronoi Edges.
 * Permission to use, copy, modify, and distribute this software for any
 * purpose without fee is hereby granted, provided that this entire notice
@@ -36,7 +36,6 @@
 #include <stdio.h>
 #include <algorithm>
 
-
 #ifndef NULL
 #define NULL 0
 #endif
@@ -45,210 +44,195 @@
 #define le 0
 #define re 1
 
-
-
-struct	Freenode	
+struct Freenode
 {
-	struct	Freenode *nextfree;
+  struct Freenode* nextfree;
 };
 
 struct FreeNodeArrayList
 {
-	struct	Freenode* memory;
-	struct	FreeNodeArrayList* next;
-
+  struct Freenode* memory;
+  struct FreeNodeArrayList* next;
 };
 
-struct	Freelist	
+struct Freelist
 {
-	struct	Freenode	*head;
-	int		nodesize;
+  struct Freenode* head;
+  int nodesize;
 };
 
-struct Point	
+struct Point
 {
-	float x,y;
-    Point(): x( 0.0 ), y( 0.0 ) { }
-    Point( float x, float y ): x( x ), y( y ) { }
+  float x, y;
+  Point() : x(0.0), y(0.0)
+  {
+  }
+  Point(float x, float y) : x(x), y(y)
+  {
+  }
 };
 
-// structure used both for sites and for vertices 
-struct Site	
+// structure used both for sites and for vertices
+struct Site
 {
-	struct	Point	coord;
-	int		sitenbr;
-	int		refcnt;
+  struct Point coord;
+  int sitenbr;
+  int refcnt;
 };
 
-
-
-struct Edge	
+struct Edge
 {
-	float   a,b,c;
-	struct	Site 	*ep[2];
-	struct	Site	*reg[2];
-	int		edgenbr;
-	int		sites[2];
-
+  float a, b, c;
+  struct Site* ep[2];
+  struct Site* reg[2];
+  int edgenbr;
+  int sites[2];
 };
 
 struct GraphEdge
 {
-	float x1,y1,x2,y2;
-	int sites[2];
-	struct GraphEdge* next;
+  float x1, y1, x2, y2;
+  int sites[2];
+  struct GraphEdge* next;
 };
 
-
-
-
-struct Halfedge 
+struct Halfedge
 {
-	struct	Halfedge	*ELleft, *ELright;
-	struct	Edge	*ELedge;
-	int		ELrefcnt;
-	char	ELpm;
-	struct	Site	*vertex;
-	float	ystar;
-	struct	Halfedge *PQnext;
+  struct Halfedge *ELleft, *ELright;
+  struct Edge* ELedge;
+  int ELrefcnt;
+  char ELpm;
+  struct Site* vertex;
+  float ystar;
+  struct Halfedge* PQnext;
 };
 
-
-
-
 class VoronoiDiagramGenerator
 {
 public:
-	VoronoiDiagramGenerator();
-	~VoronoiDiagramGenerator();
+  VoronoiDiagramGenerator();
+  ~VoronoiDiagramGenerator();
 
-	bool generateVoronoi(float *xValues, float *yValues, int numPoints, float minX, float maxX, float minY, float maxY, float minDist=0);
+  bool generateVoronoi(float* xValues, float* yValues, int numPoints, float minX, float maxX, float minY, float maxY,
+                       float minDist = 0);
 
-	void resetIterator()
-	{
-		iteratorEdges = allEdges;
-	}
+  void resetIterator()
+  {
+    iteratorEdges = allEdges;
+  }
 
-	bool getNext(float& x1, float& y1, float& x2, float& y2, int *s)
-	{
-		if(iteratorEdges == 0)
-			return false;
-		
-		x1 = iteratorEdges->x1;
-		x2 = iteratorEdges->x2;
-		y1 = iteratorEdges->y1;
-		y2 = iteratorEdges->y2;
-		std::copy(iteratorEdges->sites, iteratorEdges->sites+2, s);
+  bool getNext(float& x1, float& y1, float& x2, float& y2, int* s)
+  {
+    if (iteratorEdges == 0)
+      return false;
 
-		iteratorEdges = iteratorEdges->next;
+    x1 = iteratorEdges->x1;
+    x2 = iteratorEdges->x2;
+    y1 = iteratorEdges->y1;
+    y2 = iteratorEdges->y2;
+    std::copy(iteratorEdges->sites, iteratorEdges->sites + 2, s);
 
-		return true;
-	}
+    iteratorEdges = iteratorEdges->next;
 
+    return true;
+  }
 
 private:
-	void cleanup();
-	void cleanupEdges();
-	char *getfree(struct Freelist *fl);	
-	struct	Halfedge *PQfind();
-	int PQempty();
+  void cleanup();
+  void cleanupEdges();
+  char* getfree(struct Freelist* fl);
+  struct Halfedge* PQfind();
+  int PQempty();
 
+  struct Halfedge** ELhash;
+  struct Halfedge *HEcreate(), *ELleft(), *ELright(), *ELleftbnd();
+  struct Halfedge* HEcreate(struct Edge* e, int pm);
 
-	
-	struct	Halfedge **ELhash;
-	struct	Halfedge *HEcreate(), *ELleft(), *ELright(), *ELleftbnd();
-	struct	Halfedge *HEcreate(struct Edge *e,int pm);
+  struct Point PQ_min();
+  struct Halfedge* PQextractmin();
+  void freeinit(struct Freelist* fl, int size);
+  void makefree(struct Freenode* curr, struct Freelist* fl);
+  void geominit();
+  void plotinit();
+  bool voronoi(int triangulate);
+  void ref(struct Site* v);
+  void deref(struct Site* v);
+  void endpoint(struct Edge* e, int lr, struct Site* s);
 
+  void ELdelete(struct Halfedge* he);
+  struct Halfedge* ELleftbnd(struct Point* p);
+  struct Halfedge* ELright(struct Halfedge* he);
+  void makevertex(struct Site* v);
+  void out_triple(struct Site* s1, struct Site* s2, struct Site* s3);
 
-	struct Point PQ_min();
-	struct Halfedge *PQextractmin();	
-	void freeinit(struct Freelist *fl,int size);
-	void makefree(struct Freenode *curr,struct Freelist *fl);
-	void geominit();
-	void plotinit();
-	bool voronoi(int triangulate);
-	void ref(struct Site *v);
-	void deref(struct Site *v);
-	void endpoint(struct Edge *e,int lr,struct Site * s);
+  void PQinsert(struct Halfedge* he, struct Site* v, float offset);
+  void PQdelete(struct Halfedge* he);
+  bool ELinitialize();
+  void ELinsert(struct Halfedge* lb, struct Halfedge* newHe);
+  struct Halfedge* ELgethash(int b);
+  struct Halfedge* ELleft(struct Halfedge* he);
+  struct Site* leftreg(struct Halfedge* he);
+  void out_site(struct Site* s);
+  bool PQinitialize();
+  int PQbucket(struct Halfedge* he);
+  void clip_line(struct Edge* e);
+  char* myalloc(unsigned n);
+  int right_of(struct Halfedge* el, struct Point* p);
 
-	void ELdelete(struct Halfedge *he);
-	struct Halfedge *ELleftbnd(struct Point *p);
-	struct Halfedge *ELright(struct Halfedge *he);
-	void makevertex(struct Site *v);
-	void out_triple(struct Site *s1, struct Site *s2,struct Site * s3);
+  struct Site* rightreg(struct Halfedge* he);
+  struct Edge* bisect(struct Site* s1, struct Site* s2);
+  float dist(struct Site* s, struct Site* t);
+  struct Site* intersect(struct Halfedge* el1, struct Halfedge* el2, struct Point* p = 0);
 
-	void PQinsert(struct Halfedge *he,struct Site * v, float offset);
-	void PQdelete(struct Halfedge *he);
-	bool ELinitialize();
-	void ELinsert(struct	Halfedge *lb, struct Halfedge *newHe);
-	struct Halfedge * ELgethash(int b);
-	struct Halfedge *ELleft(struct Halfedge *he);
-	struct Site *leftreg(struct Halfedge *he);
-	void out_site(struct Site *s);
-	bool PQinitialize();
-	int PQbucket(struct Halfedge *he);
-	void clip_line(struct Edge *e);
-	char *myalloc(unsigned n);
-	int right_of(struct Halfedge *el,struct Point *p);
+  void out_bisector(struct Edge* e);
+  void out_ep(struct Edge* e);
+  void out_vertex(struct Site* v);
+  struct Site* nextone();
 
-	struct Site *rightreg(struct Halfedge *he);
-	struct Edge *bisect(struct	Site *s1,struct	Site *s2);
-	float dist(struct Site *s,struct Site *t);
-	struct Site *intersect(struct Halfedge *el1, struct Halfedge *el2, struct Point *p=0);
+  void pushGraphEdge(float x1, float y1, float x2, float y2, int s[2]);
 
-	void out_bisector(struct Edge *e);
-	void out_ep(struct Edge *e);
-	void out_vertex(struct Site *v);
-	struct Site *nextone();
+  void openpl();
+  void line(float x1, float y1, float x2, float y2, int s[2]);
+  void circle(float x, float y, float radius);
+  void range(float minX, float minY, float maxX, float maxY);
 
-	void pushGraphEdge(float x1, float y1, float x2, float y2, int s[2]);
+  struct Freelist hfl;
+  struct Halfedge *ELleftend, *ELrightend;
+  int ELhashsize;
 
-	void openpl();
-	void line(float x1, float y1, float x2, float y2, int s[2]);
-	void circle(float x, float y, float radius);
-	void range(float minX, float minY, float maxX, float maxY);
+  int triangulate, sorted, plot, debug;
+  float xmin, xmax, ymin, ymax, deltax, deltay;
 
-	struct  Freelist	hfl;
-	struct	Halfedge *ELleftend, *ELrightend;
-	int 	ELhashsize;
+  struct Site* sites;
+  int nsites;
+  int siteidx;
+  int sqrt_nsites;
+  int nvertices;
+  struct Freelist sfl;
+  struct Site* bottomsite;
 
-	int		triangulate, sorted, plot, debug;
-	float	xmin, xmax, ymin, ymax, deltax, deltay;
+  int nedges;
+  struct Freelist efl;
+  int PQhashsize;
+  struct Halfedge* PQhash;
+  int PQcount;
+  int PQmin;
 
-	struct	Site	*sites;
-	int		nsites;
-	int		siteidx;
-	int		sqrt_nsites;
-	int		nvertices;
-	struct 	Freelist sfl;
-	struct	Site	*bottomsite;
+  int ntry, totalsearch;
+  float pxmin, pxmax, pymin, pymax, cradius;
+  int total_alloc;
 
-	int		nedges;
-	struct	Freelist efl;
-	int		PQhashsize;
-	struct	Halfedge *PQhash;
-	int		PQcount;
-	int		PQmin;
+  float borderMinX, borderMaxX, borderMinY, borderMaxY;
 
-	int		ntry, totalsearch;
-	float	pxmin, pxmax, pymin, pymax, cradius;
-	int		total_alloc;
+  FreeNodeArrayList* allMemoryList;
+  FreeNodeArrayList* currentMemoryBlock;
 
-	float borderMinX, borderMaxX, borderMinY, borderMaxY;
+  GraphEdge* allEdges;
+  GraphEdge* iteratorEdges;
 
-	FreeNodeArrayList* allMemoryList;
-	FreeNodeArrayList* currentMemoryBlock;
-
-	GraphEdge* allEdges;
-	GraphEdge* iteratorEdges;
-
-	float minDistanceBetweenSites;
-	
+  float minDistanceBetweenSites;
 };
 
-int scomp(const void *p1,const void *p2);
-
+int scomp(const void* p1, const void* p2);
 
 #endif
-
-
diff --git a/include/buzz_update.h b/include/buzz_update.h
index 4b43e86..af9d41f 100644
--- a/include/buzz_update.h
+++ b/include/buzz_update.h
@@ -23,149 +23,149 @@
   } while (0)
 namespace buzz_update
 {
-  static const uint16_t CODE_REQUEST_PADDING = 250;
-  static const uint16_t MIN_UPDATE_PACKET = 251;
-  static const uint16_t UPDATE_CODE_HEADER_SIZE = 5;
-  static const uint16_t TIMEOUT_FOR_ROLLBACK = 50;
-  /*********************/
-  /*   Updater states */
-  /********************/
+static const uint16_t CODE_REQUEST_PADDING = 250;
+static const uint16_t MIN_UPDATE_PACKET = 251;
+static const uint16_t UPDATE_CODE_HEADER_SIZE = 5;
+static const uint16_t TIMEOUT_FOR_ROLLBACK = 50;
+/*********************/
+/*   Updater states */
+/********************/
 
-  typedef enum {
-    CODE_RUNNING = 0,  // Code executing
-    CODE_STANDBY,      // Standing by for others to update
-  } code_states_e;
+typedef enum {
+  CODE_RUNNING = 0,  // Code executing
+  CODE_STANDBY,      // Standing by for others to update
+} code_states_e;
 
-  /*********************/
-  /*Message types     */
-  /********************/
+/*********************/
+/*Message types     */
+/********************/
 
-  typedef enum {
-    SENT_CODE = 0,  // Broadcast code
-    RESEND_CODE,    // ReBroadcast request
-  } code_message_e;
+typedef enum {
+  SENT_CODE = 0,  // Broadcast code
+  RESEND_CODE,    // ReBroadcast request
+} code_message_e;
 
-  /*************************/
-  /*Updater message queue */
-  /*************************/
+/*************************/
+/*Updater message queue */
+/*************************/
 
-  struct updater_msgqueue_s
-  {
-    uint8_t* queue;
-    uint8_t* size;
-  };
-  typedef struct updater_msgqueue_s* updater_msgqueue_t;
+struct updater_msgqueue_s
+{
+  uint8_t* queue;
+  uint8_t* size;
+};
+typedef struct updater_msgqueue_s* updater_msgqueue_t;
 
-  struct updater_code_s
-  {
-    uint8_t* bcode;
-    uint8_t* bcode_size;
-  };
-  typedef struct updater_code_s* updater_code_t;
+struct updater_code_s
+{
+  uint8_t* bcode;
+  uint8_t* bcode_size;
+};
+typedef struct updater_code_s* updater_code_t;
 
-  /**************************/
-  /*Updater data*/
-  /**************************/
+/**************************/
+/*Updater data*/
+/**************************/
 
-  struct buzz_updater_elem_s
-  {
-    /* robot id  */
-    // uint16_t robotid;
-    /*current Bytecode content */
-    uint8_t* bcode;
-    /*old Bytecode name */
-    const char* old_bcode;
-    /*current bcode size*/
-    size_t* bcode_size;
-    /*Update patch*/
-    uint8_t* patch;
-    /* Update patch size*/
-    size_t* patch_size;
-    /*current Bytecode content */
-    uint8_t* standby_bcode;
-    /*current bcode size*/
-    size_t* standby_bcode_size;
-    /*updater out msg queue */
-    updater_msgqueue_t outmsg_queue;
-    /*updater in msg queue*/
-    updater_msgqueue_t inmsg_queue;
-    /*Current state of the updater one in code_states_e ENUM*/
-    int* mode;
-    uint8_t* update_no;
-  };
-  typedef struct buzz_updater_elem_s* buzz_updater_elem_t;
+struct buzz_updater_elem_s
+{
+  /* robot id  */
+  // uint16_t robotid;
+  /*current Bytecode content */
+  uint8_t* bcode;
+  /*old Bytecode name */
+  const char* old_bcode;
+  /*current bcode size*/
+  size_t* bcode_size;
+  /*Update patch*/
+  uint8_t* patch;
+  /* Update patch size*/
+  size_t* patch_size;
+  /*current Bytecode content */
+  uint8_t* standby_bcode;
+  /*current bcode size*/
+  size_t* standby_bcode_size;
+  /*updater out msg queue */
+  updater_msgqueue_t outmsg_queue;
+  /*updater in msg queue*/
+  updater_msgqueue_t inmsg_queue;
+  /*Current state of the updater one in code_states_e ENUM*/
+  int* mode;
+  uint8_t* update_no;
+};
+typedef struct buzz_updater_elem_s* buzz_updater_elem_t;
 
-  /**************************************************************************/
-  /*Updater routine from msg processing to file checks to be called from main*/
-  /**************************************************************************/
-  void update_routine();
+/**************************************************************************/
+/*Updater routine from msg processing to file checks to be called from main*/
+/**************************************************************************/
+void update_routine();
 
-  /************************************************/
-  /*Initalizes the updater */
-  /************************************************/
-  int init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id);
+/************************************************/
+/*Initalizes the updater */
+/************************************************/
+int init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id);
 
-  /*********************************************************/
-  /*Appends buffer of given size to in msg queue of updater*/
-  /*********************************************************/
+/*********************************************************/
+/*Appends buffer of given size to in msg queue of updater*/
+/*********************************************************/
 
-  void code_message_inqueue_append(uint8_t* msg, uint16_t size);
+void code_message_inqueue_append(uint8_t* msg, uint16_t size);
 
-  /*********************************************************/
-  /*Processes messages inside the queue of the updater*/
-  /*********************************************************/
+/*********************************************************/
+/*Processes messages inside the queue of the updater*/
+/*********************************************************/
 
-  void code_message_inqueue_process();
+void code_message_inqueue_process();
 
-  /*****************************************************/
-  /*Obtains messages from out msgs queue of the updater*/
-  /*******************************************************/
-  uint8_t* getupdater_out_msg();
+/*****************************************************/
+/*Obtains messages from out msgs queue of the updater*/
+/*******************************************************/
+uint8_t* getupdater_out_msg();
 
-  /******************************************************/
-  /*Obtains out msg queue size*/
-  /*****************************************************/
-  uint8_t* getupdate_out_msg_size();
+/******************************************************/
+/*Obtains out msg queue size*/
+/*****************************************************/
+uint8_t* getupdate_out_msg_size();
 
-  /**************************************************/
-  /*Destroys the out msg queue*/
-  /*************************************************/
-  void destroy_out_msg_queue();
+/**************************************************/
+/*Destroys the out msg queue*/
+/*************************************************/
+void destroy_out_msg_queue();
 
-  // buzz_updater_elem_t get_updater();
-  /***************************************************/
-  /*Sets bzz file name*/
-  /***************************************************/
-  void set_bzz_file(const char* in_bzz_file, bool dbg);
+// buzz_updater_elem_t get_updater();
+/***************************************************/
+/*Sets bzz file name*/
+/***************************************************/
+void set_bzz_file(const char* in_bzz_file, bool dbg);
 
-  /****************************************************/
-  /*Tests the code from a buffer*/
-  /***************************************************/
+/****************************************************/
+/*Tests the code from a buffer*/
+/***************************************************/
 
-  int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size);
+int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size);
 
-  /****************************************************/
-  /*Destroys the updater*/
-  /***************************************************/
+/****************************************************/
+/*Destroys the updater*/
+/***************************************************/
 
-  void destroy_updater();
+void destroy_updater();
 
-  /****************************************************/
-  /*Checks for updater message*/
-  /***************************************************/
+/****************************************************/
+/*Checks for updater message*/
+/***************************************************/
 
-  int is_msg_present();
+int is_msg_present();
 
-  /****************************************************/
-  /*Compiles a bzz script to bo and bdbg*/
-  /***************************************************/
+/****************************************************/
+/*Compiles a bzz script to bo and bdbg*/
+/***************************************************/
 
-  int compile_bzz(std::string bzz_file);
+int compile_bzz(std::string bzz_file);
 
-  /****************************************************/
-  /*Set number of robots in the updater*/
-  /***************************************************/
+/****************************************************/
+/*Set number of robots in the updater*/
+/***************************************************/
 
-  void updates_set_robots(int robots);
+void updates_set_robots(int robots);
 }
 #endif
diff --git a/include/buzz_utility.h b/include/buzz_utility.h
index 31197aa..c5aa69e 100644
--- a/include/buzz_utility.h
+++ b/include/buzz_utility.h
@@ -53,8 +53,12 @@ struct neitime
   double relative_rate;
   int age;
   neitime(uint64_t nht, uint64_t nlt, uint64_t mht, uint64_t mlt, double nr, double mr)
-    : nei_hardware_time(nht), nei_logical_time(nlt), node_hardware_time(mht), node_logical_time(mlt), 
-      nei_rate(nr), relative_rate(mr) {};
+    : nei_hardware_time(nht)
+    , nei_logical_time(nlt)
+    , node_hardware_time(mht)
+    , node_logical_time(mlt)
+    , nei_rate(nr)
+    , relative_rate(mr){};
   neitime()
   {
   }
@@ -101,5 +105,4 @@ int get_inmsg_size();
 std::vector<uint8_t*> get_inmsg_vector();
 
 std::string get_bvmstate();
-
 }
diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h
index 95140dc..71a1d6a 100644
--- a/include/buzzuav_closures.h
+++ b/include/buzzuav_closures.h
@@ -22,7 +22,7 @@ namespace buzzuav_closures
 int buzzros_print(buzzvm_t vm);
 void setWPlist(std::string file);
 void setVorlog(std::string path);
-void check_targets_sim(double lat, double lon, double *res);
+void check_targets_sim(double lat, double lon, double* res);
 
 /*
  * closure to move following a vector
@@ -98,7 +98,7 @@ double* getgoto();
 /*
  * returns the current grid
  */
-std::map<int, std::map<int,int>> getgrid();
+std::map<int, std::map<int, int>> getgrid();
 int voronoi_center(buzzvm_t vm);
 
 /*
diff --git a/include/roscontroller.h b/include/roscontroller.h
index 6edb678..23ef2a1 100644
--- a/include/roscontroller.h
+++ b/include/roscontroller.h
@@ -42,14 +42,14 @@
 * ROSBuzz message types
 */
 typedef enum {
-  ROS_BUZZ_MSG_NIL = 0,    // dummy msg
-  UPDATER_MESSAGE,         // Update msg
-  BUZZ_MESSAGE,            // Broadcast message
-  BUZZ_MESSAGE_TIME,       // Broadcast message with time info
+  ROS_BUZZ_MSG_NIL = 0,  // dummy msg
+  UPDATER_MESSAGE,       // Update msg
+  BUZZ_MESSAGE,          // Broadcast message
+  BUZZ_MESSAGE_TIME,     // Broadcast message with time info
 } rosbuzz_msgtype;
 
 // Time sync algo. constants
-#define COM_DELAY 100000000 // in nano seconds i.e 100 ms
+#define COM_DELAY 100000000  // in nano seconds i.e 100 ms
 #define TIME_SYNC_JUMP_THR 500000000
 #define MOVING_AVERAGE_ALPHA 0.1
 #define MAX_NUMBER_OF_ROBOTS 10
@@ -107,8 +107,8 @@ private:
     uint16_t size;
     uint64_t sent_time;
     uint64_t received_time;
-    MsgData(int mi, uint16_t ni, uint16_t s, uint64_t st, uint64_t rt):
-            msgid(mi), nid(ni), size(s),sent_time(st), received_time(rt){};
+    MsgData(int mi, uint16_t ni, uint16_t s, uint64_t st, uint64_t rt)
+      : msgid(mi), nid(ni), size(s), sent_time(st), received_time(rt){};
     MsgData(){};
   };
   typedef struct MsgData msg_data;
diff --git a/readme.md b/readme.md
index d61cdb4..6ab29db 100644
--- a/readme.md
+++ b/readme.md
@@ -1,62 +1,43 @@
-ROS Implemenation of Buzz
+ROSBuzz
 =========================
 
-What is Buzz?
-=============
-
-Buzz is a novel programming language for heterogeneous robots swarms.
-
-Buzz advocates a compositional approach, by offering primitives to define swarm behaviors both in a bottom-up and in a top-down fashion.
-
-Bottom-up primitives include robot-wise commands and manipulation of neighborhood data through mapping/reducing/filtering operations.
-
-Top-down primitives allow for the dynamic management of robot teams, and for sharing information globally across the swarm.
-
-Self-organization results from the fact that the Buzz run-time platform is purely distributed.
-
-The language can be extended to add new primitives (thus supporting heterogeneous robot swarms) and can be laid on top of other frameworks, such as ROS.
-
-More information is available at http://the.swarming.buzz/wiki/doku.php?id=start.
-
 Description:
 ============
 
-Rosbuzz package is the ROS version of Buzz. The package contains a node called “rosbuzz_node”, which implements buzz virtual machine (BVM) as a node in ROS.
+ROSBuzz is a ROS node encompassing Buzz Virtual Machine (BVM) and interfacing with ROS ecosystem for mobile robots. The only node of the package is `rosbuzz_node`. It can be used in simulation-in-the-loop using Gazebo and was tested over many platforms (Clearpath Husky, DJI M100, Intel Aero, 3DR Solos, Pleidis Spiris, etc.). More information about ROSBuzz peripheral nodes is available in [1].
 
+What is Buzz?
+=============
 
-Downloading ROS Package
-=======================
-
-    $ git clone https://github.com/MISTLab/ROSBuzz.git rosbuzz
+Buzz is a novel programming language for heterogeneous robots swarms. Buzz advocates a compositional approach, by offering primitives to define swarm behaviors both in a bottom-up and in a top-down fashion. Its official documentation and code are available [Buzz](https://github.com/MISTLab/Buzz).
 
 Requirements
 ============
 
-* Buzz : 
+* Buzz:
 
-You can download the development sources through git:
+Follow the required steps in [Buzz](https://github.com/MISTLab/ROSBuzz).
 
-    $ git clone https://github.com/MISTLab/Buzz.git buzz
+* ROS **base** binary distribution (Indigo or higher):
 
-* ROS binary distribution (Indigo or higher) with catkin (could be used with older versions of ROS with catkin but not tested)
+Follow the required steps in [ROS Kinetic](https://wiki.ros.org/kinetic/Installation/Ubuntu). Note that the guidance and camera node of DJI for the M100 require to use the Indigo distribution.
 
+* ROS additionnal dependencies:
 
-You need the following package:
-
-* mavros_msgs : 
-
-You can install using apt-get:
-
-    $ sudo apt-get install ros-<distro>-mavros ros-<distro>-mavros-extras
+```
+$ sudo apt-get install ros-<distro>-mavros ros-<distro>-mavros-extras
+```
 
 Compilation
 ===========
 
-To compile the ros package, execute the following:
-
-    $ cd catkin_ws
-    $ catkin_make
-    $ source devel/setup.bash
+```
+mkdir -p ROS_WS/src
+cd ROS_WS/src
+git clone https://github.com/MISTLab/ROSBuzz rosbuzz
+cd ..
+catkin_make
+```
     
 Run
 ===
@@ -64,42 +45,50 @@ To run the ROSBuzz package using the launch file, execute the following:
 
     $ roslaunch rosbuzz rosbuzz.launch
     
-Note : Before launching the ROSBuzz node, verify all the parameters in the launch file. A launch file using gdb is available also (rosbuzzd.launch).
+Have a look at the launch file to understand what parameters are available to suit your usage. All topics and services names are listed in `launch_config/topics.yaml`. Note : Before launching the ROSBuzz node, verify all the parameters in the launch file. A launch file using gdb is available too (rosbuzzd.launch).
 
-* Buzz scripts: Several behavioral scripts are included in the "buzz_Scripts" folder, such as "graphformGPS.bzz" uses in the ICRA publication below and the "testaloneWP.bzz" to control a single drone with a ".csv" list of waypoints. The script "empty.bzz" is a template script.
+* Buzz scripts: Several behavioral scripts are included in the "buzz_Scripts" folder, such as "graphformGPS.bzz" uses in [1] and the "testaloneWP.bzz" to control a single drone with a ".csv" list of waypoints. The script "empty.bzz" is a template script.
 
-Publisher
-=========
+Publishers
+-----------
 
 * Messages from Buzz (BVM):
-The package publishes mavros_msgs/Mavlink message "outMavlink".
+The node publishes `mavros_msgs/Mavlink` message "outMavlink".
 
 * Command to the flight controller:
-The package publishes geometry_msgs/PoseStamped message "setpoint_position/local".
+The node publishes `geometry_msgs/PoseStamped message` "setpoint_position/local".
+
+* Other information from the swarw:
+The node publishes:
+    - "bvmstate" (`std_msgs/String`)
+    - "neighbours_pos" (`rosbuzz_msgs/neigh_pos`)
+    - "fleet_status" (`mavros_msgs/Mavlink`)
 
 Subscribers
 -----------
 
-* Current position of the Robot:
-The package subscribes to sensor_msgs/NavSatFix message "global_position/global", to a std_msgs/Float64 message "global_position/rel_alt" and to a geometry_msgs/PoseStamped message "local_position/pose".
+* Information from the Robot controller (mavros compliant):
+The node subscribes to:
+    - "global_position/global" (`sensor_msgs/NavSatFix message`)
+    - "global_position/rel_alt" (`std_msgs/Float64`)
+    - "local_position/pose" (`geometry_msgs/PoseStamped`)
+    - "battery" (`sensor_msgs/BatteryState`)
+    - either "extended_state" (`mavros_msgs/ExtendedState`) or "state" (`mavros_msgs/State`)
 
 * Messages to Buzz (BVM):
-The package subscribes to mavros_msgs/Mavlink message with a topic name of "inMavlink".
+The node subscribes to `mavros_msgs/Mavlink` incoming message with name "inMavlink".
 
-* Status:
-The package subscribes to mavros_msgs/BatteryStatus message "battery" and to either a mavros_msgs/ExtendedState message "extended_state" or a mavros_msgs/State message "state".
-
-Service
+Services
 -------
 
 * Remote Controller:
-The package offers a mavros_msgs/CommandLong service "buzzcmd" to control its state. In the "misc" folder a bash script shows how to control the Buzz states from the command line.
+The package offers a service "buzzcmd" (`mavros_msgs/CommandLong`) to control it. In the "misc" folder, a bash script shows how to control the swarm state from the command line.
 
 References
 ------
-* ROS and Buzz : consensus-based behaviors for heterogeneous teams. St-Onge, D., Shankar Varadharajan, V., Li, G., Svogor, I. and Beltrame, G. arXiv : https://arxiv.org/abs/1710.08843
+* [1] ROS and Buzz : consensus-based behaviors for heterogeneous teams. St-Onge, D., Shankar Varadharajan, V., Li, G., Svogor, I. and Beltrame, G. arXiv : https://arxiv.org/abs/1710.08843
 
-* Over-The-Air Updates for Robotic Swarms. Submitted to IEEE Software (August 2017). 8pgs. Shankar Varadharajan, V., St-Onge, D., Guß, C. and Beltrame, G.
+* [2] Over-The-Air Updates for Robotic Swarms. Submitted to IEEE Software (August 2017). 8pgs. Shankar Varadharajan, V., St-Onge, D., Guß, C. and Beltrame, G.
 
 Visual Studio Code
 --------------------
diff --git a/src/VoronoiDiagramGenerator.cpp b/src/VoronoiDiagramGenerator.cpp
index 9423752..cafafc9 100644
--- a/src/VoronoiDiagramGenerator.cpp
+++ b/src/VoronoiDiagramGenerator.cpp
@@ -12,9 +12,9 @@
 * OF THIS SOFTWARE OR ITS FITNESS FOR ANY PARTICULAR PURPOSE.
 */
 
-/* 
-* This code was originally written by Stephan Fortune in C code.  I, Shane O'Sullivan, 
-* have since modified it, encapsulating it in a C++ class and, fixing memory leaks and 
+/*
+* This code was originally written by Stephan Fortune in C code.  I, Shane O'Sullivan,
+* have since modified it, encapsulating it in a C++ class and, fixing memory leaks and
 * adding accessors to the Voronoi Edges.
 * Permission to use, copy, modify, and distribute this software for any
 * purpose without fee is hereby granted, provided that this entire notice
@@ -31,830 +31,849 @@
 
 VoronoiDiagramGenerator::VoronoiDiagramGenerator()
 {
-	siteidx = 0;
-	sites = 0;
+  siteidx = 0;
+  sites = 0;
 
-	allMemoryList = new FreeNodeArrayList;
-	allMemoryList->memory = 0;
-	allMemoryList->next = 0;
-	currentMemoryBlock = allMemoryList;
-	allEdges = 0;
-	iteratorEdges = 0;
-	minDistanceBetweenSites = 0;
+  allMemoryList = new FreeNodeArrayList;
+  allMemoryList->memory = 0;
+  allMemoryList->next = 0;
+  currentMemoryBlock = allMemoryList;
+  allEdges = 0;
+  iteratorEdges = 0;
+  minDistanceBetweenSites = 0;
 }
 
 VoronoiDiagramGenerator::~VoronoiDiagramGenerator()
 {
-	cleanup();
-	cleanupEdges();
+  cleanup();
+  cleanupEdges();
 
-	if(allMemoryList != 0)
-		delete allMemoryList;
+  if (allMemoryList != 0)
+    delete allMemoryList;
 }
 
-
-
-bool VoronoiDiagramGenerator::generateVoronoi(float *xValues, float *yValues, int numPoints, float minX, float maxX, float minY, float maxY, float minDist)
+bool VoronoiDiagramGenerator::generateVoronoi(float* xValues, float* yValues, int numPoints, float minX, float maxX,
+                                              float minY, float maxY, float minDist)
 {
-	cleanup();
-	cleanupEdges();
-	int i;
+  cleanup();
+  cleanupEdges();
+  int i;
 
-	minDistanceBetweenSites = minDist;
+  minDistanceBetweenSites = minDist;
 
-	nsites=numPoints;
-	plot = 0;
-	triangulate = 0;	
-	debug = 1;
-	sorted = 0; 
-	freeinit(&sfl, sizeof (Site));
-		
-	sites = (struct Site *) myalloc(nsites*sizeof( *sites));
+  nsites = numPoints;
+  plot = 0;
+  triangulate = 0;
+  debug = 1;
+  sorted = 0;
+  freeinit(&sfl, sizeof(Site));
 
-	if(sites == 0)
-		return false;
+  sites = (struct Site*)myalloc(nsites * sizeof(*sites));
 
-	xmin = xValues[0];
-	ymin = yValues[0];
-	xmax = xValues[0];
-	ymax = yValues[0];
+  if (sites == 0)
+    return false;
 
-	for(i = 0; i< nsites; i++)
-	{
-		sites[i].coord.x = xValues[i];
-		sites[i].coord.y = yValues[i];
-		sites[i].sitenbr = i;
-		sites[i].refcnt = 0;
+  xmin = xValues[0];
+  ymin = yValues[0];
+  xmax = xValues[0];
+  ymax = yValues[0];
 
-		if(xValues[i] < xmin)
-			xmin = xValues[i];
-		else if(xValues[i] > xmax)
-			xmax = xValues[i];
+  for (i = 0; i < nsites; i++)
+  {
+    sites[i].coord.x = xValues[i];
+    sites[i].coord.y = yValues[i];
+    sites[i].sitenbr = i;
+    sites[i].refcnt = 0;
 
-		if(yValues[i] < ymin)
-			ymin = yValues[i];
-		else if(yValues[i] > ymax)
-			ymax = yValues[i];
+    if (xValues[i] < xmin)
+      xmin = xValues[i];
+    else if (xValues[i] > xmax)
+      xmax = xValues[i];
 
-		//printf("\n%f %f\n",xValues[i],yValues[i]);
-	}
-	
-	qsort(sites, nsites, sizeof (*sites), scomp);
-	
-	siteidx = 0;
-	geominit();
-	float temp = 0;
-	if(minX > maxX)
-	{
-		temp = minX;
-		minX = maxX;
-		maxX = temp;
-	}
-	if(minY > maxY)
-	{
-		temp = minY;
-		minY = maxY;
-		maxY = temp;
-	}
-	borderMinX = minX;
-	borderMinY = minY;
-	borderMaxX = maxX;
-	borderMaxY = maxY;
-	
-	siteidx = 0;
-	voronoi(triangulate);
+    if (yValues[i] < ymin)
+      ymin = yValues[i];
+    else if (yValues[i] > ymax)
+      ymax = yValues[i];
 
-	return true;
+    // printf("\n%f %f\n",xValues[i],yValues[i]);
+  }
+
+  qsort(sites, nsites, sizeof(*sites), scomp);
+
+  siteidx = 0;
+  geominit();
+  float temp = 0;
+  if (minX > maxX)
+  {
+    temp = minX;
+    minX = maxX;
+    maxX = temp;
+  }
+  if (minY > maxY)
+  {
+    temp = minY;
+    minY = maxY;
+    maxY = temp;
+  }
+  borderMinX = minX;
+  borderMinY = minY;
+  borderMaxX = maxX;
+  borderMaxY = maxY;
+
+  siteidx = 0;
+  voronoi(triangulate);
+
+  return true;
 }
 
 bool VoronoiDiagramGenerator::ELinitialize()
 {
-	int i;
-	freeinit(&hfl, sizeof **ELhash);
-	ELhashsize = 2 * sqrt_nsites;
-	ELhash = (struct Halfedge **) myalloc ( sizeof *ELhash * ELhashsize);
+  int i;
+  freeinit(&hfl, sizeof **ELhash);
+  ELhashsize = 2 * sqrt_nsites;
+  ELhash = (struct Halfedge**)myalloc(sizeof *ELhash * ELhashsize);
 
-	if(ELhash == 0)
-		return false;
+  if (ELhash == 0)
+    return false;
 
-	for(i=0; i<ELhashsize; i +=1) ELhash[i] = (struct Halfedge *)NULL;
-	ELleftend = HEcreate( (struct Edge *)NULL, 0);
-	ELrightend = HEcreate( (struct Edge *)NULL, 0);
-	ELleftend -> ELleft = (struct Halfedge *)NULL;
-	ELleftend -> ELright = ELrightend;
-	ELrightend -> ELleft = ELleftend;
-	ELrightend -> ELright = (struct Halfedge *)NULL;
-	ELhash[0] = ELleftend;
-	ELhash[ELhashsize-1] = ELrightend;
+  for (i = 0; i < ELhashsize; i += 1)
+    ELhash[i] = (struct Halfedge*)NULL;
+  ELleftend = HEcreate((struct Edge*)NULL, 0);
+  ELrightend = HEcreate((struct Edge*)NULL, 0);
+  ELleftend->ELleft = (struct Halfedge*)NULL;
+  ELleftend->ELright = ELrightend;
+  ELrightend->ELleft = ELleftend;
+  ELrightend->ELright = (struct Halfedge*)NULL;
+  ELhash[0] = ELleftend;
+  ELhash[ELhashsize - 1] = ELrightend;
 
-	return true;
+  return true;
 }
 
-
-struct Halfedge* VoronoiDiagramGenerator::HEcreate(struct Edge *e,int pm)
+struct Halfedge* VoronoiDiagramGenerator::HEcreate(struct Edge* e, int pm)
 {
-	struct Halfedge *answer;
-	answer = (struct Halfedge *) getfree(&hfl);
-	answer -> ELedge = e;
-	answer -> ELpm = pm;
-	answer -> PQnext = (struct Halfedge *) NULL;
-	answer -> vertex = (struct Site *) NULL;
-	answer -> ELrefcnt = 0;
-	return(answer);
+  struct Halfedge* answer;
+  answer = (struct Halfedge*)getfree(&hfl);
+  answer->ELedge = e;
+  answer->ELpm = pm;
+  answer->PQnext = (struct Halfedge*)NULL;
+  answer->vertex = (struct Site*)NULL;
+  answer->ELrefcnt = 0;
+  return (answer);
 }
 
-
-void VoronoiDiagramGenerator::ELinsert(struct	Halfedge *lb, struct Halfedge *newHe)
+void VoronoiDiagramGenerator::ELinsert(struct Halfedge* lb, struct Halfedge* newHe)
 {
-	newHe -> ELleft = lb;
-	newHe -> ELright = lb -> ELright;
-	(lb -> ELright) -> ELleft = newHe;
-	lb -> ELright = newHe;
+  newHe->ELleft = lb;
+  newHe->ELright = lb->ELright;
+  (lb->ELright)->ELleft = newHe;
+  lb->ELright = newHe;
 }
 
 /* Get entry from hash table, pruning any deleted nodes */
-struct Halfedge * VoronoiDiagramGenerator::ELgethash(int b)
+struct Halfedge* VoronoiDiagramGenerator::ELgethash(int b)
 {
-	struct Halfedge *he;
-	
-	if(b<0 || b>=ELhashsize) 
-		return((struct Halfedge *) NULL);
-	he = ELhash[b]; 
-	if (he == (struct Halfedge *) NULL || he->ELedge != (struct Edge *) DELETED ) 
-		return (he);
-	
-	/* Hash table points to deleted half edge.  Patch as necessary. */
-	ELhash[b] = (struct Halfedge *) NULL;
-	if ((he -> ELrefcnt -= 1) == 0) 
-		makefree((Freenode*)he, &hfl);
-	return ((struct Halfedge *) NULL);
-}	
+  struct Halfedge* he;
 
-struct Halfedge * VoronoiDiagramGenerator::ELleftbnd(struct Point *p)
-{
-	int i, bucket;
-	struct Halfedge *he;
-	
-	/* Use hash table to get close to desired halfedge */
-	bucket = (int)((p->x - xmin)/deltax * ELhashsize);	//use the hash function to find the place in the hash map that this HalfEdge should be
+  if (b < 0 || b >= ELhashsize)
+    return ((struct Halfedge*)NULL);
+  he = ELhash[b];
+  if (he == (struct Halfedge*)NULL || he->ELedge != (struct Edge*)DELETED)
+    return (he);
 
-	if(bucket<0) bucket =0;					//make sure that the bucket position in within the range of the hash array
-	if(bucket>=ELhashsize) bucket = ELhashsize - 1;
-
-	he = ELgethash(bucket);
-	if(he == (struct Halfedge *) NULL)			//if the HE isn't found, search backwards and forwards in the hash map for the first non-null entry
-	{   
-		for(i=1; 1 ; i += 1)
-		{	
-			if ((he=ELgethash(bucket-i)) != (struct Halfedge *) NULL) 
-				break;
-			if ((he=ELgethash(bucket+i)) != (struct Halfedge *) NULL) 
-				break;
-		};
-		totalsearch += i;
-	};
-	ntry += 1;
-	/* Now search linear list of halfedges for the correct one */
-	if (he==ELleftend  || (he != ELrightend && right_of(he,p)))
-	{
-		do 
-		{
-			he = he -> ELright;
-		} while (he!=ELrightend && right_of(he,p));	//keep going right on the list until either the end is reached, or you find the 1st edge which the point
-		he = he -> ELleft;				//isn't to the right of
-	}
-	else 							//if the point is to the left of the HalfEdge, then search left for the HE just to the left of the point
-		do 
-		{
-			he = he -> ELleft;
-		} while (he!=ELleftend && !right_of(he,p));
-		
-	/* Update hash table and reference counts */
-	if(bucket > 0 && bucket <ELhashsize-1)
-	{	
-		if(ELhash[bucket] != (struct Halfedge *) NULL) 
-		{
-			ELhash[bucket] -> ELrefcnt -= 1;
-		}
-		ELhash[bucket] = he;
-		ELhash[bucket] -> ELrefcnt += 1;
-	};
-	return (he);
+  /* Hash table points to deleted half edge.  Patch as necessary. */
+  ELhash[b] = (struct Halfedge*)NULL;
+  if ((he->ELrefcnt -= 1) == 0)
+    makefree((Freenode*)he, &hfl);
+  return ((struct Halfedge*)NULL);
 }
 
+struct Halfedge* VoronoiDiagramGenerator::ELleftbnd(struct Point* p)
+{
+  int i, bucket;
+  struct Halfedge* he;
+
+  /* Use hash table to get close to desired halfedge */
+  bucket = (int)((p->x - xmin) / deltax *
+                 ELhashsize);  // use the hash function to find the place in the hash map that this HalfEdge should be
+
+  if (bucket < 0)
+    bucket = 0;  // make sure that the bucket position in within the range of the hash array
+  if (bucket >= ELhashsize)
+    bucket = ELhashsize - 1;
+
+  he = ELgethash(bucket);
+  if (he == (struct Halfedge*)NULL)  // if the HE isn't found, search backwards and forwards in the hash map for the
+                                     // first non-null entry
+  {
+    for (i = 1; 1; i += 1)
+    {
+      if ((he = ELgethash(bucket - i)) != (struct Halfedge*)NULL)
+        break;
+      if ((he = ELgethash(bucket + i)) != (struct Halfedge*)NULL)
+        break;
+    };
+    totalsearch += i;
+  };
+  ntry += 1;
+  /* Now search linear list of halfedges for the correct one */
+  if (he == ELleftend || (he != ELrightend && right_of(he, p)))
+  {
+    do
+    {
+      he = he->ELright;
+    } while (he != ELrightend && right_of(he, p));  // keep going right on the list until either the end is reached, or
+                                                    // you find the 1st edge which the point
+    he = he->ELleft;  // isn't to the right of
+  }
+  else  // if the point is to the left of the HalfEdge, then search left for the HE just to the left of the point
+    do
+    {
+      he = he->ELleft;
+    } while (he != ELleftend && !right_of(he, p));
+
+  /* Update hash table and reference counts */
+  if (bucket > 0 && bucket < ELhashsize - 1)
+  {
+    if (ELhash[bucket] != (struct Halfedge*)NULL)
+    {
+      ELhash[bucket]->ELrefcnt -= 1;
+    }
+    ELhash[bucket] = he;
+    ELhash[bucket]->ELrefcnt += 1;
+  };
+  return (he);
+}
 
 /* This delete routine can't reclaim node, since pointers from hash
 table may be present.   */
-void VoronoiDiagramGenerator::ELdelete(struct Halfedge *he)
+void VoronoiDiagramGenerator::ELdelete(struct Halfedge* he)
 {
-	(he -> ELleft) -> ELright = he -> ELright;
-	(he -> ELright) -> ELleft = he -> ELleft;
-	he -> ELedge = (struct Edge *)DELETED;
+  (he->ELleft)->ELright = he->ELright;
+  (he->ELright)->ELleft = he->ELleft;
+  he->ELedge = (struct Edge*)DELETED;
 }
 
-
-struct Halfedge * VoronoiDiagramGenerator::ELright(struct Halfedge *he)
+struct Halfedge* VoronoiDiagramGenerator::ELright(struct Halfedge* he)
 {
-	return (he -> ELright);
+  return (he->ELright);
 }
 
-struct Halfedge * VoronoiDiagramGenerator::ELleft(struct Halfedge *he)
+struct Halfedge* VoronoiDiagramGenerator::ELleft(struct Halfedge* he)
 {
-	return (he -> ELleft);
+  return (he->ELleft);
 }
 
-
-struct Site * VoronoiDiagramGenerator::leftreg(struct Halfedge *he)
+struct Site* VoronoiDiagramGenerator::leftreg(struct Halfedge* he)
 {
-	if(he -> ELedge == (struct Edge *)NULL) 
-		return(bottomsite);
-	return( he -> ELpm == le ? 
-		he -> ELedge -> reg[le] : he -> ELedge -> reg[re]);
+  if (he->ELedge == (struct Edge*)NULL)
+    return (bottomsite);
+  return (he->ELpm == le ? he->ELedge->reg[le] : he->ELedge->reg[re]);
 }
 
-struct Site * VoronoiDiagramGenerator::rightreg(struct Halfedge *he)
+struct Site* VoronoiDiagramGenerator::rightreg(struct Halfedge* he)
 {
-	if(he -> ELedge == (struct Edge *)NULL) //if this halfedge has no edge, return the bottom site (whatever that is)
-		return(bottomsite);
+  if (he->ELedge == (struct Edge*)NULL)  // if this halfedge has no edge, return the bottom site (whatever that is)
+    return (bottomsite);
 
-	//if the ELpm field is zero, return the site 0 that this edge bisects, otherwise return site number 1
-	return( he -> ELpm == le ? he -> ELedge -> reg[re] : he -> ELedge -> reg[le]);
+  // if the ELpm field is zero, return the site 0 that this edge bisects, otherwise return site number 1
+  return (he->ELpm == le ? he->ELedge->reg[re] : he->ELedge->reg[le]);
 }
 
 void VoronoiDiagramGenerator::geominit()
-{	
-	float sn;
+{
+  float sn;
 
-	freeinit(&efl, sizeof(Edge));
-	nvertices = 0;
-	nedges = 0;
-	sn = (float)nsites+4;
-	sqrt_nsites = (int)sqrt(sn);
-	deltay = ymax - ymin;
-	deltax = xmax - xmin;
+  freeinit(&efl, sizeof(Edge));
+  nvertices = 0;
+  nedges = 0;
+  sn = (float)nsites + 4;
+  sqrt_nsites = (int)sqrt(sn);
+  deltay = ymax - ymin;
+  deltax = xmax - xmin;
 }
 
-
-struct Edge * VoronoiDiagramGenerator::bisect(struct Site *s1,struct Site *s2)
+struct Edge* VoronoiDiagramGenerator::bisect(struct Site* s1, struct Site* s2)
 {
-	float dx,dy,adx,ady;
-	struct Edge *newedge;	
+  float dx, dy, adx, ady;
+  struct Edge* newedge;
 
-	newedge = (struct Edge *) getfree(&efl);
-	
-	newedge -> reg[0] = s1; //store the sites that this edge is bisecting
-	newedge -> reg[1] = s2;
-	ref(s1); 
-	ref(s2);
-	newedge -> ep[0] = (struct Site *) NULL; //to begin with, there are no endpoints on the bisector - it goes to infinity
-	newedge -> ep[1] = (struct Site *) NULL;
-	
-	dx = s2->coord.x - s1->coord.x;			//get the difference in x dist between the sites
-	dy = s2->coord.y - s1->coord.y;
-	adx = dx>0 ? dx : -dx;					//make sure that the difference in positive
-	ady = dy>0 ? dy : -dy;
-	newedge -> c = (float)(s1->coord.x * dx + s1->coord.y * dy + (dx*dx + dy*dy)*0.5);//get the slope of the line
+  newedge = (struct Edge*)getfree(&efl);
 
-	if (adx>ady)
-	{	
-		newedge -> a = 1.0; newedge -> b = dy/dx; newedge -> c /= dx;//set formula of line, with x fixed to 1
-	}
-	else
-	{	
-		newedge -> b = 1.0; newedge -> a = dx/dy; newedge -> c /= dy;//set formula of line, with y fixed to 1
-	};
-	
-	newedge -> edgenbr = nedges;
-	newedge -> sites[0] = s1->sitenbr;
-	newedge -> sites[1] = s2->sitenbr;
+  newedge->reg[0] = s1;  // store the sites that this edge is bisecting
+  newedge->reg[1] = s2;
+  ref(s1);
+  ref(s2);
+  newedge->ep[0] = (struct Site*)NULL;  // to begin with, there are no endpoints on the bisector - it goes to infinity
+  newedge->ep[1] = (struct Site*)NULL;
 
-	//printf("\nbisect(%d) (%d(%f,%f) and %d(%f,%f)",nedges,s1->sitenbr,s1->coord.x,s1->coord.y,s2->sitenbr,s2->coord.x,s2->coord.y);
-	
-	nedges += 1;
-	return(newedge);
+  dx = s2->coord.x - s1->coord.x;  // get the difference in x dist between the sites
+  dy = s2->coord.y - s1->coord.y;
+  adx = dx > 0 ? dx : -dx;  // make sure that the difference in positive
+  ady = dy > 0 ? dy : -dy;
+  newedge->c = (float)(s1->coord.x * dx + s1->coord.y * dy + (dx * dx + dy * dy) * 0.5);  // get the slope of the line
+
+  if (adx > ady)
+  {
+    newedge->a = 1.0;
+    newedge->b = dy / dx;
+    newedge->c /= dx;  // set formula of line, with x fixed to 1
+  }
+  else
+  {
+    newedge->b = 1.0;
+    newedge->a = dx / dy;
+    newedge->c /= dy;  // set formula of line, with y fixed to 1
+  };
+
+  newedge->edgenbr = nedges;
+  newedge->sites[0] = s1->sitenbr;
+  newedge->sites[1] = s2->sitenbr;
+
+  // printf("\nbisect(%d) (%d(%f,%f) and
+  // %d(%f,%f)",nedges,s1->sitenbr,s1->coord.x,s1->coord.y,s2->sitenbr,s2->coord.x,s2->coord.y);
+
+  nedges += 1;
+  return (newedge);
 }
 
-//create a new site where the HalfEdges el1 and el2 intersect - note that the Point in the argument list is not used, don't know why it's there
-struct Site * VoronoiDiagramGenerator::intersect(struct Halfedge *el1, struct Halfedge *el2, struct Point *p)
+// create a new site where the HalfEdges el1 and el2 intersect - note that the Point in the argument list is not used,
+// don't know why it's there
+struct Site* VoronoiDiagramGenerator::intersect(struct Halfedge* el1, struct Halfedge* el2, struct Point* p)
 {
-	struct	Edge *e1,*e2, *e;
-	struct  Halfedge *el;
-	float d, xint, yint;
-	int right_of_site;
-	struct Site *v;
-	
-	e1 = el1 -> ELedge;
-	e2 = el2 -> ELedge;
-	if(e1 == (struct Edge*)NULL || e2 == (struct Edge*)NULL) 
-		return ((struct Site *) NULL);
+  struct Edge *e1, *e2, *e;
+  struct Halfedge* el;
+  float d, xint, yint;
+  int right_of_site;
+  struct Site* v;
 
-	//if the two edges bisect the same parent, return null
-	if (e1->reg[1] == e2->reg[1]) 
-		return ((struct Site *) NULL);
-	
-	d = e1->a * e2->b - e1->b * e2->a;
-	if (-1.0e-10<d && d<1.0e-10) 
-		return ((struct Site *) NULL);
-	
-	xint = (e1->c*e2->b - e2->c*e1->b)/d;
-	yint = (e2->c*e1->a - e1->c*e2->a)/d;
-	
-	if( (e1->reg[1]->coord.y < e2->reg[1]->coord.y) ||
-		(e1->reg[1]->coord.y == e2->reg[1]->coord.y &&
-		e1->reg[1]->coord.x < e2->reg[1]->coord.x) )
-	{	
-		el = el1; 
-		e = e1;
-	}
-	else
-	{	
-		el = el2; 
-		e = e2;
-	};
-	
-	right_of_site = xint >= e -> reg[1] -> coord.x;
-	if ((right_of_site && el -> ELpm == le) || (!right_of_site && el -> ELpm == re)) 
-		return ((struct Site *) NULL);
-	
-	//create a new site at the point of intersection - this is a new vector event waiting to happen
-	v = (struct Site *) getfree(&sfl);
-	v -> refcnt = 0;
-	v -> coord.x = xint;
-	v -> coord.y = yint;
-	return(v);
+  e1 = el1->ELedge;
+  e2 = el2->ELedge;
+  if (e1 == (struct Edge*)NULL || e2 == (struct Edge*)NULL)
+    return ((struct Site*)NULL);
+
+  // if the two edges bisect the same parent, return null
+  if (e1->reg[1] == e2->reg[1])
+    return ((struct Site*)NULL);
+
+  d = e1->a * e2->b - e1->b * e2->a;
+  if (-1.0e-10 < d && d < 1.0e-10)
+    return ((struct Site*)NULL);
+
+  xint = (e1->c * e2->b - e2->c * e1->b) / d;
+  yint = (e2->c * e1->a - e1->c * e2->a) / d;
+
+  if ((e1->reg[1]->coord.y < e2->reg[1]->coord.y) ||
+      (e1->reg[1]->coord.y == e2->reg[1]->coord.y && e1->reg[1]->coord.x < e2->reg[1]->coord.x))
+  {
+    el = el1;
+    e = e1;
+  }
+  else
+  {
+    el = el2;
+    e = e2;
+  };
+
+  right_of_site = xint >= e->reg[1]->coord.x;
+  if ((right_of_site && el->ELpm == le) || (!right_of_site && el->ELpm == re))
+    return ((struct Site*)NULL);
+
+  // create a new site at the point of intersection - this is a new vector event waiting to happen
+  v = (struct Site*)getfree(&sfl);
+  v->refcnt = 0;
+  v->coord.x = xint;
+  v->coord.y = yint;
+  return (v);
 }
 
 /* returns 1 if p is to right of halfedge e */
-int VoronoiDiagramGenerator::right_of(struct Halfedge *el,struct Point *p)
+int VoronoiDiagramGenerator::right_of(struct Halfedge* el, struct Point* p)
 {
-	struct Edge *e;
-	struct Site *topsite;
-	int right_of_site, above, fast;
-	float dxp, dyp, dxs, t1, t2, t3, yl;
-	
-	e = el -> ELedge;
-	topsite = e -> reg[1];
-	right_of_site = p -> x > topsite -> coord.x;
-	if(right_of_site && el -> ELpm == le) return(1);
-	if(!right_of_site && el -> ELpm == re) return (0);
-	
-	if (e->a == 1.0)
-	{	dyp = p->y - topsite->coord.y;
-	dxp = p->x - topsite->coord.x;
-	fast = 0;
-	if ((!right_of_site & (e->b<0.0)) | (right_of_site & (e->b>=0.0)) )
-	{	above = dyp>= e->b*dxp;	
-	fast = above;
-	}
-	else 
-	{	above = p->x + p->y*e->b > e-> c;
-	if(e->b<0.0) above = !above;
-	if (!above) fast = 1;
-	};
-	if (!fast)
-	{	dxs = topsite->coord.x - (e->reg[0])->coord.x;
-	above = e->b * (dxp*dxp - dyp*dyp) <
-		dxs*dyp*(1.0+2.0*dxp/dxs + e->b*e->b);
-	if(e->b<0.0) above = !above;
-	};
-	}
-	else  /*e->b==1.0 */
-	{	yl = e->c - e->a*p->x;
-	t1 = p->y - yl;
-	t2 = p->x - topsite->coord.x;
-	t3 = yl - topsite->coord.y;
-	above = t1*t1 > t2*t2 + t3*t3;
-	};
-	return (el->ELpm==le ? above : !above);
+  struct Edge* e;
+  struct Site* topsite;
+  int right_of_site, above, fast;
+  float dxp, dyp, dxs, t1, t2, t3, yl;
+
+  e = el->ELedge;
+  topsite = e->reg[1];
+  right_of_site = p->x > topsite->coord.x;
+  if (right_of_site && el->ELpm == le)
+    return (1);
+  if (!right_of_site && el->ELpm == re)
+    return (0);
+
+  if (e->a == 1.0)
+  {
+    dyp = p->y - topsite->coord.y;
+    dxp = p->x - topsite->coord.x;
+    fast = 0;
+    if ((!right_of_site & (e->b < 0.0)) | (right_of_site & (e->b >= 0.0)))
+    {
+      above = dyp >= e->b * dxp;
+      fast = above;
+    }
+    else
+    {
+      above = p->x + p->y * e->b > e->c;
+      if (e->b < 0.0)
+        above = !above;
+      if (!above)
+        fast = 1;
+    };
+    if (!fast)
+    {
+      dxs = topsite->coord.x - (e->reg[0])->coord.x;
+      above = e->b * (dxp * dxp - dyp * dyp) < dxs * dyp * (1.0 + 2.0 * dxp / dxs + e->b * e->b);
+      if (e->b < 0.0)
+        above = !above;
+    };
+  }
+  else /*e->b==1.0 */
+  {
+    yl = e->c - e->a * p->x;
+    t1 = p->y - yl;
+    t2 = p->x - topsite->coord.x;
+    t3 = yl - topsite->coord.y;
+    above = t1 * t1 > t2 * t2 + t3 * t3;
+  };
+  return (el->ELpm == le ? above : !above);
 }
 
-
-void VoronoiDiagramGenerator::endpoint(struct Edge *e,int lr,struct Site * s)
+void VoronoiDiagramGenerator::endpoint(struct Edge* e, int lr, struct Site* s)
 {
-	e -> ep[lr] = s;
-	ref(s);
-	if(e -> ep[re-lr]== (struct Site *) NULL) 
-		return;
+  e->ep[lr] = s;
+  ref(s);
+  if (e->ep[re - lr] == (struct Site*)NULL)
+    return;
 
-	clip_line(e);
+  clip_line(e);
 
-	deref(e->reg[le]);
-	deref(e->reg[re]);
-	makefree((Freenode*)e, &efl);
+  deref(e->reg[le]);
+  deref(e->reg[re]);
+  makefree((Freenode*)e, &efl);
 }
 
-
-float VoronoiDiagramGenerator::dist(struct Site *s,struct Site *t)
+float VoronoiDiagramGenerator::dist(struct Site* s, struct Site* t)
 {
-	float dx,dy;
-	dx = s->coord.x - t->coord.x;
-	dy = s->coord.y - t->coord.y;
-	return (float)(sqrt(dx*dx + dy*dy));
+  float dx, dy;
+  dx = s->coord.x - t->coord.x;
+  dy = s->coord.y - t->coord.y;
+  return (float)(sqrt(dx * dx + dy * dy));
 }
 
-
-void VoronoiDiagramGenerator::makevertex(struct Site *v)
+void VoronoiDiagramGenerator::makevertex(struct Site* v)
 {
-	v -> sitenbr = nvertices;
-	nvertices += 1;
-	out_vertex(v);
+  v->sitenbr = nvertices;
+  nvertices += 1;
+  out_vertex(v);
 }
 
-
-void VoronoiDiagramGenerator::deref(struct Site *v)
+void VoronoiDiagramGenerator::deref(struct Site* v)
 {
-	v -> refcnt -= 1;
-	if (v -> refcnt == 0 ) 
-		makefree((Freenode*)v, &sfl);
+  v->refcnt -= 1;
+  if (v->refcnt == 0)
+    makefree((Freenode*)v, &sfl);
 }
 
-void VoronoiDiagramGenerator::ref(struct Site *v)
+void VoronoiDiagramGenerator::ref(struct Site* v)
 {
-	v -> refcnt += 1;
+  v->refcnt += 1;
 }
 
-//push the HalfEdge into the ordered linked list of vertices
-void VoronoiDiagramGenerator::PQinsert(struct Halfedge *he,struct Site * v, float offset)
+// push the HalfEdge into the ordered linked list of vertices
+void VoronoiDiagramGenerator::PQinsert(struct Halfedge* he, struct Site* v, float offset)
 {
-	struct Halfedge *last, *next;
-	
-	he -> vertex = v;
-	ref(v);
-	he -> ystar = (float)(v -> coord.y + offset);
-	last = &PQhash[PQbucket(he)];
-	while ((next = last -> PQnext) != (struct Halfedge *) NULL &&
-		(he -> ystar  > next -> ystar  ||
-		(he -> ystar == next -> ystar && v -> coord.x > next->vertex->coord.x)))
-	{	
-		last = next;
-	};
-	he -> PQnext = last -> PQnext; 
-	last -> PQnext = he;
-	PQcount += 1;
+  struct Halfedge *last, *next;
+
+  he->vertex = v;
+  ref(v);
+  he->ystar = (float)(v->coord.y + offset);
+  last = &PQhash[PQbucket(he)];
+  while ((next = last->PQnext) != (struct Halfedge*)NULL &&
+         (he->ystar > next->ystar || (he->ystar == next->ystar && v->coord.x > next->vertex->coord.x)))
+  {
+    last = next;
+  };
+  he->PQnext = last->PQnext;
+  last->PQnext = he;
+  PQcount += 1;
 }
 
-//remove the HalfEdge from the list of vertices 
-void VoronoiDiagramGenerator::PQdelete(struct Halfedge *he)
+// remove the HalfEdge from the list of vertices
+void VoronoiDiagramGenerator::PQdelete(struct Halfedge* he)
 {
-	struct Halfedge *last;
-	
-	if(he -> vertex != (struct Site *) NULL)
-	{	
-		last = &PQhash[PQbucket(he)];
-		while (last -> PQnext != he) 
-			last = last -> PQnext;
+  struct Halfedge* last;
 
-		last -> PQnext = he -> PQnext;
-		PQcount -= 1;
-		deref(he -> vertex);
-		he -> vertex = (struct Site *) NULL;
-	};
+  if (he->vertex != (struct Site*)NULL)
+  {
+    last = &PQhash[PQbucket(he)];
+    while (last->PQnext != he)
+      last = last->PQnext;
+
+    last->PQnext = he->PQnext;
+    PQcount -= 1;
+    deref(he->vertex);
+    he->vertex = (struct Site*)NULL;
+  };
 }
 
-int VoronoiDiagramGenerator::PQbucket(struct Halfedge *he)
+int VoronoiDiagramGenerator::PQbucket(struct Halfedge* he)
 {
-	int bucket;
-	
-	bucket = (int)((he->ystar - ymin)/deltay * PQhashsize);
-	if (bucket<0) bucket = 0;
-	if (bucket>=PQhashsize) bucket = PQhashsize-1 ;
-	if (bucket < PQmin) PQmin = bucket;
-	return(bucket);
+  int bucket;
+
+  bucket = (int)((he->ystar - ymin) / deltay * PQhashsize);
+  if (bucket < 0)
+    bucket = 0;
+  if (bucket >= PQhashsize)
+    bucket = PQhashsize - 1;
+  if (bucket < PQmin)
+    PQmin = bucket;
+  return (bucket);
 }
 
-
-
 int VoronoiDiagramGenerator::PQempty()
 {
-	return(PQcount==0);
+  return (PQcount == 0);
 }
 
-
 struct Point VoronoiDiagramGenerator::PQ_min()
 {
-	struct Point answer;
-	
-	while(PQhash[PQmin].PQnext == (struct Halfedge *)NULL) {PQmin += 1;};
-	answer.x = PQhash[PQmin].PQnext -> vertex -> coord.x;
-	answer.y = PQhash[PQmin].PQnext -> ystar;
-	return (answer);
+  struct Point answer;
+
+  while (PQhash[PQmin].PQnext == (struct Halfedge*)NULL)
+  {
+    PQmin += 1;
+  };
+  answer.x = PQhash[PQmin].PQnext->vertex->coord.x;
+  answer.y = PQhash[PQmin].PQnext->ystar;
+  return (answer);
 }
 
-struct Halfedge * VoronoiDiagramGenerator::PQextractmin()
+struct Halfedge* VoronoiDiagramGenerator::PQextractmin()
 {
-	struct Halfedge *curr;
-	
-	curr = PQhash[PQmin].PQnext;
-	PQhash[PQmin].PQnext = curr -> PQnext;
-	PQcount -= 1;
-	return(curr);
-}
+  struct Halfedge* curr;
 
+  curr = PQhash[PQmin].PQnext;
+  PQhash[PQmin].PQnext = curr->PQnext;
+  PQcount -= 1;
+  return (curr);
+}
 
 bool VoronoiDiagramGenerator::PQinitialize()
 {
-	int i; 
-	
-	PQcount = 0;
-	PQmin = 0;
-	PQhashsize = 4 * sqrt_nsites;
-	PQhash = (struct Halfedge *) myalloc(PQhashsize * sizeof *PQhash);
+  int i;
 
-	if(PQhash == 0)
-		return false;
+  PQcount = 0;
+  PQmin = 0;
+  PQhashsize = 4 * sqrt_nsites;
+  PQhash = (struct Halfedge*)myalloc(PQhashsize * sizeof *PQhash);
 
-	for(i=0; i<PQhashsize; i+=1) PQhash[i].PQnext = (struct Halfedge *)NULL;
+  if (PQhash == 0)
+    return false;
 
-	return true;
+  for (i = 0; i < PQhashsize; i += 1)
+    PQhash[i].PQnext = (struct Halfedge*)NULL;
+
+  return true;
 }
 
-
-void VoronoiDiagramGenerator::freeinit(struct Freelist *fl,int size)
+void VoronoiDiagramGenerator::freeinit(struct Freelist* fl, int size)
 {
-	fl -> head = (struct Freenode *) NULL;
-	fl -> nodesize = size;
+  fl->head = (struct Freenode*)NULL;
+  fl->nodesize = size;
 }
 
-char * VoronoiDiagramGenerator::getfree(struct Freelist *fl)
+char* VoronoiDiagramGenerator::getfree(struct Freelist* fl)
 {
-	int i; 
-	struct Freenode *t;
+  int i;
+  struct Freenode* t;
 
-	if(fl->head == (struct Freenode *) NULL)
-	{	
-		t =  (struct Freenode *) myalloc(sqrt_nsites * fl->nodesize);
+  if (fl->head == (struct Freenode*)NULL)
+  {
+    t = (struct Freenode*)myalloc(sqrt_nsites * fl->nodesize);
 
-		if(t == 0)
-			return 0;
-		
-		currentMemoryBlock->next = new FreeNodeArrayList;
-		currentMemoryBlock = currentMemoryBlock->next;
-		currentMemoryBlock->memory = t;
-		currentMemoryBlock->next = 0;
+    if (t == 0)
+      return 0;
 
-		for(i=0; i<sqrt_nsites; i+=1) 	
-			makefree((struct Freenode *)((char *)t+i*fl->nodesize), fl);		
-	};
-	t = fl -> head;
-	fl -> head = (fl -> head) -> nextfree;
-	return((char *)t);
+    currentMemoryBlock->next = new FreeNodeArrayList;
+    currentMemoryBlock = currentMemoryBlock->next;
+    currentMemoryBlock->memory = t;
+    currentMemoryBlock->next = 0;
+
+    for (i = 0; i < sqrt_nsites; i += 1)
+      makefree((struct Freenode*)((char*)t + i * fl->nodesize), fl);
+  };
+  t = fl->head;
+  fl->head = (fl->head)->nextfree;
+  return ((char*)t);
 }
 
-
-
-void VoronoiDiagramGenerator::makefree(struct Freenode *curr,struct Freelist *fl)
+void VoronoiDiagramGenerator::makefree(struct Freenode* curr, struct Freelist* fl)
 {
-	curr -> nextfree = fl -> head;
-	fl -> head = curr;
+  curr->nextfree = fl->head;
+  fl->head = curr;
 }
 
 void VoronoiDiagramGenerator::cleanup()
 {
-	if(sites != 0)
-	{
-		free(sites);
-		sites = 0;
-	}
+  if (sites != 0)
+  {
+    free(sites);
+    sites = 0;
+  }
 
-	FreeNodeArrayList* current=0, *prev = 0;
+  FreeNodeArrayList *current = 0, *prev = 0;
 
-	current = prev = allMemoryList;
+  current = prev = allMemoryList;
 
-	while(current->next != 0)
-	{
-		prev = current;
-		current = current->next;
-		free(prev->memory);
-		delete prev;
-		prev = 0;
-	}
+  while (current->next != 0)
+  {
+    prev = current;
+    current = current->next;
+    free(prev->memory);
+    delete prev;
+    prev = 0;
+  }
 
-	if(current != 0 && current->memory != 0)
-	{
-		free(current->memory);
-		delete current;
-	}
+  if (current != 0 && current->memory != 0)
+  {
+    free(current->memory);
+    delete current;
+  }
 
-	allMemoryList = new FreeNodeArrayList;
-	allMemoryList->next = 0;
-	allMemoryList->memory = 0;
-	currentMemoryBlock = allMemoryList;
+  allMemoryList = new FreeNodeArrayList;
+  allMemoryList->next = 0;
+  allMemoryList->memory = 0;
+  currentMemoryBlock = allMemoryList;
 }
 
 void VoronoiDiagramGenerator::cleanupEdges()
 {
-	GraphEdge* geCurrent = 0, *gePrev = 0;
-	geCurrent = gePrev = allEdges;
+  GraphEdge *geCurrent = 0, *gePrev = 0;
+  geCurrent = gePrev = allEdges;
 
-	while(geCurrent != 0 && geCurrent->next != 0)
-	{
-		gePrev = geCurrent;
-		geCurrent = geCurrent->next;
-		delete gePrev;
-	}
-
-	allEdges = 0;
+  while (geCurrent != 0 && geCurrent->next != 0)
+  {
+    gePrev = geCurrent;
+    geCurrent = geCurrent->next;
+    delete gePrev;
+  }
 
+  allEdges = 0;
 }
 
 void VoronoiDiagramGenerator::pushGraphEdge(float x1, float y1, float x2, float y2, int s[2])
 {
-	GraphEdge* newEdge = new GraphEdge;
-	newEdge->next = allEdges;
-	allEdges = newEdge;
-	newEdge->x1 = x1;
-	newEdge->y1 = y1;
-	newEdge->x2 = x2;
-	newEdge->y2 = y2;
-	std::copy(s, s+2, newEdge->sites);
+  GraphEdge* newEdge = new GraphEdge;
+  newEdge->next = allEdges;
+  allEdges = newEdge;
+  newEdge->x1 = x1;
+  newEdge->y1 = y1;
+  newEdge->x2 = x2;
+  newEdge->y2 = y2;
+  std::copy(s, s + 2, newEdge->sites);
 }
 
-
-char * VoronoiDiagramGenerator::myalloc(unsigned n)
+char* VoronoiDiagramGenerator::myalloc(unsigned n)
 {
-	char *t=0;	
-	t=(char*)malloc(n);
-	total_alloc += n;
-	return(t);
+  char* t = 0;
+  t = (char*)malloc(n);
+  total_alloc += n;
+  return (t);
 }
 
-
 /* for those who don't have Cherry's plot */
 /* #include <plot.h> */
-void VoronoiDiagramGenerator::openpl(){}
+void VoronoiDiagramGenerator::openpl()
+{
+}
 void VoronoiDiagramGenerator::line(float x1, float y1, float x2, float y2, int s[2])
-{	
-	pushGraphEdge(x1,y1,x2,y2,s);
-
-}
-void VoronoiDiagramGenerator::circle(float x, float y, float radius){}
-void VoronoiDiagramGenerator::range(float minX, float minY, float maxX, float maxY){}
-
-
-
-void VoronoiDiagramGenerator::out_bisector(struct Edge *e)
 {
-	
-
+  pushGraphEdge(x1, y1, x2, y2, s);
 }
-
-
-void VoronoiDiagramGenerator::out_ep(struct Edge *e)
+void VoronoiDiagramGenerator::circle(float x, float y, float radius)
 {
-	
-	
 }
-
-void VoronoiDiagramGenerator::out_vertex(struct Site *v)
+void VoronoiDiagramGenerator::range(float minX, float minY, float maxX, float maxY)
 {
-	
 }
 
-
-void VoronoiDiagramGenerator::out_site(struct Site *s)
+void VoronoiDiagramGenerator::out_bisector(struct Edge* e)
 {
-	if(!triangulate & plot & !debug)
-		circle (s->coord.x, s->coord.y, cradius);
-	
 }
 
-
-void VoronoiDiagramGenerator::out_triple(struct Site *s1, struct Site *s2,struct Site * s3)
+void VoronoiDiagramGenerator::out_ep(struct Edge* e)
 {
-	
 }
 
+void VoronoiDiagramGenerator::out_vertex(struct Site* v)
+{
+}
 
+void VoronoiDiagramGenerator::out_site(struct Site* s)
+{
+  if (!triangulate & plot & !debug)
+    circle(s->coord.x, s->coord.y, cradius);
+}
+
+void VoronoiDiagramGenerator::out_triple(struct Site* s1, struct Site* s2, struct Site* s3)
+{
+}
 
 void VoronoiDiagramGenerator::plotinit()
 {
-	float dx,dy,d;
-	
-	dy = ymax - ymin;
-	dx = xmax - xmin;
-	d = (float)(( dx > dy ? dx : dy) * 1.1);
-	pxmin = (float)(xmin - (d-dx)/2.0);
-	pxmax = (float)(xmax + (d-dx)/2.0);
-	pymin = (float)(ymin - (d-dy)/2.0);
-	pymax = (float)(ymax + (d-dy)/2.0);
-	cradius = (float)((pxmax - pxmin)/350.0);
-	openpl();
-	range(pxmin, pymin, pxmax, pymax);
+  float dx, dy, d;
+
+  dy = ymax - ymin;
+  dx = xmax - xmin;
+  d = (float)((dx > dy ? dx : dy) * 1.1);
+  pxmin = (float)(xmin - (d - dx) / 2.0);
+  pxmax = (float)(xmax + (d - dx) / 2.0);
+  pymin = (float)(ymin - (d - dy) / 2.0);
+  pymax = (float)(ymax + (d - dy) / 2.0);
+  cradius = (float)((pxmax - pxmin) / 350.0);
+  openpl();
+  range(pxmin, pymin, pxmax, pymax);
 }
 
-void VoronoiDiagramGenerator::clip_line(struct Edge *e)
+void VoronoiDiagramGenerator::clip_line(struct Edge* e)
 {
-	struct Site *s1, *s2;
-	float x1=0,x2=0,y1=0,y2=0, temp = 0;;
+  struct Site *s1, *s2;
+  float x1 = 0, x2 = 0, y1 = 0, y2 = 0, temp = 0;
+  ;
 
-	x1 = e->reg[0]->coord.x;
-	x2 = e->reg[1]->coord.x;
-	y1 = e->reg[0]->coord.y;
-	y2 = e->reg[1]->coord.y;
+  x1 = e->reg[0]->coord.x;
+  x2 = e->reg[1]->coord.x;
+  y1 = e->reg[0]->coord.y;
+  y2 = e->reg[1]->coord.y;
 
-	//if the distance between the two points this line was created from is less than 
-	//the square root of 2, then ignore it
-	if(sqrt(((x2 - x1) * (x2 - x1)) + ((y2 - y1) * (y2 - y1))) < minDistanceBetweenSites)
-	{
-		return;
-	}
-	pxmin = borderMinX;
-	pxmax = borderMaxX;
-	pymin = borderMinY;
-	pymax = borderMaxY;
+  // if the distance between the two points this line was created from is less than
+  // the square root of 2, then ignore it
+  if (sqrt(((x2 - x1) * (x2 - x1)) + ((y2 - y1) * (y2 - y1))) < minDistanceBetweenSites)
+  {
+    return;
+  }
+  pxmin = borderMinX;
+  pxmax = borderMaxX;
+  pymin = borderMinY;
+  pymax = borderMaxY;
 
-	if(e -> a == 1.0 && e ->b >= 0.0)
-	{	
-		s1 = e -> ep[1];
-		s2 = e -> ep[0];
-	}
-	else 
-	{
-		s1 = e -> ep[0];
-		s2 = e -> ep[1];
-	};
+  if (e->a == 1.0 && e->b >= 0.0)
+  {
+    s1 = e->ep[1];
+    s2 = e->ep[0];
+  }
+  else
+  {
+    s1 = e->ep[0];
+    s2 = e->ep[1];
+  };
 
-	if(e -> a == 1.0)
-	{
-		y1 = pymin;
-		if (s1!=(struct Site *)NULL && s1->coord.y > pymin)
-		{
-			y1 = s1->coord.y;
-		}
-		if(y1>pymax) 
-		{
-		//	printf("\nClipped (1) y1 = %f to %f",y1,pymax);
-			y1 = pymax;
-			//return;
-		}
-		x1 = e -> c - e -> b * y1;
-		y2 = pymax;
-		if (s2!=(struct Site *)NULL && s2->coord.y < pymax) 
-			y2 = s2->coord.y;
+  if (e->a == 1.0)
+  {
+    y1 = pymin;
+    if (s1 != (struct Site*)NULL && s1->coord.y > pymin)
+    {
+      y1 = s1->coord.y;
+    }
+    if (y1 > pymax)
+    {
+      //	printf("\nClipped (1) y1 = %f to %f",y1,pymax);
+      y1 = pymax;
+      // return;
+    }
+    x1 = e->c - e->b * y1;
+    y2 = pymax;
+    if (s2 != (struct Site*)NULL && s2->coord.y < pymax)
+      y2 = s2->coord.y;
 
-		if(y2<pymin) 
-		{
-			//printf("\nClipped (2) y2 = %f to %f",y2,pymin);
-			y2 = pymin;
-			//return;
-		}
-		x2 = (e->c) - (e->b) * y2;
-		if (((x1> pxmax) & (x2>pxmax)) | ((x1<pxmin)&(x2<pxmin))) 
-		{
-			//printf("\nClipLine jumping out(3), x1 = %f, pxmin = %f, pxmax = %f",x1,pxmin,pxmax);
-			return;
-		}
-		if(x1> pxmax)
-		{	x1 = pxmax; y1 = (e -> c - x1)/e -> b;};
-		if(x1<pxmin)
-		{	x1 = pxmin; y1 = (e -> c - x1)/e -> b;};
-		if(x2>pxmax)
-		{	x2 = pxmax; y2 = (e -> c - x2)/e -> b;};
-		if(x2<pxmin)
-		{	x2 = pxmin; y2 = (e -> c - x2)/e -> b;};
-	}
-	else
-	{
-		x1 = pxmin;
-		if (s1!=(struct Site *)NULL && s1->coord.x > pxmin) 
-			x1 = s1->coord.x;
-		if(x1>pxmax) 
-		{
-			//printf("\nClipped (3) x1 = %f to %f",x1,pxmin);
-			//return;
-			x1 = pxmax;
-		}
-		y1 = e -> c - e -> a * x1;
-		x2 = pxmax;
-		if (s2!=(struct Site *)NULL && s2->coord.x < pxmax) 
-			x2 = s2->coord.x;
-		if(x2<pxmin) 
-		{
-			//printf("\nClipped (4) x2 = %f to %f",x2,pxmin);
-			//return;
-			x2 = pxmin;
-		}
-		y2 = e -> c - e -> a * x2;
-		if (((y1> pymax) & (y2>pymax)) | ((y1<pymin)&(y2<pymin))) 
-		{
-			//printf("\nClipLine jumping out(6), y1 = %f, pymin = %f, pymax = %f",y2,pymin,pymax);
-			return;
-		}
-		if(y1> pymax)
-		{	y1 = pymax; x1 = (e -> c - y1)/e -> a;};
-		if(y1<pymin)
-		{	y1 = pymin; x1 = (e -> c - y1)/e -> a;};
-		if(y2>pymax)
-		{	y2 = pymax; x2 = (e -> c - y2)/e -> a;};
-		if(y2<pymin)
-		{	y2 = pymin; x2 = (e -> c - y2)/e -> a;};
-	};
-	
-	//printf("Pushing line (%f,%f,%f,%f)\n",x1,y1,x2,y2);
-	line(x1,y1,x2,y2,e->sites);
+    if (y2 < pymin)
+    {
+      // printf("\nClipped (2) y2 = %f to %f",y2,pymin);
+      y2 = pymin;
+      // return;
+    }
+    x2 = (e->c) - (e->b) * y2;
+    if (((x1 > pxmax) & (x2 > pxmax)) | ((x1 < pxmin) & (x2 < pxmin)))
+    {
+      // printf("\nClipLine jumping out(3), x1 = %f, pxmin = %f, pxmax = %f",x1,pxmin,pxmax);
+      return;
+    }
+    if (x1 > pxmax)
+    {
+      x1 = pxmax;
+      y1 = (e->c - x1) / e->b;
+    };
+    if (x1 < pxmin)
+    {
+      x1 = pxmin;
+      y1 = (e->c - x1) / e->b;
+    };
+    if (x2 > pxmax)
+    {
+      x2 = pxmax;
+      y2 = (e->c - x2) / e->b;
+    };
+    if (x2 < pxmin)
+    {
+      x2 = pxmin;
+      y2 = (e->c - x2) / e->b;
+    };
+  }
+  else
+  {
+    x1 = pxmin;
+    if (s1 != (struct Site*)NULL && s1->coord.x > pxmin)
+      x1 = s1->coord.x;
+    if (x1 > pxmax)
+    {
+      // printf("\nClipped (3) x1 = %f to %f",x1,pxmin);
+      // return;
+      x1 = pxmax;
+    }
+    y1 = e->c - e->a * x1;
+    x2 = pxmax;
+    if (s2 != (struct Site*)NULL && s2->coord.x < pxmax)
+      x2 = s2->coord.x;
+    if (x2 < pxmin)
+    {
+      // printf("\nClipped (4) x2 = %f to %f",x2,pxmin);
+      // return;
+      x2 = pxmin;
+    }
+    y2 = e->c - e->a * x2;
+    if (((y1 > pymax) & (y2 > pymax)) | ((y1 < pymin) & (y2 < pymin)))
+    {
+      // printf("\nClipLine jumping out(6), y1 = %f, pymin = %f, pymax = %f",y2,pymin,pymax);
+      return;
+    }
+    if (y1 > pymax)
+    {
+      y1 = pymax;
+      x1 = (e->c - y1) / e->a;
+    };
+    if (y1 < pymin)
+    {
+      y1 = pymin;
+      x1 = (e->c - y1) / e->a;
+    };
+    if (y2 > pymax)
+    {
+      y2 = pymax;
+      x2 = (e->c - y2) / e->a;
+    };
+    if (y2 < pymin)
+    {
+      y2 = pymin;
+      x2 = (e->c - y2) / e->a;
+    };
+  };
+
+  // printf("Pushing line (%f,%f,%f,%f)\n",x1,y1,x2,y2);
+  line(x1, y1, x2, y2, e->sites);
 }
 
-
 /* implicit parameters: nsites, sqrt_nsites, xmin, xmax, ymin, ymax,
 deltax, deltay (can all be estimates).
 Performance suffers if they are wrong; better to make nsites,
@@ -862,148 +881,152 @@ deltax, and deltay too big than too small.  (?) */
 
 bool VoronoiDiagramGenerator::voronoi(int triangulate)
 {
-	struct Site *newsite, *bot, *top, *temp, *p;
-	struct Site *v;
-	struct Point newintstar;
-	int pm;
-	struct Halfedge *lbnd, *rbnd, *llbnd, *rrbnd, *bisector;
-	struct Edge *e;
-	
-	PQinitialize();
-	bottomsite = nextone();
-	out_site(bottomsite);
-	bool retval = ELinitialize();
+  struct Site *newsite, *bot, *top, *temp, *p;
+  struct Site* v;
+  struct Point newintstar;
+  int pm;
+  struct Halfedge *lbnd, *rbnd, *llbnd, *rrbnd, *bisector;
+  struct Edge* e;
 
-	if(!retval)
-		return false;
-	
-	newsite = nextone();
-	while(1)
-	{
+  PQinitialize();
+  bottomsite = nextone();
+  out_site(bottomsite);
+  bool retval = ELinitialize();
 
-		if(!PQempty()) 
-			newintstar = PQ_min();
-		
-		//if the lowest site has a smaller y value than the lowest vector intersection, process the site
-		//otherwise process the vector intersection		
+  if (!retval)
+    return false;
 
-		if (newsite != (struct Site *)NULL 	&& (PQempty() || newsite -> coord.y < newintstar.y
-			|| (newsite->coord.y == newintstar.y && newsite->coord.x < newintstar.x)))
-		{/* new site is smallest - this is a site event*/
-			out_site(newsite);						//output the site
-			lbnd = ELleftbnd(&(newsite->coord));				//get the first HalfEdge to the LEFT of the new site
-			rbnd = ELright(lbnd);						//get the first HalfEdge to the RIGHT of the new site
-			bot = rightreg(lbnd);						//if this halfedge has no edge, , bot = bottom site (whatever that is)
-			e = bisect(bot, newsite);					//create a new edge that bisects 
-			bisector = HEcreate(e, le);					//create a new HalfEdge, setting its ELpm field to 0			
-			ELinsert(lbnd, bisector);					//insert this new bisector edge between the left and right vectors in a linked list	
-			//printf("Newsite %d: %f,%f\n",newsite->sitenbr,newsite->coord.x,newsite->coord.y);
+  newsite = nextone();
+  while (1)
+  {
+    if (!PQempty())
+      newintstar = PQ_min();
 
-			if ((p = intersect(lbnd, bisector)) != (struct Site *) NULL) 	//if the new bisector intersects with the left edge, remove the left edge's vertex, and put in the new one
-			{	
-				PQdelete(lbnd);
-				PQinsert(lbnd, p, dist(p,newsite));
-			};
-			lbnd = bisector;						
-			bisector = HEcreate(e, re);					//create a new HalfEdge, setting its ELpm field to 1
-			ELinsert(lbnd, bisector);					//insert the new HE to the right of the original bisector earlier in the IF stmt
+    // if the lowest site has a smaller y value than the lowest vector intersection, process the site
+    // otherwise process the vector intersection
 
-			if ((p = intersect(bisector, rbnd)) != (struct Site *) NULL)	//if this new bisector intersects with the
-			{	
-				PQinsert(bisector, p, dist(p,newsite));			//push the HE into the ordered linked list of vertices
-			};
-			newsite = nextone();	
-		}
-		else if (!PQempty()) /* intersection is smallest - this is a vector event */			
-		{	
-			lbnd = PQextractmin();						//pop the HalfEdge with the lowest vector off the ordered list of vectors				
-			llbnd = ELleft(lbnd);						//get the HalfEdge to the left of the above HE
-			rbnd = ELright(lbnd);						//get the HalfEdge to the right of the above HE
-			rrbnd = ELright(rbnd);						//get the HalfEdge to the right of the HE to the right of the lowest HE 
-			bot = leftreg(lbnd);						//get the Site to the left of the left HE which it bisects
-			top = rightreg(rbnd);						//get the Site to the right of the right HE which it bisects
+    if (newsite != (struct Site*)NULL && (PQempty() || newsite->coord.y < newintstar.y ||
+                                          (newsite->coord.y == newintstar.y && newsite->coord.x < newintstar.x)))
+    {                                       /* new site is smallest - this is a site event*/
+      out_site(newsite);                    // output the site
+      lbnd = ELleftbnd(&(newsite->coord));  // get the first HalfEdge to the LEFT of the new site
+      rbnd = ELright(lbnd);                 // get the first HalfEdge to the RIGHT of the new site
+      bot = rightreg(lbnd);                 // if this halfedge has no edge, , bot = bottom site (whatever that is)
+      e = bisect(bot, newsite);             // create a new edge that bisects
+      bisector = HEcreate(e, le);           // create a new HalfEdge, setting its ELpm field to 0
+      ELinsert(lbnd, bisector);  // insert this new bisector edge between the left and right vectors in a linked list
+      // printf("Newsite %d: %f,%f\n",newsite->sitenbr,newsite->coord.x,newsite->coord.y);
 
-			out_triple(bot, top, rightreg(lbnd));		//output the triple of sites, stating that a circle goes through them
+      if ((p = intersect(lbnd, bisector)) != (struct Site*)NULL)  // if the new bisector intersects with the left edge,
+                                                                  // remove the left edge's vertex, and put in the new
+                                                                  // one
+      {
+        PQdelete(lbnd);
+        PQinsert(lbnd, p, dist(p, newsite));
+      };
+      lbnd = bisector;
+      bisector = HEcreate(e, re);  // create a new HalfEdge, setting its ELpm field to 1
+      ELinsert(lbnd, bisector);    // insert the new HE to the right of the original bisector earlier in the IF stmt
 
-			v = lbnd->vertex;						//get the vertex that caused this event
-			makevertex(v);							//set the vertex number - couldn't do this earlier since we didn't know when it would be processed
-			endpoint(lbnd->ELedge,lbnd->ELpm,v);	//set the endpoint of the left HalfEdge to be this vector
-			endpoint(rbnd->ELedge,rbnd->ELpm,v);	//set the endpoint of the right HalfEdge to be this vector
-			ELdelete(lbnd);							//mark the lowest HE for deletion - can't delete yet because there might be pointers to it in Hash Map	
-			PQdelete(rbnd);							//remove all vertex events to do with the  right HE
-			ELdelete(rbnd);							//mark the right HE for deletion - can't delete yet because there might be pointers to it in Hash Map	
-			pm = le;								//set the pm variable to zero
-			
-			if (bot->coord.y > top->coord.y)		//if the site to the left of the event is higher than the Site
-			{										//to the right of it, then swap them and set the 'pm' variable to 1
-				temp = bot; 
-				bot = top; 
-				top = temp; 
-				pm = re;
-			}
-			e = bisect(bot, top);					//create an Edge (or line) that is between the two Sites. This creates
-													//the formula of the line, and assigns a line number to it
-			bisector = HEcreate(e, pm);				//create a HE from the Edge 'e', and make it point to that edge with its ELedge field
-			ELinsert(llbnd, bisector);				//insert the new bisector to the right of the left HE
-			endpoint(e, re-pm, v);					//set one endpoint to the new edge to be the vector point 'v'.
-													//If the site to the left of this bisector is higher than the right
-													//Site, then this endpoint is put in position 0; otherwise in pos 1
-			deref(v);								//delete the vector 'v'
+      if ((p = intersect(bisector, rbnd)) != (struct Site*)NULL)  // if this new bisector intersects with the
+      {
+        PQinsert(bisector, p, dist(p, newsite));  // push the HE into the ordered linked list of vertices
+      };
+      newsite = nextone();
+    }
+    else if (!PQempty()) /* intersection is smallest - this is a vector event */
+    {
+      lbnd = PQextractmin();  // pop the HalfEdge with the lowest vector off the ordered list of vectors
+      llbnd = ELleft(lbnd);   // get the HalfEdge to the left of the above HE
+      rbnd = ELright(lbnd);   // get the HalfEdge to the right of the above HE
+      rrbnd = ELright(rbnd);  // get the HalfEdge to the right of the HE to the right of the lowest HE
+      bot = leftreg(lbnd);    // get the Site to the left of the left HE which it bisects
+      top = rightreg(rbnd);   // get the Site to the right of the right HE which it bisects
 
-			//if left HE and the new bisector don't intersect, then delete the left HE, and reinsert it 
-			if((p = intersect(llbnd, bisector)) != (struct Site *) NULL)
-			{	
-				PQdelete(llbnd);
-				PQinsert(llbnd, p, dist(p,bot));
-			};
+      out_triple(bot, top, rightreg(lbnd));  // output the triple of sites, stating that a circle goes through them
 
-			//if right HE and the new bisector don't intersect, then reinsert it 
-			if ((p = intersect(bisector, rrbnd)) != (struct Site *) NULL)
-			{	
-				PQinsert(bisector, p, dist(p,bot));
-			};
-		}
-		else break;
-	};
+      v = lbnd->vertex;  // get the vertex that caused this event
+      makevertex(v);     // set the vertex number - couldn't do this earlier since we didn't know when it would be
+                      // processed
+      endpoint(lbnd->ELedge, lbnd->ELpm, v);  // set the endpoint of the left HalfEdge to be this vector
+      endpoint(rbnd->ELedge, rbnd->ELpm, v);  // set the endpoint of the right HalfEdge to be this vector
+      ELdelete(lbnd);  // mark the lowest HE for deletion - can't delete yet because there might be pointers to it in
+                       // Hash Map
+      PQdelete(rbnd);  // remove all vertex events to do with the  right HE
+      ELdelete(
+          rbnd);  // mark the right HE for deletion - can't delete yet because there might be pointers to it in Hash Map
+      pm = le;    // set the pm variable to zero
 
-	
+      if (bot->coord.y > top->coord.y)  // if the site to the left of the event is higher than the Site
+      {                                 // to the right of it, then swap them and set the 'pm' variable to 1
+        temp = bot;
+        bot = top;
+        top = temp;
+        pm = re;
+      }
+      e = bisect(bot, top);  // create an Edge (or line) that is between the two Sites. This creates
+      // the formula of the line, and assigns a line number to it
+      bisector = HEcreate(e, pm);  // create a HE from the Edge 'e', and make it point to that edge with its ELedge
+                                   // field
+      ELinsert(llbnd, bisector);  // insert the new bisector to the right of the left HE
+      endpoint(e, re - pm, v);    // set one endpoint to the new edge to be the vector point 'v'.
+      // If the site to the left of this bisector is higher than the right
+      // Site, then this endpoint is put in position 0; otherwise in pos 1
+      deref(v);  // delete the vector 'v'
 
+      // if left HE and the new bisector don't intersect, then delete the left HE, and reinsert it
+      if ((p = intersect(llbnd, bisector)) != (struct Site*)NULL)
+      {
+        PQdelete(llbnd);
+        PQinsert(llbnd, p, dist(p, bot));
+      };
 
-	for(lbnd=ELright(ELleftend); lbnd != ELrightend; lbnd=ELright(lbnd))
-	{	
-		e = lbnd -> ELedge;
+      // if right HE and the new bisector don't intersect, then reinsert it
+      if ((p = intersect(bisector, rrbnd)) != (struct Site*)NULL)
+      {
+        PQinsert(bisector, p, dist(p, bot));
+      };
+    }
+    else
+      break;
+  };
 
-		clip_line(e);
-	};
+  for (lbnd = ELright(ELleftend); lbnd != ELrightend; lbnd = ELright(lbnd))
+  {
+    e = lbnd->ELedge;
 
-	cleanup();
+    clip_line(e);
+  };
 
-	return true;
-	
+  cleanup();
+
+  return true;
 }
 
-
-int scomp(const void *p1,const void *p2)
+int scomp(const void* p1, const void* p2)
 {
-	struct Point *s1 = (Point*)p1, *s2=(Point*)p2;
-	if(s1 -> y < s2 -> y) return(-1);
-	if(s1 -> y > s2 -> y) return(1);
-	if(s1 -> x < s2 -> x) return(-1);
-	if(s1 -> x > s2 -> x) return(1);
-	return(0);
+  struct Point *s1 = (Point *)p1, *s2 = (Point *)p2;
+  if (s1->y < s2->y)
+    return (-1);
+  if (s1->y > s2->y)
+    return (1);
+  if (s1->x < s2->x)
+    return (-1);
+  if (s1->x > s2->x)
+    return (1);
+  return (0);
 }
 
 /* return a single in-storage site */
-struct Site * VoronoiDiagramGenerator::nextone()
+struct Site* VoronoiDiagramGenerator::nextone()
 {
-	struct Site *s;
-	if(siteidx < nsites)
-	{	
-		s = &sites[siteidx];
-		siteidx += 1;
-		return(s);
-	}
-	else	
-		return( (struct Site *)NULL);
+  struct Site* s;
+  if (siteidx < nsites)
+  {
+    s = &sites[siteidx];
+    siteidx += 1;
+    return (s);
+  }
+  else
+    return ((struct Site*)NULL);
 }
-
diff --git a/src/buzz_update.cpp b/src/buzz_update.cpp
index 8624c3a..72d5787 100644
--- a/src/buzz_update.cpp
+++ b/src/buzz_update.cpp
@@ -8,626 +8,622 @@
 
 #include "buzz_update.h"
 
-namespace buzz_update{
-  /*Temp for data collection*/
-  // static int neigh=-1;
-  static struct timeval t1, t2;
-  static int timer_steps = 0;
-  // static clock_t end;
+namespace buzz_update
+{
+/*Temp for data collection*/
+// static int neigh=-1;
+static struct timeval t1, t2;
+static int timer_steps = 0;
+// static clock_t end;
 
-  /*Temp end */
-  static int fd, wd = 0;
-  static int old_update = 0;
-  static buzz_updater_elem_t updater;
-  static int no_of_robot;
-  static const char* dbgf_name;
-  static const char* bcfname;
-  static const char* old_bcfname = "old_bcode.bo";
-  static const char* bzz_file;
-  static int Robot_id = 0;
-  static int neigh = -1;
-  static int updater_msg_ready;
-  static uint16_t update_try_counter = 0;
-  static const uint16_t MAX_UPDATE_TRY = 5;
-  static size_t old_byte_code_size = 0;
-  static bool debug = false;
+/*Temp end */
+static int fd, wd = 0;
+static int old_update = 0;
+static buzz_updater_elem_t updater;
+static int no_of_robot;
+static const char* dbgf_name;
+static const char* bcfname;
+static const char* old_bcfname = "old_bcode.bo";
+static const char* bzz_file;
+static int Robot_id = 0;
+static int neigh = -1;
+static int updater_msg_ready;
+static uint16_t update_try_counter = 0;
+static const uint16_t MAX_UPDATE_TRY = 5;
+static size_t old_byte_code_size = 0;
+static bool debug = false;
 
-  /*Initialize updater*/
-  int init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id)
+/*Initialize updater*/
+int init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id)
+{
+  buzzvm_t VM = buzz_utility::get_vm();
+  buzzvm_pushs(VM, buzzvm_string_register(VM, "updates_active", 1));
+  buzzvm_gload(VM);
+  buzzobj_t obj = buzzvm_stack_at(VM, 1);
+  if (obj->o.type == BUZZTYPE_INT && obj->i.value == 1)
   {
-    buzzvm_t VM = buzz_utility::get_vm();
-    buzzvm_pushs(VM, buzzvm_string_register(VM, "updates_active", 1));
-    buzzvm_gload(VM);
-    buzzobj_t obj = buzzvm_stack_at(VM, 1);
-    if(obj->o.type == BUZZTYPE_INT && obj->i.value == 1){
-      Robot_id = robot_id;
-      dbgf_name = dbgfname;
-      bcfname = bo_filename;
-      ROS_WARN("UPDATES TURNED ON (modifying .bzz script file will update all robots)");
-      if(debug) ROS_INFO("Initializing file monitor...");
-      fd = inotify_init1(IN_NONBLOCK);
-      if (fd < 0)
-      {
-        perror("inotify_init error");
-      }
-      /*If simulation set the file monitor only for robot 0*/
-      if (SIMULATION == 1 && robot_id == 0)
-      {
-        /* watch /.bzz file for any activity and report it back to update */
-        wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS);
-      }
-      else if (SIMULATION == 0)
-      {
-        /* watch /.bzz file for any activity and report it back to update */
-        wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS);
-      }
-      /*load the .bo under execution into the updater*/
-      uint8_t* BO_BUF = 0;
-      FILE* fp = fopen(bo_filename, "rb");
-      if (!fp)
-      {
-        perror(bo_filename);
-      }
-      fseek(fp, 0, SEEK_END);
-      size_t bcode_size = ftell(fp);
-      rewind(fp);
-      BO_BUF = (uint8_t*)malloc(bcode_size);
-      if (fread(BO_BUF, 1, bcode_size, fp) < bcode_size)
-      {
-        perror(bo_filename);
-        // fclose(fp);
-        // return 0;
-      }
-      fclose(fp);
-      /*Load stand_by .bo file into the updater*/
-      uint8_t* STD_BO_BUF = 0;
-      fp = fopen(stand_by_script, "rb");
-      if (!fp)
-      {
-        perror(stand_by_script);
-      }
-      fseek(fp, 0, SEEK_END);
-      size_t stdby_bcode_size = ftell(fp);
-      rewind(fp);
-      STD_BO_BUF = (uint8_t*)malloc(stdby_bcode_size);
-      if (fread(STD_BO_BUF, 1, stdby_bcode_size, fp) < stdby_bcode_size)
-      {
-        perror(stand_by_script);
-        // fclose(fp);
-        // return 0;
-      }
-      fclose(fp);
-      /*Create the updater*/
-      updater = (buzz_updater_elem_t)malloc(sizeof(struct buzz_updater_elem_s));
-      /*Intialize the updater with the required data*/
-      updater->bcode = BO_BUF;
-      /*Store a copy of the Bcode for rollback*/
-      updater->outmsg_queue = NULL;
-      updater->inmsg_queue = NULL;
-      updater->patch = NULL;
-      updater->patch_size = (size_t*)malloc(sizeof(size_t));
-      updater->bcode_size = (size_t*)malloc(sizeof(size_t));
-      updater->update_no = (uint8_t*)malloc(sizeof(uint16_t));
-      *(uint16_t*)(updater->update_no) = 0;
-      *(size_t*)(updater->bcode_size) = bcode_size;
-      updater->standby_bcode = STD_BO_BUF;
-      updater->standby_bcode_size = (size_t*)malloc(sizeof(size_t));
-      *(size_t*)(updater->standby_bcode_size) = stdby_bcode_size;
-      updater->mode = (int*)malloc(sizeof(int));
-      *(int*)(updater->mode) = CODE_RUNNING;
-      // no_of_robot=barrier;
-      updater_msg_ready = 0;
-
-      /*Write the bcode to a file for rollback*/
-      fp = fopen("old_bcode.bo", "wb");
-      fwrite((updater->bcode), *(size_t*)updater->bcode_size, 1, fp);
-      fclose(fp);
-      return 1;
-    }
-    else{
-      ROS_WARN("UPDATES TURNED OFF... (Hint: Include update.bzz to turn on updates)");
-      return 0;
-    } 
-  }
-  /*Check for .bzz file chages*/
-  int check_update()
-  {
-    char buf[1024];
-    int check = 0;
-    int i = 0;
-    int len = read(fd, buf, 1024);
-    while (i < len)
+    Robot_id = robot_id;
+    dbgf_name = dbgfname;
+    bcfname = bo_filename;
+    ROS_WARN("UPDATES TURNED ON (modifying .bzz script file will update all robots)");
+    if (debug)
+      ROS_INFO("Initializing file monitor...");
+    fd = inotify_init1(IN_NONBLOCK);
+    if (fd < 0)
     {
-      struct inotify_event* event = (struct inotify_event*)&buf[i];
-      /*Report file changes and self deletes*/
-      if (event->mask & (IN_MODIFY | IN_DELETE_SELF))
-      {
-        /*Respawn watch if the file is self deleted */
-        inotify_rm_watch(fd, wd);
-        close(fd);
-        fd = inotify_init1(IN_NONBLOCK);
-        wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS);
-        /*To mask multiple writes from editors*/
-        if (!old_update)
-        {
-          check = 1;
-          old_update = 1;
-        }
-      }
-      /*Update index to start of next event*/
-      i += sizeof(struct inotify_event) + event->len;
+      perror("inotify_init error");
     }
-    if (!check)
-      old_update = 0;
-    return check;
-  }
-
-  int test_patch(std::string& path, std::string& name1, size_t update_patch_size, uint8_t* patch)
-  {
-    if (SIMULATION == 1)
+    /*If simulation set the file monitor only for robot 0*/
+    if (SIMULATION == 1 && robot_id == 0)
     {
-      return 1;
+      /* watch /.bzz file for any activity and report it back to update */
+      wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS);
     }
-    else
+    else if (SIMULATION == 0)
     {
-      /*Patch the old bo with new patch*/
-      std::stringstream patch_writefile;
-      patch_writefile << path << name1 << "tmp_patch.bo";
-      /*Write the patch to a file and call bsdiff to patch*/
-      FILE* fp = fopen(patch_writefile.str().c_str(), "wb");
-      fwrite(patch, update_patch_size, 1, fp);
-      fclose(fp);
-      std::stringstream patch_exsisting;
-      patch_exsisting << "bspatch " << path << name1 << ".bo " << path << name1 << "-patched.bo " << path << name1
-                      << "tmp_patch.bo";
-      if(debug) ROS_WARN("Launching bspatch command: %s", patch_exsisting.str().c_str());
-      if (system(patch_exsisting.str().c_str()))
-        return 0;
-      else
-        return 1;
+      /* watch /.bzz file for any activity and report it back to update */
+      wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS);
     }
-  }
-
-  updater_code_t obtain_patched_bo(std::string& path, std::string& name1)
-  {
-    if (SIMULATION == 1)
-    {
-      /*Read the exsisting file to simulate the patching*/
-      std::stringstream read_patched;
-      read_patched << path << name1 << ".bo";
-      if(debug){ 
-        ROS_WARN("Simulation patching ...");
-        ROS_WARN("Read file for patching %s", read_patched.str().c_str());
-      }
-      uint8_t* patched_bo_buf = 0;
-      FILE* fp = fopen(read_patched.str().c_str(), "rb");
-      if (!fp)
-      {
-        perror(read_patched.str().c_str());
-      }
-      fseek(fp, 0, SEEK_END);
-      size_t patched_size = ftell(fp);
-      rewind(fp);
-      patched_bo_buf = (uint8_t*)malloc(patched_size);
-      if (fread(patched_bo_buf, 1, patched_size, fp) < patched_size)
-      {
-        perror(read_patched.str().c_str());
-      }
-      fclose(fp);
-      /*Write the patched to a code struct and return*/
-      updater_code_t update_code = (updater_code_t)malloc(sizeof(struct updater_code_s));
-      update_code->bcode = patched_bo_buf;
-      update_code->bcode_size = (uint8_t*)malloc(sizeof(uint16_t));
-      *(uint16_t*)(update_code->bcode_size) = patched_size;
-
-      return update_code;
-    }
-
-    else
-    {
-      /*Read the new patched file*/
-      std::stringstream read_patched;
-      read_patched << path << name1 << "-patched.bo";
-      if(debug) {
-        ROS_WARN("Robot patching ...");
-        ROS_WARN("Read file for patching %s", read_patched.str().c_str());
-      }
-      uint8_t* patched_bo_buf = 0;
-      FILE* fp = fopen(read_patched.str().c_str(), "rb");
-      if (!fp)
-      {
-        perror(read_patched.str().c_str());
-      }
-      fseek(fp, 0, SEEK_END);
-      size_t patched_size = ftell(fp);
-      rewind(fp);
-      patched_bo_buf = (uint8_t*)malloc(patched_size);
-      if (fread(patched_bo_buf, 1, patched_size, fp) < patched_size)
-      {
-        perror(read_patched.str().c_str());
-      }
-      fclose(fp);
-
-      /* delete old bo file & rename new bo file */
-      remove((path + name1 + ".bo").c_str());
-      rename((path + name1 + "-patched.bo").c_str(), (path + name1 + ".bo").c_str());
-
-      /*Write the patched to a code struct and return*/
-      updater_code_t update_code = (updater_code_t)malloc(sizeof(struct updater_code_s));
-      update_code->bcode = patched_bo_buf;
-      update_code->bcode_size = (uint8_t*)malloc(sizeof(uint16_t));
-      *(uint16_t*)(update_code->bcode_size) = patched_size;
-
-      return update_code;
-    }
-  }
-
-  void code_message_outqueue_append()
-  {
-    updater->outmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s));
-    /* if size less than 250 Pad with zeors to assure transmission*/
-    uint16_t size = UPDATE_CODE_HEADER_SIZE + *(size_t*)(updater->patch_size);
-    uint16_t padding_size = (size > MIN_UPDATE_PACKET) ? 0 : MIN_UPDATE_PACKET - size;
-    size += padding_size;
-    updater->outmsg_queue->queue = (uint8_t*)malloc(size);
-    memset(updater->outmsg_queue->queue, 0, size);
-    updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t));
-    *(uint16_t*)(updater->outmsg_queue->size) = size;
-    size = 0;
-    /*Append message type*/
-    *(uint8_t*)(updater->outmsg_queue->queue + size) = SENT_CODE;
-    size += sizeof(uint8_t);
-    /*Append the update no, code size and code to out msg*/
-    *(uint16_t*)(updater->outmsg_queue->queue + size) = *(uint16_t*)(updater->update_no);
-    size += sizeof(uint16_t);
-    *(uint16_t*)(updater->outmsg_queue->queue + size) = (uint16_t) * (size_t*)(updater->patch_size);
-    size += sizeof(uint16_t);
-    memcpy(updater->outmsg_queue->queue + size, updater->patch, *(size_t*)(updater->patch_size));
-    // size += (uint16_t) * (size_t*)(updater->patch_size);
-    updater_msg_ready = 1;
-  }
-
-  void outqueue_append_code_request(uint16_t update_no)
-  {
-    updater->outmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s));
-    uint16_t size = 0;
-    updater->outmsg_queue->queue = (uint8_t*)malloc(2 * sizeof(uint16_t) + sizeof(uint8_t) + CODE_REQUEST_PADDING);
-    updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t));
-    memset(updater->outmsg_queue->queue, 0, sizeof(uint16_t) + sizeof(uint8_t) + CODE_REQUEST_PADDING);
-    /*Append message type*/
-    *(uint8_t*)(updater->outmsg_queue->queue + size) = RESEND_CODE;
-    size += sizeof(uint8_t);
-    /*Append the update no, code size and code to out msg*/
-    *(uint16_t*)(updater->outmsg_queue->queue + size) = update_no;
-    size += sizeof(uint16_t);
-    *(uint16_t*)(updater->outmsg_queue->queue + size) = update_try_counter;
-    size += sizeof(uint16_t);
-    *(uint16_t*)(updater->outmsg_queue->size) = size + CODE_REQUEST_PADDING;
-    updater_msg_ready = 1;
-    if(debug) ROS_WARN("[Debug] Requesting update no. %u for rebroadcast, try: %u", update_no, update_try_counter);
-  }
-
-  void code_message_inqueue_append(uint8_t* msg, uint16_t size)
-  {
-    updater->inmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s));
-    // ROS_INFO("[DEBUG] Updater append code of size %d", (int) size);
-    updater->inmsg_queue->queue = (uint8_t*)malloc(size);
-    updater->inmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t));
-    memcpy(updater->inmsg_queue->queue, msg, size);
-    *(uint16_t*)(updater->inmsg_queue->size) = size;
-  }
-  /*Used for data logging*/
-  /*void set_packet_id(int packet_id)
-  {
-    packet_id_ = packet_id;
-  }*/
-  void code_message_inqueue_process()
-  {
-    int size = 0;
-    updater_code_t out_code = NULL;
-    if(debug) {
-      ROS_WARN("[Debug] Updater processing in msg with mode %d", *(int*)(updater->mode));
-      ROS_WARN("[Debug] Current update no: %u, Received update no: %u", (*(uint16_t*)(updater->update_no)),
-               (*(uint16_t*)(updater->inmsg_queue->queue + sizeof(uint8_t))));
-      ROS_WARN("[Debug] Updater received patch of size %u",
-               (*(uint16_t*)(updater->inmsg_queue->queue + sizeof(uint16_t) + sizeof(uint8_t))));
-    }
-    if (*(int*)(updater->mode) == CODE_RUNNING)
-    {
-      // fprintf(stdout,"[debug]Inside inmsg code running");
-      if (*(uint8_t*)(updater->inmsg_queue->queue) == SENT_CODE)
-      {
-        size += sizeof(uint8_t);
-        if (*(uint16_t*)(updater->inmsg_queue->queue + size) > *(uint16_t*)(updater->update_no))
-        {
-          // fprintf(stdout,"[debug]Inside update number comparision");
-          uint16_t update_no = *(uint16_t*)(updater->inmsg_queue->queue + size);
-          size += sizeof(uint16_t);
-          uint16_t update_patch_size = *(uint16_t*)(updater->inmsg_queue->queue + size);
-          size += sizeof(uint16_t);
-          /*Generate patch*/
-          std::string bzzfile_name(bzz_file);
-          std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
-          std::string name1 = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1);
-          name1 = name1.substr(0, name1.find_last_of("."));
-          if (test_patch(path, name1, update_patch_size, (updater->inmsg_queue->queue + size)))
-          {
-            out_code = obtain_patched_bo(path, name1);
-
-            // fprintf(stdout,"in queue process Update no %d\n", (int) update_no);
-            // fprintf(stdout,"in queue process bcode size %d\n", (int) update_bcode_size);
-            // FILE *fp;
-            // fp=fopen("update.bo", "wb");
-            // fwrite((updater->inmsg_queue->queue+size), update_bcode_size, 1, fp);
-            // fclose(fp);
-
-            if (test_set_code((out_code->bcode), (char*)dbgf_name, (size_t)(*(uint16_t*)out_code->bcode_size)))
-            {
-              if(debug) ROS_WARN("Received patch PASSED tests!");
-              *(uint16_t*)updater->update_no = update_no;
-              /*Clear exisiting patch if any*/
-              delete_p(updater->patch);
-              /*copy the patch into the updater*/
-              updater->patch = (uint8_t*)malloc(update_patch_size);
-              memcpy(updater->patch, (updater->inmsg_queue->queue + size), update_patch_size);
-              *(size_t*)(updater->patch_size) = update_patch_size;
-              // code_message_outqueue_append();
-              neigh = 1;
-            }
-            /*clear the temp code buff*/
-            delete_p(out_code->bcode);
-            delete_p(out_code->bcode_size);
-            delete_p(out_code);
-          }
-          else
-          {
-            ROS_ERROR("Patching the .bo file failed");
-            update_try_counter++;
-            outqueue_append_code_request(update_no);
-          }
-        }
-      }
-    }
-    size = 0;
-    if (*(uint8_t*)(updater->inmsg_queue->queue) == RESEND_CODE)
-    {
-      if(debug) ROS_WARN("Patch rebroadcast request received.");
-      size += sizeof(uint8_t);
-      if (*(uint16_t*)(updater->inmsg_queue->queue + size) == *(uint16_t*)(updater->update_no))
-      {
-        size += sizeof(uint16_t);
-        if (*(uint16_t*)(updater->inmsg_queue->queue + size) > update_try_counter)
-        {
-          update_try_counter = *(uint16_t*)(updater->inmsg_queue->queue + size);
-          if(debug) ROS_WARN("Rebroadcasting patch, try : %u", update_try_counter);
-          code_message_outqueue_append();
-        }
-        if (update_try_counter > MAX_UPDATE_TRY)
-          ROS_ERROR("TODO: Max rebroadcast retry reached, ROLL BACK !!");
-      }
-    }
-    // fprintf(stdout,"in queue freed\n");
-    delete_p(updater->inmsg_queue->queue);
-    delete_p(updater->inmsg_queue->size);
-    delete_p(updater->inmsg_queue);
-  }
-
-  void create_update_patch()
-  {
-    std::stringstream genpatch;
-
-    std::string bzzfile_name(bzz_file);
-
-    std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
-
-    std::string name1 = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1);
-    name1 = name1.substr(0, name1.find_last_of("."));
-
-    std::string name2 = name1 + "-update";
-
-    /*Launch bsdiff and create a patch*/
-    genpatch << "bsdiff " << path << name1 << ".bo " << path << name2 << ".bo " << path << "diff.bo";
-    if(debug) ROS_WARN("Launching bsdiff: %s", genpatch.str().c_str());
-    system(genpatch.str().c_str());
-
-    /*Delete old files & rename new files*/
-
-    remove((path + name1 + ".bo").c_str());
-    remove((path + name1 + ".bdb").c_str());
-
-    rename((path + name2 + ".bo").c_str(), (path + name1 + ".bo").c_str());
-    rename((path + name2 + ".bdb").c_str(), (path + name1 + ".bdb").c_str());
-
-    /*Read the diff file */
-    std::stringstream patchfileName;
-    patchfileName << path << "diff.bo";
-
-    uint8_t* patch_buff = 0;
-    FILE* fp = fopen(patchfileName.str().c_str(), "rb");
+    /*load the .bo under execution into the updater*/
+    uint8_t* BO_BUF = 0;
+    FILE* fp = fopen(bo_filename, "rb");
     if (!fp)
     {
-      perror(patchfileName.str().c_str());
+      perror(bo_filename);
     }
     fseek(fp, 0, SEEK_END);
-    size_t patch_size = ftell(fp);
+    size_t bcode_size = ftell(fp);
     rewind(fp);
-    patch_buff = (uint8_t*)malloc(patch_size);
-    if (fread(patch_buff, 1, patch_size, fp) < patch_size)
+    BO_BUF = (uint8_t*)malloc(bcode_size);
+    if (fread(BO_BUF, 1, bcode_size, fp) < bcode_size)
     {
-      perror(patchfileName.str().c_str());
+      perror(bo_filename);
+      // fclose(fp);
+      // return 0;
     }
     fclose(fp);
-    delete_p(updater->patch);
-    updater->patch = patch_buff;
-    *(size_t*)(updater->patch_size) = patch_size;
+    /*Load stand_by .bo file into the updater*/
+    uint8_t* STD_BO_BUF = 0;
+    fp = fopen(stand_by_script, "rb");
+    if (!fp)
+    {
+      perror(stand_by_script);
+    }
+    fseek(fp, 0, SEEK_END);
+    size_t stdby_bcode_size = ftell(fp);
+    rewind(fp);
+    STD_BO_BUF = (uint8_t*)malloc(stdby_bcode_size);
+    if (fread(STD_BO_BUF, 1, stdby_bcode_size, fp) < stdby_bcode_size)
+    {
+      perror(stand_by_script);
+      // fclose(fp);
+      // return 0;
+    }
+    fclose(fp);
+    /*Create the updater*/
+    updater = (buzz_updater_elem_t)malloc(sizeof(struct buzz_updater_elem_s));
+    /*Intialize the updater with the required data*/
+    updater->bcode = BO_BUF;
+    /*Store a copy of the Bcode for rollback*/
+    updater->outmsg_queue = NULL;
+    updater->inmsg_queue = NULL;
+    updater->patch = NULL;
+    updater->patch_size = (size_t*)malloc(sizeof(size_t));
+    updater->bcode_size = (size_t*)malloc(sizeof(size_t));
+    updater->update_no = (uint8_t*)malloc(sizeof(uint16_t));
+    *(uint16_t*)(updater->update_no) = 0;
+    *(size_t*)(updater->bcode_size) = bcode_size;
+    updater->standby_bcode = STD_BO_BUF;
+    updater->standby_bcode_size = (size_t*)malloc(sizeof(size_t));
+    *(size_t*)(updater->standby_bcode_size) = stdby_bcode_size;
+    updater->mode = (int*)malloc(sizeof(int));
+    *(int*)(updater->mode) = CODE_RUNNING;
+    // no_of_robot=barrier;
+    updater_msg_ready = 0;
 
-    /* Delete the diff file */
-    remove(patchfileName.str().c_str());
+    /*Write the bcode to a file for rollback*/
+    fp = fopen("old_bcode.bo", "wb");
+    fwrite((updater->bcode), *(size_t*)updater->bcode_size, 1, fp);
+    fclose(fp);
+    return 1;
+  }
+  else
+  {
+    ROS_WARN("UPDATES TURNED OFF... (Hint: Include update.bzz to turn on updates)");
+    return 0;
+  }
+}
+/*Check for .bzz file chages*/
+int check_update()
+{
+  char buf[1024];
+  int check = 0;
+  int i = 0;
+  int len = read(fd, buf, 1024);
+  while (i < len)
+  {
+    struct inotify_event* event = (struct inotify_event*)&buf[i];
+    /*Report file changes and self deletes*/
+    if (event->mask & (IN_MODIFY | IN_DELETE_SELF))
+    {
+      /*Respawn watch if the file is self deleted */
+      inotify_rm_watch(fd, wd);
+      close(fd);
+      fd = inotify_init1(IN_NONBLOCK);
+      wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS);
+      /*To mask multiple writes from editors*/
+      if (!old_update)
+      {
+        check = 1;
+        old_update = 1;
+      }
+    }
+    /*Update index to start of next event*/
+    i += sizeof(struct inotify_event) + event->len;
+  }
+  if (!check)
+    old_update = 0;
+  return check;
+}
+
+int test_patch(std::string& path, std::string& name1, size_t update_patch_size, uint8_t* patch)
+{
+  if (SIMULATION == 1)
+  {
+    return 1;
+  }
+  else
+  {
+    /*Patch the old bo with new patch*/
+    std::stringstream patch_writefile;
+    patch_writefile << path << name1 << "tmp_patch.bo";
+    /*Write the patch to a file and call bsdiff to patch*/
+    FILE* fp = fopen(patch_writefile.str().c_str(), "wb");
+    fwrite(patch, update_patch_size, 1, fp);
+    fclose(fp);
+    std::stringstream patch_exsisting;
+    patch_exsisting << "bspatch " << path << name1 << ".bo " << path << name1 << "-patched.bo " << path << name1
+                    << "tmp_patch.bo";
+    if (debug)
+      ROS_WARN("Launching bspatch command: %s", patch_exsisting.str().c_str());
+    if (system(patch_exsisting.str().c_str()))
+      return 0;
+    else
+      return 1;
+  }
+}
+
+updater_code_t obtain_patched_bo(std::string& path, std::string& name1)
+{
+  if (SIMULATION == 1)
+  {
+    /*Read the exsisting file to simulate the patching*/
+    std::stringstream read_patched;
+    read_patched << path << name1 << ".bo";
+    if (debug)
+    {
+      ROS_WARN("Simulation patching ...");
+      ROS_WARN("Read file for patching %s", read_patched.str().c_str());
+    }
+    uint8_t* patched_bo_buf = 0;
+    FILE* fp = fopen(read_patched.str().c_str(), "rb");
+    if (!fp)
+    {
+      perror(read_patched.str().c_str());
+    }
+    fseek(fp, 0, SEEK_END);
+    size_t patched_size = ftell(fp);
+    rewind(fp);
+    patched_bo_buf = (uint8_t*)malloc(patched_size);
+    if (fread(patched_bo_buf, 1, patched_size, fp) < patched_size)
+    {
+      perror(read_patched.str().c_str());
+    }
+    fclose(fp);
+    /*Write the patched to a code struct and return*/
+    updater_code_t update_code = (updater_code_t)malloc(sizeof(struct updater_code_s));
+    update_code->bcode = patched_bo_buf;
+    update_code->bcode_size = (uint8_t*)malloc(sizeof(uint16_t));
+    *(uint16_t*)(update_code->bcode_size) = patched_size;
+
+    return update_code;
   }
 
-  void update_routine()
+  else
   {
-    buzzvm_t VM = buzz_utility::get_vm();
-    buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1));
-    buzzvm_pushi(VM, *(uint16_t*)(updater->update_no));
-    buzzvm_gstore(VM);
-    // fprintf(stdout,"[Debug : ]updater value = %i \n",updater->mode);
-    if (*(int*)updater->mode == CODE_RUNNING)
+    /*Read the new patched file*/
+    std::stringstream read_patched;
+    read_patched << path << name1 << "-patched.bo";
+    if (debug)
     {
-      buzzvm_function_call(VM, "updated_no_bct", 0);
-      // Check for update
-      if (check_update())
-      {
-        ROS_INFO("Update found \tUpdating script ...");
+      ROS_WARN("Robot patching ...");
+      ROS_WARN("Read file for patching %s", read_patched.str().c_str());
+    }
+    uint8_t* patched_bo_buf = 0;
+    FILE* fp = fopen(read_patched.str().c_str(), "rb");
+    if (!fp)
+    {
+      perror(read_patched.str().c_str());
+    }
+    fseek(fp, 0, SEEK_END);
+    size_t patched_size = ftell(fp);
+    rewind(fp);
+    patched_bo_buf = (uint8_t*)malloc(patched_size);
+    if (fread(patched_bo_buf, 1, patched_size, fp) < patched_size)
+    {
+      perror(read_patched.str().c_str());
+    }
+    fclose(fp);
 
-        if (compile_bzz(bzz_file))
+    /* delete old bo file & rename new bo file */
+    remove((path + name1 + ".bo").c_str());
+    rename((path + name1 + "-patched.bo").c_str(), (path + name1 + ".bo").c_str());
+
+    /*Write the patched to a code struct and return*/
+    updater_code_t update_code = (updater_code_t)malloc(sizeof(struct updater_code_s));
+    update_code->bcode = patched_bo_buf;
+    update_code->bcode_size = (uint8_t*)malloc(sizeof(uint16_t));
+    *(uint16_t*)(update_code->bcode_size) = patched_size;
+
+    return update_code;
+  }
+}
+
+void code_message_outqueue_append()
+{
+  updater->outmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s));
+  /* if size less than 250 Pad with zeors to assure transmission*/
+  uint16_t size = UPDATE_CODE_HEADER_SIZE + *(size_t*)(updater->patch_size);
+  uint16_t padding_size = (size > MIN_UPDATE_PACKET) ? 0 : MIN_UPDATE_PACKET - size;
+  size += padding_size;
+  updater->outmsg_queue->queue = (uint8_t*)malloc(size);
+  memset(updater->outmsg_queue->queue, 0, size);
+  updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t));
+  *(uint16_t*)(updater->outmsg_queue->size) = size;
+  size = 0;
+  /*Append message type*/
+  *(uint8_t*)(updater->outmsg_queue->queue + size) = SENT_CODE;
+  size += sizeof(uint8_t);
+  /*Append the update no, code size and code to out msg*/
+  *(uint16_t*)(updater->outmsg_queue->queue + size) = *(uint16_t*)(updater->update_no);
+  size += sizeof(uint16_t);
+  *(uint16_t*)(updater->outmsg_queue->queue + size) = (uint16_t) * (size_t*)(updater->patch_size);
+  size += sizeof(uint16_t);
+  memcpy(updater->outmsg_queue->queue + size, updater->patch, *(size_t*)(updater->patch_size));
+  // size += (uint16_t) * (size_t*)(updater->patch_size);
+  updater_msg_ready = 1;
+}
+
+void outqueue_append_code_request(uint16_t update_no)
+{
+  updater->outmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s));
+  uint16_t size = 0;
+  updater->outmsg_queue->queue = (uint8_t*)malloc(2 * sizeof(uint16_t) + sizeof(uint8_t) + CODE_REQUEST_PADDING);
+  updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t));
+  memset(updater->outmsg_queue->queue, 0, sizeof(uint16_t) + sizeof(uint8_t) + CODE_REQUEST_PADDING);
+  /*Append message type*/
+  *(uint8_t*)(updater->outmsg_queue->queue + size) = RESEND_CODE;
+  size += sizeof(uint8_t);
+  /*Append the update no, code size and code to out msg*/
+  *(uint16_t*)(updater->outmsg_queue->queue + size) = update_no;
+  size += sizeof(uint16_t);
+  *(uint16_t*)(updater->outmsg_queue->queue + size) = update_try_counter;
+  size += sizeof(uint16_t);
+  *(uint16_t*)(updater->outmsg_queue->size) = size + CODE_REQUEST_PADDING;
+  updater_msg_ready = 1;
+  if (debug)
+    ROS_WARN("[Debug] Requesting update no. %u for rebroadcast, try: %u", update_no, update_try_counter);
+}
+
+void code_message_inqueue_append(uint8_t* msg, uint16_t size)
+{
+  updater->inmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s));
+  // ROS_INFO("[DEBUG] Updater append code of size %d", (int) size);
+  updater->inmsg_queue->queue = (uint8_t*)malloc(size);
+  updater->inmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t));
+  memcpy(updater->inmsg_queue->queue, msg, size);
+  *(uint16_t*)(updater->inmsg_queue->size) = size;
+}
+/*Used for data logging*/
+/*void set_packet_id(int packet_id)
+{
+  packet_id_ = packet_id;
+}*/
+void code_message_inqueue_process()
+{
+  int size = 0;
+  updater_code_t out_code = NULL;
+  if (debug)
+  {
+    ROS_WARN("[Debug] Updater processing in msg with mode %d", *(int*)(updater->mode));
+    ROS_WARN("[Debug] Current update no: %u, Received update no: %u", (*(uint16_t*)(updater->update_no)),
+             (*(uint16_t*)(updater->inmsg_queue->queue + sizeof(uint8_t))));
+    ROS_WARN("[Debug] Updater received patch of size %u",
+             (*(uint16_t*)(updater->inmsg_queue->queue + sizeof(uint16_t) + sizeof(uint8_t))));
+  }
+  if (*(int*)(updater->mode) == CODE_RUNNING)
+  {
+    // fprintf(stdout,"[debug]Inside inmsg code running");
+    if (*(uint8_t*)(updater->inmsg_queue->queue) == SENT_CODE)
+    {
+      size += sizeof(uint8_t);
+      if (*(uint16_t*)(updater->inmsg_queue->queue + size) > *(uint16_t*)(updater->update_no))
+      {
+        // fprintf(stdout,"[debug]Inside update number comparision");
+        uint16_t update_no = *(uint16_t*)(updater->inmsg_queue->queue + size);
+        size += sizeof(uint16_t);
+        uint16_t update_patch_size = *(uint16_t*)(updater->inmsg_queue->queue + size);
+        size += sizeof(uint16_t);
+        /*Generate patch*/
+        std::string bzzfile_name(bzz_file);
+        std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
+        std::string name1 = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1);
+        name1 = name1.substr(0, name1.find_last_of("."));
+        if (test_patch(path, name1, update_patch_size, (updater->inmsg_queue->queue + size)))
         {
-          ROS_ERROR("Error in compiling script, resuming old script.");
+          out_code = obtain_patched_bo(path, name1);
+
+          // fprintf(stdout,"in queue process Update no %d\n", (int) update_no);
+          // fprintf(stdout,"in queue process bcode size %d\n", (int) update_bcode_size);
+          // FILE *fp;
+          // fp=fopen("update.bo", "wb");
+          // fwrite((updater->inmsg_queue->queue+size), update_bcode_size, 1, fp);
+          // fclose(fp);
+
+          if (test_set_code((out_code->bcode), (char*)dbgf_name, (size_t)(*(uint16_t*)out_code->bcode_size)))
+          {
+            if (debug)
+              ROS_WARN("Received patch PASSED tests!");
+            *(uint16_t*)updater->update_no = update_no;
+            /*Clear exisiting patch if any*/
+            delete_p(updater->patch);
+            /*copy the patch into the updater*/
+            updater->patch = (uint8_t*)malloc(update_patch_size);
+            memcpy(updater->patch, (updater->inmsg_queue->queue + size), update_patch_size);
+            *(size_t*)(updater->patch_size) = update_patch_size;
+            // code_message_outqueue_append();
+            neigh = 1;
+          }
+          /*clear the temp code buff*/
+          delete_p(out_code->bcode);
+          delete_p(out_code->bcode_size);
+          delete_p(out_code);
         }
         else
         {
-          std::string bzzfile_name(bzz_file);
-          stringstream bzzfile_in_compile;
-          std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
-          std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1);
-          name = name.substr(0, name.find_last_of("."));
-          bzzfile_in_compile << path << name << "-update.bo";
-          uint8_t* BO_BUF = 0;
-          FILE* fp = fopen(bzzfile_in_compile.str().c_str(), "rb");  
-          if (!fp)
-          {
-            perror(bzzfile_in_compile.str().c_str());
-          }
-          fseek(fp, 0, SEEK_END);
-          size_t bcode_size = ftell(fp);
-          rewind(fp);
-          BO_BUF = (uint8_t*)malloc(bcode_size);
-          if (fread(BO_BUF, 1, bcode_size, fp) < bcode_size)
-          {
-            perror(bcfname);
-          }
-          fclose(fp);
-          if (test_set_code(BO_BUF, dbgf_name, bcode_size))
-          {
-            uint16_t update_no = *(uint16_t*)(updater->update_no);
-            *(uint16_t*)(updater->update_no) = update_no + 1;
-
-            create_update_patch();
-            VM = buzz_utility::get_vm();
-            if(debug) ROS_INFO("Current Update no %d", *(uint16_t*)(updater->update_no));
-            buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1));
-            buzzvm_pushi(VM, *(uint16_t*)(updater->update_no));
-            buzzvm_gstore(VM);
-            neigh = -1;
-            if(debug) ROS_INFO("Broadcasting patch ...");
-            code_message_outqueue_append();
-          }
-          delete_p(BO_BUF);
+          ROS_ERROR("Patching the .bo file failed");
+          update_try_counter++;
+          outqueue_append_code_request(update_no);
         }
       }
     }
-
-    else
+  }
+  size = 0;
+  if (*(uint8_t*)(updater->inmsg_queue->queue) == RESEND_CODE)
+  {
+    if (debug)
+      ROS_WARN("Patch rebroadcast request received.");
+    size += sizeof(uint8_t);
+    if (*(uint16_t*)(updater->inmsg_queue->queue + size) == *(uint16_t*)(updater->update_no))
     {
-      timer_steps++;
-      buzzvm_pushs(VM, buzzvm_string_register(VM, "barrier_val", 1));
-      buzzvm_gload(VM);
-      buzzobj_t tObj = buzzvm_stack_at(VM, 1);
-      buzzvm_pop(VM);
-      ROS_INFO("Barrier ............. No. of robots deployed: %i", tObj->i.value);
-      if (tObj->i.value == no_of_robot)
+      size += sizeof(uint16_t);
+      if (*(uint16_t*)(updater->inmsg_queue->queue + size) > update_try_counter)
       {
-        ROS_WARN("Patch deployment successful, rolling forward");
-        *(int*)(updater->mode) = CODE_RUNNING;
-        gettimeofday(&t2, NULL);
-        // collect_data();
-        buzz_utility::buzz_update_set((updater)->bcode, (char*)dbgf_name, *(size_t*)(updater->bcode_size));
-        // buzzvm_function_call(m_tBuzzVM, "updated", 0);
-        update_try_counter = 0;
-        timer_steps = 0;
-      }
-      else if (timer_steps > TIMEOUT_FOR_ROLLBACK)
-      {
-        ROS_ERROR("TIME OUT Reached, rolling back");
-        /*Time out hit deceided to roll back*/
-        *(int*)(updater->mode) = CODE_RUNNING;
-        buzz_utility::buzz_script_set(old_bcfname, dbgf_name, (int)VM->robot);
-        update_try_counter = 0;
-        timer_steps = 0;
+        update_try_counter = *(uint16_t*)(updater->inmsg_queue->queue + size);
+        if (debug)
+          ROS_WARN("Rebroadcasting patch, try : %u", update_try_counter);
+        code_message_outqueue_append();
       }
+      if (update_try_counter > MAX_UPDATE_TRY)
+        ROS_ERROR("TODO: Max rebroadcast retry reached, ROLL BACK !!");
     }
   }
+  // fprintf(stdout,"in queue freed\n");
+  delete_p(updater->inmsg_queue->queue);
+  delete_p(updater->inmsg_queue->size);
+  delete_p(updater->inmsg_queue);
+}
 
-  uint8_t* getupdater_out_msg()
+void create_update_patch()
+{
+  std::stringstream genpatch;
+
+  std::string bzzfile_name(bzz_file);
+
+  std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
+
+  std::string name1 = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1);
+  name1 = name1.substr(0, name1.find_last_of("."));
+
+  std::string name2 = name1 + "-update";
+
+  /*Launch bsdiff and create a patch*/
+  genpatch << "bsdiff " << path << name1 << ".bo " << path << name2 << ".bo " << path << "diff.bo";
+  if (debug)
+    ROS_WARN("Launching bsdiff: %s", genpatch.str().c_str());
+  system(genpatch.str().c_str());
+
+  /*Delete old files & rename new files*/
+
+  remove((path + name1 + ".bo").c_str());
+  remove((path + name1 + ".bdb").c_str());
+
+  rename((path + name2 + ".bo").c_str(), (path + name1 + ".bo").c_str());
+  rename((path + name2 + ".bdb").c_str(), (path + name1 + ".bdb").c_str());
+
+  /*Read the diff file */
+  std::stringstream patchfileName;
+  patchfileName << path << "diff.bo";
+
+  uint8_t* patch_buff = 0;
+  FILE* fp = fopen(patchfileName.str().c_str(), "rb");
+  if (!fp)
   {
-    return (uint8_t*)updater->outmsg_queue->queue;
+    perror(patchfileName.str().c_str());
   }
-
-  uint8_t* getupdate_out_msg_size()
+  fseek(fp, 0, SEEK_END);
+  size_t patch_size = ftell(fp);
+  rewind(fp);
+  patch_buff = (uint8_t*)malloc(patch_size);
+  if (fread(patch_buff, 1, patch_size, fp) < patch_size)
   {
-    // fprintf(stdout,"[Debug  from get out size in util: ]size = %i \n",*(uint16_t*)updater->outmsg_queue->size);
-    return (uint8_t*)updater->outmsg_queue->size;
+    perror(patchfileName.str().c_str());
   }
+  fclose(fp);
+  delete_p(updater->patch);
+  updater->patch = patch_buff;
+  *(size_t*)(updater->patch_size) = patch_size;
 
-  int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size)
+  /* Delete the diff file */
+  remove(patchfileName.str().c_str());
+}
+
+void update_routine()
+{
+  buzzvm_t VM = buzz_utility::get_vm();
+  buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1));
+  buzzvm_pushi(VM, *(uint16_t*)(updater->update_no));
+  buzzvm_gstore(VM);
+  // fprintf(stdout,"[Debug : ]updater value = %i \n",updater->mode);
+  if (*(int*)updater->mode == CODE_RUNNING)
   {
-    if (buzz_utility::buzz_update_init_test(BO_BUF, dbgfname, bcode_size))
+    buzzvm_function_call(VM, "updated_no_bct", 0);
+    // Check for update
+    if (check_update())
     {
-      if(debug) ROS_WARN("Initializtion test passed");
-      if (buzz_utility::update_step_test())
+      ROS_INFO("Update found \tUpdating script ...");
+
+      if (compile_bzz(bzz_file))
       {
-        /*data logging*/
-        old_byte_code_size = *(size_t*)updater->bcode_size;
-        /*data logging*/
-        if(debug) ROS_WARN("Step test passed");
-        *(int*)(updater->mode) = CODE_STANDBY;
-        // fprintf(stdout,"updater value = %i\n",updater->mode);
-        delete_p(updater->bcode);
-        updater->bcode = (uint8_t*)malloc(bcode_size);
-        memcpy(updater->bcode, BO_BUF, bcode_size);
-        *(size_t*)updater->bcode_size = bcode_size;
-        buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname,
-                                            (size_t) * (size_t*)(updater->standby_bcode_size));
-        buzzvm_t VM = buzz_utility::get_vm();
-        gettimeofday(&t1, NULL);
-        buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1));
-        buzzvm_pushi(VM, no_of_robot);
-        buzzvm_gstore(VM);
-        return 1;
+        ROS_ERROR("Error in compiling script, resuming old script.");
       }
-      /*Unable to step something wrong*/
       else
       {
-        if (*(int*)(updater->mode) == CODE_RUNNING)
+        std::string bzzfile_name(bzz_file);
+        stringstream bzzfile_in_compile;
+        std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
+        std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1);
+        name = name.substr(0, name.find_last_of("."));
+        bzzfile_in_compile << path << name << "-update.bo";
+        uint8_t* BO_BUF = 0;
+        FILE* fp = fopen(bzzfile_in_compile.str().c_str(), "rb");
+        if (!fp)
         {
-          ROS_ERROR("Step test failed, resuming old script");
-          buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (size_t) * (size_t*)(updater->bcode_size));
+          perror(bzzfile_in_compile.str().c_str());
         }
-        else
+        fseek(fp, 0, SEEK_END);
+        size_t bcode_size = ftell(fp);
+        rewind(fp);
+        BO_BUF = (uint8_t*)malloc(bcode_size);
+        if (fread(BO_BUF, 1, bcode_size, fp) < bcode_size)
         {
-          /*You will never reach here*/
-          ROS_ERROR("Step test failed, returning to standby");
-          buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname,
-                                              (size_t) * (size_t*)(updater->standby_bcode_size));
-          buzzvm_t VM = buzz_utility::get_vm();
-          buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1));
-          buzzvm_pushi(VM, no_of_robot);
+          perror(bcfname);
+        }
+        fclose(fp);
+        if (test_set_code(BO_BUF, dbgf_name, bcode_size))
+        {
+          uint16_t update_no = *(uint16_t*)(updater->update_no);
+          *(uint16_t*)(updater->update_no) = update_no + 1;
+
+          create_update_patch();
+          VM = buzz_utility::get_vm();
+          if (debug)
+            ROS_INFO("Current Update no %d", *(uint16_t*)(updater->update_no));
+          buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1));
+          buzzvm_pushi(VM, *(uint16_t*)(updater->update_no));
           buzzvm_gstore(VM);
+          neigh = -1;
+          if (debug)
+            ROS_INFO("Broadcasting patch ...");
+          code_message_outqueue_append();
         }
-        return 0;
+        delete_p(BO_BUF);
       }
     }
+  }
+
+  else
+  {
+    timer_steps++;
+    buzzvm_pushs(VM, buzzvm_string_register(VM, "barrier_val", 1));
+    buzzvm_gload(VM);
+    buzzobj_t tObj = buzzvm_stack_at(VM, 1);
+    buzzvm_pop(VM);
+    ROS_INFO("Barrier ............. No. of robots deployed: %i", tObj->i.value);
+    if (tObj->i.value == no_of_robot)
+    {
+      ROS_WARN("Patch deployment successful, rolling forward");
+      *(int*)(updater->mode) = CODE_RUNNING;
+      gettimeofday(&t2, NULL);
+      // collect_data();
+      buzz_utility::buzz_update_set((updater)->bcode, (char*)dbgf_name, *(size_t*)(updater->bcode_size));
+      // buzzvm_function_call(m_tBuzzVM, "updated", 0);
+      update_try_counter = 0;
+      timer_steps = 0;
+    }
+    else if (timer_steps > TIMEOUT_FOR_ROLLBACK)
+    {
+      ROS_ERROR("TIME OUT Reached, rolling back");
+      /*Time out hit deceided to roll back*/
+      *(int*)(updater->mode) = CODE_RUNNING;
+      buzz_utility::buzz_script_set(old_bcfname, dbgf_name, (int)VM->robot);
+      update_try_counter = 0;
+      timer_steps = 0;
+    }
+  }
+}
+
+uint8_t* getupdater_out_msg()
+{
+  return (uint8_t*)updater->outmsg_queue->queue;
+}
+
+uint8_t* getupdate_out_msg_size()
+{
+  // fprintf(stdout,"[Debug  from get out size in util: ]size = %i \n",*(uint16_t*)updater->outmsg_queue->size);
+  return (uint8_t*)updater->outmsg_queue->size;
+}
+
+int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size)
+{
+  if (buzz_utility::buzz_update_init_test(BO_BUF, dbgfname, bcode_size))
+  {
+    if (debug)
+      ROS_WARN("Initializtion test passed");
+    if (buzz_utility::update_step_test())
+    {
+      /*data logging*/
+      old_byte_code_size = *(size_t*)updater->bcode_size;
+      /*data logging*/
+      if (debug)
+        ROS_WARN("Step test passed");
+      *(int*)(updater->mode) = CODE_STANDBY;
+      // fprintf(stdout,"updater value = %i\n",updater->mode);
+      delete_p(updater->bcode);
+      updater->bcode = (uint8_t*)malloc(bcode_size);
+      memcpy(updater->bcode, BO_BUF, bcode_size);
+      *(size_t*)updater->bcode_size = bcode_size;
+      buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname,
+                                          (size_t) * (size_t*)(updater->standby_bcode_size));
+      buzzvm_t VM = buzz_utility::get_vm();
+      gettimeofday(&t1, NULL);
+      buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1));
+      buzzvm_pushi(VM, no_of_robot);
+      buzzvm_gstore(VM);
+      return 1;
+    }
+    /*Unable to step something wrong*/
     else
     {
       if (*(int*)(updater->mode) == CODE_RUNNING)
       {
-        ROS_ERROR("Initialization test failed, resuming old script");
-        buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (int)*(size_t*)(updater->bcode_size));
+        ROS_ERROR("Step test failed, resuming old script");
+        buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (size_t) * (size_t*)(updater->bcode_size));
       }
       else
       {
         /*You will never reach here*/
-        ROS_ERROR("Initialization test failed, returning to standby");
+        ROS_ERROR("Step test failed, returning to standby");
         buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname,
                                             (size_t) * (size_t*)(updater->standby_bcode_size));
         buzzvm_t VM = buzz_utility::get_vm();
@@ -638,87 +634,108 @@ namespace buzz_update{
       return 0;
     }
   }
-
-  void destroy_out_msg_queue()
+  else
+  {
+    if (*(int*)(updater->mode) == CODE_RUNNING)
+    {
+      ROS_ERROR("Initialization test failed, resuming old script");
+      buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (int)*(size_t*)(updater->bcode_size));
+    }
+    else
+    {
+      /*You will never reach here*/
+      ROS_ERROR("Initialization test failed, returning to standby");
+      buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname,
+                                          (size_t) * (size_t*)(updater->standby_bcode_size));
+      buzzvm_t VM = buzz_utility::get_vm();
+      buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1));
+      buzzvm_pushi(VM, no_of_robot);
+      buzzvm_gstore(VM);
+    }
+    return 0;
+  }
+}
+
+void destroy_out_msg_queue()
+{
+  // fprintf(stdout,"out queue freed\n");
+  delete_p(updater->outmsg_queue->queue);
+  delete_p(updater->outmsg_queue->size);
+  delete_p(updater->outmsg_queue);
+  updater_msg_ready = 0;
+}
+
+int is_msg_present()
+{
+  return updater_msg_ready;
+}
+/*buzz_updater_elem_t get_updater()
+{
+  return updater;
+}*/
+void destroy_updater()
+{
+  delete_p(updater->bcode);
+  delete_p(updater->bcode_size);
+  delete_p(updater->standby_bcode);
+  delete_p(updater->standby_bcode_size);
+  delete_p(updater->mode);
+  delete_p(updater->update_no);
+  if (updater->outmsg_queue)
   {
-    // fprintf(stdout,"out queue freed\n");
     delete_p(updater->outmsg_queue->queue);
     delete_p(updater->outmsg_queue->size);
     delete_p(updater->outmsg_queue);
-    updater_msg_ready = 0;
   }
+  if (updater->inmsg_queue)
+  {
+    delete_p(updater->inmsg_queue->queue);
+    delete_p(updater->inmsg_queue->size);
+    delete_p(updater->inmsg_queue);
+  }
+  inotify_rm_watch(fd, wd);
+  close(fd);
+}
 
-  int is_msg_present()
-  {
-    return updater_msg_ready;
-  }
-  /*buzz_updater_elem_t get_updater()
-  {
-    return updater;
-  }*/
-  void destroy_updater()
-  {
-    delete_p(updater->bcode);
-    delete_p(updater->bcode_size);
-    delete_p(updater->standby_bcode);
-    delete_p(updater->standby_bcode_size);
-    delete_p(updater->mode);
-    delete_p(updater->update_no);
-    if (updater->outmsg_queue)
-    {
-      delete_p(updater->outmsg_queue->queue);
-      delete_p(updater->outmsg_queue->size);
-      delete_p(updater->outmsg_queue);
-    }
-    if (updater->inmsg_queue)
-    {
-      delete_p(updater->inmsg_queue->queue);
-      delete_p(updater->inmsg_queue->size);
-      delete_p(updater->inmsg_queue);
-    }
-    inotify_rm_watch(fd, wd);
-    close(fd);
-  }
+void set_bzz_file(const char* in_bzz_file, bool dbg)
+{
+  debug = dbg;
+  bzz_file = in_bzz_file;
+}
 
-  void set_bzz_file(const char* in_bzz_file, bool dbg)
-  {
-    debug=dbg;
-    bzz_file = in_bzz_file;
-  }
+void updates_set_robots(int robots)
+{
+  no_of_robot = robots;
+}
 
-  void updates_set_robots(int robots)
-  {
-    no_of_robot = robots;
-  }
+/*--------------------------------------------------------
+/ Create Buzz bytecode from the bzz script input
+/-------------------------------------------------------*/
+int compile_bzz(std::string bzz_file)
+{
+  /*Compile the buzz code .bzz to .bo*/
+  std::string bzzfile_name(bzz_file);
+  stringstream bzzfile_in_compile;
+  std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
+  std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1);
+  name = name.substr(0, name.find_last_of("."));
+  bzzfile_in_compile << "bzzc -I " << path << "include/";  //<<" "<<path<< name<<".basm";
+  bzzfile_in_compile << " -b " << path << name << "-update.bo";
+  bzzfile_in_compile << " -d " << path << name << "-update.bdb ";
+  bzzfile_in_compile << bzzfile_name;
+  ROS_WARN("Launching buzz compilation for update: %s", bzzfile_in_compile.str().c_str());
+  return system(bzzfile_in_compile.str().c_str());
+}
 
-  /*--------------------------------------------------------
-  / Create Buzz bytecode from the bzz script input
-  /-------------------------------------------------------*/
-  int compile_bzz(std::string bzz_file)
-  {
-    /*Compile the buzz code .bzz to .bo*/
-    std::string bzzfile_name(bzz_file);
-    stringstream bzzfile_in_compile;
-    std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
-    std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1);
-    name = name.substr(0, name.find_last_of("."));
-    bzzfile_in_compile << "bzzc -I " << path << "include/";  //<<" "<<path<< name<<".basm";
-    bzzfile_in_compile << " -b " << path << name << "-update.bo";
-    bzzfile_in_compile << " -d " << path << name << "-update.bdb ";
-    bzzfile_in_compile << bzzfile_name;
-    ROS_WARN("Launching buzz compilation for update: %s", bzzfile_in_compile.str().c_str());
-    return system(bzzfile_in_compile.str().c_str());
-  }
-
-  /*void collect_data(std::ofstream& logger)
-  {
-    double time_spent = (t2.tv_sec - t1.tv_sec) * 1000.0;  //(double)(end - begin) / CLOCKS_PER_SEC;
-    time_spent += (t2.tv_usec - t1.tv_usec) / 1000.0;
-    // RID,update trigger,time steps taken,old byte code size, new bytecode size, patch size,update number,
-    //
-  Patch_packets_received_counter,Patch_request_packets_received,Patch_packets_sent_counter,Patch_request_packets_sent_counter
-    logger << (int)no_of_robot << "," << neigh << "," << (double)time_spent << "," << (int)timer_steps << ","
-           << old_byte_code_size << "," << *(size_t*)updater->bcode_size << "," << *(size_t*)updater->patch_size << ","
-           << (int)*(uint8_t*)updater->update_no << "," << (int)packet_id_;
-  }*/
+/*void collect_data(std::ofstream& logger)
+{
+  double time_spent = (t2.tv_sec - t1.tv_sec) * 1000.0;  //(double)(end - begin) / CLOCKS_PER_SEC;
+  time_spent += (t2.tv_usec - t1.tv_usec) / 1000.0;
+  // RID,update trigger,time steps taken,old byte code size, new bytecode size, patch size,update number,
+  //
+Patch_packets_received_counter,Patch_request_packets_received,Patch_packets_sent_counter,Patch_request_packets_sent_counter
+  logger << (int)no_of_robot << "," << neigh << "," << (double)time_spent << "," << (int)timer_steps << ","
+         << old_byte_code_size << "," << *(size_t*)updater->bcode_size << "," << *(size_t*)updater->patch_size << ","
+         << (int)*(uint8_t*)updater->update_no << "," << (int)packet_id_;
+}*/
 }
\ No newline at end of file
diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp
index 458ddfd..af2606a 100644
--- a/src/buzz_utility.cpp
+++ b/src/buzz_utility.cpp
@@ -17,7 +17,7 @@ static buzzvm_t VM = 0;
 static char* BO_FNAME = 0;
 static uint8_t* BO_BUF = 0;
 static buzzdebug_t DBG_INFO = 0;
-static uint32_t MAX_MSG_SIZE = 190;//250;  // Maximum Msg size for sending update packets
+static uint32_t MAX_MSG_SIZE = 190;  // 250;  // Maximum Msg size for sending update packets
 static uint8_t Robot_id = 0;
 static std::vector<uint8_t*> IN_MSG;
 std::map<int, Pos_struct> users_map;
@@ -572,7 +572,8 @@ int get_inmsg_size()
   return IN_MSG.size();
 }
 
-std::vector<uint8_t*> get_inmsg_vector(){
+std::vector<uint8_t*> get_inmsg_vector()
+{
   return IN_MSG;
 }
 
@@ -590,11 +591,12 @@ string get_bvmstate()
 ---------------------------------------*/
 {
   std::string uav_state = "Unknown";
-  if(VM && VM->strings){
+  if (VM && VM->strings)
+  {
     buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1));
     buzzvm_gload(VM);
     buzzobj_t obj = buzzvm_stack_at(VM, 1);
-    if(obj->o.type == BUZZTYPE_STRING)
+    if (obj->o.type == BUZZTYPE_STRING)
       uav_state = string(obj->s.value.str);
     else
       uav_state = "Not Available";
@@ -603,8 +605,8 @@ string get_bvmstate()
   return uav_state;
 }
 
-int get_swarmsize() {
+int get_swarmsize()
+{
   return (int)buzzdict_size(VM->swarmmembers) + 1;
 }
-
 }
diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp
index baf6f4f..fd04090 100644
--- a/src/buzzuav_closures.cpp
+++ b/src/buzzuav_closures.cpp
@@ -42,11 +42,15 @@ static bool logVoronoi = false;
 std::ofstream voronoicsv;
 
 struct Point
-{ 
-    float x; 
-    float y;
-    Point(): x( 0.0 ), y( 0.0 ) { }
-    Point( float x, float y ): x( x ), y( y ) { }
+{
+  float x;
+  float y;
+  Point() : x(0.0), y(0.0)
+  {
+  }
+  Point(float x, float y) : x(x), y(y)
+  {
+  }
 };
 string WPlistname = "";
 
@@ -54,7 +58,7 @@ std::map<int, buzz_utility::RB_struct> targets_map;
 std::map<int, buzz_utility::RB_struct> wplist_map;
 std::map<int, buzz_utility::Pos_struct> neighbors_map;
 std::map<int, buzz_utility::neighbors_status> neighbors_status_map;
-std::map<int, std::map<int,int>> grid;
+std::map<int, std::map<int, int>> grid;
 
 /****************************************/
 /****************************************/
@@ -123,7 +127,8 @@ void setVorlog(string path)
 / set the absolute path for a csv list of waypoints
 ----------------------------------------------------------- */
 {
-  voronoicsv.open(path + "/log/voronoi_"+std::to_string(buzz_utility::get_robotid())+".csv", std::ios_base::trunc | std::ios_base::out);
+  voronoicsv.open(path + "/log/voronoi_" + std::to_string(buzz_utility::get_robotid()) + ".csv",
+                  std::ios_base::trunc | std::ios_base::out);
   logVoronoi = true;
 }
 
@@ -152,15 +157,18 @@ void rb_from_gps(double nei[], double out[], double cur[])
   out[2] = 0.0;
 }
 
-void gps_from_vec(double vec[], double gps[]) {
+void gps_from_vec(double vec[], double gps[])
+{
   double Vrange = sqrt(vec[0] * vec[0] + vec[1] * vec[1]);
   double Vbearing = constrainAngle(atan2(vec[1], vec[0]));
-  double latR = cur_pos[0]*M_PI/180.0;
-  double lonR = cur_pos[1]*M_PI/180.0;
-  double target_lat = asin(sin(latR) * cos(Vrange/EARTH_RADIUS) + cos(latR) * sin(Vrange/EARTH_RADIUS) * cos(Vbearing));
-  double target_lon = lonR + atan2(sin(Vbearing) * sin(Vrange/EARTH_RADIUS) * cos(latR), cos(Vrange/EARTH_RADIUS) - sin(latR) * sin(target_lat));
-  gps[0] = target_lat*180.0/M_PI;
-  gps[1] = target_lon*180.0/M_PI;
+  double latR = cur_pos[0] * M_PI / 180.0;
+  double lonR = cur_pos[1] * M_PI / 180.0;
+  double target_lat =
+      asin(sin(latR) * cos(Vrange / EARTH_RADIUS) + cos(latR) * sin(Vrange / EARTH_RADIUS) * cos(Vbearing));
+  double target_lon = lonR + atan2(sin(Vbearing) * sin(Vrange / EARTH_RADIUS) * cos(latR),
+                                   cos(Vrange / EARTH_RADIUS) - sin(latR) * sin(target_lat));
+  gps[0] = target_lat * 180.0 / M_PI;
+  gps[1] = target_lon * 180.0 / M_PI;
   gps[2] = cur_pos[2];
 }
 
@@ -215,7 +223,7 @@ void parse_gpslist()
   fin.close();
 }
 
-void check_targets_sim(double lat, double lon, double *res)
+void check_targets_sim(double lat, double lon, double* res)
 /*
 / check if a listed target is close
 ----------------------------------------------------------- */
@@ -225,16 +233,19 @@ void check_targets_sim(double lat, double lon, double *res)
   for (it = wplist_map.begin(); it != wplist_map.end(); ++it)
   {
     double rb[3];
-    double ref[2]={lat, lon};
-    double tar[2]={it->second.latitude, it->second.longitude};
+    double ref[2] = { lat, lon };
+    double tar[2] = { it->second.latitude, it->second.longitude };
     rb_from_gps(tar, rb, ref);
-    if(rb[0] < visibility_radius && (buzz_utility::get_bvmstate()=="WAYPOINT" && it->second.r==0)){
+    if (rb[0] < visibility_radius && (buzz_utility::get_bvmstate() == "WAYPOINT" && it->second.r == 0))
+    {
       ROS_WARN("FOUND A TARGET IN WAYPOINT!!! [%i]", it->first);
       res[0] = it->first;
       res[1] = it->second.latitude;
       res[2] = it->second.longitude;
       res[3] = it->second.altitude;
-    } else if(rb[0] < visibility_radius && (buzz_utility::get_bvmstate()=="DEPLOY" && it->second.r==1)){
+    }
+    else if (rb[0] < visibility_radius && (buzz_utility::get_bvmstate() == "DEPLOY" && it->second.r == 1))
+    {
       ROS_WARN("FOUND A TARGET IN WAYPOINT!!! [%i]", it->first);
       res[0] = it->first;
       res[1] = it->second.latitude;
@@ -253,21 +264,23 @@ int buzz_exportmap(buzzvm_t vm)
   buzzvm_lnum_assert(vm, 1);
   // Get the parameter
   buzzvm_lload(vm, 1);
-  buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE);    // dictionary
+  buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE);  // dictionary
   buzzobj_t t = buzzvm_stack_at(vm, 1);
-  for(int32_t i = 1; i <= buzzdict_size(t->t.value); ++i) {
+  for (int32_t i = 1; i <= buzzdict_size(t->t.value); ++i)
+  {
     buzzvm_dup(vm);
     buzzvm_pushi(vm, i);
     buzzvm_tget(vm);
     std::map<int, int> row;
-    for(int32_t j = 1; j <= buzzdict_size(buzzvm_stack_at(vm, 1)->t.value); ++j) {
+    for (int32_t j = 1; j <= buzzdict_size(buzzvm_stack_at(vm, 1)->t.value); ++j)
+    {
       buzzvm_dup(vm);
       buzzvm_pushi(vm, j);
       buzzvm_tget(vm);
-      row.insert(std::pair<int,int>(j, 100.0 - round(buzzvm_stack_at(vm, 1)->f.value*100.0)));
+      row.insert(std::pair<int, int>(j, 100.0 - round(buzzvm_stack_at(vm, 1)->f.value * 100.0)));
       buzzvm_pop(vm);
     }
-    grid.insert(std::pair<int,std::map<int, int>>(i,row));
+    grid.insert(std::pair<int, std::map<int, int>>(i, row));
     buzzvm_pop(vm);
   }
   // DEBUG
@@ -277,252 +290,279 @@ int buzz_exportmap(buzzvm_t vm)
 
 /*
  *  Geofence(): test for a point in a polygon
- *     TAKEN from https://www.geeksforgeeks.org/how-to-check-if-a-given-point-lies-inside-a-polygon/  
+ *     TAKEN from https://www.geeksforgeeks.org/how-to-check-if-a-given-point-lies-inside-a-polygon/
  */
 
-// Given three colinear points p, q, r, the function checks if 
-// point q lies on line segment 'pr' 
-bool onSegment(Point p, Point q, Point r) 
-{ 
-    if (q.x <= max(p.x, r.x) && q.x >= min(p.x, r.x) && 
-            q.y <= max(p.y, r.y) && q.y >= min(p.y, r.y)) 
-        return true; 
-    return false; 
-}
-// To find orientation of ordered triplet (p, q, r). 
-// The function returns following values 
-// 0 --> p, q and r are colinear 
-// 1 --> Clockwise 
-// 2 --> Counterclockwise 
-int orientation(Point p, Point q, Point r) 
-{ 
-    int val =round((q.y - p.y) * (r.x - q.x)*100 - 
-              (q.x - p.x) * (r.y - q.y)*100); 
-  
-    if (val == 0) return 0;  // colinear 
-    return (val > 0)? 1: 2; // clock or counterclock wise 
-}
-// The function that returns true if line segment 'p1q1' 
-// and 'p2q2' intersect. 
-bool doIntersect(Point p1, Point q1, Point p2, Point q2) 
-{ 
-    // Find the four orientations needed for general and 
-    // special cases 
-    int o1 = orientation(p1, q1, p2); 
-    int o2 = orientation(p1, q1, q2); 
-    int o3 = orientation(p2, q2, p1); 
-    int o4 = orientation(p2, q2, q1); 
-
-    //ROS_WARN("(%f,%f)->(%f,%f), 1:%d,2:%d,3:%d,4:%d",p1.x,p1.y,q1.x,q1.y,o1,o2,o3,o4);
-  
-    // General case   
-    if (o1 != o2 && o3 != o4) 
-        return true; 
-  
-    // Special Cases 
-    // p1, q1 and p2 are colinear and p2 lies on segment p1q1 
-    if (o1 == 0 && onSegment(p1, p2, q1)) return true; 
-  
-    // p1, q1 and p2 are colinear and q2 lies on segment p1q1 
-    if (o2 == 0 && onSegment(p1, q2, q1)) return true; 
-  
-    // p2, q2 and p1 are colinear and p1 lies on segment p2q2 
-    if (o3 == 0 && onSegment(p2, p1, q2)) return true; 
-  
-     // p2, q2 and q1 are colinear and q1 lies on segment p2q2 
-    if (o4 == 0 && onSegment(p2, q1, q2)) return true; 
-  
-    return false; // Doesn't fall in any of the above cases 
-}
-
-float clockwise_angle_of( const Point& p )
+// Given three colinear points p, q, r, the function checks if
+// point q lies on line segment 'pr'
+bool onSegment(Point p, Point q, Point r)
 {
-  return atan2(p.y,p.x);
+  if (q.x <= max(p.x, r.x) && q.x >= min(p.x, r.x) && q.y <= max(p.y, r.y) && q.y >= min(p.y, r.y))
+    return true;
+  return false;
 }
-
-bool clockwise_compare_points( const Point& a, const Point& b )
+// To find orientation of ordered triplet (p, q, r).
+// The function returns following values
+// 0 --> p, q and r are colinear
+// 1 --> Clockwise
+// 2 --> Counterclockwise
+int orientation(Point p, Point q, Point r)
 {
-  return clockwise_angle_of( a ) < clockwise_angle_of( b );
+  int val = round((q.y - p.y) * (r.x - q.x) * 100 - (q.x - p.x) * (r.y - q.y) * 100);
+
+  if (val == 0)
+    return 0;                // colinear
+  return (val > 0) ? 1 : 2;  // clock or counterclock wise
+}
+// The function that returns true if line segment 'p1q1'
+// and 'p2q2' intersect.
+bool doIntersect(Point p1, Point q1, Point p2, Point q2)
+{
+  // Find the four orientations needed for general and
+  // special cases
+  int o1 = orientation(p1, q1, p2);
+  int o2 = orientation(p1, q1, q2);
+  int o3 = orientation(p2, q2, p1);
+  int o4 = orientation(p2, q2, q1);
+
+  // ROS_WARN("(%f,%f)->(%f,%f), 1:%d,2:%d,3:%d,4:%d",p1.x,p1.y,q1.x,q1.y,o1,o2,o3,o4);
+
+  // General case
+  if (o1 != o2 && o3 != o4)
+    return true;
+
+  // Special Cases
+  // p1, q1 and p2 are colinear and p2 lies on segment p1q1
+  if (o1 == 0 && onSegment(p1, p2, q1))
+    return true;
+
+  // p1, q1 and p2 are colinear and q2 lies on segment p1q1
+  if (o2 == 0 && onSegment(p1, q2, q1))
+    return true;
+
+  // p2, q2 and p1 are colinear and p1 lies on segment p2q2
+  if (o3 == 0 && onSegment(p2, p1, q2))
+    return true;
+
+  // p2, q2 and q1 are colinear and q1 lies on segment p2q2
+  if (o4 == 0 && onSegment(p2, q1, q2))
+    return true;
+
+  return false;  // Doesn't fall in any of the above cases
 }
 
-void sortclose_polygon(vector <Point> *P){
-  std::sort( P->begin(), P->end(), clockwise_compare_points );
+float clockwise_angle_of(const Point& p)
+{
+  return atan2(p.y, p.x);
+}
+
+bool clockwise_compare_points(const Point& a, const Point& b)
+{
+  return clockwise_angle_of(a) < clockwise_angle_of(b);
+}
+
+void sortclose_polygon(vector<Point>* P)
+{
+  std::sort(P->begin(), P->end(), clockwise_compare_points);
   P->push_back((*P)[0]);
 }
 
-
-float pol_area(vector <Point> vert) {
+float pol_area(vector<Point> vert)
+{
   float a = 0.0;
-  //ROS_INFO("Polygone %d edges area.",vert.size());
-  vector <Point>::iterator it;
-  vector <Point>::iterator next;
-  for (it = vert.begin(); it != vert.end()-1; ++it){
-    next = it+1;
+  // ROS_INFO("Polygone %d edges area.",vert.size());
+  vector<Point>::iterator it;
+  vector<Point>::iterator next;
+  for (it = vert.begin(); it != vert.end() - 1; ++it)
+  {
+    next = it + 1;
     a += it->x * next->y - next->x * it->y;
   }
   a *= 0.5;
-  //ROS_INFO("Polygon area: %f",a);
+  // ROS_INFO("Polygon area: %f",a);
   return a;
 }
 
-double* polygone_center(vector <Point> vert, double *c) {
+double* polygone_center(vector<Point> vert, double* c)
+{
   float A = pol_area(vert);
   int i1 = 1;
-  vector <Point>::iterator it;
-  vector <Point>::iterator next;
-  for (it = vert.begin(); it != vert.end()-1; ++it){
-    next = it+1;
-    float t = it->x*next->y - next->x*it->y;
-    c[0] += (it->x+next->x) * t;
-    c[1] += (it->y+next->y) * t;
+  vector<Point>::iterator it;
+  vector<Point>::iterator next;
+  for (it = vert.begin(); it != vert.end() - 1; ++it)
+  {
+    next = it + 1;
+    float t = it->x * next->y - next->x * it->y;
+    c[0] += (it->x + next->x) * t;
+    c[1] += (it->y + next->y) * t;
   }
   c[0] = c[0] / (6.0 * A);
   c[1] = c[1] / (6.0 * A);
   return c;
 }
 
-double numerator  ( Point A, Point C, Point E, Point F ) { return (A.y - C.y) * (F.x - E.x) - (A.x - C.x) * (F.y - E.y); }
-double denominator( Point A, Point B, Point C, Point D ) { return (B.x - A.x) * (D.y - C.y) - (B.y - A.y) * (D.x - C.x); }
+double numerator(Point A, Point C, Point E, Point F)
+{
+  return (A.y - C.y) * (F.x - E.x) - (A.x - C.x) * (F.y - E.y);
+}
+double denominator(Point A, Point B, Point C, Point D)
+{
+  return (B.x - A.x) * (D.y - C.y) - (B.y - A.y) * (D.x - C.x);
+}
 
-void getintersection(Point S, Point D, std::vector <Point> Poly, Point *I) {
-	//printf("Points for intersection 1(%f,%f->%f,%f) and 2(%f,%f->%f,%f)\n",q1.x,q1.y,p1.x,p1.y,q2.x,q2.y,p2.x,p2.y);
+void getintersection(Point S, Point D, std::vector<Point> Poly, Point* I)
+{
+  // printf("Points for intersection 1(%f,%f->%f,%f) and 2(%f,%f->%f,%f)\n",q1.x,q1.y,p1.x,p1.y,q2.x,q2.y,p2.x,p2.y);
   bool parallel = false;
   bool collinear = false;
-	std::vector <Point>::iterator itc;
-	std::vector <Point>::iterator next;
-  for (itc = Poly.begin(); itc != Poly.end()-1; ++itc) {
-    next = itc+1;
-    if (doIntersect((*itc), (*next), S, D)) 
+  std::vector<Point>::iterator itc;
+  std::vector<Point>::iterator next;
+  for (itc = Poly.begin(); itc != Poly.end() - 1; ++itc)
+  {
+    next = itc + 1;
+    if (doIntersect((*itc), (*next), S, D))
     {
       // Uses the determinant of the two lines. For more information, refer to one of the following:
       // https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection#Given_two_points_on_each_line
       // http://www.faqs.org/faqs/graphics/algorithms-faq/ (Subject 1.03)
-      
-      double d = denominator( S, D, (*itc), (*next) );
-      
-      if (std::abs( d ) < 0.000000001)
+
+      double d = denominator(S, D, (*itc), (*next));
+
+      if (std::abs(d) < 0.000000001)
       {
-        parallel  = true;
-        collinear = abs(numerator( S, D, (*itc), (*next) )) < 0.000000001;
+        parallel = true;
+        collinear = abs(numerator(S, D, (*itc), (*next))) < 0.000000001;
         return;
       }
-      
-      double r = numerator( S, (*itc), (*itc), (*next) ) / d;
-      double s = numerator( S, (*itc), S, D ) / d;
 
-      //ROS_INFO("-- (%f,%f)",S.x + r * (D.x - S.x), S.y + r * (D.y - S.y));
-      (*I)=Point(S.x + r * (D.x - S.x), S.y + r * (D.y - S.y));
+      double r = numerator(S, (*itc), (*itc), (*next)) / d;
+      double s = numerator(S, (*itc), S, D) / d;
+
+      // ROS_INFO("-- (%f,%f)",S.x + r * (D.x - S.x), S.y + r * (D.y - S.y));
+      (*I) = Point(S.x + r * (D.x - S.x), S.y + r * (D.y - S.y));
     }
   }
-  if(parallel || collinear)
-    ROS_WARN("Lines are Collinear (%d) or Parallels (%d)",collinear,parallel);
+  if (parallel || collinear)
+    ROS_WARN("Lines are Collinear (%d) or Parallels (%d)", collinear, parallel);
 }
 
-bool isSiteout(Point S, std::vector <Point> Poly) {
+bool isSiteout(Point S, std::vector<Point> Poly)
+{
   bool onedge = false;
 
-	// Create a point for line segment from p to infinite 
-  Point extreme = {10000, S.y};
+  // Create a point for line segment from p to infinite
+  Point extreme = { 10000, S.y };
 
-  // Count intersections of the above line with sides of polygon 
-  int count = 0; 
-	std::vector <Point>::iterator itc;
-	std::vector <Point>::iterator next;
-	for (itc = Poly.begin(); itc != Poly.end()-1; ++itc) {
-    next = itc+1;
+  // Count intersections of the above line with sides of polygon
+  int count = 0;
+  std::vector<Point>::iterator itc;
+  std::vector<Point>::iterator next;
+  for (itc = Poly.begin(); itc != Poly.end() - 1; ++itc)
+  {
+    next = itc + 1;
 
-    // Check if the line segment from 'p' to 'extreme' intersects 
-    // with the line segment from 'polygon[i]' to 'polygon[next]' 
-    if (doIntersect((*itc), (*next), S, extreme)) 
-    { 
-        // If the point 'p' is colinear with line segment 'i-next', 
-        // then check if it lies on segment. If it lies, return true, 
-        // otherwise false 
-        if (orientation((*itc), S, (*next)) == 0) {
-          onedge = onSegment((*itc), S, (*next));
-          if(onedge)
-            break;
-        }
-        count++; 
+    // Check if the line segment from 'p' to 'extreme' intersects
+    // with the line segment from 'polygon[i]' to 'polygon[next]'
+    if (doIntersect((*itc), (*next), S, extreme))
+    {
+      // If the point 'p' is colinear with line segment 'i-next',
+      // then check if it lies on segment. If it lies, return true,
+      // otherwise false
+      if (orientation((*itc), S, (*next)) == 0)
+      {
+        onedge = onSegment((*itc), S, (*next));
+        if (onedge)
+          break;
+      }
+      count++;
     }
   }
 
-  return ((count%2 == 0) && !onedge);
+  return ((count % 2 == 0) && !onedge);
 }
 
 int buzzuav_geofence(buzzvm_t vm)
 {
-    Point P;
-    buzzvm_lnum_assert(vm, 1);
-    // Get the parameter
-    buzzvm_lload(vm, 1);
-    buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE);    // dictionary
-    buzzobj_t t = buzzvm_stack_at(vm, 1);
+  Point P;
+  buzzvm_lnum_assert(vm, 1);
+  // Get the parameter
+  buzzvm_lload(vm, 1);
+  buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE);  // dictionary
+  buzzobj_t t = buzzvm_stack_at(vm, 1);
 
-    if(buzzdict_size(t->t.value) < 5) {
-      ROS_ERROR("Wrong Geofence input size (%i).", buzzdict_size(t->t.value));
-      return buzzvm_ret0(vm);
+  if (buzzdict_size(t->t.value) < 5)
+  {
+    ROS_ERROR("Wrong Geofence input size (%i).", buzzdict_size(t->t.value));
+    return buzzvm_ret0(vm);
+  }
+  std::vector<Point> polygon_bound;
+  for (int32_t i = 0; i < buzzdict_size(t->t.value); ++i)
+  {
+    Point tmp;
+    buzzvm_dup(vm);
+    buzzvm_pushi(vm, i);
+    buzzvm_tget(vm);
+
+    buzzvm_dup(vm);
+    buzzvm_pushs(vm, buzzvm_string_register(vm, "x", 1));
+    buzzvm_tget(vm);
+    if (i == 0)
+    {
+      P.x = buzzvm_stack_at(vm, 1)->f.value;
+      // printf("px=%f\n",P.x);
     }
-	  std::vector <Point> polygon_bound;
-    for(int32_t i = 0; i < buzzdict_size(t->t.value); ++i) {
-      Point tmp;
-      buzzvm_dup(vm);
-      buzzvm_pushi(vm, i);
-      buzzvm_tget(vm);
-    
-      buzzvm_dup(vm);
-      buzzvm_pushs(vm, buzzvm_string_register(vm, "x", 1));
-      buzzvm_tget(vm);
-      if(i==0){
-        P.x = buzzvm_stack_at(vm, 1)->f.value;
-        //printf("px=%f\n",P.x);
-      }else{
-        tmp.x = buzzvm_stack_at(vm, 1)->f.value;
-        //printf("c%dx=%f\n",i,tmp.x);
-      }
-      buzzvm_pop(vm);
-      buzzvm_dup(vm);
-      buzzvm_pushs(vm, buzzvm_string_register(vm, "y", 1));
-      buzzvm_tget(vm);
-      //ROS_INFO("[%i]---y-->%i",buzz_utility::get_robotid(), tmp);
-      if(i==0){
-        P.y = buzzvm_stack_at(vm, 1)->f.value;
-        //printf("py=%f\n",P.y);
-      }else{
-        tmp.y = buzzvm_stack_at(vm, 1)->f.value;
-        //printf("c%dy=%f\n",i,tmp.y);
-      }
-      buzzvm_pop(vm);
-
-      if(i!=0)
-        polygon_bound.push_back(tmp);
-
-      buzzvm_pop(vm);
+    else
+    {
+      tmp.x = buzzvm_stack_at(vm, 1)->f.value;
+      // printf("c%dx=%f\n",i,tmp.x);
     }
-    sortclose_polygon(&polygon_bound);
-
-    // Check if we are in the zone
-    if(isSiteout(P, polygon_bound)){
-      Point Intersection;
-      getintersection(Point(0.0, 0.0) , P, polygon_bound, &Intersection);
-      double gps[3];
-      double d[2]={Intersection.x,Intersection.y};
-      gps_from_vec(d, gps);
-      set_gpsgoal(gps);
-      ROS_WARN("Geofencing trigered, not going any further (%f,%f)!",d[0],d[1]);
+    buzzvm_pop(vm);
+    buzzvm_dup(vm);
+    buzzvm_pushs(vm, buzzvm_string_register(vm, "y", 1));
+    buzzvm_tget(vm);
+    // ROS_INFO("[%i]---y-->%i",buzz_utility::get_robotid(), tmp);
+    if (i == 0)
+    {
+      P.y = buzzvm_stack_at(vm, 1)->f.value;
+      // printf("py=%f\n",P.y);
     }
+    else
+    {
+      tmp.y = buzzvm_stack_at(vm, 1)->f.value;
+      // printf("c%dy=%f\n",i,tmp.y);
+    }
+    buzzvm_pop(vm);
+
+    if (i != 0)
+      polygon_bound.push_back(tmp);
+
+    buzzvm_pop(vm);
+  }
+  sortclose_polygon(&polygon_bound);
+
+  // Check if we are in the zone
+  if (isSiteout(P, polygon_bound))
+  {
+    Point Intersection;
+    getintersection(Point(0.0, 0.0), P, polygon_bound, &Intersection);
+    double gps[3];
+    double d[2] = { Intersection.x, Intersection.y };
+    gps_from_vec(d, gps);
+    set_gpsgoal(gps);
+    ROS_WARN("Geofencing trigered, not going any further (%f,%f)!", d[0], d[1]);
+  }
 
   return buzzvm_ret0(vm);
 }
 
-int voronoi_center(buzzvm_t vm) {
-
+int voronoi_center(buzzvm_t vm)
+{
   float dist_max = 300;
 
-	buzzvm_lnum_assert(vm, 1);
+  buzzvm_lnum_assert(vm, 1);
   // Get the parameter
   buzzvm_lload(vm, 1);
-  buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE);    // dictionary
+  buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE);  // dictionary
   buzzobj_t t = buzzvm_stack_at(vm, 1);
 
   buzzvm_dup(vm);
@@ -531,161 +571,191 @@ int voronoi_center(buzzvm_t vm) {
   int Poly_vert = buzzvm_stack_at(vm, 1)->i.value;
   buzzvm_pop(vm);
 
-	std::vector <Point> polygon_bound;
-  for(int32_t i = 0; i < Poly_vert; ++i) {
+  std::vector<Point> polygon_bound;
+  for (int32_t i = 0; i < Poly_vert; ++i)
+  {
     buzzvm_dup(vm);
     buzzvm_pushi(vm, i);
     buzzvm_tget(vm);
-   
+
     buzzvm_dup(vm);
     buzzvm_pushs(vm, buzzvm_string_register(vm, "x", 1));
     buzzvm_tget(vm);
-    //ROS_INFO("---x-->%f",buzzvm_stack_at(vm, 1)->f.value);
+    // ROS_INFO("---x-->%f",buzzvm_stack_at(vm, 1)->f.value);
     float tmpx = buzzvm_stack_at(vm, 1)->f.value;
     buzzvm_pop(vm);
     buzzvm_dup(vm);
     buzzvm_pushs(vm, buzzvm_string_register(vm, "y", 1));
     buzzvm_tget(vm);
-    //ROS_INFO("---y-->%f",buzzvm_stack_at(vm, 1)->f.value);
+    // ROS_INFO("---y-->%f",buzzvm_stack_at(vm, 1)->f.value);
     float tmpy = buzzvm_stack_at(vm, 1)->f.value;
     buzzvm_pop(vm);
 
     polygon_bound.push_back(Point(tmpx, tmpy));
-    //ROS_INFO("[%i] Polygon vertex: %f, %f", buzz_utility::get_robotid(),tmpx,tmpy);
+    // ROS_INFO("[%i] Polygon vertex: %f, %f", buzz_utility::get_robotid(),tmpx,tmpy);
 
     buzzvm_pop(vm);
   }
   sortclose_polygon(&polygon_bound);
-  
-  int count = buzzdict_size(t->t.value)-(Poly_vert+1);
-  ROS_WARN("NP: %d, Sites: %d", Poly_vert, count);
-  float *xValues = new float[count];
-  float *yValues = new float[count];
-  for(int32_t i = 0; i < count; ++i) {
-    int index = i + Poly_vert;
-    buzzvm_dup(vm);
-    buzzvm_pushi(vm, index);
-    buzzvm_tget(vm);
-   
-    buzzvm_dup(vm);
-    buzzvm_pushs(vm, buzzvm_string_register(vm, "x", 1));
-    buzzvm_tget(vm);
-    //ROS_INFO("---x-->%f",buzzvm_stack_at(vm, 1)->f.value);
-    xValues[i] = buzzvm_stack_at(vm, 1)->f.value;
-    buzzvm_pop(vm);
-    buzzvm_dup(vm);
-    buzzvm_pushs(vm, buzzvm_string_register(vm, "y", 1));
-    buzzvm_tget(vm);
-    //ROS_INFO("---y-->%f",buzzvm_stack_at(vm, 1)->f.value);
-    yValues[i] = buzzvm_stack_at(vm, 1)->f.value;
-    buzzvm_pop(vm);
 
-    buzzvm_pop(vm);
-  }
-  
+  int count = buzzdict_size(t->t.value) - (Poly_vert + 1);
+
   // Check if we are in the zone
-  if(isSiteout(Point(0,0), polygon_bound) || count < 3) {
-    //ROS_WARN("Not in the Zone!!!");
+  if (isSiteout(Point(0, 0), polygon_bound) || count < 3)
+  {
+    // ROS_WARN("Not in the Zone!!!");
     double goal_tmp[2];
-    do{
-      goal_tmp[0] = polygon_bound[0].x + (rand()%100)/100.0*(polygon_bound[2].x- polygon_bound[0].x);
-      goal_tmp[1] = polygon_bound[0].y + (rand()%100)/100.0*(polygon_bound[2].y- polygon_bound[0].y);
-      //ROS_WARN(" in the Zone (%f,%f)!",goal_tmp[0],goal_tmp[1]);
-    } while(isSiteout(Point(goal_tmp[0],goal_tmp[1]), polygon_bound));
-    ROS_WARN("Sending at a random location in the Zone (%f,%f)!",goal_tmp[0],goal_tmp[1]);
+    do
+    {
+      goal_tmp[0] = polygon_bound[0].x + (rand() % 100) / 100.0 * (polygon_bound[2].x - polygon_bound[0].x);
+      goal_tmp[1] = polygon_bound[0].y + (rand() % 100) / 100.0 * (polygon_bound[2].y - polygon_bound[0].y);
+      // ROS_WARN(" in the Zone (%f,%f)!",goal_tmp[0],goal_tmp[1]);
+    } while (isSiteout(Point(goal_tmp[0], goal_tmp[1]), polygon_bound));
+    ROS_WARN("Sending at a random location in the Zone (%f,%f)!", goal_tmp[0], goal_tmp[1]);
     double gps[3];
     gps_from_vec(goal_tmp, gps);
     set_gpsgoal(gps);
     return buzzvm_ret0(vm);
   }
 
-	VoronoiDiagramGenerator vdg;
-  ROS_WARN("[%i] Voronoi Bounded tessellation starting with %i sites...", buzz_utility::get_robotid(),count);
-	vdg.generateVoronoi(xValues, yValues, count, -dist_max, dist_max, -dist_max, dist_max, 3.0);
-  if(logVoronoi)  voronoicsv << ros::Time::now().toNSec() << ",";
-	vdg.resetIterator();
-  //ROS_WARN("[%i] Voronoi Bounded tessellation done!", buzz_utility::get_robotid());
 
-  std::vector <Point>::iterator itc, next;
-	for (itc = polygon_bound.begin(); itc != polygon_bound.end()-1; ++itc) {
-    next = itc+1;
-    if(logVoronoi)  voronoicsv << itc->x << "," << itc->y << "," << next->x << "," << next->y << "," << 0 << "," << 0 << ",";
+  ROS_WARN("NP: %d, Sites: %d", Poly_vert, count);
+  float* xValues = new float[count];
+  float* yValues = new float[count];
+  for (int32_t i = 0; i < count; ++i)
+  {
+    int index = i + Poly_vert;
+    buzzvm_dup(vm);
+    buzzvm_pushi(vm, index);
+    buzzvm_tget(vm);
+
+    buzzvm_dup(vm);
+    buzzvm_pushs(vm, buzzvm_string_register(vm, "x", 1));
+    buzzvm_tget(vm);
+    // ROS_INFO("---x-->%f",buzzvm_stack_at(vm, 1)->f.value);
+    xValues[i] = buzzvm_stack_at(vm, 1)->f.value;
+    buzzvm_pop(vm);
+    buzzvm_dup(vm);
+    buzzvm_pushs(vm, buzzvm_string_register(vm, "y", 1));
+    buzzvm_tget(vm);
+    // ROS_INFO("---y-->%f",buzzvm_stack_at(vm, 1)->f.value);
+    yValues[i] = buzzvm_stack_at(vm, 1)->f.value;
+    buzzvm_pop(vm);
+
+    buzzvm_pop(vm);
   }
 
-	float x1,y1,x2,y2;
+  VoronoiDiagramGenerator vdg;
+  ROS_WARN("[%i] Voronoi Bounded tessellation starting with %i sites...", buzz_utility::get_robotid(), count);
+  vdg.generateVoronoi(xValues, yValues, count, -dist_max, dist_max, -dist_max, dist_max, 3.0);
+  if (logVoronoi)
+    voronoicsv << ros::Time::now().toNSec() << ",";
+  vdg.resetIterator();
+  // ROS_WARN("[%i] Voronoi Bounded tessellation done!", buzz_utility::get_robotid());
+
+  std::vector<Point>::iterator itc, next;
+  for (itc = polygon_bound.begin(); itc != polygon_bound.end() - 1; ++itc)
+  {
+    next = itc + 1;
+    if (logVoronoi)
+      voronoicsv << itc->x << "," << itc->y << "," << next->x << "," << next->y << "," << 0 << "," << 0 << ",";
+  }
+
+  float x1, y1, x2, y2;
   int s[2];
-  vector <Point> cell_vert;
+  vector<Point> cell_vert;
   Point Intersection;
-  int i=0;
-	while(vdg.getNext(x1,y1,x2,y2,s))
-	{
-		//ROS_INFO("GOT Line (%f,%f)->(%f,%f) between sites %d,%d",x1,y1,x2,y2,s[0],s[1]);
-    if(sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1))<0.1)
+  int i = 0;
+  while (vdg.getNext(x1, y1, x2, y2, s))
+  {
+    // ROS_INFO("GOT Line (%f,%f)->(%f,%f) between sites %d,%d",x1,y1,x2,y2,s[0],s[1]);
+    if (sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1)) < 0.1)
       continue;
-    bool isout1 = isSiteout(Point(x1,y1), polygon_bound);
-    bool isout2 = isSiteout(Point(x2,y2), polygon_bound);
-    if(isout1 && isout2){
-      //ROS_INFO("Line out of area!");
+    bool isout1 = isSiteout(Point(x1, y1), polygon_bound);
+    bool isout2 = isSiteout(Point(x2, y2), polygon_bound);
+    if (isout1 && isout2)
+    {
+      // ROS_INFO("Line out of area!");
       continue;
-    }else if(isout1) {
-      getintersection(Point(x2,y2), Point(x1,y1), polygon_bound, &Intersection);
+    }
+    else if (isout1)
+    {
+      getintersection(Point(x2, y2), Point(x1, y1), polygon_bound, &Intersection);
       x1 = Intersection.x;
       y1 = Intersection.y;
-      //ROS_INFO("Site out 1 -> (%f,%f)", x1, y1);
-	  }else if(isout2) {
-      getintersection(Point(x1,y1), Point(x2,y2), polygon_bound, &Intersection);
+      // ROS_INFO("Site out 1 -> (%f,%f)", x1, y1);
+    }
+    else if (isout2)
+    {
+      getintersection(Point(x1, y1), Point(x2, y2), polygon_bound, &Intersection);
       x2 = Intersection.x;
       y2 = Intersection.y;
-      //ROS_INFO("Site out 2 -> (%f,%f)", x2, y2);
+      // ROS_INFO("Site out 2 -> (%f,%f)", x2, y2);
     }
-    if(logVoronoi)  voronoicsv << x1 << "," << y1 << "," << x2 << "," << y2 << "," << s[0] << "," << s[1] << ",";
+    if (logVoronoi)
+      voronoicsv << x1 << "," << y1 << "," << x2 << "," << y2 << "," << s[0] << "," << s[1] << ",";
     i++;
-    if((s[0]==0 || s[1]==0)) {
-      if(cell_vert.empty()){
-        cell_vert.push_back(Point(x1,y1));
-        cell_vert.push_back(Point(x2,y2));
-      } else {
+    if ((s[0] == 0 || s[1] == 0))
+    {
+      if (cell_vert.empty())
+      {
+        cell_vert.push_back(Point(x1, y1));
+        cell_vert.push_back(Point(x2, y2));
+      }
+      else
+      {
         bool alreadyin = false;
-        vector <Point>::iterator itc;
-        for (itc = cell_vert.begin(); itc != cell_vert.end(); ++itc) {
+        vector<Point>::iterator itc;
+        for (itc = cell_vert.begin(); itc != cell_vert.end(); ++itc)
+        {
           double dist = sqrt((itc->x - x1) * (itc->x - x1) + (itc->y - y1) * (itc->y - y1));
-          if(dist < 0.1) {
+          if (dist < 0.1)
+          {
             alreadyin = true;
             break;
           }
         }
-        if(!alreadyin)
+        if (!alreadyin)
           cell_vert.push_back(Point(x1, y1));
         alreadyin = false;
-        for (itc = cell_vert.begin(); itc != cell_vert.end(); ++itc) {
+        for (itc = cell_vert.begin(); itc != cell_vert.end(); ++itc)
+        {
           double dist = sqrt((itc->x - x2) * (itc->x - x2) + (itc->y - y2) * (itc->y - y2));
-          if(dist < 0.1) {
+          if (dist < 0.1)
+          {
             alreadyin = true;
             break;
           }
         }
-        if(!alreadyin)
+        if (!alreadyin)
           cell_vert.push_back(Point(x2, y2));
       }
     }
-	}
-  if(cell_vert.size()<3){
-    ROS_WARN("[%i] Voronoi Bounded tessellation failed (%d)!", buzz_utility::get_robotid(),cell_vert.size());
+  }
+  if (cell_vert.size() < 3)
+  {
+    ROS_WARN("[%i] Voronoi Bounded tessellation failed (%d)!", buzz_utility::get_robotid(), cell_vert.size());
+    delete xValues;
+    delete yValues;
     return buzzvm_ret0(vm);
   }
-  std::sort( cell_vert.begin(), cell_vert.end(), clockwise_compare_points );
+  std::sort(cell_vert.begin(), cell_vert.end(), clockwise_compare_points);
   cell_vert.push_back(cell_vert[0]);
-  
-  double center_dist[2] = {0.0, 0.0};
+
+  double center_dist[2] = { 0.0, 0.0 };
   polygone_center(cell_vert, center_dist);
-  if(logVoronoi)  voronoicsv << center_dist[0] << "," << center_dist[1] << std::endl;
-  center_dist[0]/=2;
-  center_dist[1]/=2;
+  if (logVoronoi)
+    voronoicsv << center_dist[0] << "," << center_dist[1] << std::endl;
+  center_dist[0] /= 2;
+  center_dist[1] /= 2;
   double gps[3];
   gps_from_vec(center_dist, gps);
-  //ROS_INFO("[%i] Voronoi cell center: %f, %f, %f, %f", buzz_utility::get_robotid(), center_dist[0], center_dist[1], gps[0], gps[1]);
+  // ROS_INFO("[%i] Voronoi cell center: %f, %f, %f, %f", buzz_utility::get_robotid(), center_dist[0], center_dist[1],
+  // gps[0], gps[1]);
   set_gpsgoal(gps);
 
+  delete xValues;
+  delete yValues;
   return buzzvm_ret0(vm);
 }
 
@@ -769,7 +839,8 @@ int buzzuav_addNeiStatus(buzzvm_t vm)
   buzzvm_lload(vm, 1);  // state table
   buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE);
   buzzobj_t t = buzzvm_stack_at(vm, 1);
-  if(buzzdict_size(t->t.value) != 5) {
+  if (buzzdict_size(t->t.value) != 5)
+  {
     ROS_ERROR("Wrong neighbor status size.");
     return buzzvm_ret0(vm);
   }
@@ -800,7 +871,7 @@ int buzzuav_addNeiStatus(buzzvm_t vm)
   buzzvm_tget(vm);
   newRS.flight_status = buzzvm_stack_at(vm, 1)->i.value;
   buzzvm_pop(vm);
-  
+
   map<int, buzz_utility::neighbors_status>::iterator it = neighbors_status_map.find(id);
   if (it != neighbors_status_map.end())
     neighbors_status_map.erase(it);
@@ -911,12 +982,17 @@ void set_gpsgoal(double goal[3])
 {
   double rb[3];
   rb_from_gps(goal, rb, cur_pos);
-  if (fabs(rb[0]) < 250.0) {
-    goto_gpsgoal[0] = goal[0];goto_gpsgoal[1] = goal[1];goto_gpsgoal[2] = goal[2];
-    ROS_INFO("[%i] Set GPS GOAL TO ---- %f %f %f (%f %f, %f %f)", buzz_utility::get_robotid(), goal[0], goal[1], goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]);
-  } else
-    ROS_WARN("[%i] GPS GOAL TOO FAR !!-- %f %f %f (%f %f, %f %f)", buzz_utility::get_robotid(), goal[0], goal[1], goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]);
-
+  if (fabs(rb[0]) < 250.0)
+  {
+    goto_gpsgoal[0] = goal[0];
+    goto_gpsgoal[1] = goal[1];
+    goto_gpsgoal[2] = goal[2];
+    ROS_INFO("[%i] Set GPS GOAL TO ---- %f %f %f (%f %f, %f %f)", buzz_utility::get_robotid(), goal[0], goal[1],
+             goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]);
+  }
+  else
+    ROS_WARN("[%i] GPS GOAL TOO FAR !!-- %f %f %f (%f %f, %f %f)", buzz_utility::get_robotid(), goal[0], goal[1],
+             goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]);
 }
 
 int buzzuav_arm(buzzvm_t vm)
@@ -987,7 +1063,7 @@ double* getgoto()
   return goto_pos;
 }
 
-std::map<int, std::map<int,int>> getgrid()
+std::map<int, std::map<int, int>> getgrid()
 /*
 / return the grid
 /-------------------------------------------------------------*/
@@ -1199,7 +1275,8 @@ void update_neighbors(buzzvm_t vm)
 }
 
 // Clear neighbours pos
-void clear_neighbours_pos(){
+void clear_neighbours_pos()
+{
   neighbors_map.clear();
 }
 
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index 50be762..d42e5cd 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -12,8 +12,8 @@ namespace rosbuzz_node
 {
 const string roscontroller::CAPTURE_SRV_DEFAULT_NAME = "/image_sender/capture_image";
 
-roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv):
-logical_clock(ros::Time()), previous_step_time(ros::Time())
+roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv)
+  : logical_clock(ros::Time()), previous_step_time(ros::Time())
 /*
 / roscontroller class Constructor
 ---------------*/
@@ -26,7 +26,7 @@ logical_clock(ros::Time()), previous_step_time(ros::Time())
   cur_pos.longitude = 0;
   cur_pos.latitude = 0;
   cur_pos.altitude = 0;
-  
+
   //  Obtain parameters from ros parameter server
   Rosparameters_get(n_c_priv);
   //  Initialize publishers, subscribers and client
@@ -35,9 +35,9 @@ logical_clock(ros::Time()), previous_step_time(ros::Time())
   std::string fname = Compile_bzz(bzzfile_name);
   bcfname = fname + ".bo";
   dbgfname = fname + ".bdb";
-  buzz_update::set_bzz_file(bzzfile_name.c_str(),debug);
+  buzz_update::set_bzz_file(bzzfile_name.c_str(), debug);
   //  Initialize variables
-  if(setmode)
+  if (setmode)
     SetMode("LOITER", 0);
   goto_pos = buzzuav_closures::getgoto();
 
@@ -67,7 +67,7 @@ logical_clock(ros::Time()), previous_step_time(ros::Time())
   logical_clock.fromSec(0);
   logical_time_rate = 0;
   time_sync_jumped = false;
-  out_msg_time=0;
+  out_msg_time = 0;
   // Create log dir and open log file
   initcsvlog();
   buzzuav_closures::setWPlist(WPfile);
@@ -126,7 +126,7 @@ void roscontroller::RosControllerRun()
     //  set ROS loop rate
     ros::Rate loop_rate(BUZZRATE);
     // check for BVMSTATE variable
-    if(buzz_utility::get_bvmstate()=="Not Available")
+    if (buzz_utility::get_bvmstate() == "Not Available")
       ROS_ERROR("BVMSTATE undeclared in .bzz file, please set BVMSTATE.");
     //  DEBUG
     // ROS_WARN("[%i] -----------------------STARTING MAIN LOOP!", robot_id);
@@ -138,7 +138,8 @@ void roscontroller::RosControllerRun()
       grid_publisher();
       send_MPpayload();
       //  Check updater state and step code
-      if(update) buzz_update::update_routine();
+      if (update)
+        buzz_update::update_routine();
       if (time_step == BUZZRATE)
       {
         time_step = 0;
@@ -159,26 +160,29 @@ void roscontroller::RosControllerRun()
 
       // log data
       logtocsv();
-      
+
       //  Call Step from buzz script
       buzz_utility::buzz_script_step();
       //  Force a refresh on neighbors array once in a while
-      if (timer_step >= 20*BUZZRATE){
+      if (timer_step >= 20 * BUZZRATE)
+      {
         clear_pos();
         timer_step = 0;
-      } else
+      }
+      else
         timer_step++;
       //  Prepare messages and publish them
       prepare_msg_and_publish();
       // Call the flight controler service
       flight_controller_service_call();
       // Broadcast local position to FCU
-      if(BClpose && !setmode)
-	      SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], goto_pos[3]);
+      if (BClpose && !setmode)
+        SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], goto_pos[3]);
       //  Set ROBOTS variable (swarm size)
       get_number_of_robots();
       buzz_utility::set_robot_var(no_of_robots);
-      if(update) buzz_update::updates_set_robots(no_of_robots);
+      if (update)
+        buzz_update::updates_set_robots(no_of_robots);
       // get_xbee_status();  // commented out because it may slow down the node too much, to be tested
 
       ros::spinOnce();
@@ -203,19 +207,19 @@ void roscontroller::initcsvlog()
 / Create the CSV log file
 /-------------------------------------------------------*/
 {
-  std::string path =
-      bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
-  path = path.substr(0, bzzfile_name.find_last_of("\\/"))+"/log/";
-  std::string folder_check="mkdir -p "+path;
+  std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
+  path = path.substr(0, bzzfile_name.find_last_of("\\/")) + "/log/";
+  std::string folder_check = "mkdir -p " + path;
   system(folder_check.c_str());
-  for(int i=5;i>0;i--){
-    rename((path +"logger_"+ std::to_string((uint8_t)robot_id)+"_"+std::to_string(i-1)+".log").c_str(),
-     (path +"logger_"+ std::to_string((uint8_t)robot_id)+"_"+std::to_string(i)+".log").c_str());
+  for (int i = 5; i > 0; i--)
+  {
+    rename((path + "logger_" + std::to_string((uint8_t)robot_id) + "_" + std::to_string(i - 1) + ".log").c_str(),
+           (path + "logger_" + std::to_string((uint8_t)robot_id) + "_" + std::to_string(i) + ".log").c_str());
   }
-  path += "logger_"+std::to_string(robot_id)+"_0.log";
+  path += "logger_" + std::to_string(robot_id) + "_0.log";
   log.open(path.c_str(), std::ios_base::trunc | std::ios_base::out);
-  // set log data double precision 
-  log <<std::setprecision(15) << std::fixed;
+  // set log data double precision
+  log << std::setprecision(15) << std::fixed;
 }
 
 void roscontroller::logtocsv()
@@ -225,28 +229,25 @@ void roscontroller::logtocsv()
 {
   // hardware time now
   log << ros::Time::now().toNSec() << ",";
-  
-  log << cur_pos.latitude << "," << cur_pos.longitude << ","
-  << cur_pos.altitude << ",";
+
+  log << cur_pos.latitude << "," << cur_pos.longitude << "," << cur_pos.altitude << ",";
   log << (int)no_of_robots << ",";
   log << neighbours_pos_map.size() << ",";
   log << (int)inmsgdata.size() << "," << message_number << ",";
-  log << out_msg_time <<",";
-  log << out_msg_size<<",";
+  log << out_msg_time << ",";
+  log << out_msg_size << ",";
   log << buzz_utility::get_bvmstate();
-  
-  map<int, buzz_utility::Pos_struct>::iterator it =
-  neighbours_pos_map.begin();
+
+  map<int, buzz_utility::Pos_struct>::iterator it = neighbours_pos_map.begin();
   for (; it != neighbours_pos_map.end(); ++it)
   {
-      log << "," << it->first << ",";
-      log << (double)it->second.x << "," << (double)it->second.y
-      << "," << (double)it->second.z;
+    log << "," << it->first << ",";
+    log << (double)it->second.x << "," << (double)it->second.y << "," << (double)it->second.z;
   }
-  for (std::vector<msg_data>::iterator it = inmsgdata.begin() ; it != inmsgdata.end(); ++it)
+  for (std::vector<msg_data>::iterator it = inmsgdata.begin(); it != inmsgdata.end(); ++it)
   {
-      log << "," << (int)it->nid << "," << (int)it->msgid << "," << (int)it->size << "," << 
-      it->sent_time << ","<< it->received_time;
+    log << "," << (int)it->nid << "," << (int)it->msgid << "," << (int)it->size << "," << it->sent_time << ","
+        << it->received_time;
   }
   inmsgdata.clear();
   log << std::endl;
@@ -395,7 +396,7 @@ void roscontroller::PubandServ(ros::NodeHandle& n_c, ros::NodeHandle& node_handl
   }
   if (node_handle.getParam("services/modeclient", topic))
   {
-    if(setmode)
+    if (setmode)
       mode_client = n_c.serviceClient<mavros_msgs::SetMode>(topic);
   }
   else
@@ -480,11 +481,10 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c, ros::NodeHandle& n_
   // Publishers and service Clients
 
   PubandServ(n_c, n_c_priv);
-  
+
   ROS_INFO("Ready to receive Mav Commands from RC client");
-  
+
   capture_srv = n_c.serviceClient<mavros_msgs::CommandBool>(capture_srv_name);
-  
 
   multi_msg = true;
 }
@@ -583,11 +583,12 @@ void roscontroller::neighbours_pos_publisher()
     // cout<<"iterator it val: "<< it-> first << " After convertion: "
     // <<(uint8_t) buzz_utility::get_rid_uint8compac(it->first)<<endl;
     // std::cout<<"long obt"<<neigh_tmp.longitude<<endl;
-    
+
     // Check if any simulated target are in range
-    double tf[4] = {-1, 0, 0, 0};
+    double tf[4] = { -1, 0, 0, 0 };
     buzzuav_closures::check_targets_sim((it->second).x, (it->second).y, tf);
-    if(tf[0]!=-1){
+    if (tf[0] != -1)
+    {
       buzz_utility::Pos_struct pos_tmp;
       pos_tmp.x = tf[1];
       pos_tmp.y = tf[2];
@@ -641,24 +642,25 @@ void roscontroller::grid_publisher()
 / Publish current Grid from Buzz script
 /----------------------------------------------------*/
 {
-  std::map<int, std::map<int,int>> grid = buzzuav_closures::getgrid();
-  std::map<int, std::map<int,int>>::iterator itr = grid.begin();
+  std::map<int, std::map<int, int>> grid = buzzuav_closures::getgrid();
+  std::map<int, std::map<int, int>>::iterator itr = grid.begin();
   int g_w = itr->second.size();
   int g_h = grid.size();
 
-  if(g_w!=0 && g_h!=0) {
+  if (g_w != 0 && g_h != 0)
+  {
     // DEBUG
-    //ROS_INFO("------> Publishing a grid of %i x %i", g_h, g_w);
+    // ROS_INFO("------> Publishing a grid of %i x %i", g_h, g_w);
     auto current_time = ros::Time::now();
     nav_msgs::OccupancyGrid grid_msg;
     grid_msg.header.frame_id = "/world";
     grid_msg.header.stamp = current_time;
     grid_msg.info.map_load_time = current_time;  // Same as header stamp as we do not load the map.
-    grid_msg.info.resolution = 0.01;//gridMap.getResolution();
+    grid_msg.info.resolution = 0.01;             // gridMap.getResolution();
     grid_msg.info.width = g_w;
     grid_msg.info.height = g_h;
-    grid_msg.info.origin.position.x = round(g_w/2.0) * grid_msg.info.resolution;
-    grid_msg.info.origin.position.y = round(g_h/2.0) * grid_msg.info.resolution;
+    grid_msg.info.origin.position.x = round(g_w / 2.0) * grid_msg.info.resolution;
+    grid_msg.info.origin.position.y = round(g_h / 2.0) * grid_msg.info.resolution;
     grid_msg.info.origin.position.z = 0.0;
     grid_msg.info.origin.orientation.x = 0.0;
     grid_msg.info.origin.orientation.y = 0.0;
@@ -666,19 +668,21 @@ void roscontroller::grid_publisher()
     grid_msg.info.origin.orientation.w = 1.0;
     grid_msg.data.resize(g_w * g_h);
 
-    for (itr=grid.begin(); itr!=grid.end(); ++itr) {
-      std::map<int,int>::iterator itc = itr->second.begin();
-      for (itc=itr->second.begin(); itc!=itr->second.end(); ++itc) {
-        grid_msg.data[(itr->first-1)*g_w+itc->first] = itc->second;
+    for (itr = grid.begin(); itr != grid.end(); ++itr)
+    {
+      std::map<int, int>::iterator itc = itr->second.begin();
+      for (itc = itr->second.begin(); itc != itr->second.end(); ++itc)
+      {
+        grid_msg.data[(itr->first - 1) * g_w + itc->first] = itc->second;
         // DEBUG
-        //ROS_INFO("--------------> index: %i (%i,%i): %i", (itr->first-1)*g_w+itc->first, itr->first, itc->first, grid_msg.data[(itr->first-1)*g_w+itc->first]);
+        // ROS_INFO("--------------> index: %i (%i,%i): %i", (itr->first-1)*g_w+itc->first, itr->first, itc->first,
+        // grid_msg.data[(itr->first-1)*g_w+itc->first]);
       }
     }
     grid_pub.publish(grid_msg);
   }
 }
 
-
 void roscontroller::Arm()
 /*
 / Functions handling the local MAV ROS flight controller
@@ -729,13 +733,13 @@ with size.........  |   /
   else
     message_number++;
 
-  //header variables
+  // header variables
   uint16_t tmphead[4];
   tmphead[1] = (uint16_t)message_number;
-  //uint32_t stime = (uint32_t)(ros::Time::now().toSec());   //(uint32_t)(logical_clock.toSec() * 100000);
-  memcpy((void*)(tmphead+2),(void*)&stime,sizeof(uint32_t));
+  // uint32_t stime = (uint32_t)(ros::Time::now().toSec());   //(uint32_t)(logical_clock.toSec() * 100000);
+  memcpy((void*)(tmphead + 2), (void*)&stime, sizeof(uint32_t));
   uint64_t rheader[1];
-  rheader[0]=0;
+  rheader[0] = 0;
   payload_out.sysid = (uint8_t)robot_id;
   payload_out.msgid = (uint32_t)message_number;
 
@@ -755,7 +759,7 @@ with size.........  |   /
   {
     payload_out.payload64.push_back(payload_out_ptr[i]);
   }
-  //Store out msg time
+  // Store out msg time
   out_msg_time = ros::Time::now().toNSec();
   out_msg_size = out[0];
   // publish prepared messages in respective topic
@@ -825,7 +829,8 @@ script
       cmd_srv.request.command = buzzuav_closures::getcmd();
       if (!armstate)
       {
-        if(setmode){
+        if (setmode)
+        {
           SetMode("LOITER", 0);
           ros::Duration(0.5).sleep();
         }
@@ -834,13 +839,13 @@ script
         // Registering HOME POINT.
         home = cur_pos;
         // Initialize GPS goal for safety.
-        double gpspgoal[3] = {cur_pos.latitude,cur_pos.longitude,cur_pos.altitude};
+        double gpspgoal[3] = { cur_pos.latitude, cur_pos.longitude, cur_pos.altitude };
         buzzuav_closures::set_gpsgoal(gpspgoal);
-	      BClpose = true;
+        BClpose = true;
       }
       if (current_mode != "GUIDED" && setmode)
         SetMode("GUIDED", 3000);  // added for compatibility with 3DR Solo
-      if(setmode)
+      if (setmode)
       {
         if (mav_client.call(cmd_srv))
         {
@@ -862,7 +867,7 @@ script
         armstate = 0;
         Arm();
       }
-      else if(cur_pos.altitude < 0.4) //disarm only when close to ground
+      else if (cur_pos.altitude < 0.4)  // disarm only when close to ground
       {
         armstate = 0;
         Arm();
@@ -894,17 +899,17 @@ script
     case COMPONENT_ARM_DISARM:
       if (!armstate)
       {
-        if(setmode)
+        if (setmode)
           SetMode("LOITER", 0);
         armstate = 1;
         Arm();
       }
       break;
 
-    case COMPONENT_ARM_DISARM+1:
+    case COMPONENT_ARM_DISARM + 1:
       if (armstate)
       {
-        if(setmode)
+        if (setmode)
           SetMode("LOITER", 0);
         armstate = 0;
         Arm();
@@ -913,7 +918,7 @@ script
 
     case NAV_SPLINE_WAYPOINT:
       goto_pos = buzzuav_closures::getgoto();
-      if(setmode) 
+      if (setmode)
         SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], goto_pos[3]);
       break;
 
@@ -950,12 +955,11 @@ void roscontroller::clear_pos()
 /Refresh neighbours Position for every ten step
 /---------------------------------------------*/
 {
-  
-    neighbours_pos_map.clear();
-    raw_neighbours_pos_map.clear();
-    buzzuav_closures::clear_neighbours_pos();
-    // raw_neighbours_pos_map.clear(); // TODO: currently not a problem, but
-    // have to clear !
+  neighbours_pos_map.clear();
+  raw_neighbours_pos_map.clear();
+  buzzuav_closures::clear_neighbours_pos();
+  // raw_neighbours_pos_map.clear(); // TODO: currently not a problem, but
+  // have to clear !
 }
 
 void roscontroller::neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr)
@@ -1088,21 +1092,17 @@ void roscontroller::local_pos_callback(const geometry_msgs::PoseStamped::ConstPt
   cur_pos.x = msg->pose.position.x;
   cur_pos.y = msg->pose.position.y;
 
-  if(!BClpose)
+  if (!BClpose)
   {
-  goto_pos[0]=cur_pos.x;
-  goto_pos[1]=cur_pos.y;
-  goto_pos[2]=0.0;
-  BClpose = true;
+    goto_pos[0] = cur_pos.x;
+    goto_pos[1] = cur_pos.y;
+    goto_pos[2] = 0.0;
+    BClpose = true;
   }
 
-  buzzuav_closures::set_currentNEDpos(msg->pose.position.y,msg->pose.position.x);
+  buzzuav_closures::set_currentNEDpos(msg->pose.position.y, msg->pose.position.x);
   //  cur_pos.z = pose->pose.position.z; // Using relative altitude topic instead
-  tf::Quaternion q(
-    msg->pose.orientation.x,
-    msg->pose.orientation.y,
-    msg->pose.orientation.z,
-    msg->pose.orientation.w);
+  tf::Quaternion q(msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z, msg->pose.orientation.w);
   tf::Matrix3x3 m(q);
   double roll, pitch, yaw;
   m.getRPY(roll, pitch, yaw);
@@ -1157,8 +1157,8 @@ void roscontroller::SetLocalPosition(float x, float y, float z, float yaw)
   moveMsg.pose.orientation.w = q[3];
 
   // To prevent drifting from stable position, uncomment
-  //if(fabs(x)>0.005 || fabs(y)>0.005) {
-    localsetpoint_nonraw_pub.publish(moveMsg);
+  // if(fabs(x)>0.005 || fabs(y)>0.005) {
+  localsetpoint_nonraw_pub.publish(moveMsg);
   //}
 }
 
@@ -1197,15 +1197,15 @@ void roscontroller::SetStreamRate(int id, int rate, int on_off)
   message.request.stream_id = id;
   message.request.message_rate = rate;
   message.request.on_off = on_off;
-  int timeout = 20; // 2sec at 10Hz
+  int timeout = 20;  // 2sec at 10Hz
 
-  while (!stream_client.call(message) && timeout>0)
+  while (!stream_client.call(message) && timeout > 0)
   {
     ROS_INFO("Set stream rate call failed!, trying again...");
     ros::Duration(0.1).sleep();
     timeout--;
   }
-  if(timeout<1)
+  if (timeout < 1)
     ROS_ERROR("Set stream rate timed out!");
   else
     ROS_WARN("Set stream rate call successful");
@@ -1228,12 +1228,12 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg)
   // decode msg header
   uint64_t rhead = msg->payload64[0];
   uint16_t r16head[4];
-  memcpy(r16head,&rhead, sizeof(uint64_t));
+  memcpy(r16head, &rhead, sizeof(uint64_t));
   uint16_t mtype = r16head[0];
   uint16_t mid = r16head[1];
-  uint32_t temptime=0;
-  memcpy(&temptime, r16head+2, sizeof(uint32_t));
-  //float stime = (float)temptime/(float)100000;
+  uint32_t temptime = 0;
+  memcpy(&temptime, r16head + 2, sizeof(uint32_t));
+  // float stime = (float)temptime/(float)100000;
   // if(debug) ROS_INFO("Received Msg: sent time %f for id %u",stime, mid);
   // Check for Updater message, if updater message push it into updater FIFO
   if ((uint64_t)msg->payload64[0] == (uint64_t)UPDATER_MESSAGE)
@@ -1284,7 +1284,7 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg)
     //  Extract robot id of the neighbour
     uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + index));
     // store in msg data for data log
-    msg_data inm(mid,out[1],out[0]*sizeof(uint64_t),stime,ros::Time::now().toNSec());
+    msg_data inm(mid, out[1], out[0] * sizeof(uint64_t), stime, ros::Time::now().toNSec());
     inmsgdata.push_back(inm);
 
     if (debug)