/**
* @file			filter.c
* @brief		various filtering algorithm
* @author		Patrick Roth - roth@stettbacher.ch
* @copyright	Stettbacher Signal Processing AG
* 
* @remarks
*
* <PRE>
* This library 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 2.1 of the License, or (at your option) any later version.
*
* This library 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
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
* </PRE>
*
*/

#include <stdio.h>
#include <string.h>
#include <stdint.h>
#include <stdlib.h>
#include <math.h>

#define MAX_GAUSS_KERNEL_SIZE			25			///< maximum gaussian kernel size (must be an odd number)


/**
 * Filter pixel with given 3x3 kernel. Fixed-point is used.
 * 
 * @param a11 kernel weight at position 1/1
 * @param a12 kernel weight at position 1/2
 * @param a13 kernel weight at position 1/3
 * @param a21 kernel weight at position 2/1
 * @param a22 kernel weight at position 2/2
 * @param a23 kernel weight at position 2/3
 * @param a31 kernel weight at position 3/1
 * @param a32 kernel weight at position 3/2
 * @param a33 kernel weight at position 3/3
 * @param p11 pixel value at position 1/1
 * @param p12 pixel value at position 1/2
 * @param p13 pixel value at position 1/3
 * @param p21 pixel value at position 2/1
 * @param p22 pixel value at position 2/2
 * @param p23 pixel value at position 2/3
 * @param p31 pixel value at position 3/1
 * @param p32 pixel value at position 3/2
 * @param p33 pixel value at position 3/3 
 * @param shift_fact The shifting factor defines how many number of bits the kernel and pixel were shifted to left.
 * @return filtered pixel value
 */
static inline int16_t calc_filter3x3(const int16_t a11, const int16_t a12, const int16_t a13,
									 const int16_t a21, const int16_t a22, const int16_t a23,
									 const int16_t a31, const int16_t a32, const int16_t a33,
									 const int16_t p11, const int16_t p12, const int16_t p13,
									 const int16_t p21, const int16_t p22, const int16_t p23,
									 const int16_t p31, const int16_t p32, const int16_t p33,
									 const int shift_fact) {
	
	int16_t out;
	
	out = (a11*p11 + a12*p12 + a13*p13 + a21*p21 + a22*p22 + a23*p23 + a31*p31 + a32*p32 + a33*p33) >> shift_fact;
	return out;
}


/**
 * Apply sobel kernel to given 3x3 pixels.
 * 
 * @param p11 pixel value at position 1/1
 * @param p12 pixel value at position 1/2
 * @param p13 pixel value at position 1/3
 * @param p21 pixel value at position 2/1
 * @param p22 pixel value at position 2/2
 * @param p23 pixel value at position 2/3
 * @param p31 pixel value at position 3/1
 * @param p32 pixel value at position 3/2
 * @param p33 pixel value at position 3/3 
 * @return filtered pixel value
 * 
 * NOTE
 * For performance reasons the return value is ret = abs(sobel_x) + (abs sobel_y) instead of ret = sqrt(abs(sobel_x) + (abs sobel_y))
 */
static inline int16_t calc_filter_sobel(const int16_t p11, const int16_t p12, const int16_t p13,
										const int16_t p21, const int16_t p22, const int16_t p23,
										const int16_t p31, const int16_t p32, const int16_t p33) {
	
	int16_t sobel_y, sobel_x;
	
	sobel_x = (-1)*p11 + p13 - 2*p21 + 2*p23 - p31 + p33;
	if(sobel_x < 0) {
		sobel_x = sobel_x * -1;
	}
	
	sobel_y = (-1)*p11 - 2*p12 - p13 + p31 + 2*p32 + p33;
	if(sobel_y < 0) {
		sobel_y = sobel_y * -1;
	}
	
	return (sobel_x+sobel_y);
}


/**
 * Filter pixel at given image by applying NxN kernel. Each pixel at the image is a 32 bit signed value. The step size
 * defines the offset to the next pixel. E. g. a step size of 3 means the image uses 3 32-bit channels.
 * The image and the kernel uses fixed-point values. Therfore a shifting factor is used.
 * 
 * NOTE
 * The kernel heigth and width must be an odd value (e. g. 3x5 is accepted). This function doesn't make any sanity checks
 * due to performance reason. 
 * 
 * @param img start image address
 * @param height image height in number of pixels
 * @param width image width in number of pixels
 * @param step_size step size
 * @param coord_y y pixel coordinate to apply kernel
 * @param coord_x x pixel coordinate to apply kernel
 * @param a pointer to filter kernel (the kernel values must be shifted correctly)
 * @param kernel_height kernel height in number of pixels
 * @param kernel_width kernel width in number of pixels
 * @param shift_fact shifting factor
 * @return filtered pixel value
 */
