/**
* @file			color_space.c
* @brief		color space conversion utilities
* @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>

#if (WITH_SIMD == 1)
#include <immintrin.h>		// see /usr/lib64/gcc/x86_64-suse-linux/4.7/include/immintrin.h
#endif // WITH_SIMD

#include "color.h"
#include "color_pipe_private.h"


/**
 * RGB image (16 bit per color channel) to YUV color space conversion (scalar code).
 * 
 * @param img_yuv On return: image in YUV color space (this buffer must be allocated externly)
 * @param img_rgb RGB image
 * @param height image height in number of pixels
 * @param width image width in number of pixels
 */
static void rgb16_to_yuv_scalar(int16_t *img_yuv, const uint16_t *img_rgb, const int height, const int width)
#include "alg_rgb_to_yuv.h"


#ifdef __SSSE3__
/**
 * RGB image (8 bit per color channel) to YUV color space conversion (vector code).
 * 
 * @param img_yuv On return: image in YUV color space (this buffer must be allocated externly)
 * @param img_rgb RGB image
 * @param height image height in number of pixels
 * @param width image width in number of pixels
 */
static void rgb8_to_yuv_vector(int16_t *img_yuv, const uint8_t *img_rgb, const int height, const int width) {
	int i, size_mod8;
	uint8_t *input_img;
	int px_blue, px_green, px_red;
	int px_y, px_u, px_v;
	
	const int scale_fact = 7;		// more than 7 is not possible due to overflow
	
	const int y_r = (int)roundf(RGB2YUV_COEFF_Y_RED*(1<<scale_fact));
	const int y_g = (int)roundf(RGB2YUV_COEFF_Y_GREEN*(1<<scale_fact));
	const int y_b = (int)roundf(RGB2YUV_COEFF_Y_BLUE*(1<<scale_fact));
	
	const int u_r = (int)roundf(RGB2YUV_COEFF_U_RED*(1<<scale_fact));
	const int u_g = (int)roundf(RGB2YUV_COEFF_U_GREEN*(1<<scale_fact));
	const int u_b = (int)roundf(RGB2YUV_COEFF_U_BLUE*(1<<scale_fact));
	
	const int v_r = (int)roundf(RGB2YUV_COEFF_V_RED*(1<<scale_fact));
	const int v_g = (int)roundf(RGB2YUV_COEFF_V_GREEN*(1<<scale_fact));
	const int v_b = (int)roundf(RGB2YUV_COEFF_V_BLUE*(1<<scale_fact));
	
	__m128i px_buf1, px_buf2;
	__m128i mask_red1, mask_red2, red1, red2, red;
	__m128i mask_green1, mask_green2, green1, green2, green;
	__m128i mask_blue1, mask_blue2, blue1, blue2, blue;
	
	__m128i ch_y, coeff_y_red, coeff_y_green, coeff_y_blue;
	__m128i ch_u, coeff_u_red, coeff_u_green, coeff_u_blue;
	__m128i ch_v, coeff_v_red, coeff_v_green, coeff_v_blue;
	__m128i red_tmp, green_tmp, blue_tmp;
	
	__m128i mask_y1, mask_y2, mask_y3, y1, y2, y3;
	__m128i mask_u1, mask_u2, mask_u3, u1, u2, u3;
	__m128i mask_v1, mask_v2, mask_v3, v1, v2, v3;
	
	__m128i yuv1, yuv2, yuv3;
	
	
	input_img = (uint8_t*)img_rgb;
	size_mod8 = (width*height/8)*8;				// image size must be divisible by 8 because 8 pixel are processed in parallel
	
	mask_red1 = _mm_set_epi8(-1, -1, -1, -1, -1, 15, -1, 12, -1, 9, -1, 6, -1, 3, -1, 0);
	mask_red2 = _mm_set_epi8(-1, 5, -1, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1);
	
	mask_green1 = _mm_set_epi8(-1, -1, -1, -1, -1, -1, -1, 13, -1, 10, -1, 7, -1, 4, -1, 1);
	mask_green2 = _mm_set_epi8(-1, 6, -1, 3, -1, 0, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1);
	
	mask_blue1 = _mm_set_epi8(-1, -1, -1, -1, -1, -1, -1, 14, -1, 11, -1, 8, -1, 5, -1, 2);
	mask_blue2 = _mm_set_epi8(-1, 7, -1, 4, -1, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1);
	
	mask_y1 = _mm_set_epi8(-1, -1, 5, 4, -1, -1, -1, -1, 3, 2, -1, -1, -1, -1, 1, 0);
	mask_y2 = _mm_set_epi8(11, 10, -1, -1, -1, -1, 9, 8, -1, -1, -1, -1, 7, 6, -1, -1);
	mask_y3 = _mm_set_epi8(-1, -1, -1, -1, 15, 14, -1, -1, -1, -1, 13, 12, -1, -1, -1, -1);
	
	mask_u1 = _mm_set_epi8(5, 4, -1, -1, -1, -1, 3, 2, -1, -1, -1, -1, 1, 0, -1, -1);
	mask_u2 = _mm_set_epi8(-1, -1, -1, -1, 9, 8, -1, -1, -1, -1, 7, 6, -1, -1, -1, -1);
	mask_u3 = _mm_set_epi8(-1, -1, 15, 14, -1, -1, -1, -1, 13, 12, -1, -1, -1, -1, 11, 10);
	
	mask_v1 = _mm_set_epi8(-1, -1, -1, -1, 3, 2, -1, -1, -1, -1, 1, 0, -1, -1, -1, -1);
	mask_v2 = _mm_set_epi8(-1, -1, 9, 8, -1, -1, -1, -1, 7, 6, -1, -1, -1, -1, 5, 4);
	mask_v3 = _mm_set_epi8(15, 14, -1, -1, -1, -1, 13, 12, -1, -1, -1, -1, 11, 10, -1, -1);
	
	
	/*
	 * RGB -> YUV transformation coefficient
	 * The values are scaled defined by scale_fact.
	 * 
	 * y = 0.299*red + 0.587*green + 0.114*blue
	 * u = -0.299*red - 0.587*green + 0.886*blue
	 * v = 0.701*red - 0.587*green - 0.114*blue
	 */
	coeff_y_red = _mm_set1_epi16((short)y_r);
	coeff_y_green = _mm_set1_epi16((short)y_g);
	coeff_y_blue = _mm_set1_epi16((short)y_b);
	
	coeff_u_red = _mm_set1_epi16((short)u_r);
	coeff_u_green = _mm_set1_epi16((short)u_g);
	coeff_u_blue = _mm_set1_epi16((short)u_b);
	
	coeff_v_red = _mm_set1_epi16((short)v_r);
	coeff_v_green = _mm_set1_epi16((short)v_g);
	coeff_v_blue = _mm_set1_epi16((short)v_b);
	
	
	// process 8 RGB-pixel-pair in parallel
	for(i = 0; i < size_mod8; i+=8) {
			
		// load 128 bit pixel value into SIMD register
		px_buf1 = _mm_lddqu_si128((__m128i*)(input_img));
		input_img += 16;
		px_buf2 = _mm_lddqu_si128((__m128i*)(input_img));
		input_img += 8;
		
		// get first 6 red pixels
		red1 = _mm_shuffle_epi8(px_buf1, mask_red1);
		
		// get next 2 red pixels
		red2 = _mm_shuffle_epi8(px_buf2, mask_red2);
		
		// combine to 8 red pixels
		red = _mm_or_si128(red1, red2);
		
		
		// get first 5 green pixels
		green1 = _mm_shuffle_epi8(px_buf1, mask_green1);
		
		// get next 3 green pixels
		green2 = _mm_shuffle_epi8(px_buf2, mask_green2);
		
		// combine to 8 green pixels
		green = _mm_or_si128(green1, green2);
		
		
		// get first 5 blue pixels
		blue1 = _mm_shuffle_epi8(px_buf1, mask_blue1);
		
		// get next 3 blue pixels
		blue2 = _mm_shuffle_epi8(px_buf2, mask_blue2);
		
		// combine to 8 blue pixels
		blue = _mm_or_si128(blue1, blue2);
		
		
		/*
		 * calculate 8 Y-channel pixels
		 */
		red_tmp = _mm_mullo_epi16(coeff_y_red, red);
		green_tmp = _mm_mullo_epi16(coeff_y_green, green);
		blue_tmp = _mm_mullo_epi16(coeff_y_blue, blue);
		ch_y = _mm_add_epi16(_mm_add_epi16(red_tmp, green_tmp), blue_tmp);
		ch_y = _mm_srai_epi16(ch_y, scale_fact);
		
		/*
		 * calculate 8 U-channel pixels
		 */
		red_tmp = _mm_mullo_epi16(coeff_u_red, red);
		green_tmp = _mm_mullo_epi16(coeff_u_green, green);
		blue_tmp = _mm_mullo_epi16(coeff_u_blue, blue);
		ch_u = _mm_add_epi16(_mm_add_epi16(red_tmp, green_tmp), blue_tmp);
		ch_u = _mm_srai_epi16(ch_u, scale_fact);
		
		/*
		 * calculate 8 V-channel pixels
		 */
		red_tmp = _mm_mullo_epi16(coeff_v_red, red);
		green_tmp = _mm_mullo_epi16(coeff_v_green, green);
		blue_tmp = _mm_mullo_epi16(coeff_v_blue, blue);
		ch_v = _mm_add_epi16(_mm_add_epi16(red_tmp, green_tmp), blue_tmp);
		ch_v = _mm_srai_epi16(ch_v, scale_fact);
		
		
		/*
		 * Store separate YUV buffer to one YUV image stream
		 */
		y1 = _mm_shuffle_epi8(ch_y, mask_y1);
		y2 = _mm_shuffle_epi8(ch_y, mask_y2);
		y3 = _mm_shuffle_epi8(ch_y, mask_y3);
		
		u1 = _mm_shuffle_epi8(ch_u, mask_u1);
		u2 = _mm_shuffle_epi8(ch_u, mask_u2);
		u3 = _mm_shuffle_epi8(ch_u, mask_u3);
		
		v1 = _mm_shuffle_epi8(ch_v, mask_v1);
		v2 = _mm_shuffle_epi8(ch_v, mask_v2);
		v3 = _mm_shuffle_epi8(ch_v, mask_v3);
		
		yuv1 = _mm_or_si128(y1,  _mm_or_si128(u1, v1));
		yuv2 = _mm_or_si128(y2,  _mm_or_si128(u2, v2));
		yuv3 = _mm_or_si128(y3,  _mm_or_si128(u3, v3));
		
		/*
		 * Store 3 YUV SIMD register into memory
		 */
		_mm_store_si128((__m128i*)img_yuv, yuv1);
		img_yuv += 8;
		_mm_store_si128((__m128i*)img_yuv, yuv2);
		img_yuv += 8;
		_mm_store_si128((__m128i*)img_yuv, yuv3);
		img_yuv += 8;
	}
	
	
	for(i = size_mod8; i < (width*height); i++) {
		
		// put each RGB color on stack
		px_red = *input_img;
		input_img++;
		px_green = *input_img;
		input_img++;
		px_blue = *input_img;
		
		// color space conversion from  RGB to YUV
		px_y = (y_r*px_red + y_g*px_green + y_b*px_blue) >> scale_fact;
		px_u = (u_r*px_red + u_g*px_green + u_b*px_blue) >> scale_fact;
		px_v = (v_r*px_red + v_g*px_green + v_b*px_blue) >> scale_fact;
		
		*img_yuv = px_y;
		img_yuv++;
		*img_yuv = px_u;
		img_yuv++;
		*img_yuv = px_v;
		img_yuv++;
	}
}

