From 06285246b7b8d0c05acc0793676ae295caed4dcb Mon Sep 17 00:00:00 2001
From: Jean-Baptiste Mouret <mouret@isir.upmc.fr>
Date: Fri, 15 Apr 2016 13:44:10 +0200
Subject: [PATCH] indent

---
 display.cpp            | 539 ++++++++++++++++++++---------------------
 display.hpp            |  95 ++++----
 fastsim_keyboard.cpp   |  39 ++-
 goal.hpp               |  28 ++-
 illuminated_switch.hpp | 123 +++++-----
 laser.cpp              |  31 ++-
 laser.hpp              |  66 +++--
 light_sensor.cpp       |  38 ++-
 light_sensor.hpp       |  40 +--
 linear_camera.cpp      |  23 +-
 linear_camera.hpp      |  32 +--
 main.cpp               |  49 ++--
 map.cpp                | 205 ++++++++--------
 map.hpp                | 162 +++++++------
 misc.hpp               |  52 ++--
 posture.hpp            | 117 ++++-----
 radar.cpp              |  24 +-
 radar.hpp              |  32 ++-
 robot.cpp              |  72 +++---
 robot.hpp              | 114 ++++++---
 simu_fastsim.hpp       | 112 +++++----
 test_fastsim.cpp       |  69 +++---
 22 files changed, 1048 insertions(+), 1014 deletions(-)

diff --git a/display.cpp b/display.cpp
index 4ddbd83..a2ab074 100644
--- a/display.cpp
+++ b/display.cpp
@@ -3,18 +3,18 @@
 ** Login : <mouret@mexico>
 ** Started on  Sat Jan 12 20:40:12 2008 Jeanbaptiste MOURET
 ** $Id$
-** 
+**
 ** Copyright (C) 2008 Jeanbaptiste MOURET
 ** This program is free software; you can redistribute it and/or modify
 ** it under the terms of the GNU General Public License as published by
 ** the Free Software Foundation; either version 2 of the License, or
 ** (at your option) any later version.
-** 
+**
 ** This program is distributed in the hope that it will be useful,
 ** but WITHOUT ANY WARRANTY; without even the implied warranty of
 ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 ** GNU General Public License for more details.
-** 
+**
 ** You should have received a copy of the GNU General Public License
 ** along with this program; if not, write to the Free Software
 ** Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
@@ -24,47 +24,41 @@
 
 #ifdef USE_SDL
 