static int16_t calc_filterNxN(const int16_t *img, const int height, const int width, const int step_size,
							  const int coord_y, const int coord_x, const int16_t *a, const int kernel_height, const int kernel_width,
							  const int shift_fact) {
								
	int y, x, y_start, y_end, x_start, x_end;
	int index_img, index_kernel;
	int64_t out;
	
	y_start = coord_y-kernel_height/2;
	y_end = y_start+kernel_height;
	x_start = coord_x-kernel_width/2;
	x_end = x_start+kernel_width;
	
	index_kernel = 0;
	out = 0;
	for(y = y_start; y < y_end; y++) {
		index_img = (y*width + x_start)*step_size;
		for(x = x_start; x < x_end; x++) {
			out += a[index_kernel]*img[index_img];
			index_img += step_size;
			index_kernel++;
		}
	}
	return (out>>shift_fact);
}


/**
 * Apply sobel filter (edge detection) on given input image of type: 3 channels, 16 bit signed fixed-point per channel
 * 
 * @param img_sobel On return: sobel filtered image
 * @param img_in input image to be filtered
 * @param height image height in number of pixels
 * @param width image width in number of pixels
 * @param skip_ch1 set to 0 if channel 1 should be filtered
 * @param skip_ch2 set to 0 if channel 2 should be filtered
 * @param skip_ch3 set to 0 if channel 3 should be filtered
 */
void filter_sobel_3s16(int16_t *img_sobel, const int16_t *img_in, const int height, const int width,
					   const int skip_ch1, const int skip_ch2, const int skip_ch3) {
	
	int y, x, index_upper, index_center, index_lower, index_left, index_right;
	
	for(y = 1; y < (height-1); y++) {
		
		index_upper = (y-1)*width*3;
		index_center = y*width*3;
		index_lower = (y+1)*width*3;
		
		
		for(x = 1; x < (width-1); x++) {
			
			if(!skip_ch1) {
				img_sobel[index_center+3] = calc_filter_sobel(	img_in[index_upper], img_in[index_upper+3], img_in[index_upper+6],
																img_in[index_center], img_in[index_center+3], img_in[index_center+6],
																img_in[index_lower], img_in[index_lower+3], img_in[index_lower+6]);
			}
			else {
				img_sobel[index_center+3] = 0;
			}
			
			index_upper++;
			index_center++;
			index_lower++;
			
			if(!skip_ch2) {
				img_sobel[index_center+3] = calc_filter_sobel(	img_in[index_upper], img_in[index_upper+3], img_in[index_upper+6],
																img_in[index_center], img_in[index_center+3], img_in[index_center+6],
																img_in[index_lower], img_in[index_lower+3], img_in[index_lower+6]);
			}
			else {
				img_sobel[index_center+3] = 0;
			}
			
			index_upper++;
			index_center++;
			index_lower++;
			
			if(!skip_ch3) {
				img_sobel[index_center+3] = calc_filter_sobel(	img_in[index_upper], img_in[index_upper+3], img_in[index_upper+6],
																img_in[index_center], img_in[index_center+3], img_in[index_center+6],
																img_in[index_lower], img_in[index_lower+3], img_in[index_lower+6]);
			}
			else {
				img_sobel[index_center+3] = 0;
			}
			
			index_upper++;
			index_center++;
			index_lower++;
		}
	}
	
	
	/*
	 * Image border are set to 0
	 */
	index_upper = 0;
	index_lower = (height-1)*width*3;
	for(x = 0; x < width; x++) {
		
		// horizontal upper border
		img_sobel[index_upper] = 0;
		img_sobel[index_upper+1] = 0;
		img_sobel[index_upper+2] = 0;
		index_upper += 3;
		
		// horizontal lower border
		img_sobel[index_lower] = 0;
		img_sobel[index_lower+1] = 0;
		img_sobel[index_lower+2] = 0;
		index_lower += 3;
	}
	
	index_left = 0;
	index_right = width*3-3;
	for(y = 0; y < height; y++) {
		
		// verticel left border
		img_sobel[index_left] = 0;;
		img_sobel[index_left+1] = 0;
		img_sobel[index_left+2] = 0;
		index_left += width*3;
		
		// verticel right border
		img_sobel[index_right] = 0;;
		img_sobel[index_right+1] = 0;
		img_sobel[index_right+2] = 0;
		index_right += width*3;
	}
}