#else

/**
 * RGB image (8 bit per color channel) to YUV color space conversion (scalar code).
 * 
 * @param img_yuv On return: image in YUV color space (this buffer must be allocated externly)
 * @param img_rgb RGB image
 * @param height image height in number of pixels
 * @param width image width in number of pixels
 */
static void rgb8_to_yuv_scalar(int16_t *img_yuv, const uint8_t *img_rgb, const int height, const int width)
#include "alg_rgb_to_yuv.h"

#endif // __SSSE3__


/**
 * YUV to RGB (8 bit per color channel) color space conversion.
 * 
 * @param img_rgb On return: image in RGB space (this buffer must be allocated externly)
 * @param img_yuv YUV image
 * @param height image height in number of pixels
 * @param width image width in number of pixels
 * @return 0 on success otherwise -1
 */
static void yuv_to_rgb8(uint8_t *img_rgb, const int16_t *img_yuv, const int height, const int  width, const int pix_max)
#include "alg_yuv_to_rgb.h"


/**
 * YUV to RGB (8 bit per color channel) color space conversion.
 * 
 * @param img_rgb On return: image in RGB space (this buffer must be allocated externly)
 * @param img_yuv YUV image
 * @param height image height in number of pixels
 * @param width image width in number of pixels
 * @return 0 on success otherwise -1
 */
