diff --git a/coreslam.c b/coreslam.c
new file mode 100644
index 0000000..4790491
--- /dev/null
+++ b/coreslam.c
@@ -0,0 +1,544 @@
+/*
+coreslam.c adapted from CoreSLAM.c, CoreSLAM_state.c, and CoreSLAM.h 
+downloaded from openslam.org on 01 January 2014.  Contains implementations 
+of scan and map methods.
+
+Copyright (C) 2014 Simon D. Levy
+
+This code is free software: you can redistribute it and/or modify
+it under the terms of the GNU Lesser General Public License as 
+published by the Free Software Foundation, either version 3 of the 
+License, or (at your option) any later version.
+
+This code 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 Lesser General Public License 
+along with this code.  If not, see <http:#www.gnu.org/licenses/>.
+
+Change log
+
+07-FEB-2014 : Simon D. Levy  - Initial release
+01-MAR-2014 : SDL - Converted millimeters to meters for API
+21-JUN-2014 : SDL - Added support for SSE and NEON
+10-JUL-2014 : SDL - Changed Laser scan rate and angles from int to double
+21-JUL-2014 : SDL - Made RMHC position search avoid looping when max count is zero
+23-JUL-2014 : SDL - Simplified laser detection angle min, max to total angle
+07-SEP-2014 : SDL - Migrated to github
+*/
+
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <time.h>
+#include <string.h>
+#include <math.h>
+
+#include "coreslam.h"
+#include "coreslam_internals.h"
+
+#include "random.h"
+
+/* Local helpers--------------------------------------------------- */
+
+static void * safe_malloc(size_t size)
+{
+    void * v = malloc(size);
+    
+    if (!v)
+    {
+        fprintf(stderr, "Unable to allocate %lu bytes\n", (unsigned long)size);
+        exit(1);
+    }
+    
+    return v;
+}
+
+
+static double * double_alloc(int size)
+{
+    return (double *)safe_malloc(size * sizeof(double));
+}
+
+static float * float_alloc(int size)
+{
+    return (float *)safe_malloc(size * sizeof(float));
+}
+
+
+static void 
+swap(int * a, int * b)
+{
+    int tmp = *a;
+    *a = *b;
+    *b = tmp;
+}
+
+
+static int roundup(double x)
+{
+    return (int)floor(x + 0.5);
+}
+
+
+static int 
+out_of_bounds(int value, int bound)
+{
+    return value  < 0 || value >= bound;
+}
+
+
+
+static int 
+clip(int *xyc, int * yxc, int xy, int yx, int map_size)
+{
+    
+    if (*xyc < 0) 
+    {
+        if (*xyc == xy) 
+        {
+            return 1;
+        }
+        *yxc += (*yxc - yx) * (- *xyc) / (*xyc - xy);
+        *xyc = 0;
+    }
+    
+    if (*xyc >= map_size)
+    {
+        if (*xyc == xy) 
+        {
+            return 1;
+        }
+        *yxc += (*yxc - yx) * (map_size - 1 - *xyc) / (*xyc - xy);
+        *xyc = map_size - 1;
+    }
+    
+    return 0;
+}
+
+
+static void
+map_laser_ray(
+    pixel_t * map_pixels,  
+    int map_size, 
+    int x1, 
+    int y1, 
+    int x2, 
+    int y2, 
+    int xp, 
+    int yp, 
+    int value, 
+    int alpha)
+{                    
+
+    if (out_of_bounds(x1, map_size) || out_of_bounds(y1, map_size))
+    {
+        return; 
+    }
+    
+    int x2c = x2; 
+    int y2c = y2;
+    
+    if (clip(&x2c, &y2c, x1, y1, map_size) || clip(&y2c, &x2c, y1, x1, map_size)) 
+    {
+        return;
+    }
+    
+    int dx = abs(x2 - x1); 
+    int dy = abs(y2 - y1);
+    int dxc = abs(x2c - x1); 
+    int dyc = abs(y2c - y1);
+    int incptrx = (x2 > x1) ? 1 : -1;
+    int incptry = (y2 > y1) ? map_size : -map_size;
+    int sincv = (value > NO_OBSTACLE) ? 1 : -1; 
+        
+    int derrorv = 0;
+    
+    if (dx > dy) 
+    {
+        derrorv = abs(xp - x2);
+    } 
+    else 
+    {
+        swap(&dx, &dy); 
+        swap(&dxc, &dyc); 
+        swap(&incptrx, &incptry);    
+        derrorv = abs(yp - y2);
+    }
+    
+    if (!derrorv)
+    {   /* XXX should probably throw an exception */
+        fprintf(stderr, "map_update: No error gradient: try increasing hole width\n");
+        exit(1);
+    }
+    
+    int error = 2 * dyc - dxc;
+    int horiz = 2 * dyc;
+    int diago = 2 * (dyc - dxc);
+    int errorv = derrorv / 2;
+        
+    int incv = (value - NO_OBSTACLE) / derrorv;   
+  
+    int incerrorv = value - NO_OBSTACLE - derrorv * incv;     
+    
+    pixel_t * ptr = map_pixels + y1 * map_size + x1;
+    int pixval = NO_OBSTACLE;
+    
+    int x = 0;
+    for (x = 0; x <= dxc; x++, ptr += incptrx) 
+    {
+        if (x > dx - 2 * derrorv) 
+        {
+            if (x <= dx - derrorv) 
+            {
+                pixval += incv;
+                errorv += incerrorv;
+                if (errorv > derrorv) 
+                {
+                    pixval += sincv;
+                    errorv -= derrorv; 
+                }
+            } 
+            else 
+            {
+                pixval -= incv;
+                errorv -= incerrorv;
+                if (errorv < 0) 
+                {
+                    pixval -= sincv;
+                    errorv += derrorv; 
+                }
+            }
+        } 
+        
+        /* Integration into the map */
+        *ptr = ((256 - alpha) * (*ptr) + alpha * pixval) >> 8;      
+        
+        if (error > 0) 
+        {
+            ptr += incptry;
+            error += diago;
+        } else 
+        {
+            error += horiz;
+        }
+    }    
+}
+
+
+static void 
+scan_update_xy(
+    scan_t * scan,
+    int offset,
+    int distance,
+    int scanval,
+    laser_t laser,
+    double horz_mm,
+    double rotation)
+{
+    int j;
+    for (j=0; j<scan->span; ++j)
+    {    
+        double k = (double)(offset*scan->span+j) * laser.detection_angle_degrees / 
+                   (laser.scan_size * scan->span - 1);
+        double angle = radians(-laser.detection_angle_degrees/2 + k * rotation); 
+        
+        double x = distance * cos(angle) - k * horz_mm;
+        double y = distance * sin(angle);
+        
+        scan->value[scan->npoints] = scanval;
+        
+        scan->x_mm[scan->npoints] = x;
+        scan->y_mm[scan->npoints] = y;
+        scan->npoints++;
+    }
+}
+
+
+/* Exported functions --------------------------------------------------------*/
+
+int * 
+int_alloc(
+    int size)
+{
+    return (int *)safe_malloc(size * sizeof(int));
+}
+
+void 
+map_init(
+    map_t * map, 
+    int size_pixels, 
+    double size_meters)
+{    
+    int npix = size_pixels * size_pixels;
+    
+    map->pixels = (pixel_t *)safe_malloc(npix * sizeof(pixel_t));
+    
+    int k;
+    for (k=0; k<npix; ++k)
+    {
+        map->pixels[k] = (OBSTACLE + NO_OBSTACLE) / 2;
+    }    
+    
+    map->size_pixels = size_pixels;
+    map->size_meters = size_meters;    
+    
+    /* precompute scale for efficiency */
+    map->scale_pixels_per_mm =  size_pixels / (size_meters * 1000);    
+}
+
+void
+map_free(
+    map_t * map)
+{
+    free(map->pixels);
+}
+
+void map_string(
+    map_t map,
+    char * str)
+{
+    sprintf(str, "size = %d x %d pixels | = %f meters", 
+        map.size_pixels, map.size_pixels, map.size_meters);
+}
+
+void 
+map_update(
+    map_t * map,
+    scan_t * scan, 
+    position_t position,
+    int map_quality,
+    double hole_width_mm)
+{            
+    
+    double position_theta_radians = radians(position.theta_degrees);
+    double costheta = cos(position_theta_radians);
+    double sintheta = sin(position_theta_radians);
+    
+    int x1 = roundup(position.x_mm * map->scale_pixels_per_mm);
+    int y1 = roundup(position.y_mm * map->scale_pixels_per_mm);
+    
+    int i = 0;
+    for (i = 0; i != scan->npoints; i++) 
+    {
+        double x2p = costheta * scan->x_mm[i] - sintheta * scan->y_mm[i];
+        double y2p = sintheta * scan->x_mm[i] + costheta * scan->y_mm[i];
+        
+        int xp = roundup((position.x_mm + x2p) * map->scale_pixels_per_mm);
+        int yp = roundup((position.y_mm + y2p) * map->scale_pixels_per_mm);
+        
+        
+        double dist = sqrt(x2p * x2p + y2p * y2p);
+        double add = hole_width_mm / 2 / dist;
+        x2p *= map->scale_pixels_per_mm * (1 + add);
+        y2p *= map->scale_pixels_per_mm * (1 + add); 
+
+        
+        int x2 = roundup(position.x_mm * map->scale_pixels_per_mm + x2p);
+        int y2 = roundup(position.y_mm * map->scale_pixels_per_mm + y2p);
+        
+        int value = OBSTACLE;
+        int q = map_quality;
+        
+        if (scan->value[i] == NO_OBSTACLE) 
+        { 
+            q = map_quality / 4;
+            value = NO_OBSTACLE;
+        }
+       
+        map_laser_ray(map->pixels, map->size_pixels, x1, y1, x2, y2, xp, yp, value, q);       
+    }            
+}    
+
+void
+map_get(
+    map_t * map, 
+    char * bytes)
+{    
+    int k;
+    for (k=0; k<map->size_pixels*map->size_pixels; ++k)
+    {
+        bytes[k] = map->pixels[k] >> 8;
+    }
+
+}
+
+
+void
+map_set(
+    map_t * map, 
+    char * bytes)
+{    
+    int k;
+    for (k=0; k<map->size_pixels*map->size_pixels; ++k)
+    {
+        map->pixels[k] = bytes[k];
+        map->pixels[k] <<= 8;
+    }
+}
+
+void 
+scan_init(
+    scan_t * scan, 
+    int size,
+    int span)
+{
+    scan->x_mm = double_alloc(size*span);
+    scan->y_mm = double_alloc(size*span); 
+    scan->value = int_alloc(size*span);
+    scan->span = span;
+    
+    /* assure size multiple of 4 for SSE */
+    scan->obst_x_mm = float_alloc(size*span+4);
+    scan->obst_y_mm = float_alloc(size*span+4);
+}
+
+
+void 
+scan_free(
+    scan_t * scan)
+{
+    free(scan->x_mm);
+    free(scan->y_mm);
+    free(scan->value);
+    
+    free(scan->obst_x_mm);
+    free(scan->obst_y_mm);
+}
+
+void scan_string(
+    scan_t scan,
+    char * str)
+{
+    sprintf(str, "%d obstacle points | %d free points", scan.obst_npoints, scan.npoints-scan.obst_npoints);    
+}
+
+void 
+scan_update(
+    scan_t * scan, 
+    int * lidar_mm, 
+    laser_t laser,
+    double hole_width_mm,
+    double velocities_dxy_mm,
+    double velocities_dtheta_degrees)
+{        
+    scan->npoints = 0;
+    scan->obst_npoints = 0;
+    
+    /* Take velocity into account */
+    int degrees_per_second = laser.scan_rate_hz * 360;
+    double horz_mm = velocities_dxy_mm / degrees_per_second;
+    double rotation = 1 + velocities_dtheta_degrees / degrees_per_second;
+       
+    /* Span the laser scans to better cover the space */
+    int i;
+    for (i=laser.detection_margin+1; i<laser.scan_size-laser.detection_margin; ++i)
+    {
+        int lidar_value_mm = lidar_mm[i];
+        
+        /* No obstacle */
+        if (lidar_value_mm == 0)
+        {
+            scan_update_xy(scan, i, laser.distance_no_detection_mm, NO_OBSTACLE, laser, horz_mm, rotation);
+        }
+        
+        /* Obstacle */
+        else if (lidar_value_mm > hole_width_mm / 2)
+        {
+            int oldstart = scan->npoints;
+        
+            scan_update_xy(scan, i, lidar_value_mm, OBSTACLE, laser, horz_mm, rotation);
+            
+            /* Store obstacles separately for SSE */
+            int j;
+            for (j=oldstart; j<scan->npoints; ++j)
+            {
+                if (scan->value[j] == OBSTACLE)
+                {                
+                    scan->obst_x_mm[scan->obst_npoints] = scan->x_mm[j];
+                    scan->obst_y_mm[scan->obst_npoints] = scan->y_mm[j];
+                    scan->obst_npoints++;
+                }
+            }
+        }
+    }      
+}
+
+void 
+laser_string(
+    laser_t laser, 
+    char * str)
+{
+    sprintf(str, 
+        "scan_size=%d | scan_rate=%3.3f hz | detection_angle=%3.3f deg | " 
+        "distance_no_detection=%7.4f mm | detection_margin=%d | offset=%4.4f m",
+        laser.scan_size,
+        laser.scan_rate_hz, 
+        laser.detection_angle_degrees, 
+        laser.distance_no_detection_mm,
+        laser.detection_margin, 
+        laser.offset_mm
+        );
+}    
+
+position_t 
+rmhc_position_search(
+    position_t start_pos,
+	map_t * map,
+    scan_t * scan,
+    laser_t laser,
+	double sigma_xy_mm,
+	double sigma_theta_degrees,
+	int max_search_iter,
+	void * randomizer)
+{   	    
+	position_t currentpos = start_pos;
+	position_t bestpos = start_pos;
+	position_t lastbestpos = start_pos;
+	
+	int current_distance = distance_scan_to_map(map, scan, currentpos);
+	
+	int lowest_distance =  current_distance;   
+	int last_lowest_distance = current_distance;   
+		
+	int counter = 0;
+	
+	while (counter < max_search_iter) 
+	{  	    
+		currentpos = lastbestpos;
+		
+		currentpos.x_mm = random_normal(randomizer, currentpos.x_mm, sigma_xy_mm);
+		currentpos.y_mm = random_normal(randomizer, currentpos.y_mm, sigma_xy_mm); 	
+		currentpos.theta_degrees = random_normal(randomizer, currentpos.theta_degrees, sigma_theta_degrees);
+		
+		current_distance = distance_scan_to_map(map, scan, currentpos);
+		
+		/* -1 indicates infinity */		
+		if ((current_distance > -1) && (current_distance < lowest_distance))
+		{
+			lowest_distance = current_distance;
+			bestpos = currentpos;
+		} 
+		else 
+		{
+			counter++;
+		}
+		
+		if (counter > max_search_iter / 3) 
+		{
+			if (lowest_distance < last_lowest_distance) 
+			{
+				lastbestpos = bestpos;
+				last_lowest_distance = lowest_distance;
+				counter = 0;
+				sigma_xy_mm *= 0.5;
+				sigma_theta_degrees *= 0.5;
+			}
+		}
+		
+	}
+    
+	return bestpos;
+}