/**
* @file			projection.c
* @brief		projective transformation
* @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 <math.h>

#include "color_pipe_private.h"


/**
 * Pixel value interpolation of RGB image (8 bit per color channel).
 * If a pixel coordinate with a fraction part is of interest, do interpolate the correct value from their neighbor's pixel.
 * 
 * E. g. the pixel coordinate x/y = 1.8/2.3 gives the following weights:
 *          +------+------+
 *          |      |      |
 *          | 14%  | 56%  |		14% = 20%*70%, 56% = 80%*70%
 *          |      |      |
 *          +------+------+
 *          |      |      |
 *          | 6%   | 24%  |		6% = 20%*30%, 24% = 80%*30%
 *          |      |      |
 *          +------+------+
 * 
 * The weights are applied to the neighors and the resulting pixel value is saved at the given location.
 * 
 * NOTE
 * The input and output image must have the same pixel size.
 * 
 * @param img_out On return: image with interpolated values
 * @param x saved interpolated pixel value at this x-coordinate
 * @param y saved interpolated pixel value at this y-coordinate
 * @param height image height of input and output image in number of pixels
 * @param width image width of input and output image in number of pixels
 * @param img_in input image to interpolate pixel values
 * @param coord_x x-coordinate to interpolate
 * @param coord_y y-coordinate to interpolate
 * @param scale_fact coordinates are scaled by this factor
 */
static void interpolate_rgb8_scalar(uint8_t *img_out, const int x, const int y, const int height, const int width,
						 const uint8_t *img_in, const int coord_x, const int coord_y, const int scale_fact)
#include "alg_interpolate_rgb_scalar.h"


/**
 * Pixel value interpolation of RGB image (16 bit per color channel).
 * If a pixel coordinate with a fraction part is of interest, do interpolate the correct value from their neighbor's pixel.
 * 
 * E. g. the pixel coordinate x/y = 1.8/2.3 gives the following weights:
 *          +------+------+
 *          |      |      |
 *          | 14%  | 56%  |		14% = 20%*70%, 56% = 80%*70%
 *          |      |      |
 *          +------+------+
 *          |      |      |
 *          | 6%   | 24%  |		6% = 20%*30%, 24% = 80%*30%
 *          |      |      |
 *          +------+------+
 * 
 * The weights are applied to the neighors and the resulting pixel value is saved at the given location.
 * 
 * NOTE
 * The input and output image must have the same pixel size.
 * 
 * @param img_out On return: image with interpolated values
 * @param x saved interpolated pixel value at this x-coordinate
 * @param y saved interpolated pixel value at this y-coordinate
 * @param height image height of input and output image in number of pixels
 * @param width image width of input and output image in number of pixels
 * @param img_in input image to interpolate pixel values
 * @param coord_x x-coordinate to interpolate
 * @param coord_y y-coordinate to interpolate
 * @param scale_fact coordinates are scaled by this factor
 */
static void interpolate_rgb16_scalar(uint16_t *img_out, const int x, const int y, const int height, const int width,
						  const uint16_t *img_in, const int coord_x, const int coord_y, const int scale_fact)
#include "alg_interpolate_rgb_scalar.h"


/**
 * Pixel value interpolation of monochrome image (8 bit per pixel).
 * If a pixel coordinate with a fraction part is of interest, do interpolate the correct value from their neighbor's pixel.
 * 
 * E. g. the pixel coordinate x/y = 1.8/2.3 gives the following weights:
 *          +------+------+
 *          |      |      |
 *          | 14%  | 56%  |		14% = 20%*70%, 56% = 80%*70%
 *          |      |      |
 *          +------+------+
 *          |      |      |
 *          | 6%   | 24%  |		6% = 20%*30%, 24% = 80%*30%
 *          |      |      |
 *          +------+------+
 * 
 * The weights are applied to the neighors and the resulting pixel value is saved at the given location.
 * 
 * NOTE
 * The input and output image must have the same pixel size.
 * 
 * @param img_out On return: image with interpolated values
 * @param x saved interpolated pixel value at this x-coordinate
 * @param y saved interpolated pixel value at this y-coordinate
 * @param height image height of input and output image in number of pixels
 * @param width image width of input and output image in number of pixels
 * @param img_in input image to interpolate pixel values
 * @param coord_x x-coordinate to interpolate
 * @param coord_y y-coordinate to interpolate
 * @param scale_fact coordinates are scaled by this factor
 */
static void interpolate_mono8_scalar(uint8_t *img_out, const int x, const int y, const int height, const int width,
						 const uint8_t *img_in, const int coord_x, const int coord_y, const int scale_fact)