static void yuv_to_rgb16(uint16_t *img_rgb, const int16_t *img_yuv, const int height, const int  width, const int pix_max)
#include "alg_yuv_to_rgb.h"


/**
 * Apply color correction matrix on given RGB image (8 bit per color channel).
 * 
 * @param img_calib On return: color calibrated image
 * @param img_uncalib input image to calibrate
 * @param color_bit_depth color channel bit depth (all channel have the same bit depth)
 * @param height image height in number of pixels
 * @param width image width in number of pixels
 * @param a 3x3 color correction matrix
 */
static void rgb_color_correction16(uint16_t *img_calib, const uint16_t *img_uncalib,
								   const int color_bit_depth, const int height, const int width, float a[3][3])
#include "alg_ccm.h"


/**
 * Apply color correction matrix on given RGB image (8 bit per color channel).
 * 
 * @param img_calib On return: color calibrated image
 * @param img_uncalib input image to calibrate
 * @param color_bit_depth color channel bit depth (all channel have the same bit depth)
 * @param height image height in number of pixels
 * @param width image width in number of pixels
 * @param a 3x3 color correction matrix
 */
static void rgb_color_correction8(uint8_t *img_calib, const uint8_t *img_uncalib,
								  const int color_bit_depth, const int height, const int width, float a[3][3])
