/**
* @file			alg_interpolate_rgb_scalar.h
* @brief		RGB pixel interpolation (scalar code)
* @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>
*
*/


/*
 * This code inplements an RGB pixel interpolator and is independent of the pixel-bit-depth. Including this code at the
 * C-Source file will define the pixel-bit-depth (see camera_calib.c).
 */
// 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)
// 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)
{
	int x_start, y_start, x_end, y_end, index_start, index_end;
	int wheight_x, wheight_y, a11, a12, a21, a22;
	int red, green, blue;
	int index;
	const int max_val = (1<<scale_fact);
	
	
	// calculate pixel index of destination image (calibrated image)
	index = ((y*width)+x)*3;
	
	if((coord_x>>scale_fact) > width || (coord_y>>scale_fact) > height || coord_x < 0 || coord_y < 0) {
		// out of range --> return black value
		img_out[index] = 0;
		img_out[index+1] = 0;
		img_out[index+2] = 0;
		return;
	}
	
	red = 0;
	green = 0;
	blue = 0;
	
	x_start = coord_x>>scale_fact;
	y_start = coord_y>>scale_fact;
	x_end = x_start + 1;
	y_end = y_start + 1;
	index_start = (y_start*width + x_start)*3;
	index_end = (y_end*width + x_end)*3;
	
	// calculate wheights
	wheight_x = coord_x % max_val;
	wheight_y = coord_y % max_val;
	a11 = (max_val - wheight_x)*(max_val - wheight_y);
	a12 = wheight_x*(max_val - wheight_y);
	a21 = (max_val - wheight_x)*wheight_y;
	a22 = wheight_x*wheight_y;
	
	/*
	 * handle border region separately
	 */
	if(x_end < width || y_end < height) {
		// pixels are not lying on border region
		red = 		img_in[index_start]*a11 +
					img_in[index_start+3]*a12 +
					img_in[index_end-3]*a21 +
					img_in[index_end]*a22;
					
		green =		img_in[index_start+1]*a11 +
					img_in[index_start+1+3]*a12 +
					img_in[index_end+1-3]*a21 +
					img_in[index_end+1]*a22;
					
		blue =		img_in[index_start+2]*a11 +
					img_in[index_start+2+3]*a12 +
					img_in[index_end+2-3]*a21 +
					img_in[index_end+2]*a22;
	}
	
	img_out[index] = red >> (2*scale_fact);
	img_out[index+1] = green >> (2*scale_fact);
	img_out[index+2] = blue >> (2*scale_fact);
}