/*
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;
}