#include "alg_ccm.h"


/**
 * RGB to YUV color space conversion.
 * 
 * @param img_yuv On return: image in YUV color space (this buffer must be allocated externly)
 * @param img_rgb RGB image
 * @param height image height in number of pixels
 * @param width image width in number of pixels
 * @param bit_channel bits per color channel
 * @return 0 on success otherwise -1
 */
int color_rgb_to_yuv(int16_t *img_yuv, const void *img_rgb, const int height, const int width, const int bit_channel) {
	
	if(bit_channel <= 8) {
#ifdef __SSSE3__
		rgb8_to_yuv_vector(img_yuv, img_rgb, height, width);
#else
		rgb8_to_yuv_scalar(img_yuv, img_rgb, height, width);
#endif
	}
	else if(bit_channel <= 16) {
		rgb16_to_yuv_scalar(img_yuv, img_rgb, height, width);
	}
	return 0;
}


/**
 * YUV to RGB color space conversion.
 * 
 * @param img_rgb On return: image in RGB space (this buffer must be allocated externly)
 * @param img_yuv YUV image
 * @param height image height in number of pixels
 * @param width image width in number of pixels
 * @param bit_channel bits per color channel of RGB image
 * @return 0 on success otherwise -1
 */
int color_yuv_to_rgb(void *img_rgb, const int16_t *img_yuv, const int height, const int width, const int bit_channel) {
	int ret = 0;
	int bpp = (1<<bit_channel) - 1;
	
	if(bit_channel <= 8) {
		yuv_to_rgb8(img_rgb, img_yuv, height, width, bpp);
	}
	else if(bit_channel <= 16) {
		yuv_to_rgb16(img_rgb, img_yuv, height, width, bpp);
	}
	else {
		printf("%s: Color space conversion on images with %d bits per color channel not implemented yet\n", __func__, bit_channel);
		ret = -1;
	}
	return ret;
}

	
/**
 * Apply color calibration.
 * 
 * @param color_calib required color calibration data
 * @return 0 on success otherwise -1
 */
int color_calib(struct color_calib_data_t *color_calib) {
	
	int ret, bit_channel;
	
	
	bit_channel = color_calib->bit_channel;
	ret = 0;
	
	if(bit_channel <= 8) {
		rgb_color_correction8(color_calib->img_calib, color_calib->img_in, 8, color_calib->height, color_calib->width, color_calib->a);
	}
	else if(bit_channel <= 16) {
		rgb_color_correction16(color_calib->img_calib, color_calib->img_in, 12, color_calib->height, color_calib->width, color_calib->a);
	}
	else {
		printf("%s: Color calibration not possible on images with %d bits per color channel\n", __func__, bit_channel);
		ret = -1;
	}
	return ret;
}