#include "alg_interpolate_mono_scalar.h"


/**
 * Pixel value interpolation of monochrome image (16 bit per pixel).
 * If a pixel coordinate with a fraction part is of interest, do interpolate the correct value from their neighbor's pixel.
 * 
 * E. g. the pixel coordinate x/y = 1.8/2.3 gives the following weights:
 *          +------+------+
 *          |      |      |
 *          | 14%  | 56%  |		14% = 20%*70%, 56% = 80%*70%
 *          |      |      |
 *          +------+------+
 *          |      |      |
 *          | 6%   | 24%  |		6% = 20%*30%, 24% = 80%*30%
 *          |      |      |
 *          +------+------+
 * 
 * The weights are applied to the neighors and the resulting pixel value is saved at the given location.
 * 
 * NOTE
 * The input and output image must have the same pixel size.
 * 
 * @param img_out On return: image with interpolated values
 * @param x saved interpolated pixel value at this x-coordinate
 * @param y saved interpolated pixel value at this y-coordinate
 * @param height image height of input and output image in number of pixels
 * @param width image width of input and output image in number of pixels
 * @param img_in input image to interpolate pixel values
 * @param coord_x x-coordinate to interpolate
 * @param coord_y y-coordinate to interpolate
 * @param scale_fact coordinates are scaled by this factor
 */
static void interpolate_mono16_scalar(uint16_t *img_out, const int x, const int y, const int height, const int width,
						  const uint16_t *img_in, const int coord_x, const int coord_y, const int scale_fact)
#include "alg_interpolate_mono_scalar.h"


/**
 * Apply projective transformaion.
 * 
 * @param data projective transformation data
 */ 
static void apply_projection(struct projection_data_t *data) {
	int x, y, x_corr, y_corr;
	const int width = data->width;
	const int height = data->height;
	int bit_channel =  data->bit_channel;
	struct coord_t *map = data->map;
	void *img_calib = data->img_out;
	void *img_uncalib = data->img_in;
	const int scale_fact = data->map_scale_fact;
	const int is_color = data->is_color;
	
	for(y = 0; y < height; y++) {
		for(x = 0; x < width; x++) {
			x_corr = map->x;
			y_corr = map->y;
			map++;
			
			if(bit_channel <= 8) {
				if(is_color) {
					interpolate_rgb8_scalar(img_calib, x, y, height, width, img_uncalib, x_corr, y_corr, scale_fact);
				}
				else {
					interpolate_mono8_scalar(img_calib, x, y, height, width, img_uncalib, x_corr, y_corr, scale_fact);
				}
			}
			else if(bit_channel <= 16) {
				if(is_color) {
					interpolate_rgb16_scalar(img_calib, x, y, height, width, img_uncalib, x_corr, y_corr, scale_fact);
				}
				else {
					interpolate_mono16_scalar(img_calib, x, y, height, width, img_uncalib, x_corr, y_corr, scale_fact);
				}
			}
		}
	}
}


/**
 * Initialize perspective transformation map.
 * 
 * @param data projective transformation data
 */ 
static void init_map(struct projection_data_t *data) {
	int x, y;
	struct coord_t *map = data->map;
	const int scale_fact = (1 << (data->map_scale_fact));
	const int width = data->width;
	const int height = data->height;
	float x_corr, y_corr;
	const float c00 = data->c_inv[0][0];
	const float c01 = data->c_inv[0][1];
	const float c02 = data->c_inv[0][2];
	const float c10 = data->c_inv[1][0];
	const float c11 = data->c_inv[1][1];
	const float c12 = data->c_inv[1][2];
	const float c20 = data->c_inv[2][0];
	const float c21 = data->c_inv[2][1];
	const float c22 = data->c_inv[2][2];
	
	for(y = 0; y < height; y++) {
		for(x = 0; x < width; x++) {
			x_corr = (c00*x+c01*y+c02)/(c20*x+c21*y+c22);
			y_corr = (c10*x+c11*y+c12)/(c20*x+c21*y+c22);
			
			// apply scaling factor
			map->x = (int)roundf(x_corr*scale_fact);
			map->y = (int)roundf(y_corr*scale_fact);
			map++;
		}
	}
}


/**
 * Apply projective transformation.
 * 
 * @param data required data for projective transformation
 * @return 0 on success otherwise -1
 */
int projection(struct projection_data_t *data) {
	
	/*
	 * Create projective transformation map if needed.
	 */ 
	if(data->map_init == 0) {
		data->map_scale_fact = 6;	// scale by 9 means 2^6 = 64
		init_map(data);
		data->map_init = 1;
	}
	
	apply_projection(data);
	return 0;
}