/**
 * Apply gauss low-pass filter on given input image of type: 3 channels, 16 bit signed fixed-point per channel.
 * An odd kernel size is expected.
 * 
 * @param img_gauss On return: gauss low-pass filtered image
 * @param img_in input image to be filtered
 * @param height image height in number of pixels
 * @param width image width in number of pixels
 * @param kernel_size filter kernel size (minimum of 3, maximum see @ref MAX_GAUSS_KERNEL_SIZE)
 * @param spread gaussian curve spreading value, as higher as bigger spread (spread = sigma)
 * @param skip_ch1 set to 0 if channel 1 should be filtered
 * @param skip_ch2 set to 0 if channel 2 should be filtered
 * @param skip_ch3 set to 0 if channel 3 should be filtered
 */
void filter_gauss_3s16(int16_t *img_gauss, const int16_t *img_in, const int height, const int width, int kernel_size, const float spread,
					   const int skip_ch1, const int skip_ch2, const int skip_ch3) {
	
	int y, x, index, index_upper, index_center, index_lower;
	int kernel_start, kernel_end;
	float a[MAX_GAUSS_KERNEL_SIZE*MAX_GAUSS_KERNEL_SIZE];
	int16_t a_s16[MAX_GAUSS_KERNEL_SIZE*MAX_GAUSS_KERNEL_SIZE];
	float sum;
	int x_start, x_end, y_start, y_end;
	const int shift_fact = 10;		// 2^10 = 1024--> about 3 digits
	
	
	if((kernel_size%2) == 0) {
		// even kernel size!!
		kernel_size++;
	}
	
	if(kernel_size < 3) {
		kernel_size = 3;
	}
	else if(kernel_size > MAX_GAUSS_KERNEL_SIZE) {
		kernel_size = MAX_GAUSS_KERNEL_SIZE;
	}
	
	
	/*
	 * Generate 2D-gaussian kernel depending on given size.
	 * Norm kernel, that sum over each kernel element is 1.0.
	 */
	kernel_start = -1*kernel_size/2;
	kernel_end = -1*kernel_start;
	index = 0;
	sum = 0;
	for(y = kernel_start; y <= kernel_end; y++) {
		for(x = kernel_start; x <= kernel_end; x++) {
			a[index] = (1.0/(2*M_PI*spread*spread))*expf((-1)*((x*x+y*y)/(2*spread*spread)));
			sum += a[index];
			index++;
		}
	}
	index = 0;
	for(y = 0; y < kernel_size; y++) {
		for(x = 0; x < kernel_size; x++) {
			a[index] /= sum;
			a_s16[index] = (int16_t)roundf(a[index] * (1<<shift_fact));
// 			printf("XXX a[%d,%d] = %.20f --> %d\n", y, x, a[index], a_s16[index]);
			index++;
		}
	}
	
	
	/*
	 * This loop filters the image without border region.
	 */
	y_start = kernel_size/2;
	y_end = height-y_start;
	x_start = kernel_size/2;
	x_end = width-x_start;
	for(y = y_start; y < y_end; y++) {
		
		index = (y*width+x_start)*3;
		index_upper = (y-1)*width*3;
		index_center = y*width*3;
		index_lower = (y+1)*width*3;
		
		for(x = x_start; x < x_end; x++) {
			
			if(!skip_ch1) {
				if(kernel_size == 3) {
					img_gauss[index_center] = calc_filter3x3(	a_s16[0], a_s16[1], a_s16[2], a_s16[3], a_s16[4], a_s16[5], a_s16[6], a_s16[7], a_s16[8],
																img_in[index_upper], img_in[index_upper+3], img_in[index_upper+6],
																img_in[index_center], img_in[index_center+3], img_in[index_center+6],
																img_in[index_lower], img_in[index_lower+3], img_in[index_lower+6],
																shift_fact);
				}
				else {
					img_gauss[index] = calc_filterNxN(img_in, height, width, 3, y, x, a_s16, kernel_size, kernel_size, shift_fact);
				}
			}
			else {
				img_gauss[index] = 0;
			}
			
			index++;
			index_upper++;
			index_center++;
			index_lower++;
			
			if(!skip_ch2) {
				if(kernel_size == 3) {
					img_gauss[index_center] = calc_filter3x3(	a_s16[0], a_s16[1], a_s16[2], a_s16[3], a_s16[4], a_s16[5], a_s16[6], a_s16[7], a_s16[8],
																img_in[index_upper], img_in[index_upper+3], img_in[index_upper+6],
																img_in[index_center], img_in[index_center+3], img_in[index_center+6],
																img_in[index_lower], img_in[index_lower+3], img_in[index_lower+6],
																shift_fact);
				}
				else {
					img_gauss[index] = calc_filterNxN(img_in+1, height, width, 3, y, x, a_s16, kernel_size, kernel_size, shift_fact);
				}
			}
			else {
				img_gauss[index] = 0;
			}
			
			index++;
			index_upper++;
			index_center++;
			index_lower++;
			
			if(!skip_ch3) {
				if(kernel_size == 3) {
					img_gauss[index_center] = calc_filter3x3(	a_s16[0], a_s16[1], a_s16[2], a_s16[3], a_s16[4], a_s16[5], a_s16[6], a_s16[7], a_s16[8],
																img_in[index_upper], img_in[index_upper+3], img_in[index_upper+6],
																img_in[index_center], img_in[index_center+3], img_in[index_center+6],
																img_in[index_lower], img_in[index_lower+3], img_in[index_lower+6],
																shift_fact);
				}
				else {
					img_gauss[index] = calc_filterNxN(img_in+2, height, width, 3, y, x, a_s16, kernel_size, kernel_size, shift_fact);
				}
			}
			else {
				img_gauss[index] = 0;
			}
			
			index++;
			index_upper++;
			index_center++;
			index_lower++;
		}
	}
	
	
	/*
	 * Image border are not filtered
	 */
	// handler upper horizontal border area
	index = 0;
	for(y = 0; y < y_start; y++) {
		for(x = 0; x < width; x++) {
			img_gauss[index] = img_in[index];
			img_gauss[index+1] = img_in[index+1];
			img_gauss[index+2] = img_in[index+2];
			index += 3;
		}
	}
	
	// handler lower horizontal border area
	index = y_end*width*3;
	for(y = y_end; y < height; y++) {
		for(x = 0; x < width; x++) {
			img_gauss[index] = img_in[index];
			img_gauss[index+1] = img_in[index+1];
			img_gauss[index+2] = img_in[index+2];
			index += 3;
		}
	}
	
	// handler left vertical border area
	for(y = 0; y < height; y++) {
		index = y*width*3;
		for(x = 0; x < x_start; x++) {
			img_gauss[index] = img_in[index];
			img_gauss[index+1] = img_in[index+1];
			img_gauss[index+2] = img_in[index+2];
			index += 3;
		}
	}
	
	// handler right vertical border area
	for(y = 0; y < height; y++) {
		index = (y*width+x_end)*3;
		for(x = x_end; x < width; x++) {
			img_gauss[index] = img_in[index];
			img_gauss[index+1] = img_in[index+1];
			img_gauss[index+2] = img_in[index+2];
			index += 3;
		}
	}
}