-namespace fastsim
-{
+namespace fastsim {
 
-  void Display :: _events()
-  {
+  void Display :: _events() {
     SDL_Event event;
-    while (SDL_PollEvent(&event))
-      {
-	switch (event.type) 
-	  {
-	  case SDL_QUIT:
-	    _quit();
-	  case SDL_KEYDOWN:
-	    if (event.key.keysym.sym == SDLK_ESCAPE)
-	      _quit();
-	    break;
-	  }
+    while (SDL_PollEvent(&event)) {
+      switch (event.type) {
+      case SDL_QUIT:
+        _quit();
+      case SDL_KEYDOWN:
+        if (event.key.keysym.sym == SDLK_ESCAPE)
+          _quit();
+        break;
       }
+    }
   }
 
-  void Display :: _blit_map()
-  {
+  void Display :: _blit_map() {
     for (unsigned i = 0; i < _map->get_pixel_w(); ++i)
       for (unsigned j = 0; j < _map->get_pixel_h(); ++j)
-	if (_map->get_pixel(i, j) == Map::obstacle)
-	  _put_pixel(_map_bmp, i, j, 0, 0, 0);
-	else
-	  _put_pixel(_map_bmp, i, j, 255, 255, 255);
+        if (_map->get_pixel(i, j) == Map::obstacle)
+          _put_pixel(_map_bmp, i, j, 0, 0, 0);
+        else
+          _put_pixel(_map_bmp, i, j, 255, 255, 255);
     SDL_BlitSurface(_map_bmp, 0, _screen, 0);
     SDL_UpdateRect(_screen, 0, 0, _w, _h);
   }
 
-  Display :: Display(const boost::shared_ptr<Map>& m, const Robot& r) : 
-    _map(m), _robot(r)
-  {
+  Display :: Display(const boost::shared_ptr<Map>& m, const Robot& r) :
+    _map(m), _robot(r) {
     _w = _map->get_pixel_w();
     _h = _map->get_pixel_h();
     if (SDL_Init(SDL_INIT_VIDEO) == -1)
       throw Exception(SDL_GetError());
-    _screen = SDL_SetVideoMode(_w, _h, 
-			       32, SDL_SWSURFACE);
+    _screen = SDL_SetVideoMode(_w, _h,
+                               32, SDL_SWSURFACE);
     if (!_screen)
       throw Exception(SDL_GetError());
 
@@ -81,90 +75,91 @@ namespace fastsim
     amask = 0xff000000;
 #endif
 
-    _map_bmp = SDL_CreateRGBSurface(SDL_SWSURFACE, _w, _h, 32, 
-				    rmask, gmask, bmask, amask);
+    _map_bmp = SDL_CreateRGBSurface(SDL_SWSURFACE, _w, _h, 32,
+                                    rmask, gmask, bmask, amask);
     _blit_map();
     _bb_to_sdl(_robot.get_bb(), &_prev_bb);
   }
-  
+
   void Display :: _line(SDL_Surface* surf,
-			int x0, int y0, int x1, int y1, 
-			Uint32 color)
-  {
-    x0 = std::max(x0, 0); x0 = std::min(x0, surf->w - 1);
-    x1 = std::max(x1, 0); x1 = std::min(x1, surf->w - 1);
-    y0 = std::max(y0, 0); y0 = std::min(y0, surf->h - 1);
-    y1 = std::max(y1, 0); y1 = std::min(y1, surf->h - 1);
+                        int x0, int y0, int x1, int y1,
+                        Uint32 color) {
+    x0 = std::max(x0, 0);
+    x0 = std::min(x0, surf->w - 1);
+    x1 = std::max(x1, 0);
+    x1 = std::min(x1, surf->w - 1);
+    y0 = std::max(y0, 0);
+    y0 = std::min(y0, surf->h - 1);
+    y1 = std::max(y1, 0);
+    y1 = std::min(y1, surf->h - 1);
     int dy = y1 - y0;
     int dx = x1 - x0;
     int stepx, stepy;
 
-    if (dy < 0) { dy = -dy;  stepy = -1; } else { stepy = 1; }
-    if (dx < 0) { dx = -dx;  stepx = -1; } else { stepx = 1; }
+    if (dy < 0) {
+      dy = -dy;
+      stepy = -1;
+    } else {
+      stepy = 1;
+    }
+    if (dx < 0) {
+      dx = -dx;
+      stepx = -1;
+    } else {
+      stepx = 1;
+    }
     dy <<= 1; // dy is now 2*dy
     dx <<= 1; // dx is now 2*dx
 
     _put_pixel(surf, color, x0, y0);
-    if (dx > dy) 
-      {
-	int fraction = dy - (dx >> 1); // same as 2*dy - dx
-	while (x0 != x1)
-	  {
-	    if (fraction >= 0)
-	      {
-		y0 += stepy;
-		fraction -= dx; // -= 2*dx
-	      }
-	    x0 += stepx;
-	    fraction += dy; //  -= 2*dy
-	    _put_pixel(surf, color, x0, y0);
-	  }
-      } 
-    else 
-      {
-	int fraction = dx - (dy >> 1);
-	while (y0 != y1)
-	  {
-	    if (fraction >= 0)
-	      {
-		x0 += stepx;
-		fraction -= dy;
-	      }
-	    y0 += stepy;
-	    fraction += dx;
-	    _put_pixel(surf, color, x0, y0);
-	  }
+    if (dx > dy) {
+      int fraction = dy - (dx >> 1); // same as 2*dy - dx
+      while (x0 != x1) {
+        if (fraction >= 0) {
+          y0 += stepy;
+          fraction -= dx; // -= 2*dx
+        }
+        x0 += stepx;
+        fraction += dy; //  -= 2*dy
+        _put_pixel(surf, color, x0, y0);
+      }
+    } else {
+      int fraction = dx - (dy >> 1);
+      while (y0 != y1) {
+        if (fraction >= 0) {
+          x0 += stepx;
+          fraction -= dy;
+        }
+        y0 += stepy;
+        fraction += dx;
+        _put_pixel(surf, color, x0, y0);
       }
+    }
   }
 
   void Display :: _try_pixel(bool& res,
-			     SDL_Surface* surf,
-			     Uint32 color, int x, int y)
-  {
-
-    if (x >= 0 && y >= 0 
-	&& x < _map->get_pixel_w() 
-	&& y < _map->get_pixel_h() 
-	&& _map->get_pixel(x, y) == Map::free)
-      {
-	_put_pixel(surf, color, x, y);
-	res = false;
-      }
-    else
+                             SDL_Surface* surf,
+                             Uint32 color, int x, int y) {
+
+    if (x >= 0 && y >= 0
+        && x < _map->get_pixel_w()
+        && y < _map->get_pixel_h()
+        && _map->get_pixel(x, y) == Map::free) {
+      _put_pixel(surf, color, x, y);
+      res = false;
+    } else
       res = true;
   }
 
   void Display :: _circle_points(SDL_Surface* surf,
-				 int cx, int cy, int x, int y, Uint32 color)
-  {
+                                 int cx, int cy, int x, int y, Uint32 color) {
 
     if (x == 0) {
       _put_pixel(surf, color, cx, cy + y);
       _put_pixel(surf, color, cx, cy - y);
       _put_pixel(surf, color, cx + y, cy);
       _put_pixel(surf, color, cx - y, cy);
-    }
-    else if (x == y) {
+    } else if (x == y) {
       _put_pixel(surf, color, cx + x, cy + y);
       _put_pixel(surf, color, cx - x, cy + y);
       _put_pixel(surf, color, cx + x, cy - y);
@@ -182,34 +177,30 @@ namespace fastsim
   }
 
   void Display :: _circle(SDL_Surface *surf,
-			  int x_center, int y_center, int radius,
-			  Uint32 color)
-  {    
+                          int x_center, int y_center, int radius,
+                          Uint32 color) {
     int x = 0;
     int y = radius;
     int p = (5 - radius * 4) / 4;
-    
+
     _circle_points(surf, x_center, y_center, x, y, color);
-    while (x < y) 
-      {
-	x++;
-	if (p < 0) 
-	  p += 2 * x + 1;
-	else
-	  {
-	    y--;
-	    p += 2 * (x - y) + 1;
-	  }
-	_circle_points(surf, x_center, y_center, x, y, color);
+    while (x < y) {
+      x++;
+      if (p < 0)
+        p += 2 * x + 1;
+      else {
+        y--;
+        p += 2 * (x - y) + 1;
       }
+      _circle_points(surf, x_center, y_center, x, y, color);
+    }
   }
   /// see http://en.wikipedia.org/wiki/Midpoint_circle_algorithm
   // transform setPixel(x0 + x, y0 + y); setPixel(x0 - x, y0 + y);
   // to  _line(x0 - x,, y0+y, x0 + x, y0 + y)
   void Display :: _disc(SDL_Surface *surf,
-			int x_center, int y_center, int radius,
-			Uint32 color)
-  {
+                        int x_center, int y_center, int radius,
+                        Uint32 color) {
     int f = 1 - radius;
     int ddF_x = 1;
     int ddF_y = -2 * radius;
@@ -219,37 +210,34 @@ namespace fastsim
     int y0 = y_center;
 
     _line(surf, x0, y0 - radius, x0, y0 + radius, color);
-    _line(surf, x0 - radius, y0, x0 + radius, y0, color);    
-    while(x < y)
-      {
-	// ddF_x == 2 * x + 1;
-	// ddF_y == -2 * y;
-	// f == x*x + y*y - radius*radius + 2*x - y + 1;
-	if(f >= 0) 
-	  {
-	    y--;
-	    ddF_y += 2;
-	    f += ddF_y;
-	  }
-	x++;
-	ddF_x += 2;
-	f += ddF_x;   
-	_line(surf, x0 - x, y0 + y, x0 + x, y0 + y, color);
-	_line(surf, x0 - x, y0 - y, x0 + x, y0 - y, color);
-	_line(surf, x0 - y, y0 + x, x0 + y, y0 + x, color);
-	_line(surf, x0 - y, y0 - x, x0 + y, y0 - x, color);
+    _line(surf, x0 - radius, y0, x0 + radius, y0, color);
+    while(x < y) {
+      // ddF_x == 2 * x + 1;
+      // ddF_y == -2 * y;
+      // f == x*x + y*y - radius*radius + 2*x - y + 1;
+      if(f >= 0) {
+        y--;
+        ddF_y += 2;
+        f += ddF_y;
       }
+      x++;
+      ddF_x += 2;
+      f += ddF_x;
+      _line(surf, x0 - x, y0 + y, x0 + x, y0 + y, color);
+      _line(surf, x0 - x, y0 - y, x0 + x, y0 - y, color);
+      _line(surf, x0 - y, y0 + x, x0 + y, y0 + x, color);
+      _line(surf, x0 - y, y0 - x, x0 + y, y0 - x, color);
+    }
   }
 
 
 
-  void Display :: _disp_bb()
-  {
+  void Display :: _disp_bb() {
     unsigned x = _map->real_to_pixel(_robot.get_bb().x);
     unsigned y = _map->real_to_pixel(_robot.get_bb().y);
     unsigned w = _map->real_to_pixel(_robot.get_bb().w);
     unsigned h = _map->real_to_pixel(_robot.get_bb().h);
-    
+
     assert(x >= 0);
     assert(y >= 0);
     assert(x + w < _screen->w);
@@ -257,64 +245,57 @@ namespace fastsim
     _line(_screen, x, y, x + w, y, 0);
     _line(_screen, x + w, y, x + w, y + h, 0);
     _line(_screen, x + w, y + h, x, y + h, 0);
-    _line(_screen, x, y + h, x, y, 0);    
+    _line(_screen, x, y + h, x, y, 0);
   }
 
-  void Display :: _disp_goals()
-  {
-    for (size_t i = 0; i < _map->get_goals().size(); ++i)
-      {
-	const Goal& goal = _map->get_goals()[i];
-	unsigned x = _map->real_to_pixel(goal.get_x());
-	unsigned y = _map->real_to_pixel(goal.get_y());
-	unsigned diam = _map->real_to_pixel(goal.get_diam());
-	Uint8 r = 0, g = 0, b = 0;
-	switch (goal.get_color())
-	  {
-	  case 0:
-	    r = 255;
-	    break;
-	  case 1:
-	    g = 255;
-	    break;
-	  case 2:
-	    b = 255;
-	    break;
-	  default:
-	    assert(0);
-	  }
-	_circle(_screen, x, y, diam, r, g, b);
-      }    
+  void Display :: _disp_goals() {
+    for (size_t i = 0; i < _map->get_goals().size(); ++i) {
+      const Goal& goal = _map->get_goals()[i];
+      unsigned x = _map->real_to_pixel(goal.get_x());
+      unsigned y = _map->real_to_pixel(goal.get_y());
+      unsigned diam = _map->real_to_pixel(goal.get_diam());
+      Uint8 r = 0, g = 0, b = 0;
+      switch (goal.get_color()) {
+      case 0:
+        r = 255;
+        break;
+      case 1:
+        g = 255;
+        break;
+      case 2:
+        b = 255;
+        break;
+      default:
+        assert(0);
+      }
+      _circle(_screen, x, y, diam, r, g, b);
+    }
   }
 
-  void Display :: _disp_radars()
-  {
+  void Display :: _disp_radars() {
     unsigned r = _map->real_to_pixel(_robot.get_radius())  / 2;
     unsigned x = _map->real_to_pixel(_robot.get_pos().x());
     unsigned y = _map->real_to_pixel(_robot.get_pos().y());
 
-    for (size_t i = 0; i < _robot.get_radars().size(); ++i)
-      {
-	const Radar& radar = _robot.get_radars()[i];
-	if (radar.get_activated_slice() != -1)
-	  {
-	    float a1 = _robot.get_pos().theta() + radar.get_inc() * radar.get_activated_slice();
-	    float a2 = _robot.get_pos().theta() + radar.get_inc() * (radar.get_activated_slice() + 1);
-	    _line(_screen,
-		  cos(a1) * r + x, sin(a1) * r + y,
-		  cos(a2) * r + x, sin(a2) * r + y,
-		  0x0000FF);
-	    const Goal& g = _map->get_goals()[radar.get_color()];
-	    unsigned gx = _map->real_to_pixel(g.get_x());
-	    unsigned gy = _map->real_to_pixel(g.get_y());	    
-	    _line(_screen, x, y, gx, gy, 0x0000FF);
-	  }
-
+    for (size_t i = 0; i < _robot.get_radars().size(); ++i) {
+      const Radar& radar = _robot.get_radars()[i];
+      if (radar.get_activated_slice() != -1) {
+        float a1 = _robot.get_pos().theta() + radar.get_inc() * radar.get_activated_slice();
+        float a2 = _robot.get_pos().theta() + radar.get_inc() * (radar.get_activated_slice() + 1);
+        _line(_screen,
+              cos(a1) * r + x, sin(a1) * r + y,
+              cos(a2) * r + x, sin(a2) * r + y,
+              0x0000FF);
+        const Goal& g = _map->get_goals()[radar.get_color()];
+        unsigned gx = _map->real_to_pixel(g.get_x());
+        unsigned gy = _map->real_to_pixel(g.get_y());
+        _line(_screen, x, y, gx, gy, 0x0000FF);
       }
-      
+
+    }
+
   }
-  void Display :: _disp_bumpers()
-  {
+  void Display :: _disp_bumpers() {
     // convert to pixel
     unsigned x = _map->real_to_pixel(_robot.get_pos().x());
     unsigned y = _map->real_to_pixel(_robot.get_pos().y());
@@ -323,136 +304,129 @@ namespace fastsim
     Uint32 cb_left = SDL_MapRGB(_screen->format, _robot.get_left_bumper() ? 255 : 0, 0, 0);
     Uint32 cb_right = SDL_MapRGB(_screen->format, _robot.get_right_bumper() ? 255 : 0, 0, 0);
     _line(_screen,
-	  (int) (r * cosf(theta + M_PI / 2.0f) + x),
-	  (int) (r * sinf(theta + M_PI / 2.0f) + y),
-	  (int) (r * cosf(theta) + x),
-	  (int) (r * sinf(theta) + y),
-	  cb_left);
+          (int) (r * cosf(theta + M_PI / 2.0f) + x),
+          (int) (r * sinf(theta + M_PI / 2.0f) + y),
+          (int) (r * cosf(theta) + x),
+          (int) (r * sinf(theta) + y),
+          cb_left);
     _line(_screen,
-	  (int) (r * cosf(theta - M_PI / 2.0f) + x),
-	  (int) (r * sinf(theta - M_PI / 2.0f) + y),
-	  (int) (r * cosf(theta) + x),
-	  (int) (r * sinf(theta) + y),
-	  cb_right);
+          (int) (r * cosf(theta - M_PI / 2.0f) + x),
+          (int) (r * sinf(theta - M_PI / 2.0f) + y),
+          (int) (r * cosf(theta) + x),
+          (int) (r * sinf(theta) + y),
+          cb_right);
   }
 
-  Uint32 Display :: _color_from_id(SDL_Surface* surf, int x)
-  {
-    std::bitset<4> bs = x;	
+  Uint32 Display :: _color_from_id(SDL_Surface* surf, int x) {
+    std::bitset<4> bs = x;
     Uint8 k = 1 - bs[4];
     Uint8 r = bs[0] * 128 + 127 * k;
     Uint8 g = bs[1] * 128 + 127 * k;
     Uint8 b = bs[2] * 128 + 127 * k;
-    
+
     return SDL_MapRGB(surf->format, r, g, b);
   }
-  
-  void Display :: _disp_switches()
-  {
-    for (size_t i = 0; i < _map->get_illuminated_switches().size(); ++i)
-      {
-	const IlluminatedSwitch& sw = *_map->get_illuminated_switches()[i];
-	unsigned x = _map->real_to_pixel(sw.get_x());
-	unsigned y = _map->real_to_pixel(sw.get_y());
-	unsigned rad = _map->real_to_pixel(sw.get_radius());
-	Uint32 color = _color_from_id(_screen, sw.get_color());
-
-	_circle(_screen, x, y, rad, color);
-	if (sw.get_on())
-	  _disc(_screen, x, y, rad, color);
-      }
+
+  void Display :: _disp_switches() {
+    for (size_t i = 0; i < _map->get_illuminated_switches().size(); ++i) {
+      const IlluminatedSwitch& sw = *_map->get_illuminated_switches()[i];
+      unsigned x = _map->real_to_pixel(sw.get_x());
+      unsigned y = _map->real_to_pixel(sw.get_y());
+      unsigned rad = _map->real_to_pixel(sw.get_radius());
+      Uint32 color = _color_from_id(_screen, sw.get_color());
+
+      _circle(_screen, x, y, rad, color);
+      if (sw.get_on())
+        _disc(_screen, x, y, rad, color);
+    }
   }
 
-  void Display :: _disp_lasers()
-  {
-    for (size_t i = 0; i < _robot.get_lasers().size(); ++i)
-      {
-	unsigned x_laser = _map->real_to_pixel(_robot.get_pos().x() 
-					       + _robot.get_lasers()[i].get_gap_dist() 
-					       * cosf(_robot.get_pos().theta() 
-						      + _robot.get_lasers()[i].get_gap_angle()));
-	unsigned y_laser = _map->real_to_pixel(_robot.get_pos().y() 
-					       + _robot.get_lasers()[i].get_gap_dist() 
-					       * sinf(_robot.get_pos().theta()
-						      + _robot.get_lasers()[i].get_gap_angle()));
-	_line(_screen, x_laser, y_laser,
-	      _robot.get_lasers()[i].get_x_pixel(),
-	      _robot.get_lasers()[i].get_y_pixel(),
-	      0xFF00000);
-      }
+  void Display :: _disp_lasers() {
+    for (size_t i = 0; i < _robot.get_lasers().size(); ++i) {
+      unsigned x_laser = _map->real_to_pixel(_robot.get_pos().x()
+                                             + _robot.get_lasers()[i].get_gap_dist()
+                                             * cosf(_robot.get_pos().theta()
+                                                 + _robot.get_lasers()[i].get_gap_angle()));
+      unsigned y_laser = _map->real_to_pixel(_robot.get_pos().y()
+                                             + _robot.get_lasers()[i].get_gap_dist()
+                                             * sinf(_robot.get_pos().theta()
+                                                 + _robot.get_lasers()[i].get_gap_angle()));
+      _line(_screen, x_laser, y_laser,
+            _robot.get_lasers()[i].get_x_pixel(),
+            _robot.get_lasers()[i].get_y_pixel(),
+            0xFF00000);
+    }
   }
 
-  void Display :: _disp_light_sensors()
-  {
-    for (size_t i = 0; i < _robot.get_light_sensors().size(); ++i)
-      {
-	const LightSensor& ls = _robot.get_light_sensors()[i];
-	unsigned x_ls = _map->real_to_pixel(_robot.get_pos().x());
-	unsigned y_ls = _map->real_to_pixel(_robot.get_pos().y());
-	unsigned x_ls1 = _map->real_to_pixel(_robot.get_pos().x() 
-					       + 200./(float)ls.get_color() 
-					       * cosf(_robot.get_pos().theta() 
-						      + ls.get_angle()-ls.get_range()/2.0));
-	unsigned y_ls1 = _map->real_to_pixel(_robot.get_pos().y() 
-					       + 200./(float)ls.get_color()
-					       * sinf(_robot.get_pos().theta()
-						      + ls.get_angle()-ls.get_range()/2.0));	
-	_line(_screen, x_ls, y_ls, x_ls1, y_ls1, _color_from_id(_screen, ls.get_color()));
-	unsigned x_ls2 = _map->real_to_pixel(_robot.get_pos().x() 
-					       + 200./(float)ls.get_color() 
-					       * cosf(_robot.get_pos().theta() 
-						      + ls.get_angle()+ls.get_range()/2.0));
-	unsigned y_ls2 = _map->real_to_pixel(_robot.get_pos().y() 
-					       + 200./(float)ls.get_color() 
-					       * sinf(_robot.get_pos().theta()
-						      + ls.get_angle()+ls.get_range()/2.0));	
-	_line(_screen, x_ls, y_ls, x_ls2, y_ls2, _color_from_id(_screen, ls.get_color()));
-	_line(_screen, x_ls1, y_ls1, x_ls2, y_ls2, _color_from_id(_screen, ls.get_color()));
-
-	if (ls.get_activated())
-	  {
-	    const IlluminatedSwitch& is = 
-	      *_map->get_illuminated_switches()[ls.get_num()];
-	    unsigned x_is = _map->real_to_pixel(is.get_x());
-	    unsigned y_is = _map->real_to_pixel(is.get_y());
-	    _line(_screen, x_ls, y_ls, x_is, y_is, _color_from_id(_screen, is.get_color()));
-	  }
+  void Display :: _disp_light_sensors() {
+    for (size_t i = 0; i < _robot.get_light_sensors().size(); ++i) {
+      const LightSensor& ls = _robot.get_light_sensors()[i];
+      unsigned x_ls = _map->real_to_pixel(_robot.get_pos().x());
+      unsigned y_ls = _map->real_to_pixel(_robot.get_pos().y());
+      unsigned x_ls1 = _map->real_to_pixel(_robot.get_pos().x()
+                                           + 200./(float)ls.get_color()
+                                           * cosf(_robot.get_pos().theta()
+                                               + ls.get_angle()-ls.get_range()/2.0));
+      unsigned y_ls1 = _map->real_to_pixel(_robot.get_pos().y()
+                                           + 200./(float)ls.get_color()
+                                           * sinf(_robot.get_pos().theta()
+                                               + ls.get_angle()-ls.get_range()/2.0));
+      _line(_screen, x_ls, y_ls, x_ls1, y_ls1, _color_from_id(_screen, ls.get_color()));
+      unsigned x_ls2 = _map->real_to_pixel(_robot.get_pos().x()
+                                           + 200./(float)ls.get_color()
+                                           * cosf(_robot.get_pos().theta()
+                                               + ls.get_angle()+ls.get_range()/2.0));
+      unsigned y_ls2 = _map->real_to_pixel(_robot.get_pos().y()
+                                           + 200./(float)ls.get_color()
+                                           * sinf(_robot.get_pos().theta()
+                                               + ls.get_angle()+ls.get_range()/2.0));
+      _line(_screen, x_ls, y_ls, x_ls2, y_ls2, _color_from_id(_screen, ls.get_color()));
+      _line(_screen, x_ls1, y_ls1, x_ls2, y_ls2, _color_from_id(_screen, ls.get_color()));
+
+      if (ls.get_activated()) {
+        const IlluminatedSwitch& is =
+          *_map->get_illuminated_switches()[ls.get_num()];
+        unsigned x_is = _map->real_to_pixel(is.get_x());
+        unsigned y_is = _map->real_to_pixel(is.get_y());
+        _line(_screen, x_ls, y_ls, x_is, y_is, _color_from_id(_screen, is.get_color()));
       }
+    }
   }
 
-  void Display :: _disp_camera()
-  {
-    static const int pw = 20;
+  void Display :: _disp_camera() {
+    BOOST_STATIC_CONSTEXPR int pw = 20;
     if (!_robot.use_camera())
       return;
     unsigned x_ls = _map->real_to_pixel(_robot.get_pos().x());
     unsigned y_ls = _map->real_to_pixel(_robot.get_pos().y());
     float a1 = _robot.get_pos().theta() + _robot.get_camera().get_angular_range() / 2.0;
-    _line(_screen, x_ls, y_ls, cos(a1) * 200 + x_ls, 
-	  sin(a1) * 200 + y_ls, 0x0000ff);
+    _line(_screen, x_ls, y_ls, cos(a1) * 200 + x_ls,
+          sin(a1) * 200 + y_ls, 0x0000ff);
     float a2 = _robot.get_pos().theta() - _robot.get_camera().get_angular_range() / 2.0;
-    _line(_screen, x_ls, y_ls, cos(a2) * 200 + x_ls, 
-	  sin(a2) * 200 + y_ls, 0x0000ff);
-
-    for (size_t i = 0; i < _robot.get_camera().pixels().size(); ++i)
-      {
-	int pix = _robot.get_camera().pixels()[i];	
-	Uint32 color = pix == -1 ? 0xffffff : _color_from_id(_screen, pix);
-	SDL_Rect r; r.x = i * pw; r.y = 0; r.w = pw; r.h = pw;
-	SDL_FillRect(_screen, &r, color);
-      }
+    _line(_screen, x_ls, y_ls, cos(a2) * 200 + x_ls,
+          sin(a2) * 200 + y_ls, 0x0000ff);
+
+    for (size_t i = 0; i < _robot.get_camera().pixels().size(); ++i) {
+      int pix = _robot.get_camera().pixels()[i];
+      Uint32 color = pix == -1 ? 0xffffff : _color_from_id(_screen, pix);
+      SDL_Rect r;
+      r.x = i * pw;
+      r.y = 0;
+      r.w = pw;
+      r.h = pw;
+      SDL_FillRect(_screen, &r, color);
+    }
 
   }
 
-  void Display :: update()
-  {
+  void Display :: update() {
     _events();
     // convert to pixel
     unsigned x = _map->real_to_pixel(_robot.get_pos().x());
     unsigned y = _map->real_to_pixel(_robot.get_pos().y());
     unsigned r = _map->real_to_pixel(_robot.get_radius());
     float theta = _robot.get_pos().theta();
-    
+
     // erase robot
     SDL_BlitSurface(_map_bmp, &_prev_bb, _screen, &_prev_bb);
     // erase all
@@ -463,30 +437,30 @@ namespace fastsim
 
     // goals
     _disp_goals();
-    
+
     // illuminated switches
     _disp_switches();
-    
+
     // light sensor
     _disp_light_sensors();
 
     // radars
     _disp_radars();
-    
+
     // camera
     _disp_camera();
 
     // draw the circle again (robot)
-	unsigned int col=_robot.color();
+    unsigned int col=_robot.color();
     _disc(_screen, x, y, r, _color_from_id(_screen,col));
-	_circle(_screen,x,y,r,255,0,0);
+    _circle(_screen,x,y,r,255,0,0);
     // direction
     Uint32 color = SDL_MapRGB(_screen->format, 0, 255, 0);
     _line(_screen, x, y, (int) (r * cosf(theta) + x), (int)(r * sinf(theta) + y), color);
 
     // bumpers
     _disp_bumpers();
-        
+
     SDL_Rect rect;
     _bb_to_sdl(_robot.get_bb(), &rect);
     using namespace std;
@@ -494,14 +468,15 @@ namespace fastsim
     rect.y = max(0, min((int)rect.y, (int)_prev_bb.y));
     rect.w = max(rect.w, _prev_bb.w);
     rect.h = max(rect.h, _prev_bb.h);
-    
+
     if (rect.x + rect.w > _w) rect.w = _w;
     if (rect.y + rect.h > _h) rect.h = _h;
-    
+
     // the fast one
     SDL_UpdateRect(_screen, rect.x, rect.y, rect.w, rect.h);
     // the slow one
     SDL_UpdateRect(_screen, 0, 0, _screen->w, _screen->h);
+
     _bb_to_sdl(_robot.get_bb(), &_prev_bb);
 
   }
diff --git a/display.hpp b/display.hpp
index 657e0d4..a0673e7 100644
--- a/display.hpp
+++ b/display.hpp
@@ -3,18 +3,18 @@
 ** Login : <mouret@asuncion.lip6.fr>
 ** Started on  Mon Jan 14 16:42:14 2008 Jean-Baptiste MOURET
 ** $Id$
-** 
+**
 ** Copyright (C) 2008 Jean-Baptiste MOURET
 ** This program is free software; you can redistribute it and/or modify
 ** it under the terms of the GNU General Public License as published by
 ** the Free Software Foundation; either version 2 of the License, or
 ** (at your option) any later version.
-** 
+**
 ** This program is distributed in the hope that it will be useful,
 ** but WITHOUT ANY WARRANTY; without even the implied warranty of
 ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 ** GNU General Public License for more details.
-** 
+**
 ** You should have received a copy of the GNU General Public License
 ** along with this program; if not, write to the Free Software
 ** Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
@@ -39,95 +39,92 @@
 #include "robot.hpp"
 #include <boost/shared_ptr.hpp>
 
-namespace fastsim
-{
-  class Display
-  {
+namespace fastsim {
+  class Display {
 
-  public:
+   public:
 #ifdef USE_SDL
     Display(const boost::shared_ptr<Map>& m, const Robot& r);
-    ~Display()
-    {
+    ~Display() {
       SDL_FreeSurface(_map_bmp);
       SDL_FreeSurface(_screen);
       SDL_Quit();
-    }    
+    }
     void update();
-    void update_map() 
-    {
+    void update_map() {
       _blit_map();
     }
 #else
     Display(const boost::shared_ptr<Map>& m, const Robot& r) : _map(m), _robot(r) {}
     ~Display() {}
-    void update(){}
-    void update_map(){}
+    void update() {}
+    void update_map() {}
 #endif
-  protected:
+   protected:
     const boost::shared_ptr<Map>& _map;
     const Robot& _robot;
 #ifdef USE_SDL
     void _events();
     void _bb_to_sdl(const Robot::BoundingBox& bb,
-		    SDL_Rect *r)
-    {
+                    SDL_Rect *r) {
       r->x = (Sint16) _map->real_to_pixel(bb.x);
       r->y = (Sint16) _map->real_to_pixel(bb.y);
       r->w = (Sint16) _map->real_to_pixel(bb.w);
       r->h = (Sint16) _map->real_to_pixel(bb.h);
     }
 
-    void _quit()
-    {
+    void _quit() {
       SDL_Quit();
       exit(0);
     }
     void _put_pixel(SDL_Surface* surf,
-		    Uint32 color, unsigned x, unsigned y)
-    {
+                    Uint32 color, unsigned x, unsigned y) {
       if (x >= surf->w || x < 0 || y >= surf->h || y < 0)
-	return;
+        return;
       Uint32 *bufp = (Uint32*)surf->pixels  + y * surf->pitch / 4 + x;
       *bufp = color;
     }
     void _put_pixel(SDL_Surface* surf,
-		    unsigned x, unsigned y,
-		    Uint8 r, Uint8 g, Uint8 b)
-    {  _put_pixel(surf, SDL_MapRGB(surf->format, r, g, b), x, y); }
-    
+                    unsigned x, unsigned y,
+                    Uint8 r, Uint8 g, Uint8 b) {
+      _put_pixel(surf, SDL_MapRGB(surf->format, r, g, b), x, y);
+    }
+
     void _blit_map();
 
     // used by _circle
     void _circle_points(SDL_Surface* surf,
-			int cx, int cy, int x, int y, Uint32 color);
+                        int cx, int cy, int x, int y, Uint32 color);
     void _circle(SDL_Surface* surf,
-		 int x_center, int y_center, int radius,
-		 Uint8 r, Uint8 g, Uint8 b)
-    { _circle(surf, x_center, y_center, radius, SDL_MapRGB(surf->format, r, g, b)); }
+                 int x_center, int y_center, int radius,
+                 Uint8 r, Uint8 g, Uint8 b) {
+      _circle(surf, x_center, y_center, radius, SDL_MapRGB(surf->format, r, g, b));
+    }
     void _circle(SDL_Surface* surf,
-		 int x_center, int y_center, int radius,
-		 Uint32 color);
+                 int x_center, int y_center, int radius,
+                 Uint32 color);
     //
     void _disc(SDL_Surface* surf,
-	       int x_center, int y_center, int radius,
-	       Uint8 r, Uint8 g, Uint8 b)
-    { _disc(surf, x_center, y_center, radius, SDL_MapRGB(surf->format, r, g, b)); }
+               int x_center, int y_center, int radius,
+               Uint8 r, Uint8 g, Uint8 b) {
+      _disc(surf, x_center, y_center, radius, SDL_MapRGB(surf->format, r, g, b));
+    }
     void _disc(SDL_Surface* surf,
-	       int x_center, int y_center, int radius,
-	       Uint32 color);
+               int x_center, int y_center, int radius,
+               Uint32 color);
     //
-    void _line(SDL_Surface* surf, int x0, int y0, int x1, int y1, 
-	       Uint8 r, Uint8 g, Uint8 b)
-    { _line(surf, x0, y0, x1, y1, SDL_MapRGB(surf->format, r, g, b)); }
-    void _line(SDL_Surface* surf, int x0, int y0, int x1, int y1, 
-	       Uint32 color);
-  
+    void _line(SDL_Surface* surf, int x0, int y0, int x1, int y1,
+               Uint8 r, Uint8 g, Uint8 b) {
+      _line(surf, x0, y0, x1, y1, SDL_MapRGB(surf->format, r, g, b));
+    }
+    void _line(SDL_Surface* surf, int x0, int y0, int x1, int y1,
+               Uint32 color);
+
     void _try_pixel(bool& res,
-		    SDL_Surface* surf,
-		    Uint32 color,
-		    int x, int y);
-    
+                    SDL_Surface* surf,
+                    Uint32 color,
+                    int x, int y);
+
     //
     Uint32 _color_from_id(SDL_Surface* surf, int id);
     // disp sensor values
@@ -139,7 +136,7 @@ namespace fastsim
     void _disp_lasers();
     void _disp_light_sensors();
     void _disp_camera();
-    // 
+    //
     int _w, _h;
     SDL_Surface* _screen, *_map_bmp;
     SDL_Rect _prev_bb;
diff --git a/fastsim_keyboard.cpp b/fastsim_keyboard.cpp
index ccf69cd..db8b551 100644
--- a/fastsim_keyboard.cpp
+++ b/fastsim_keyboard.cpp
@@ -2,18 +2,16 @@
 #include "simu_fastsim.hpp"
 
 using namespace sferes;
-struct Params
-{
-  struct simu
-  {
+struct Params {
+  struct simu {
     SFERES_STRING(map_name, "modules/fastsim/cuisine.pbm");
+    SFERES_CONST float dt = 0.01;
   };
 };
 
 
 
-int main(int argc, char *argv[])
-{
+int main(int argc, char *argv[]) {
   using namespace fastsim;
   typedef simu::Fastsim<Params> simu_t;
   simu_t s;
@@ -37,21 +35,20 @@ int main(int argc, char *argv[])
   s.init_view();
 
   int numkey;
-  while(true)
-    {
-      SDL_PumpEvents();
-      Uint8* keys = SDL_GetKeyState(&numkey);
-      if (keys[SDLK_UP])
-	s.move_robot(1.0, 1.0);
-      if (keys[SDLK_DOWN])
-	s.move_robot(-1.0, -1.0);
-      if (keys[SDLK_LEFT])
-       	s.move_robot(1.0, -1.0);
-      if (keys[SDLK_RIGHT])
-       	s.move_robot(-1.0, 1.0);
-      s.refresh();
-      s.refresh_view();
-    }
+  while(true) {
+    SDL_PumpEvents();
+    Uint8* keys = SDL_GetKeyState(&numkey);
+    if (keys[SDLK_UP])
+      s.move_robot(1.0, 1.0);
+    if (keys[SDLK_DOWN])
+      s.move_robot(-1.0, -1.0);
+    if (keys[SDLK_LEFT])
+      s.move_robot(1.0, -1.0);
+    if (keys[SDLK_RIGHT])
+      s.move_robot(-1.0, 1.0);
+    s.refresh();
+    s.refresh_view();
+  }
 
   return 0;
 }
diff --git a/goal.hpp b/goal.hpp
index f3fc253..d070d49 100644
--- a/goal.hpp
+++ b/goal.hpp
@@ -1,21 +1,27 @@
 #ifndef FASTSIM_GOAL_HPP_
 #define FASTSIM_GOAL_HPP_
 
-namespace fastsim
-{
-  class Goal
-  { 
-  public:
+namespace fastsim {
+  class Goal {
+   public:
     Goal(float x, float y, float diam, int color) :
       _x(x), _y(y), _diam(diam), _color(color) {}
-    float get_x() const { return _x; }
-    float get_y() const { return _y; }
-    float get_diam() const { return _diam; }
-    int   get_color() const { return _color; }
-  private:
+    float get_x() const {
+      return _x;
+    }
+    float get_y() const {
+      return _y;
+    }
+    float get_diam() const {
+      return _diam;
+    }
+    int   get_color() const {
+      return _color;
+    }
+   private:
     float _x, _y;
     float _diam;
-    int _color; 
+    int _color;
   };
 }
 
diff --git a/illuminated_switch.hpp b/illuminated_switch.hpp
index c53bb00..a8d3690 100644
--- a/illuminated_switch.hpp
+++ b/illuminated_switch.hpp
@@ -4,65 +4,80 @@
 #include <vector>
 #include "posture.hpp"
 
-namespace fastsim
-{
-   // an illimuninated light switch
-  class IlluminatedSwitch
-  {
-    public:
-       // in WORLD coordinate (NOT pixel)
-      IlluminatedSwitch(int color, float radius, float x, float y, bool on) :
-        _color(color),
-        _radius(radius),
-        _x(x), _y(y),
-        _on(on), _activated(false)
-      {
-      }
+namespace fastsim {
+  // an illimuninated light switch
+  class IlluminatedSwitch {
+   public:
+    // in WORLD coordinate (NOT pixel)
+    IlluminatedSwitch(int color, float radius, float x, float y, bool on) :
+      _color(color),
+      _radius(radius),
+      _x(x), _y(y),
+      _on(on), _activated(false) {
+    }
 
-      void try_to_activate(const Posture& robot_pos)
-      {
-        float d = robot_pos.dist_to(_x, _y);
-        if (_on && d < _radius)
-          activate();
-      }
-      void activate()
-      {
-        _activated = true;
-        for (size_t i = 0; i < _linked_lights.size(); ++i)
-          _linked_lights[i]->set_on(true);
-      }
-      void deactivate()
-      {
-        _activated = false;
-        for (size_t i = 0; i < _linked_lights.size(); ++i)
-          _linked_lights[i]->set_on(false);
-      }
-      void set_on(bool x = true) { _on = x; }
-      void set_off() { set_on(false); }
-      bool get_on() const { return _on;  }
-      bool get_off() const { return !_on; }
-      int  get_color() const { return _color; }
-      float get_radius() const { return _radius; }
-      float get_x() const { return _x; }
-      float get_y() const { return _y; }
-      void set_pos(float x, float y) { _x = x; _y = y; }
-      bool get_activated() const { return _activated; }
-      void link(boost::shared_ptr<IlluminatedSwitch> o) { _linked_lights.push_back(o); }
-    protected:
-      int _color;
-      float _radius;
-      float _x, _y;
-      bool _on;
-      bool _activated;
-      std::vector<boost::shared_ptr<IlluminatedSwitch> > _linked_lights;
+    void try_to_activate(const Posture& robot_pos) {
+      float d = robot_pos.dist_to(_x, _y);
+      if (_on && d < _radius)
+        activate();
+    }
+    void activate() {
+      _activated = true;
+      for (size_t i = 0; i < _linked_lights.size(); ++i)
+        _linked_lights[i]->set_on(true);
+    }
+    void deactivate() {
+      _activated = false;
+      for (size_t i = 0; i < _linked_lights.size(); ++i)
+        _linked_lights[i]->set_on(false);
+    }
+    void set_on(bool x = true) {
+      _on = x;
+    }
+    void set_off() {
+      set_on(false);
+    }
+    bool get_on() const {
+      return _on;
+    }
+    bool get_off() const {
+      return !_on;
+    }
+    int  get_color() const {
+      return _color;
+    }
+    float get_radius() const {
+      return _radius;
+    }
+    float get_x() const {
+      return _x;
+    }
+    float get_y() const {
+      return _y;
+    }
+    void set_pos(float x, float y) {
+      _x = x;
+      _y = y;
+    }
+    bool get_activated() const {
+      return _activated;
+    }
+    void link(boost::shared_ptr<IlluminatedSwitch> o) {
+      _linked_lights.push_back(o);
+    }
+   protected:
+    int _color;
+    float _radius;
+    float _x, _y;
+    bool _on;
+    bool _activated;
+    std::vector<boost::shared_ptr<IlluminatedSwitch> > _linked_lights;
   };
 
-  struct ClosestSwitch_f
-  {
+  struct ClosestSwitch_f {
     ClosestSwitch_f(float x, float y) : _x(x), _y(y) {}
     bool operator()(const boost::shared_ptr<IlluminatedSwitch> i1,
-		    const boost::shared_ptr<IlluminatedSwitch> i2)
-    {
+                    const boost::shared_ptr<IlluminatedSwitch> i2) {
       float x1 = i1->get_x() - _x;
       float y1 = i1->get_y() - _y;
       float x2 = i2->get_x() - _x;
diff --git a/laser.cpp b/laser.cpp
index a6e41ea..5230181 100644
--- a/laser.cpp
+++ b/laser.cpp
@@ -3,18 +3,18 @@
 ** Login : <mandor@ithaqua>
 ** Started on  Wed Jun  3 00:41:29 2009 mandor
 ** $Id$
-** 
+**
 ** Copyright (C) 2009 mandor
 ** This program is free software; you can redistribute it and/or modify
 ** it under the terms of the GNU General Public License as published by
 ** the Free Software Foundation; either version 2 of the License, or
 ** (at your option) any later version.
-** 
+**
 ** This program is distributed in the hope that it will be useful,
 ** but WITHOUT ANY WARRANTY; without even the implied warranty of
 ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 ** GNU General Public License for more details.
-** 
+**
 ** You should have received a copy of the GNU General Public License
 ** along with this program; if not, write to the Free Software
 ** Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
@@ -23,15 +23,13 @@
 #include <iostream>
 #include "laser.hpp"
 
-namespace fastsim
-{
+namespace fastsim {
   float Laser :: update(const Posture& pos,
-			const boost::shared_ptr<Map>& m)
-  {
+                        const boost::shared_ptr<Map>& m) {
     float x2 = cosf(_angle + pos.theta()) * _range + pos.x() + _gap_dist * cosf(_gap_angle + pos.theta());
     float y2 = sinf(_angle + pos.theta()) * _range + pos.y() + _gap_dist * sinf(_gap_angle + pos.theta());
 
-    
+
 
     // convert to pixel
     int xx1 = m->real_to_pixel(pos.x() + _gap_dist * cosf(_gap_angle + pos.theta()));
@@ -46,7 +44,7 @@ namespace fastsim
     bool inter = m->check_inter_pixel(xx1, yy1, xx2, yy2, _x_pixel, _y_pixel);
     // _x_pixel = std::min(std::max(0, _x_pixel), (int)m->get_pixel_w());
     // _y_pixel = std::min(std::max(0, _y_pixel), (int)m->get_pixel_h())
-      ;
+    ;
     assert(_x_pixel >= 0);
     assert(_y_pixel >= 0);
 
@@ -54,16 +52,15 @@ namespace fastsim
     _x = m->pixel_to_real(_x_pixel);
     _y = m->pixel_to_real(_y_pixel);
 
-    // 
+    //
     if (!inter)
       _dist = -1;
-    else
-      {
-	float px = pos.x() + _gap_dist * cosf(_gap_angle + pos.theta()) - _x;
-	float py = pos.y() + _gap_dist * sinf(_gap_angle + pos.theta()) - _y;    
-	_dist = sqrtf(px * px  + py * py);
-      }
+    else {
+      float px = pos.x() + _gap_dist * cosf(_gap_angle + pos.theta()) - _x;
+      float py = pos.y() + _gap_dist * sinf(_gap_angle + pos.theta()) - _y;
+      _dist = sqrtf(px * px  + py * py);
+    }
     return _dist;
   }
-  
+
 }
diff --git a/laser.hpp b/laser.hpp
index 91115b5..5b085b1 100644
--- a/laser.hpp
+++ b/laser.hpp
@@ -3,18 +3,18 @@
 ** Login : <mandor@ithaqua>
 ** Started on  Wed Jun  3 00:39:20 2009 mandor
 ** $Id$
-** 
+**
 ** Copyright (C) 2009 mandor
 ** This program is free software; you can redistribute it and/or modify
 ** it under the terms of the GNU General Public License as published by
 ** the Free Software Foundation; either version 2 of the License, or
 ** (at your option) any later version.
-** 
+**
 ** This program is distributed in the hope that it will be useful,
 ** but WITHOUT ANY WARRANTY; without even the implied warranty of
 ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 ** GNU General Public License for more details.
-** 
+**
 ** You should have received a copy of the GNU General Public License
 ** along with this program; if not, write to the Free Software
 ** Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
@@ -26,44 +26,60 @@
 #include "posture.hpp"
 #include <boost/shared_ptr.hpp>
 
-namespace fastsim
-{
-  class Laser
-  {
-  public:
+namespace fastsim {
+  class Laser {
+   public:
     Laser(float angle, float range, float gap_dist = 0.0f, float gap_angle = 0.0f) :
       _angle(angle), _range(range),
       _gap_dist(gap_dist), _gap_angle(gap_angle),
-      _x(0), _y(0), 
+      _x(0), _y(0),
       _x_pixel(0), _y_pixel(0),
       _dist(-1) {}
     float update(const Posture& pos,
-		 const boost::shared_ptr<Map>& map);
-    float get_dist() const { return _dist; }
-    float get_angle() const { return _angle; }
-    float get_range() const { return _range; }
-    float get_gap_dist() const { return _gap_dist; }
-    float get_gap_angle() const { return _gap_angle; }
+                 const boost::shared_ptr<Map>& map);
+    float get_dist() const {
+      return _dist;
+    }
+    float get_angle() const {
+      return _angle;
+    }
+    float get_range() const {
+      return _range;
+    }
+    float get_gap_dist() const {
+      return _gap_dist;
+    }
+    float get_gap_angle() const {
+      return _gap_angle;
+    }
     // intersection point in world coordinates
-    float get_x() const { return _x; }
-    float get_y() const { return _y; }
+    float get_x() const {
+      return _x;
+    }
+    float get_y() const {
+      return _y;
+    }
     // intersection point in pixel coordinates
-    int get_x_pixel() const { return _x_pixel; }
-    int get_y_pixel() const { return _y_pixel; }
-  protected:
+    int get_x_pixel() const {
+      return _x_pixel;
+    }
+    int get_y_pixel() const {
+      return _y_pixel;
+    }
+   protected:
     float _angle;
     float _range;
     float _gap_dist, _gap_angle;//polar coordinates of the laser (reference = robot)
     float _x, _y;
     int _x_pixel, _y_pixel;
     float _dist;
-    // 
+    //
     bool _try_pixel(const boost::shared_ptr<Map>& m, int x, int y);
     bool _line_inter(const boost::shared_ptr<Map>& m,
-		     int y1, int x1, // src
-		     int y2, int x2, // dest
-		     int& x_res, int& y_res //res
-		     );
+                     int y1, int x1, // src
+                     int y2, int x2, // dest
+                     int& x_res, int& y_res //res
+                    );
 
   };
 }
diff --git a/light_sensor.cpp b/light_sensor.cpp
index 06a0cf1..d4641d4 100644
--- a/light_sensor.cpp
+++ b/light_sensor.cpp
@@ -2,31 +2,27 @@
 #include "light_sensor.hpp"
 
 
-namespace fastsim
-{
+namespace fastsim {
   int LightSensor :: update(const Posture& pos,
-			    const boost::shared_ptr<Map>& map)
-  {
+                            const boost::shared_ptr<Map>& map) {
     const std::vector<Map::ill_sw_t>& isv = map->get_illuminated_switches();
     _activated = false;
     for (size_t i = 0; i < isv.size(); ++i)
-      if (isv[i]->get_color() == get_color() && isv[i]->get_on())
-	{
-	  float angle = 
-	    normalize_angle(atan2(isv[i]->get_y() - pos.y(), 
-				  isv[i]->get_x() - pos.x())
-			    - pos.theta());
-	  float x_res = 0, y_res = 0;// ignored
-	  if (angle > normalize_angle(_angle - _range / 2.0f)
-	      && angle < normalize_angle(_angle + _range / 2.0f)
-	      && !map->check_inter_real(isv[i]->get_x(), isv[i]->get_y(),
-					pos.x(), pos.y(),
-					x_res, y_res))
-	    {
-	      _activated = true;
-	      _num=i;
-	    }	  
-	}
+      if (isv[i]->get_color() == get_color() && isv[i]->get_on()) {
+        float angle =
+          normalize_angle(atan2(isv[i]->get_y() - pos.y(),
+                                isv[i]->get_x() - pos.x())
+                          - pos.theta());
+        float x_res = 0, y_res = 0;// ignored
+        if (angle > normalize_angle(_angle - _range / 2.0f)
+            && angle < normalize_angle(_angle + _range / 2.0f)
+            && !map->check_inter_real(isv[i]->get_x(), isv[i]->get_y(),
+                                      pos.x(), pos.y(),
+                                      x_res, y_res)) {
+          _activated = true;
+          _num=i;
+        }
+      }
     return _activated;
   }
 }
diff --git a/light_sensor.hpp b/light_sensor.hpp
index 31f55e8..6534006 100644
--- a/light_sensor.hpp
+++ b/light_sensor.hpp
@@ -5,26 +5,34 @@
 #include "posture.hpp"
 #include "map.hpp"
 
-namespace fastsim
-{
+namespace fastsim {
   /// detect an colored illuminated switch in the given direction
   /// (angle) and within the angular range. There is no range limit.
   /// light sensors DONT'T SEE TROUGH OBSTACLES
-  class LightSensor
-  {
-  public:
+  class LightSensor {
+   public:
     LightSensor(int color, float angle, float range) :
       _color(color), _angle(angle), _range(range),
-      _activated(false),_num(0)
-    {}
+      _activated(false),_num(0) {
+    }
     int update(const Posture& pos,
-	       const boost::shared_ptr<Map>& map);
-    int	get_color() const { return _color; }
-    float get_angle() const { return _angle; }
-    float get_range() const { return _range; }
-    bool get_activated() const { return _activated; }
-	unsigned int get_num() const {return _num;}
-  protected:
+               const boost::shared_ptr<Map>& map);
+    int	get_color() const {
+      return _color;
+    }
+    float get_angle() const {
+      return _angle;
+    }
+    float get_range() const {
+      return _range;
+    }
+    bool get_activated() const {
+      return _activated;
+    }
+    unsigned int get_num() const {
+      return _num;
+    }
+   protected:
     // the "color" (i.e. light identifier) detected
     int _color;
     //
@@ -33,8 +41,8 @@ namespace fastsim
     float _range;
     //
     bool _activated;
-	// sensor number (for display only)
-	unsigned int _num;
+    // sensor number (for display only)
+    unsigned int _num;
   };
 }
 #endif
diff --git a/linear_camera.cpp b/linear_camera.cpp
index d1dbf4d..bbc1744 100644
--- a/linear_camera.cpp
+++ b/linear_camera.cpp
@@ -2,21 +2,18 @@
 #include "linear_camera.hpp"
 
 
-namespace fastsim
-{
-  
+namespace fastsim {
+
   void LinearCamera :: update(const Posture& pos,
-                             const boost::shared_ptr<Map>& map)
-  {
+                              const boost::shared_ptr<Map>& map) {
     float inc = _angular_range / _pixels.size();
     float r = -_angular_range / 2.0f;
-    for (size_t i = 0; i < _pixels.size(); ++i, r += inc)
-      {
-	float alpha = r + pos.theta();
-	float xr = cos(alpha) * 10000 + pos.x();//range = 10000
-	float yr = sin(alpha) * 10000 + pos.x();
-	assert(i < _pixels.size());
-	_pixels[i] = map->check_inter_is(pos.x(), pos.y(), xr, yr);
-      }
+    for (size_t i = 0; i < _pixels.size(); ++i, r += inc) {
+      float alpha = r + pos.theta();
+      float xr = cos(alpha) * 10000 + pos.x();//range = 10000
+      float yr = sin(alpha) * 10000 + pos.x();
+      assert(i < _pixels.size());
+      _pixels[i] = map->check_inter_is(pos.x(), pos.y(), xr, yr);
+    }
   }
 }
diff --git a/linear_camera.hpp b/linear_camera.hpp
index f98bad7..3294e55 100644
--- a/linear_camera.hpp
+++ b/linear_camera.hpp
@@ -5,22 +5,26 @@
 #include "posture.hpp"
 #include "map.hpp"
 
-namespace fastsim
-{
-  class LinearCamera
-  {
-    public:
-    LinearCamera() : _angular_range(M_PI / 2), _pixels(12)
-    { std::fill(_pixels.begin(), _pixels.end(), -1); }
+namespace fastsim {
+  class LinearCamera {
+   public:
+    LinearCamera() : _angular_range(M_PI / 2), _pixels(12) {
+      std::fill(_pixels.begin(), _pixels.end(), -1);
+    }
     LinearCamera(float angular_range, int nb_pixels) :
-      _angular_range(angular_range), _pixels(nb_pixels)
-    { std::fill(_pixels.begin(), _pixels.end(), -1); }
-   
+      _angular_range(angular_range), _pixels(nb_pixels) {
+      std::fill(_pixels.begin(), _pixels.end(), -1);
+    }
+
     void update(const Posture& pos,
-	       const boost::shared_ptr<Map>& map);
-    const std::vector<int>& pixels() const { return _pixels; }
-    float get_angular_range() const { return _angular_range; }
-  protected:
+                const boost::shared_ptr<Map>& map);
+    const std::vector<int>& pixels() const {
+      return _pixels;
+    }
+    float get_angular_range() const {
+      return _angular_range;
+    }
+   protected:
     float _angular_range;
     std::vector<int> _pixels;
   };
diff --git a/main.cpp b/main.cpp
index 046715f..331ab8e 100644
--- a/main.cpp
+++ b/main.cpp
@@ -3,18 +3,18 @@
 ** Login : <mouret@asuncion.lip6.fr>
 ** Started on  Mon Jan 14 16:44:52 2008 Jean-Baptiste MOURET
 ** $Id$
-** 
+**
 ** Copyright (C) 2008 Jean-Baptiste MOURET
 ** This program is free software; you can redistribute it and/or modify
 ** it under the terms of the GNU General Public License as published by
 ** the Free Software Foundation; either version 2 of the License, or
 ** (at your option) any later version.
-** 
+**
 ** This program is distributed in the hope that it will be useful,
 ** but WITHOUT ANY WARRANTY; without even the implied warranty of
 ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 ** GNU General Public License for more details.
-** 
+**
 ** You should have received a copy of the GNU General Public License
 ** along with this program; if not, write to the Free Software
 ** Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
@@ -23,32 +23,27 @@
 #include <iostream>
 #include "fastsim.hpp"
 
-int main()
-{
-  try
-    {
-      using namespace fastsim;
-      boost::shared_ptr<Map> m = 
-	boost::shared_ptr<Map>(new Map("envs/maze_hard.pbm", 600));
-      m->add_goal(Goal(100, 100, 10, 0));
-      Robot r(20.0f, Posture(60, 600-60, 0));
-      r.add_laser(Laser(M_PI / 4.0, 100.0f));
-      r.add_laser(Laser(-M_PI / 4.0, 100.0f));
-      r.add_laser(Laser(0.0f, 100.0f));
-      r.add_radar(Radar(0, 4));
-      Display d(m, r);
+int main() {
+  try {
+    using namespace fastsim;
+    boost::shared_ptr<Map> m =
+      boost::shared_ptr<Map>(new Map("envs/maze_hard.pbm", 600));
+    m->add_goal(Goal(100, 100, 10, 0));
+    Robot r(20.0f, Posture(60, 600-60, 0));
+    r.add_laser(Laser(M_PI / 4.0, 100.0f));
+    r.add_laser(Laser(-M_PI / 4.0, 100.0f));
+    r.add_laser(Laser(0.0f, 100.0f));
+    r.add_radar(Radar(0, 4));
+    Display d(m, r);
 
-      float x = 30;
-      for (int i = 0; i < 10000; ++i)
-	{
-	  d.update();
-	  r.move(1.0, 1.1, m);
-	}
-    }
-  catch (fastsim::Exception e)
-    {
-      std::cerr<<"error : "<<e.get_msg()<<std::endl;
+    float x = 30;
+    for (int i = 0; i < 10000; ++i) {
+      d.update();
+      r.move(1.0, 1.1, m);
     }
+  } catch (fastsim::Exception e) {
+    std::cerr<<"error : "<<e.get_msg()<<std::endl;
+  }
   return 0;
 }
 
diff --git a/map.cpp b/map.cpp
index ae3a09e..ad5d397 100644
--- a/map.cpp
+++ b/map.cpp
@@ -3,18 +3,18 @@
 ** Login : <mouret@asuncion.lip6.fr>
 ** Started on  Mon Jan 14 16:39:08 2008 Jean-Baptiste MOURET
 ** $Id$
-** 
+**
 ** Copyright (C) 2008 Jean-Baptiste MOURET
 ** This program is free software; you can redistribute it and/or modify
 ** it under the terms of the GNU General Public License as published by
 ** the Free Software Foundation; either version 2 of the License, or
 ** (at your option) any later version.
-** 
+**
 ** This program is distributed in the hope that it will be useful,
 ** but WITHOUT ANY WARRANTY; without even the implied warranty of
 ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 ** GNU General Public License for more details.
-** 
+**
 ** You should have received a copy of the GNU General Public License
 ** along with this program; if not, write to the Free Software
 ** Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
@@ -23,10 +23,8 @@
 #include <iostream>
 #include "map.hpp"
 
-namespace fastsim
-{
-  void Map::_read_file(const std::string& fname)
-  {
+namespace fastsim {
+  void Map::_read_file(const std::string& fname) {
     std::string str;
     std::ifstream ifs(fname.c_str());
     if (!ifs.good())
@@ -43,18 +41,17 @@ namespace fastsim
     ifs.read((char*)buffer, k);
     for (int i = 0; i < k - 1; ++i)
       for (int j = 0; j < 8; ++j)
-	_data[i * 8 + j] = _get_bit(buffer[i + 1], j) ? obstacle : free;
+        _data[i * 8 + j] = _get_bit(buffer[i + 1], j) ? obstacle : free;
   }
-  
+
   // we use the "triangle method"
-  // concept : 
+  // concept :
   // * area = base * height / 2
   // * area = cross product
   // -> if height < radius, intersection
   bool Map::_check_inter_ray_circle(float x1, float y1,
-				    float x2, float y2,
-				    float xr, float yr, float radius) const
-  {
+                                    float x2, float y2,
+                                    float xr, float yr, float radius) const {
     // check if the object is on the right side of the camera
     float dot = (x2 - x1) * (xr - x1) + (y2 - y1) * (yr - y1);
     if (dot < 0)
@@ -68,51 +65,47 @@ namespace fastsim
       return true;
     return false;
   }
-  
+
   int Map::check_inter_is(float x1, float y1,
-			  float x2, float y2) const 
-  {
+                          float x2, float y2) const {
     // list intersections with rays
     std::vector<ill_sw_t> res;
-    for (size_t i = 0; i < _illuminated_switches.size(); ++i)
-      {
-	ill_sw_t isv = _illuminated_switches[i];
-	float xtmp, ytmp;
-	if (isv->get_on()
-	    && _check_inter_ray_circle(x1, y1, x2, y2,
-				      isv->get_x(), isv->get_y(),
-				      isv->get_radius())
-	    && !check_inter_real(x1, y1, isv->get_x(), isv->get_y(), xtmp, ytmp))
-	  res.push_back(isv);	    
-      }
+    for (size_t i = 0; i < _illuminated_switches.size(); ++i) {
+      ill_sw_t isv = _illuminated_switches[i];
+      float xtmp, ytmp;
+      if (isv->get_on()
+          && _check_inter_ray_circle(x1, y1, x2, y2,
+                                     isv->get_x(), isv->get_y(),
+                                     isv->get_radius())
+          && !check_inter_real(x1, y1, isv->get_x(), isv->get_y(), xtmp, ytmp))
+        res.push_back(isv);
+    }
     if (res.empty())
       return -1;
     // return the closest
     std::sort(res.begin(), res.end(), ClosestSwitch_f(x1, y1));
     return res[0]->get_color();
   }
-  
-  
-  bool Map :: _try_pixel(int x, int y) const
-  {
-    if (x >= 0 && y >= 0 
-	&& x < get_pixel_w() 
-	&& y < get_pixel_h() 
-	&& get_pixel(x, y) == free)
+
+
+  bool Map :: _try_pixel(int x, int y) const {
+    if (x >= 0 && y >= 0
+        && x < get_pixel_w()
+        && y < get_pixel_h()
+        && get_pixel(x, y) == free)
       return false;
     else
       return true;
   }
 
 
-  
+
   // see
   // http://lifc.univ-fcomte.fr/~dedu/projects/bresenham/index.html
   // In PIXEL coordinates
   bool Map :: check_inter_pixel(int y1, int x1,
-				int y2, int x2,
-				int& y_res, int& x_res) const
-  {
+                                int y2, int x2,
+                                int& y_res, int& x_res) const {
     int i;               // loop counter
     int ystep, xstep;    // the step on y and x axis
     int error;           // the error accumulated during the increment
@@ -122,84 +115,82 @@ namespace fastsim
     int dx = x2 - x1;
     int dy = y2 - y1;
     bool inter = _try_pixel(y1, x1);
-    if (dy < 0) { ystep = -1; dy = -dy; } else ystep = 1;
-    if (dx < 0) { xstep = -1; dx = -dx; } else xstep = 1;
+    if (dy < 0) {
+      ystep = -1;
+      dy = -dy;
+    } else ystep = 1;
+    if (dx < 0) {
+      xstep = -1;
+      dx = -dx;
+    } else xstep = 1;
     ddy = dy * 2;
     ddx = dx * 2;
-    if (ddx >= ddy) // first octant (0 <= slope <= 1)
-      {  
-	errorprev = error = dx;
-	for (i = 0 ; i < dx ; i++)
-	  {  // do not use the first point (already done)
-	    x += xstep;
-	    error += ddy;
-	    if (error > ddx)
-	      {
-		y += ystep;
-		error -= ddx;
-		if (error + errorprev < ddx)  // bottom square also
-		  inter = inter || _try_pixel(y - ystep, x);
-		else if (error + errorprev > ddx)  // left square also
-		  inter = inter || _try_pixel(y, x - xstep);
-		else // corner: bottom and left squares also
-		  {
-		    inter = inter || _try_pixel(y - ystep, x);
-		    inter = inter || _try_pixel(y, x - xstep);
-		  }
-	      }
-	    inter = inter || _try_pixel(y, x);
-	    errorprev = error;
-	    if (inter)
-	      {
-		x_res = x;
-		y_res = y;
-		return true;
-	      }
-	  }
+    if (ddx >= ddy) { // first octant (0 <= slope <= 1)
+      errorprev = error = dx;
+      for (i = 0 ; i < dx ; i++) {
+        // do not use the first point (already done)
+        x += xstep;
+        error += ddy;
+        if (error > ddx) {
+          y += ystep;
+          error -= ddx;
+          if (error + errorprev < ddx)  // bottom square also
+            inter = inter || _try_pixel(y - ystep, x);
+          else if (error + errorprev > ddx)  // left square also
+            inter = inter || _try_pixel(y, x - xstep);
+          else { // corner: bottom and left squares also
+            inter = inter || _try_pixel(y - ystep, x);
+            inter = inter || _try_pixel(y, x - xstep);
+          }
+        }
+        inter = inter || _try_pixel(y, x);
+        errorprev = error;
+        if (inter) {
+          x_res = x;
+          y_res = y;
+          return true;
+        }
       }
-    else
-      {  // the same as above
-	errorprev = error = dy;
-	for (i = 0 ; i < dy ; i++)
-	  {
-	    y += ystep;
-	    error += ddx;
-	    if (error > ddy){
-	      x += xstep;
-	      error -= ddy;
-	      if (error + errorprev < ddy)
-		inter = inter || _try_pixel(y, x - xstep);
-	      else if (error + errorprev > ddy)
-		inter = inter || _try_pixel(y - ystep, x);
-	      else
-		{
-		  inter = inter || _try_pixel(y, x - xstep);
-		  inter = inter || _try_pixel(y -  ystep, x);
-		}
-	    }
-	    inter = inter || _try_pixel(y, x);
-	    errorprev = error;
-	    if (inter)
-	      {
-		x_res = x;
-		y_res = y;
-		return true;
-	      }
-	  }
+    } else {
+      // the same as above
+      errorprev = error = dy;
+      for (i = 0 ; i < dy ; i++) {
+        y += ystep;
+        error += ddx;
+        if (error > ddy) {
+          x += xstep;
+          error -= ddy;
+          if (error + errorprev < ddy)
+            inter = inter || _try_pixel(y, x - xstep);
+          else if (error + errorprev > ddy)
+            inter = inter || _try_pixel(y - ystep, x);
+          else {
+            inter = inter || _try_pixel(y, x - xstep);
+            inter = inter || _try_pixel(y -  ystep, x);
+          }
+        }
+        inter = inter || _try_pixel(y, x);
+        errorprev = error;
+        if (inter) {
+          x_res = x;
+          y_res = y;
+          return true;
+        }
       }
+    }
     return false;
   }
 
-    // Draws a rectangle with (x,y) the upper left point and (lx,ly) the size
+  // Draws a rectangle with (x,y) the upper left point and (lx,ly) the size
   void Map:: draw_rect(int x, int y, int lx, int ly) {
     int i,j;
 
-    for (i=0;i<lx;i++)
-      for (j=0;j<ly;j++) {
-	if ((x+i) >= 0 && (y+j) >= 0 
-	    && (x+i) < get_pixel_w() 
-	    && (y+j) < get_pixel_h())
-	  set_pixel(x+i,y+j,obstacle);
+    for (i=0; i<lx; i++)
+      for (j=0; j<ly; j++) {
+        if ((x+i) >= 0 && (y+j) >= 0
+            && (x+i) < get_pixel_w()
+            && (y+j) < get_pixel_h())
+          set_pixel(x+i,y+j,obstacle);
       }
   }
 
diff --git a/map.hpp b/map.hpp
index 36b52f6..c0e9fe2 100644
--- a/map.hpp
+++ b/map.hpp
@@ -3,18 +3,18 @@
 ** Login : <mouret@asuncion.lip6.fr>
 ** Started on  Mon Jan 14 16:38:00 2008 Jean-Baptiste MOURET
 ** $Id$
-** 
+**
 ** Copyright (C) 2008 Jean-Baptiste MOURET
 ** This program is free software; you can redistribute it and/or modify
 ** it under the terms of the GNU General Public License as published by
 ** the Free Software Foundation; either version 2 of the License, or
 ** (at your option) any later version.
-** 
+**
 ** This program is distributed in the hope that it will be useful,
 ** but WITHOUT ANY WARRANTY; without even the implied warranty of
 ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 ** GNU General Public License for more details.
-** 
+**
 ** You should have received a copy of the GNU General Public License
 ** along with this program; if not, write to the Free Software
 ** Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
@@ -34,139 +34,149 @@
 #include "goal.hpp"
 #include "illuminated_switch.hpp"
 
-namespace fastsim
-{
+namespace fastsim {
 
   // 0, 0 is top left
   // map must be a square
-  class Map
-  {
-  public:
+  class Map {
+   public:
     enum status_t { free = 0, obstacle = 255 };
     Map(const char* fname, float real_w) :
       _real_w(real_w),
-      _real_h(real_w)
-    {
-      try
-	{
-	  _read_file(fname); 
-	}
-      catch (Exception e)
-	{
-	  std::cerr<<"error : "<<e.get_msg()<<std::endl;
-	}
+      _real_h(real_w) {
+      try {
+        _read_file(fname);
+      } catch (Exception e) {
+        std::cerr<<"error : "<<e.get_msg()<<std::endl;
+      }
       assert(_w == _h);
       _fx = _w / _real_w;
     }
-    // copy ONLY the picture (no goal, illuminated switches, etc) 
+    // copy ONLY the picture (no goal, illuminated switches, etc)
     // REASON:
     // we want to avoid reading the data data but we don't want to
     // require good copy constructors for all objects
-    Map(const Map& o) : 
+    Map(const Map& o) :
       _data(o._data),
       _w(o._w),
       _h(o._h),
       _real_w(o._real_w),
       _real_h(o._real_h),
-      _fx(o._fx)
-    {
+      _fx(o._fx) {
       // check that we are copying an empty map
       assert(_goals.size() == 0);
       assert(_illuminated_switches.size() == 0);
     }
-    status_t get_pixel(unsigned x, unsigned y) const 
-    {
-      return (y * _h + x >= 0 && y * _h + x < _data.size()) ? 
-	_data[y * _h + x]
-	: _data[_data.size()-1]; 
-    }
-    void set_pixel(unsigned x, unsigned y, status_t v) 
-    {
-      if (y * _h + x >= 0 && y * _h + x < _data.size()) 
-	_data[y * _h + x] = v;
-	
-    }
-    status_t get_real(float x, float y) const { return get_pixel(real_to_pixel(x), real_to_pixel(y)); }
-    int real_to_pixel(float x) const { return (unsigned) roundf(x * _fx); }
-    float pixel_to_real(unsigned i) const { return i / _fx; }
-    float get_real_w() const { return _real_w;}
-    float get_real_h() const { return _real_h;}
-    unsigned get_pixel_w() const { return _w;}
-    unsigned get_pixel_h() const { return _h;}
+    status_t get_pixel(unsigned x, unsigned y) const {
+      return (y * _h + x >= 0 && y * _h + x < _data.size()) ?
+             _data[y * _h + x]
+             : _data[_data.size()-1];
+    }
+    void set_pixel(unsigned x, unsigned y, status_t v) {
+      if (y * _h + x >= 0 && y * _h + x < _data.size())
+        _data[y * _h + x] = v;
+
+    }
+    status_t get_real(float x, float y) const {
+      return get_pixel(real_to_pixel(x), real_to_pixel(y));
+    }
+    int real_to_pixel(float x) const {
+      return (unsigned) roundf(x * _fx);
+    }
+    float pixel_to_real(unsigned i) const {
+      return i / _fx;
+    }
+    float get_real_w() const {
+      return _real_w;
+    }
+    float get_real_h() const {
+      return _real_h;
+    }
+    unsigned get_pixel_w() const {
+      return _w;
+    }
+    unsigned get_pixel_h() const {
+      return _h;
+    }
     // see
     // http://lifc.univ-fcomte.fr/~dedu/projects/bresenham/index.html
     // in PIXEL coordinates
     bool check_inter_pixel(int x1, int y1,
-			   int x2, int y2,
-			   int& x_res, int& y_res) const;
+                           int x2, int y2,
+                           int& x_res, int& y_res) const;
     // in world coordinates
     bool check_inter_real(float x1, float y1,
-			  float x2, float y2,
-			  float& x_res, float &y_res) const
-    {
+                          float x2, float y2,
+                          float& x_res, float &y_res) const {
       int px = 0, py = 0;
       bool res = check_inter_pixel(real_to_pixel(x1), real_to_pixel(y1),
-				   real_to_pixel(x2), real_to_pixel(y2),
-				   px, py);
+                                   real_to_pixel(x2), real_to_pixel(y2),
+                                   px, py);
       x_res = pixel_to_real(px);
       y_res = pixel_to_real(py);
       return res;
     }
     // in world coordinate, check intersection with an illuminated switch (on)
-    // return the color of the closest illuminated switch if there is an 
+    // return the color of the closest illuminated switch if there is an
     // intersection, -1 otherwise
     int check_inter_is(float x1, float y1,
-		       float x2, float y2) const ;
-    
-    const std::vector<Goal>& get_goals() const { return _goals; }
-    void add_goal(const Goal& g) { _goals.push_back(g); }
+                       float x2, float y2) const ;
+
+    const std::vector<Goal>& get_goals() const {
+      return _goals;
+    }
+    void add_goal(const Goal& g) {
+      _goals.push_back(g);
+    }
 
     typedef boost::shared_ptr<IlluminatedSwitch> ill_sw_t;
-    void add_illuminated_switch(ill_sw_t is) 
-    { _illuminated_switches.push_back(is); }
-    const std::vector<ill_sw_t>& get_illuminated_switches() const 
-    { return _illuminated_switches; }
-    ill_sw_t get_illuminated_switch_by_color(int color)
-    {
+    void add_illuminated_switch(ill_sw_t is) {
+      _illuminated_switches.push_back(is);
+    }
+    const std::vector<ill_sw_t>& get_illuminated_switches() const {
+      return _illuminated_switches;
+    }
+    ill_sw_t get_illuminated_switch_by_color(int color) {
       for (size_t i = 0; i < _illuminated_switches.size(); ++i)
-	if (_illuminated_switches[i]->get_color() == color)
-	  return _illuminated_switches[i];
+        if (_illuminated_switches[i]->get_color() == color)
+          return _illuminated_switches[i];
       assert(0);
       return ill_sw_t();
     }
-    void clear_illuminated_switches() 
-    { _illuminated_switches.clear(); }
+    void clear_illuminated_switches() {
+      _illuminated_switches.clear();
+    }
 
-    void update(const Posture& robot_pos)
-    {
+    void update(const Posture& robot_pos) {
       for (size_t i = 0; i < _illuminated_switches.size(); ++i)
-	_illuminated_switches[i]->try_to_activate(robot_pos);
+        _illuminated_switches[i]->try_to_activate(robot_pos);
     }
-    void terrain_switch(const std::string &fname)
-    {
+    void terrain_switch(const std::string &fname) {
       _read_file(fname);
     }
 
     // Draws a rectangle with (x,y) the upper left point and (lx,ly) the size
     void draw_rect(int x, int y, int lx, int ly);
-    
-  protected:
 
-    // 
+   protected:
+
+    //
     void _read_file(const std::string &fname);
-    bool _get_bit(char c, int i) const { return (c & (1 << (7 - i))); }
+    bool _get_bit(char c, int i) const {
+      return (c & (1 << (7 - i)));
+    }
     //
     bool _try_pixel(int x, int y) const;
     //
     bool _check_inter_ray_circle(float x1, float y1,
-				 float x2, float y2,
-				 float xr, float yr, float radius) const;
- 
+                                 float x2, float y2,
+                                 float xr, float yr, float radius) const;
+
     //
     std::vector<status_t> _data;
     std::vector<Goal> _goals;
     std::vector<ill_sw_t> _illuminated_switches;
-    // 
+    //
     int _w, _h;
     float _real_w, _real_h;
     float _fx;
diff --git a/misc.hpp b/misc.hpp
index b2b986c..07bc1c7 100644
--- a/misc.hpp
+++ b/misc.hpp
@@ -3,18 +3,18 @@
 ** Login : <mouret@asuncion.lip6.fr>
 ** Started on  Mon Jan 14 16:38:42 2008 Jean-Baptiste MOURET
 ** $Id$
-** 
+**
 ** Copyright (C) 2008 Jean-Baptiste MOURET
 ** This program is free software; you can redistribute it and/or modify
 ** it under the terms of the GNU General Public License as published by
 ** the Free Software Foundation; either version 2 of the License, or
 ** (at your option) any later version.
-** 
+**
 ** This program is distributed in the hope that it will be useful,
 ** but WITHOUT ANY WARRANTY; without even the implied warranty of
 ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 ** GNU General Public License for more details.
-** 
+**
 ** You should have received a copy of the GNU General Public License
 ** along with this program; if not, write to the Free Software
 ** Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
@@ -23,37 +23,35 @@
 #ifndef   	FASTSIM_MISC_HH_
 # define   	FASTSIM_MISC_HH_
 
-namespace fastsim
-{
-  class Exception
-  {
-  public:
+namespace fastsim {
+  class Exception {
+   public:
     Exception(const char* msg) : _msg(msg) {};
     Exception(const std::string& str) : _msg(str) {}
-    const std::string& get_msg() const { return _msg; }
-  protected:
+    const std::string& get_msg() const {
+      return _msg;
+    }
+   protected:
     std::string _msg;
   };
 
   template<typename T>
-  inline T normalize_angle(T a)
-  {
-     while (a > M_PI)
-       a -= 2*M_PI;
-     while (a < -M_PI)
-       a += 2*M_PI;
-     return a;
-  }  
-  
+  inline T normalize_angle(T a) {
+    while (a > M_PI)
+      a -= 2*M_PI;
+    while (a < -M_PI)
+      a += 2*M_PI;
+    return a;
+  }
+
   template<typename T>
-  inline T normalize_angle_2pi(T a)
-  {
-     while (a > 2 * M_PI)
-       a -= 2 * M_PI;
-     while (a < 0)
-       a += 2 * M_PI;
-     return a;
-  }  
+  inline T normalize_angle_2pi(T a) {
+    while (a > 2 * M_PI)
+      a -= 2 * M_PI;
+    while (a < 0)
+      a += 2 * M_PI;
+    return a;
+  }
 
 }
 
diff --git a/posture.hpp b/posture.hpp
index cb2a5e6..1ec2ae2 100644
--- a/posture.hpp
+++ b/posture.hpp
@@ -4,18 +4,18 @@
 ** Login : <mouret@asuncion.lip6.fr>
 ** Started on  Mon Jan 14 16:39:35 2008 Jean-Baptiste MOURET
 ** $Id$
-** 
+**
 ** Copyright (C) 2008 Jean-Baptiste MOURET
 ** This program is free software; you can redistribute it and/or modify
 ** it under the terms of the GNU General Public License as published by
 ** the Free Software Foundation; either version 2 of the License, or
 ** (at your option) any later version.
-** 
+**
 ** This program is distributed in the hope that it will be useful,
 ** but WITHOUT ANY WARRANTY; without even the implied warranty of
 ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 ** GNU General Public License for more details.
-** 
+**
 ** You should have received a copy of the GNU General Public License
 ** along with this program; if not, write to the Free Software
 ** Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
@@ -25,51 +25,57 @@
 # define   	POSTURE_HH_
 #include <cmath>
 
-namespace fastsim
-{
-  class Posture
-  {
-  public:
-    Posture(float x, float y, float theta) : 
+namespace fastsim {
+  class Posture {
+   public:
+    Posture(float x, float y, float theta) :
       _x(x), _y(y),
-      _theta(theta)
-    { }
-    Posture(){}
-    Posture(const Posture& p) : 
-      _x(p._x), 
-      _y(p._y), 
-      _theta(p._theta)
-    {}
-    Posture& operator=(const Posture& o)
-    {
+      _theta(theta) {
+    }
+    Posture() {}
+    Posture(const Posture& p) :
+      _x(p._x),
+      _y(p._y),
+      _theta(p._theta) {
+    }
+    Posture& operator=(const Posture& o) {
       _x = o._x;
       _y = o._y;
       _theta = o._theta;
       return *this;
     }
     //
-    float theta() const { return _theta;}
-    void set_theta(float t) { _theta = t; }
-    float x() const { return _x; }
-    float y() const { return _y; }
-    float get_x() const { return _x; }
-    float get_y() const { return _y; }
-    
+    float theta() const {
+      return _theta;
+    }
+    void set_theta(float t) {
+      _theta = t;
+    }
+    float x() const {
+      return _x;
+    }
+    float y() const {
+      return _y;
+    }
+    float get_x() const {
+      return _x;
+    }
+    float get_y() const {
+      return _y;
+    }
+
     //
-    float dist_to(const Posture& p) const
-    {
+    float dist_to(const Posture& p) const {
       return dist_to(p.x(), p.y());
     }
-    float dist_to(float x, float y) const
-    {
+    float dist_to(float x, float y) const {
       float xx = _x - x;
       float yy = _y - y;
       return sqrtf(xx * xx + yy * yy);
     }
 
-    // 
-    const Posture& rotate(float theta)
-    {
+    //
+    const Posture& rotate(float theta) {
       float x_ = cos(theta) * x() - sin(theta) * y();
       float y_ = cos(theta) * y() + sin(theta) * x();
       _x = x_;
@@ -77,49 +83,44 @@ namespace fastsim
       _theta += theta;
       return *this;
     }
-    Posture operator+(const Posture& o) const
-    {
+    Posture operator+(const Posture& o) const {
       Posture p;
       p._x = _x + o._x;
       p._y = _y + o._y;
       p._theta   = normalize_angle(_theta + o._theta);
       return p;
     }
-    const Posture& move(float d_l, float d_r, 
-			float wheels_dist)
-    {
+    const Posture& move(float d_l, float d_r,
+                        float wheels_dist) {
       Posture old_pos = *this;
       float alpha = (d_r - d_l) / wheels_dist;
       Posture p;
 
-      if (fabs(alpha) > 1e-10)
-	{
-	  float	r = (d_l / alpha) +  wheels_dist / 2;
-	  float	d_x = (cos(alpha) - 1) * r;
-	  float	d_y = sin(alpha) * r;
-	  p = Posture(d_x, d_y, alpha);
-	  p.rotate(old_pos.theta() - M_PI / 2);
-	  p.set_theta(normalize_angle(alpha));
-	}
-      else
-	p = Posture(d_l * cos(old_pos.theta()),
-		    d_l * sin(old_pos.theta()),
-		    0);
+      if (fabs(alpha) > 1e-10) {
+        float	r = (d_l / alpha) +  wheels_dist / 2;
+        float	d_x = (cos(alpha) - 1) * r;
+        float	d_y = sin(alpha) * r;
+        p = Posture(d_x, d_y, alpha);
+        p.rotate(old_pos.theta() - M_PI / 2);
+        p.set_theta(normalize_angle(alpha));
+      } else
+        p = Posture(d_l * cos(old_pos.theta()),
+                    d_l * sin(old_pos.theta()),
+                    0);
       *this = p + old_pos;
       return *this;
     }
 
     template<typename T>
-    static inline T normalize_angle(T a)
-    {
+    static inline T normalize_angle(T a) {
       while (a > M_PI)
-	a -= 2*M_PI;
+        a -= 2*M_PI;
       while (a < -M_PI)
-	a += 2*M_PI;
+        a += 2*M_PI;
       return a;
-    }  
-  protected:
-    float _x, _y;    
+    }
+   protected:
+    float _x, _y;
     float _theta;
   };
 
diff --git a/radar.cpp b/radar.cpp
index a72e4de..4299b45 100644
--- a/radar.cpp
+++ b/radar.cpp
@@ -2,29 +2,25 @@
 #include "radar.hpp"
 
 
-namespace fastsim
-{
+namespace fastsim {
   int Radar :: update(const Posture& pos,
-		      const boost::shared_ptr<Map>& map)
-  {
+                      const boost::shared_ptr<Map>& map) {
     const Goal& g = map->get_goals()[_color];
     float angle = normalize_angle_2pi(atan2(g.get_y() - pos.y(), g.get_x() - pos.x())
-				  - pos.theta());
+                                      - pos.theta());
     float x, y;
     bool not_visible = _through_walls ? false :
-      map->check_inter_real(pos.x(), pos.y(), g.get_x(), g.get_y(), x, y);
-    if (not_visible && !_through_walls)
-    {
+                       map->check_inter_real(pos.x(), pos.y(), g.get_x(), g.get_y(), x, y);
+    if (not_visible && !_through_walls) {
       _activated_slice = -1;
       return -1;
     }
     float xi = _inc;
-    for (size_t i = 0; i < _nb_slices + 1; ++i)
-      {
-	if (angle < xi)
-	  return (_activated_slice = i % _nb_slices);
-	xi += _inc;
-      }
+    for (size_t i = 0; i < _nb_slices + 1; ++i) {
+      if (angle < xi)
+        return (_activated_slice = i % _nb_slices);
+      xi += _inc;
+    }
     //    assert(0);
     return -1;
   }
diff --git a/radar.hpp b/radar.hpp
index d421826..4f0b90e 100644
--- a/radar.hpp
+++ b/radar.hpp
@@ -5,25 +5,31 @@
 #include "posture.hpp"
 #include "map.hpp"
 
-namespace fastsim
-{
+namespace fastsim {
   /// a radar detects a given goal in one of the n slices
   /// radar SEE TROUGH OBSTACLES
-  class Radar
-  {
-  public:
+  class Radar {
+   public:
     Radar(int color, int nb_slices, bool through_walls = true) :
       _color(color), _nb_slices(nb_slices), _inc(2 * M_PI / nb_slices),
       _activated_slice(0),
-      _through_walls(through_walls)
-    {}
+      _through_walls(through_walls) {
+    }
     int update(const Posture& pos,
-	       const boost::shared_ptr<Map>& map);
-    int get_activated_slice() const { return _activated_slice; }
-    int get_nb_slices() const { return _nb_slices; }
-    int get_color() const { return _color; }
-    float get_inc() const { return _inc; }
-  protected:
+               const boost::shared_ptr<Map>& map);
+    int get_activated_slice() const {
+      return _activated_slice;
+    }
+    int get_nb_slices() const {
+      return _nb_slices;
+    }
+    int get_color() const {
+      return _color;
+    }
+    float get_inc() const {
+      return _inc;
+    }
+   protected:
     // the "color" (i.e. goal identifier) detected by this "radar"
     int _color;
     // the number of pie-slices
diff --git a/robot.cpp b/robot.cpp
index 05d5055..b9eb6aa 100644
--- a/robot.cpp
+++ b/robot.cpp
@@ -3,18 +3,18 @@
 ** Login : <mouret@asuncion.lip6.fr>
 ** Started on  Mon Jan 14 16:40:38 2008 Jean-Baptiste MOURET
 ** $Id$
-** 
+**
 ** Copyright (C) 2008 Jean-Baptiste MOURET
 ** This program is free software; you can redistribute it and/or modify
 ** it under the terms of the GNU General Public License as published by
 ** the Free Software Foundation; either version 2 of the License, or
 ** (at your option) any later version.
-** 
+**
 ** This program is distributed in the hope that it will be useful,
 ** but WITHOUT ANY WARRANTY; without even the implied warranty of
 ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 ** GNU General Public License for more details.
-** 
+**
 ** You should have received a copy of the GNU General Public License
 ** along with this program; if not, write to the Free Software
 ** Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
@@ -26,21 +26,18 @@
 
 #include "robot.hpp"
 
-namespace fastsim
-{
-  void Robot :: move(float v1, float v2, const boost::shared_ptr<Map>& m)
-  {
+namespace fastsim {
+  void Robot :: move(float v1, float v2, const boost::shared_ptr<Map>& m) {
     Posture prev = _pos;
     _pos.move(v1, v2, _radius * 2);
     _update_bb();
     // update bumpers & go back if there is a collision
-    if(_check_collision(m))
-      {
-	float theta = _pos.theta();
-	_pos = prev;
-	_pos.set_theta(theta);
-	_collision = true;
-      }
+    if(_check_collision(m)) {
+      float theta = _pos.theta();
+      _pos = prev;
+      _pos.set_theta(theta);
+      _collision = true;
+    }
     // update lasers
     for (size_t i = 0; i < _lasers.size(); ++i)
       _lasers[i].update(_pos, m);
@@ -55,15 +52,13 @@ namespace fastsim
       _camera.update(_pos, m);
   }
 
-  void Robot :: _update_bb()
-  {
+  void Robot :: _update_bb() {
     // robot bb
     _bb.x = _pos.x() - _radius - 4;
     _bb.y = _pos.y() - _radius - 4;
   }
 
-  bool Robot :: _check_collision(const boost::shared_ptr<Map>& m)
-  {
+  bool Robot :: _check_collision(const boost::shared_ptr<Map>& m) {
     // pixel wise
     int rp = m->real_to_pixel(_radius);
     int r = rp * rp;
@@ -73,36 +68,33 @@ namespace fastsim
     int bby = m->real_to_pixel(_bb.y);
     int bbw = m->real_to_pixel(_bb.x + _bb.w);
     int bbh = m->real_to_pixel(_bb.y + _bb.h);
-    
+
     typedef std::pair<int, int> p_t;
     std::list<p_t > coll_points;
     for (int i = bbx; i < bbw; ++i)
       for (int j = bby; j < bbh; ++j)
-	if (m->get_pixel(i, j) == Map::obstacle)
-	  {
-	    float d1 = (i - x);
-	    float d2 = (j - y);
-	    if (d1 * d1 + d2 * d2 <= r)
-	      coll_points.push_back(p_t(i, j));
-	  }
+        if (m->get_pixel(i, j) == Map::obstacle) {
+          float d1 = (i - x);
+          float d2 = (j - y);
+          if (d1 * d1 + d2 * d2 <= r)
+            coll_points.push_back(p_t(i, j));
+        }
     _left_bumper = false;
     _right_bumper = false;
     if (coll_points.empty())
       return false;
-    else
-      {
-	// bumpers
-	for (std::list<p_t>::const_iterator it = coll_points.begin();
-	     it != coll_points.end(); ++it)
-	  {
-	    float a = Posture::normalize_angle(atan2(it->second - y, 
-						     it->first - x) - _pos.theta());
-	    if (a > 0 && a < M_PI / 2.0)
-	      _left_bumper = true;
-	    if (a < 0 && a > -M_PI / 2.0)
-	      _right_bumper = true;
-	  }
-	return true;
+    else {
+      // bumpers
+      for (std::list<p_t>::const_iterator it = coll_points.begin();
+           it != coll_points.end(); ++it) {
+        float a = Posture::normalize_angle(atan2(it->second - y,
+                                           it->first - x) - _pos.theta());
+        if (a > 0 && a < M_PI / 2.0)
+          _left_bumper = true;
+        if (a < 0 && a > -M_PI / 2.0)
+          _right_bumper = true;
       }
+      return true;
+    }
   }
 }
diff --git a/robot.hpp b/robot.hpp
index be69a33..66b31c1 100644
--- a/robot.hpp
+++ b/robot.hpp
@@ -3,18 +3,18 @@
 ** Login : <mouret@asuncion.lip6.fr>
 ** Started on  Mon Jan 14 16:40:06 2008 Jean-Baptiste MOURET
 ** $Id$
-** 
+**
 ** Copyright (C) 2008 Jean-Baptiste MOURET
 ** This program is free software; you can redistribute it and/or modify
 ** it under the terms of the GNU General Public License as published by
 ** the Free Software Foundation; either version 2 of the License, or
 ** (at your option) any later version.
-** 
+**
 ** This program is distributed in the hope that it will be useful,
 ** but WITHOUT ANY WARRANTY; without even the implied warranty of
 ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 ** GNU General Public License for more details.
-** 
+**
 ** You should have received a copy of the GNU General Public License
 ** along with this program; if not, write to the Free Software
 ** Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
@@ -32,73 +32,109 @@
 #include "light_sensor.hpp"
 #include "linear_camera.hpp"
 
-namespace fastsim
-{
-  class Robot
-  {
-  public:
-    struct BoundingBox { float x, y, w, h; };
-    Robot(float radius) : 
+namespace fastsim {
+  class Robot {
+   public:
+    struct BoundingBox {
+      float x, y, w, h;
+    };
+    Robot(float radius) :
       _radius(radius),
       _pos(0, 0, 0),
       _left_bumper(false),
       _right_bumper(false),
       _use_camera(false),
       _collision(false),
-      _color(1)
-    {
+      _color(1) {
       _bb.w = _radius * 2 + 8;
-      _bb.h = _radius * 2 + 8;      
+      _bb.h = _radius * 2 + 8;
       _update_bb();
     }
     Robot(float radius, const Posture& pos) :
-      _radius(radius), _pos(pos), 
+      _radius(radius), _pos(pos),
       _left_bumper(false),
       _right_bumper(false),
       _use_camera(false),
-      _collision(false)
-    {
+      _collision(false) {
       _bb.w = _radius * 2 + 8;
-      _bb.h = _radius * 2 + 8;      
+      _bb.h = _radius * 2 + 8;
       _update_bb();
     }
     //
-    void reinit()
-    {
+    void reinit() {
       _collision = false;
       _left_bumper = false;
       _right_bumper = false;
     }
     void move(float v1, float v2, const boost::shared_ptr<Map>& m);
-    const Posture& get_pos() const { return _pos; }
-    void set_pos(const Posture& pos) { _pos = pos; }
-    const BoundingBox& get_bb() const { return _bb; }
-    float get_radius() const { return _radius; }
-    bool get_collision() const { return _collision;}
+    const Posture& get_pos() const {
+      return _pos;
+    }
+    void set_pos(const Posture& pos) {
+      _pos = pos;
+    }
+    const BoundingBox& get_bb() const {
+      return _bb;
+    }
+    float get_radius() const {
+      return _radius;
+    }
+    bool get_collision() const {
+      return _collision;
+    }
     //
-    bool get_left_bumper() const { return _left_bumper; }
-    bool get_right_bumper() const { return _right_bumper; }
+    bool get_left_bumper() const {
+      return _left_bumper;
+    }
+    bool get_right_bumper() const {
+      return _right_bumper;
+    }
 
     // lasers
-    void add_laser(const Laser& l) { _lasers.push_back(l); }
-    const std::vector<Laser>& get_lasers() const { return _lasers; }
+    void add_laser(const Laser& l) {
+      _lasers.push_back(l);
+    }
+    const std::vector<Laser>& get_lasers() const {
+      return _lasers;
+    }
 
     // radars
-    void add_radar(const Radar& r){ _radars.push_back(r); }
-    const std::vector<Radar>& get_radars() const { return _radars; }
+    void add_radar(const Radar& r) {
+      _radars.push_back(r);
+    }
+    const std::vector<Radar>& get_radars() const {
+      return _radars;
+    }
 
     // light sensors
-    void add_light_sensor(const LightSensor& l) { _light_sensors.push_back(l); }
-    const std::vector<LightSensor>& get_light_sensors() const { return _light_sensors; }
-    void set_color(unsigned int color) {_color=color;}
-    unsigned int color() const {return _color;}
+    void add_light_sensor(const LightSensor& l) {
+      _light_sensors.push_back(l);
+    }
+    const std::vector<LightSensor>& get_light_sensors() const {
+      return _light_sensors;
+    }
+    void set_color(unsigned int color) {
+      _color=color;
+    }
+    unsigned int color() const {
+      return _color;
+    }
 
     // camera
-    void use_camera(const LinearCamera& c) { _camera = c; _use_camera = true; }
-    void use_camera() { _use_camera = true; }
-    const LinearCamera& get_camera() const { return _camera; }
-    bool use_camera() const { return _use_camera; }
-  protected:
+    void use_camera(const LinearCamera& c) {
+      _camera = c;
+      _use_camera = true;
+    }
+    void use_camera() {
+      _use_camera = true;
+    }
+    const LinearCamera& get_camera() const {
+      return _camera;
+    }
+    bool use_camera() const {
+      return _use_camera;
+    }
+   protected:
     bool _check_collision(const boost::shared_ptr<Map>& m);
     void _update_bb();
     float _radius;
diff --git a/simu_fastsim.hpp b/simu_fastsim.hpp
index 566ebf7..88ae2f4 100644
--- a/simu_fastsim.hpp
+++ b/simu_fastsim.hpp
@@ -4,62 +4,68 @@
 #include <sferes/simu/simu.hpp>
 #include "fastsim.hpp"
 
-namespace sferes
-{
-  namespace simu
-  {
-    SFERES_SIMU(Fastsim, Simu)
-    {
-      public:
-        Fastsim() :
-          _robot(20.0f)
-        {
-          _map = boost::shared_ptr<fastsim::Map>(new fastsim::Map(Params::simu::map_name(), 600.0f));
-        }
-        Fastsim(const fastsim::Map & m) :
-          _robot(20.0f)
-        {
-          _map = boost::shared_ptr<fastsim::Map>(new fastsim::Map(m));
-        }
-        ~Fastsim(){}
-        void init()
-        {}
-        void refresh() { _map->update(_robot.get_pos()); }
-        void init_view(bool dump = false)
-        {
-          _display =
-            boost::shared_ptr<fastsim::Display>(new fastsim::Display(_map, _robot));
-        }
-        void set_map(const boost::shared_ptr<fastsim::Map>& map) { _map = map; }
-        void refresh_view()
-        {
-          _display->update();
-        }
-        void refresh_map_view()
-        {
-          _display->update_map();
-        }
-        void switch_map()
-        {
-          _map->terrain_switch(Params::simu::alt_map_name());
-        }
-        void reset_map()
-        {
-          _map->terrain_switch(Params::simu::map_name());
-        }
-        void move_robot(float v1, float v2) { _robot.move(v1, v2, _map); }
+namespace sferes {
+  namespace simu {
+    SFERES_SIMU(Fastsim, Simu) {
+    public:
+      Fastsim() :
+        _robot(20.0f) {
+        _map = boost::shared_ptr<fastsim::Map>(new fastsim::Map(Params::simu::map_name(), 600.0f));
+      }
+      Fastsim(const fastsim::Map & m) :
+        _robot(20.0f) {
+        _map = boost::shared_ptr<fastsim::Map>(new fastsim::Map(m));
+      }
+      ~Fastsim() {}
+      void init() {
+      }
+      void refresh() {
+        _map->update(_robot.get_pos());
+      }
+      void init_view(bool dump = false) {
+        _display =
+          boost::shared_ptr<fastsim::Display>(new fastsim::Display(_map, _robot));
+      }
+      void set_map(const boost::shared_ptr<fastsim::Map>& map) {
+        _map = map;
+      }
+      void refresh_view() {
+        _display->update();
+      }
+      void refresh_map_view() {
+        _display->update_map();
+      }
+      void switch_map() {
+        _map->terrain_switch(Params::simu::alt_map_name());
+      }
+      void reset_map() {
+        _map->terrain_switch(Params::simu::map_name());
+      }
+      void move_robot(float v1, float v2) {
+        _robot.move(v1, v2, _map);
+      }
 
-        boost::shared_ptr<fastsim::Map> map() { return _map; }
-        const boost::shared_ptr<fastsim::Map> map() const { return _map; }
+      boost::shared_ptr<fastsim::Map> map() {
+        return _map;
+      }
+      const boost::shared_ptr<fastsim::Map> map() const {
+        return _map;
+      }
 
-        fastsim::Robot& robot() { return _robot; }
-        const fastsim::Robot& robot() const { return _robot; }
+      fastsim::Robot& robot() {
+        return _robot;
+      }
+      const fastsim::Robot& robot() const {
+        return _robot;
+      }
 
-        fastsim::Display& display() {return *_display; }
-      protected:
-        fastsim::Robot _robot;
-        boost::shared_ptr<fastsim::Map> _map;
-        boost::shared_ptr<fastsim::Display> _display;
+      fastsim::Display& display() {
+        return *_display;
+      }
+    protected:
+      fastsim::Robot _robot;
+      boost::shared_ptr<fastsim::Map> _map;
+      boost::shared_ptr<fastsim::Display> _display;
 
     };
   }
diff --git a/test_fastsim.cpp b/test_fastsim.cpp
index 63d980b..54fb836 100644
--- a/test_fastsim.cpp
+++ b/test_fastsim.cpp
@@ -1,6 +1,6 @@
 
 #ifndef APPLE
-#define BOOST_TEST_DYN_LINK 
+#define BOOST_TEST_DYN_LINK
 #define BOOST_TEST_MODULE fastsim
 
 
@@ -14,23 +14,21 @@
 
 using namespace sferes;
 //using namespace sferes::ctrl;
-struct Params
-{
-  struct simu
-  {
+struct Params {
+  struct simu {
     SFERES_STRING(map_name, "modules/fastsim/cuisine.pbm");
+    SFERES_CONST float dt = 0.01;
   };
 };
 
 
-BOOST_AUTO_TEST_CASE(fastsim_intersection)
-{
+BOOST_AUTO_TEST_CASE(fastsim_intersection) {
   using namespace fastsim;
   typedef simu::Fastsim<Params> simu_t;
   simu_t s;
   s.robot().set_pos(Posture(200, 100, 0));
   s.robot().use_camera();
-  Map::ill_sw_t s1 = Map::ill_sw_t(new IlluminatedSwitch(1, 10, 300, 250, true)); 
+  Map::ill_sw_t s1 = Map::ill_sw_t(new IlluminatedSwitch(1, 10, 300, 250, true));
   Map::ill_sw_t s2 = Map::ill_sw_t(new IlluminatedSwitch(2, 30, 500, 250, true));
   Map::ill_sw_t s3 = Map::ill_sw_t(new IlluminatedSwitch(3, 10, 200, 150, true));
   Map::ill_sw_t s4 = Map::ill_sw_t(new IlluminatedSwitch(4, 10, 400, 350, true));
@@ -38,16 +36,16 @@ BOOST_AUTO_TEST_CASE(fastsim_intersection)
   s.map()->add_illuminated_switch(s2);
   s.map()->add_illuminated_switch(s3);
   s.map()->add_illuminated_switch(s4);
-  
+
   int c1 = s.map()->check_inter_is(250, 250, 1000, 250);
   BOOST_CHECK_EQUAL(c1, 1);
-  
+
   int c2 = s.map()->check_inter_is(250, 250, 250, 0);
   BOOST_CHECK_EQUAL(c2, -1);
-  
+
   int c3 = s.map()->check_inter_is(200, 350, 200, 100);
   BOOST_CHECK_EQUAL(c3, 3);
-  
+
   // s.init_view();
   // while(true)
   //   {
@@ -56,13 +54,11 @@ BOOST_AUTO_TEST_CASE(fastsim_intersection)
   //   }
 }
 
-#ifdef APPLE 
-int main(int argc, char *argv[])
-{
-	std::cout<<"WARNING: BOOST TEST framework desactivated on macos to avoid problems with SDL."<<std::endl;
+#ifdef APPLE
+int main(int argc, char *argv[]) {
+  std::cout<<"WARNING: BOOST TEST framework desactivated on macos to avoid problems with SDL."<<std::endl;
 #else
-BOOST_AUTO_TEST_CASE(fastsim_simu_refresh)
-{
+BOOST_AUTO_TEST_CASE(fastsim_simu_refresh) {
 #endif
   using namespace fastsim;
   typedef simu::Fastsim<Params> simu_t;
@@ -84,31 +80,30 @@ BOOST_AUTO_TEST_CASE(fastsim_simu_refresh)
   s.map()->add_illuminated_switch(s2);
   s.map()->add_illuminated_switch(s3);
   s.map()->add_illuminated_switch(s4);
-  
+
   s.robot().add_light_sensor(LightSensor(1, M_PI / 3, M_PI / 3));
   s.robot().add_light_sensor(LightSensor(1, -M_PI / 3, M_PI / 3));
   s.robot().add_light_sensor(LightSensor(2, M_PI / 3, M_PI / 3));
   s.robot().add_light_sensor(LightSensor(2, -M_PI / 3, M_PI / 3));
-			     
+
 
   s.init_view();
-  for (size_t i = 0; i < 10000; ++i)
-    {
-      if (s.robot().get_left_bumper())
-	s.move_robot(2.0, -2.0);
-      else if (s.robot().get_right_bumper())
-	s.move_robot(2.0, -2.0);
-      else if (rand()/(RAND_MAX + 1.0) < 0.05)
-	s.move_robot(3.0, -3.0);
-      else if (rand()/(RAND_MAX + 1.0) < 0.05)
-	s.move_robot(-3.0, 3.0);
-      else
-	s.move_robot(2.0, 2.0);
-	std::cout<<"i="<<i<<std::endl;
-      s.refresh();
-      s.refresh_view();
-    }
+  for (size_t i = 0; i < 10000; ++i) {
+    if (s.robot().get_left_bumper())
+      s.move_robot(2.0, -2.0);
+    else if (s.robot().get_right_bumper())
+      s.move_robot(2.0, -2.0);
+    else if (rand()/(RAND_MAX + 1.0) < 0.05)
+      s.move_robot(3.0, -3.0);
+    else if (rand()/(RAND_MAX + 1.0) < 0.05)
+      s.move_robot(-3.0, 3.0);
+    else
+      s.move_robot(2.0, 2.0);
+    std::cout<<"i="<<i<<std::endl;
+    s.refresh();
+    s.refresh_view();
+  }
 #ifdef APPLE
-	return 0;
+  return 0;
 #endif
 }