#include "Raw_Processor_implC.h"

/*
void mul_matrix_01(const mul_matrix_function_variable *variable){
	int pos_row, comp_pointer, end_pointer;
	float fcomp0, fcomp1, fcomp2;
	const float *matrix = variable->matrix;
	int icomp0, icomp1, icomp2;
	unsigned short *scomp0 = variable->comp0;
	unsigned short *scomp1 = variable->comp1;
	unsigned short *scomp2 = variable->comp2;


	for(pos_row = variable->start_row; pos_row < variable->number_row; pos_row++){
		comp_pointer 	= pos_row * variable->linestride + variable->start_col;
		end_pointer		=  comp_pointer + variable->linestride;
		for(;comp_pointer < end_pointer; comp_pointer ++){
			fcomp0 = scomp0[comp_pointer];
			fcomp1 = scomp1[comp_pointer];
			fcomp2 = scomp2[comp_pointer];

			icomp0 = (int)(fcomp0 * matrix[0] + fcomp1 * matrix[1] + fcomp2 * matrix[2]);
			icomp1 = (int)(fcomp0 * matrix[3] + fcomp1 * matrix[4] + fcomp2 * matrix[5]);
			icomp2 = (int)(fcomp0 * matrix[6] + fcomp1 * matrix[7] + fcomp2 * matrix[8]);

			scomp0[comp_pointer] = (unsigned short) CROP(icomp0);
			scomp1[comp_pointer] = (unsigned short) CROP(icomp1);
			scomp2[comp_pointer] = (unsigned short) CROP(icomp2);
		}
	}
}
*/

inline unsigned short crop(float in){
	if(in <= 0xFFFF){
		if(in < 0){
			return 0;
		}else{
			return (unsigned short) in;
		}
	}else{
		return 0xFFFF;
	}
	return 0;
}

void mul_matrix_01(const mul_matrix_function_variable *variable){
	int pos_row;
	float fcomp0, fcomp1, fcomp2;
	const float *matrix = variable->matrix;
	//unsigned int icomp0, icomp1, icomp2;
	unsigned short *scomp0, *scomp0_end;
	unsigned short *scomp1;
	unsigned short *scomp2;


	for(pos_row = variable->start_row; pos_row < variable->number_row; pos_row++){
		scomp0 = &(variable->comp0[pos_row * variable->linestride + variable->start_col]);
		scomp1 = &(variable->comp1[pos_row * variable->linestride + variable->start_col]);
		scomp2 = &(variable->comp2[pos_row * variable->linestride + variable->start_col]);

		scomp0_end = scomp0 + variable->number_col;
		for(;scomp0 < scomp0_end; scomp0 += 1, scomp1 += 1, scomp2 += 1){
			fcomp0 = *scomp0;
			fcomp1 = *scomp1;
			fcomp2 = *scomp2;

			*scomp0 = crop(fcomp0 * matrix[0] + fcomp1 * matrix[1] + fcomp2 * matrix[2]);
			*scomp1	= crop(fcomp0 * matrix[3] + fcomp1 * matrix[4] + fcomp2 * matrix[5]);
			*scomp2 = crop(fcomp0 * matrix[6] + fcomp1 * matrix[7] + fcomp2 * matrix[8]);

			/*
			icomp0 = (unsigned int)(fcomp0 * matrix[0] + fcomp1 * matrix[1] + fcomp2 * matrix[2]);
			icomp1 = (unsigned int)(fcomp0 * matrix[3] + fcomp1 * matrix[4] + fcomp2 * matrix[5]);
			icomp2 = (unsigned int)(fcomp0 * matrix[6] + fcomp1 * matrix[7] + fcomp2 * matrix[8]);


			*scomp0 = (unsigned short) CROP(icomp0);
			*scomp1 = (unsigned short) CROP(icomp1);
			*scomp2 = (unsigned short) CROP(icomp2);
*/

		}
	}
}