/**
 * Generate binary image from given image of type: 3 channels, 16 bit signed fixed-point per channel.
 * All pixel values above the given threshold are set to the defined value. Otherwise the values are set to 0.
 * 
 * @param img_gauss On return: gauss low-pass filtered image
 * @param img_in input image to be filtered
 * @param height image height in number of pixels
 * @param width image width in number of pixels
 * @param threshold binary threshold
 * @param bin_value Pixel values above given threshold are set to this value. Values below the threshold are set to 0.
 * @param skip_ch1 set to 0 if channel 1 should be filtered
 * @param skip_ch2 set to 0 if channel 2 should be filtered
 * @param skip_ch3 set to 0 if channel 3 should be filtered
 */
void filter_binary_3s16(int8_t *img_bin, const int16_t *img_in, const int height, const int width, const int16_t threshold, const int8_t bin_value,
						const int skip_ch1, const int skip_ch2, const int skip_ch3) {
	
	int y, x, index;
	
	index = 0;
		
	for(y = 0; y < height; y++) {
		for(x = 0; x < width; x++) {
			if(!skip_ch1) {
				if(img_in[index] >= threshold) {
					img_bin[index] = bin_value;
				}
				else {
					img_bin[index] = 0;
				}
			}
			else {
				img_bin[index] = 0;
			}
			
			index++;
			
			if(!skip_ch2) {
				if(img_in[index] >= threshold) {
					img_bin[index] = bin_value;
				}
				else {
					img_bin[index] = 0;
				}
			}
			else {
				img_bin[index] = 0;
			}
			
			index++;
			
			if(!skip_ch3) {
				if(img_in[index] >= threshold) {
					img_bin[index] = bin_value;
				}
				else {
					img_bin[index] = 0;
				}
			}
			else {
				img_bin[index] = 0;
			}
			
			index++;
		}
	}
	
}