#include "Raw_Processor.h"
#include "TIFF_reader.h"
#include "Raw_Processor_implC.h"
#include "threadwrap.h"

#include <sys/time.h>
#include <unistd.h>

namespace MobilePhotoTool{

	const float Raw_Processor::RGBtoXYZ[9] ={	0.4124564f,  0.3575761f,  0.1804375f,
												0.2126729f,  0.7151522f,  0.0721750f,
												0.0193339f,  0.1191920f,  0.9503041f};

	/*
	const float Raw_Processor::RGBtoXYZ[9] ={	 2.36460f, -0.51515f,  0.00520f,
												-0.89653f,  1.42640f, -0.01441f,
												-0.46807f,  0.08875f,  1.00921f};
		*/
	const float Raw_Processor::failPixel = 1.f;

	Raw_Processor::Raw_Processor(MPT_Exception *exception, IFD *raw_ifd, IFD *ifd0)
	{

		init(exception, raw_ifd, ifd0);
		MPT_EXCEPTION_TEST;

		/*
		createBuffer(exception);
		MPT_EXCEPTION_TEST;

		basicPipline(exception);
		MPT_EXCEPTION_TEST;
		 */
	}

	void Raw_Processor::init(MPT_Exception *exception, IFD *raw_ifd, IFD *ifd0)
	{
		unsigned int pos, size;
		this->pixel			= NULL;
		this->linestride	= NULL;
		this->start_pointer	= NULL;
		this->histogramm	= NULL;
		this->ifd0			= ifd0;
		this->zooms			= NULL;
		this->zoomssize		= 0;
		this->zoomfactor	= 0.f;

		if(raw_ifd->is_raw(exception)){
			MPT_EXCEPTION_TEST;
			this->raw_ifd	= raw_ifd;
			this->length	= raw_ifd->getImageLength(exception);
			MPT_EXCEPTION_TEST;
			this->width		= raw_ifd->getImageWidth(exception);
			MPT_EXCEPTION_TEST;

			if(!TIFF_reader::test_width_length(exception, this->length, this->width)){
				MPT_EXCEPTION("The image size is not supported.");
			}
			size = this->length * this->width;

			this->number_components	= this->get_number_components(exception);
			MPT_EXCEPTION_TEST;

/*
			this->start_pointer		= new unsigned int[this->number_components];
			this->linestride		= new unsigned int[this->number_components];
			this->pixel				= new pixeltype*[this->number_components];
			for( pos = 0; pos < this->number_components; pos += 1){
				this->pixel[pos] = NULL;
			}

			for( pos = 0; pos < this->number_components; pos += 1){
				this->pixel[pos] = new pixeltype[size];
				for(pos2 = 0; pos2 < size; pos2 += 1){
					this->pixel[pos][pos2] = pixeltypeMIN;
				}

				this->start_pointer[pos] = 0;
				this->linestride[pos] = this->width;
			}
*/

			this->demosaic_type = BILINEAR;
			this->demosaic_type = AHD;
			this->histogramm	= new unsigned int*[this->number_components];
			for(pos = 0; pos < this->number_components; pos += 1){
				this->histogramm[pos] = NULL;
			}
			for(pos = 0; pos < this->number_components; pos += 1){
				this->histogramm[pos] = new unsigned int[this->histogramm_size + 1];
			}

			this->raw_ifd->getDefaultCropOrigin(exception, this->cropOrigin);
			MPT_EXCEPTION_TEST;
			if(	this->cropOrigin[1] != 1 ||
				this->cropOrigin[3] != 1){

				MPT_EXCEPTION("Unknown format.");
			}
			this->raw_ifd->getDefaultCropSize(exception, this->cropSize);
			if(	this->cropSize[1] != 1 ||
				this->cropSize[3] != 1){

				MPT_EXCEPTION("Unknown format.");
			}
			MPT_EXCEPTION_TEST;

			/*
			set_zoom(exception, 1.f, NULL);
			MPT_EXCEPTION_TEST;
*/

			create_CameraToXYZ(exception);
			MPT_EXCEPTION_TEST;

		}else{
			MPT_EXCEPTION("This is not a Raw ifd.");
		}
	}

	/**
	 *
	 */
	void Raw_Processor::init(MPT_Exception *exception){
		createBuffer(exception);
		MPT_EXCEPTION_TEST;

		basicPipline(exception);
		MPT_EXCEPTION_TEST;
	}


	/**
	 *
	 */
	void Raw_Processor::createBuffer(MPT_Exception *exception)
	{
		unsigned int pos, pos2, size;

		this->start_pointer		= new unsigned int[this->number_components];
		if(this->start_pointer == NULL){
			MPT_EXCEPTION("Out of memory");
		}

		this->linestride		= new unsigned int[this->number_components];
		if(this->linestride == NULL){
			MPT_EXCEPTION("Out of memory");
		}
		this->pixel				= new pixeltype*[this->number_components];
		if(this->pixel == NULL){
			MPT_EXCEPTION("Out of memory");
		}

		for( pos = 0; pos < this->number_components; pos += 1){
			this->pixel[pos] = NULL;
		}

		size = this->length * this->width;

		for( pos = 0; pos < this->number_components; pos += 1){
			this->pixel[pos] = new pixeltype[size];
			if(this->pixel[pos] == NULL){
				MPT_EXCEPTION("Out of memory");
			}

			for(pos2 = 0; pos2 < size; pos2 += 1){
				this->pixel[pos][pos2] = pixeltypeMIN;
			}

			this->start_pointer[pos] = 0;
			this->linestride[pos] = this->width;
		}
	}

	/*
	 * Calculate the a basic RGB image
	 */
	void Raw_Processor::basicPipline(MPT_Exception *exception)
	{
		struct timeval tv1, tv2;
		long long ti1, ti2, ti3;
		if(demosaic_type == BILINEAR){
			loadRaw(exception);
			start_demosaik(exception);
			MPT_EXCEPTION_TEST;
		}else if(demosaic_type == AHD){
			timerclear(&tv1);
			timerclear(&tv2);
			gettimeofday(&tv1, NULL);

			demosaik_AHD(exception);

			gettimeofday(&tv2, NULL);

			ti1 = (long long)tv1.tv_sec * 1000000 + (long long)tv1.tv_usec;
			ti2 = (long long)tv2.tv_sec * 1000000 + (long long)tv2.tv_usec;
			ti3 = ti2 - ti1;
			printf("Direkt Zeitmessung = %lld\n", ti3);
		}

		this->mul_matrix_03(this->CamtoRGB);


		//updata_histogramm(exception);
		//MPT_EXCEPTION_TEST;
		//set_white_point(exception);
		//MPT_EXCEPTION_TEST;
		setup_gammacurve(pixeltypeMAX);
		gamma_correction(exception);

		//set_data(0xFFFF);

		//this->write_ppm();
	}

/*
	Raw_Processor *Raw_Processor::create_Raw_Processor(MPT_Exception *exception, IFD *raw_ifd, IFD *ifd0){
		Raw_Processor *processor = NULL;
#ifdef __OPENCL__
		processor = new Raw_Processor_OpenCL(exception, raw_ifd, ifd0);
#else
		processor = new Raw_Processor(exception, raw_ifd, ifd0);
#endif
		return processor;
	}
*/

	Raw_Processor::~Raw_Processor(void)
	{
		unsigned int pos;

		if(this->pixel != NULL){
			for(pos = 0; pos < this->number_components; pos += 1){
				if(this->pixel[pos] != NULL){
					delete []this->pixel[pos];
				}
			}
		}

		if(this->histogramm != NULL){
			for(pos = 0; pos < this->number_components; pos +=1){
				if(this->histogramm[pos] != NULL){
					delete []this->histogramm[pos];
				}
			}
			delete []this->histogramm;
		}

		delete []this->pixel;
		delete []this->start_pointer;
		delete []this->linestride;

		if(this->zooms != NULL){
			free(this->zooms);
		}
	}

	void *Raw_Processor::loadRaw_thread(void *obj)
	{
		void *back = NULL;
		demosaik_AHD_variable *variable = (demosaik_AHD_variable*)obj;
		Raw_Processor *thiss 			= variable->thiss;
		MPT_Exception  *exception 			= variable->exception;


		unsigned int *bits_per_sample = NULL;
		size_t num_bits_per_sample = 0;
		unsigned int *pattern = NULL;
		size_t num_pattern = 0;
		unsigned int cfa_layout = 0;
		unsigned int read_pos, read_pos_end, read_pos_start;
		unsigned int block_size = variable->block_size;
		unsigned int bytes_per_row;
		unsigned int real_block_size;
		unsigned int block_pos;
		unsigned char *temp_data, *temp_data_work;
		unsigned short *data1, *data2;
		unsigned int temp_data_size;
		unsigned int pos_target;
		unsigned int width, pos_width;
		unsigned short temp;
		float factor1, factor2;

		bits_per_sample = thiss->raw_ifd->getBitsPerSample(exception, &num_bits_per_sample);
		MPT_EXCEPTION_TEST back;
		if(num_bits_per_sample == 0){
			MPT_EXCEPTION("Unknow CFA") back;
		}
		cfa_layout	= thiss->raw_ifd->getCFALayout(exception);
		MPT_EXCEPTION_TEST back;



		if(cfa_layout == 1 && bits_per_sample[0] == 12){

			pattern = thiss->raw_ifd->getCFAPattern(exception, &num_pattern);
			if(num_pattern != 4){
				MPT_EXCEPTION("Unknown CFAPattern.") back;
			}
			for(size_t pos = 0; pos < num_pattern; pos += 1){
				if(pattern[pos] > 2){
					MPT_EXCEPTION("Unknown CFAPattern.") back;
				}
			}

			width 			= thiss->width;
			bytes_per_row	= (width + width / 2);
			temp_data_size 	= bytes_per_row * block_size;
			temp_data		= (unsigned char*)malloc(temp_data_size);



			read_pos_start	= 0;
			read_pos_end	= thiss->length;
			for(read_pos 	= read_pos_start, pos_target = 0; read_pos < read_pos_end; read_pos += block_size){
				if(read_pos + block_size <= thiss->length){
					real_block_size = block_size;
				}else{
					real_block_size = thiss->length - read_pos;
				}
				thiss->raw_ifd->getRawData_ts(exception, read_pos, real_block_size,temp_data, temp_data_size);
				MPT_EXCEPTION_TEST back;

				for(block_pos = 0; block_pos < real_block_size; block_pos++){
					if(block_pos % 2 == 0){
						//data1 = variable->memory[0];
						//data2 = variable->memory[1];
						data1 = variable->buffer[16]->get_Row(pos_target);
						data2 = variable->buffer[17]->get_Row(pos_target);
						factor1 = thiss->factor[0];
						factor2	= thiss->factor[1];

					}else{
						//data1 = variable->memory[2];
						//data2 = variable->memory[3];
						data1 = variable->buffer[18]->get_Row(pos_target);
						data2 = variable->buffer[19]->get_Row(pos_target);
						factor1	= thiss->factor[1];
						factor2 = thiss->factor[2];
						pos_target += 1;
					}
					//data1 = data1 + pos_target * variable->linestride;
					//data2 = data2 + pos_target * variable->linestride;
					temp_data_work = temp_data + block_pos * bytes_per_row;
					/*
					if(block_pos % 2 == 1){
						pos_target += 1;
					}
					*/

					for(pos_width = 0; pos_width < width; pos_width += 2){
						temp	= *temp_data_work;
						temp	<<= 4;
						temp_data_work += 1;
						temp	|= *temp_data_work >> 4;
						*data1 	= crop( (float)((temp << 4) + 7) * factor1);
						data1 	+= 1;

						temp	= *temp_data_work & 0x0F;
						temp	<<= 8;
						temp_data_work += 1;
						temp	|= *temp_data_work;
						*data2	= crop((float)((temp << 4) + 7) * factor2);
						data2 	+= 1;
						temp_data_work	+= 1;
					}
				}
				variable->set_maximum_read_length(read_pos + real_block_size);

			}

			free(temp_data);

		}



		if(pattern != NULL){
			delete []pattern;
		}
		if(bits_per_sample != NULL){
			delete []bits_per_sample;
		}

		return back;
	}

	void Raw_Processor::loadRaw(MPT_Exception *exception)
	{
		float divident, mul[4], *mul1;
		pixeltype *comp[3];
		unsigned int *pattern = NULL, *patternuse;
		unsigned char *data;
		unsigned int temp, posLength, posWidth, posbuffer;
		size_t	numPattern, numsample;
		unsigned int *bitsPerSample = NULL;

		bitsPerSample = this->raw_ifd->getBitsPerSample(exception, &numsample);

		if(	this->raw_ifd->getCFALayout(exception) == 1 &&
			bitsPerSample[0] == 12){
			MPT_EXCEPTION_TEST;

			pattern = this->raw_ifd->getCFAPattern(exception, &numPattern);
			MPT_EXCEPTION_TEST;
			if(numPattern != 4 ){
				MPT_EXCEPTION("Unknown CFAPattern.");
			}
			for(size_t pos = 0; pos < numPattern; pos += 1){
				if(pattern[pos] > 2){
					MPT_EXCEPTION("Unknown CFAPattern.");
				}
			}
			divident = (float)((1 << 12) - 1);

			mul[0] = pixeltypeMAX / divident * this->factor[0];
			mul[1] = pixeltypeMAX / divident * this->factor[1];
			mul[2] = mul[1];
			mul[3] = pixeltypeMAX / divident * this->factor[2];

			data = this->raw_ifd->getRawData(exception);
			MPT_EXCEPTION_TEST;
			for(posbuffer = 0, posLength = 0; posLength < length; posLength += 1){
				comp[0] = &(this->pixel[0][start_pointer[0] + posLength * linestride[0]]);
				comp[1] = &(this->pixel[1][start_pointer[1] + posLength * linestride[1]]);
				comp[2] = &(this->pixel[2][start_pointer[2] + posLength * linestride[2]]);

				if(posLength %  2 == 0){
					patternuse 	= pattern;
					mul1		= mul;
				}else{
					patternuse	= &pattern[2];
					mul1		= &mul[2];
				}
				comp[patternuse[1]] += 1;

				for(posWidth = 0; posWidth < width; ){
					temp =((unsigned int) data[posbuffer]) << 4;
					posbuffer += 1;
					temp |=  ((unsigned int) data[posbuffer]) >> 4;
					*comp[patternuse[posWidth % 2]] = Mathe::crop(temp * mul1[0]);
					comp[patternuse[posWidth % 2]] += 2;
					posWidth += 1;

					temp = ((unsigned int) data[posbuffer] & 0x0F) << 8;
					posbuffer += 1;
					temp |= ((unsigned int) data[posbuffer]);
					posbuffer += 1;
					*comp[patternuse[posWidth % 2]] = Mathe::crop(temp * mul1[1]);
					comp[patternuse[posWidth % 2]] += 2;
					posWidth += 1;
				}
			}
			delete []data;
		}else{
			if(bitsPerSample != NULL){
				delete []bitsPerSample;
			}
			MPT_EXCEPTION("Not Supported Raw format.");
		}

		if(pattern != NULL){
			delete []pattern;
		}
		if(bitsPerSample != NULL){
			delete []bitsPerSample;
		}
	}

	void Raw_Processor::updata_RGB(MPT_Exception *exception, unsigned int *data, size_t size){
		pixeltype *comp[3];
		unsigned int  posWidth, posLength, posData, inttemp;
		pixeltype temp;


		if(size >= width * length){

			for(posLength = 0, posData = 0; posLength < length; posLength += 1){
				comp[0] = &(this->pixel[0][start_pointer[0] + posLength * linestride[0]]);
				comp[1] = &(this->pixel[1][start_pointer[1] + posLength * linestride[1]]);
				comp[2] = &(this->pixel[2][start_pointer[2] + posLength * linestride[2]]);
				posData = posLength * width;

				for(posWidth = 0; posWidth < width; posWidth += 1){
					inttemp = 0;
					temp = *comp[0] >> 8;
					inttemp = ((unsigned int)temp) << 16;


					temp = *comp[1] >> 8;
					inttemp |= ((unsigned int)temp) << 8;


					temp = *comp[2] >> 8;
					inttemp |= ((unsigned int)temp);

					data[posData] =  inttemp;
					comp[0] += 1;
					comp[1] += 1;
					comp[2] += 1;
					posData += 1;
				}
			}
		}else{
			MPT_EXCEPTION("Size does not fit.");
		}
	}

	unsigned int Raw_Processor::get_Length(MPT_Exception *)
	{
		return this->length;
	}

	unsigned int Raw_Processor::get_Width(MPT_Exception *)
	{
		return this->width;
	}

	unsigned int Raw_Processor::get_number_components(MPT_Exception *exception)
	{
		unsigned int *pattern, numberr = 0;
		size_t number;

		pattern = this->raw_ifd->getCFAPattern(exception, &number);
		MPT_EXCEPTION_TEST 0;
		if(number == 4){
			if(	pattern[0] == 0 &&
				pattern[1] == 1 &&
				pattern[2] == 1 &&
				pattern[3] == 2){

				numberr = 3;
			}else{
				if(pattern != NULL){
					delete []pattern;
				}
				MPT_EXCEPTION("Unsupported CFA pattern") 0;
			}
		}else{
			if(pattern != NULL){
				delete []pattern;
			}
			MPT_EXCEPTION("Unsupported CFA pattern") 0;
		}
		if(pattern != NULL){
			delete []pattern;
		}
		return numberr;
	}

	void Raw_Processor::bilinear_green_01(MPT_Exception *exception)
	{
		unsigned int *pattern, even_start, odd_start,start, pos_row, length, green, linestride;
		size_t pattern_number;
		pixeltype *comp, *end;

		green = 1;
		even_start = odd_start = 0;
		linestride = this->linestride[green];
		pattern = this->raw_ifd->getCFAPattern(exception, &pattern_number);
		MPT_EXCEPTION_TEST;
		if(pattern_number == 4){
			if(	pattern[1] == 1 &&
				pattern[2] == 1){
					even_start = 2;
					odd_start = 1;
			}else{
				if(pattern != NULL){
					delete []pattern;
				}
				MPT_EXCEPTION("Unsupported CFA pattern");
			}

		}else{
			if(pattern != NULL){
				delete []pattern;
			}
			MPT_EXCEPTION("Unsupported CFA pattern");
		}

		length = this->length - 1;
		for(pos_row = 1; pos_row < length; pos_row += 1){
			if(pos_row % 2 == 0){
				start = even_start;
			}else{
				start = odd_start;
			}
			comp = &(this->pixel[green][pos_row * linestride + this->start_pointer[green] + start]);
			end = comp + (this->width - 1 - start);
			for(;comp < end; comp += 2){
				*comp = (*(comp + 1) + *(comp - 1) + *(comp - linestride) + *(comp + linestride)) / 4.0f;
			}
		}
		if(pattern != NULL){
			delete []pattern;
		}
	}

	void Raw_Processor::billinear_red_01(MPT_Exception *)
	{
		unsigned int  pos_row, length, red, linestride;
		pixeltype *comp, *end;

		red = 0;
		linestride = this->linestride[red];

		length = this->length - 1;
		for(pos_row = 2; pos_row < length; pos_row += 2){
			comp	= &(this->pixel[red][pos_row * linestride + this->start_pointer[red] + 1]);
			end		= comp + this->width - 2;
			for(;comp < end; comp += 2){
				*comp	= (*(comp + 1) + *(comp - 1)) / 2.0f;
			}
		}

		for(pos_row = 1; pos_row < length; pos_row += 2){
			comp	= &(this->pixel[red][pos_row * linestride + this->start_pointer[red] + 1]);
			end		= comp + this->width - 2;
			for(;comp < end; comp += 1){
				*comp	= (*(comp + linestride) + *(comp - linestride)) / 2.0f;
			}
		}
	}

	void Raw_Processor::billinear_blue_01(MPT_Exception *)
	{
		unsigned int  pos_row, length, blue, linestride;
		pixeltype *comp, *end;

		blue = 2;
		linestride = this->linestride[blue];

		length = this->length - 1;
		for(pos_row = 1; pos_row < length; pos_row += 2){
			comp	= &(this->pixel[blue][pos_row * linestride + this->start_pointer[blue] + 2]);
			end		= comp + this->width - 3;
			for(;comp < end; comp += 2){
				*comp	= (*(comp + 1) + *(comp - 1)) / 2.0f;
			}
		}

		for(pos_row = 2; pos_row < length; pos_row += 2){
			comp	= &(this->pixel[blue][pos_row * linestride + this->start_pointer[blue] + 1]);
			end		= comp + this->width - 2;
			for(;comp < end; comp += 1){
				*comp	= (*(comp + linestride) + *(comp - linestride)) / 2.0f;
			}
		}
	}

	void Raw_Processor::start_demosaik(MPT_Exception *exception){

		if(this->demosaic_type == BILINEAR){
			bilinear_green_01(exception);
			MPT_EXCEPTION_TEST;
			billinear_red_01(exception);
			MPT_EXCEPTION_TEST;
			billinear_blue_01(exception);
			MPT_EXCEPTION_TEST;
		}

	}

	void Raw_Processor::updata_histogramm(MPT_Exception *)
	{
		unsigned int pos_comp, pos_length, temp, *histo, histogramm_max;
		pixeltype *comp, *pos, *end;
		float factor;

		histogramm_max = histogramm_size - 1;
		factor = (float)histogramm_max/ (float) pixeltypeMAX ;


		for(pos_comp = 0; pos_comp < this->number_components; pos_comp += 1){
			comp	= this->pixel[pos_comp];
			histo	= this->histogramm[pos_comp];

			for(unsigned int i = 0; i < histogramm_size ; i += 1){
				histo[i] = 0;
			}

			for(pos_length = this->cropOrigin[2] ; pos_length < this->cropSize[2]; pos_length += 1){
				pos = comp + (this->start_pointer[pos_comp] + this->cropOrigin[0]  + pos_length * this->linestride[pos_comp]);
				end = pos + this->cropSize[0];
				for(; pos < end; pos += 1){
					if(*pos > 0.0f){
						temp = (unsigned int)(*pos * factor);
						if(temp > histogramm_max ){
							histo[histogramm_max] += 1;
						}else{
							histo[temp] += 1;
						}
					}else{
						histo[0] += 1;
					}
				}
			}
		}
	}


	void Raw_Processor::set_white_point(MPT_Exception *)
	{
		unsigned int pos_comp, *histo, size, fail, count, histogramm_max;
		pixeltype *comp;

		histogramm_max = histogramm_size - 1;
		size = this->cropSize[0] * this->cropSize[2];
		fail = (unsigned int)( (float)size * failPixel / 100.f);

		this->RGB_white_point[this->number_components] = 0.0f;

		for(pos_comp = 0; pos_comp < this->number_components; pos_comp += 1){
			comp	= this->pixel[pos_comp];
			histo	= this->histogramm[pos_comp];

			count = 0;
			for(unsigned int i = histogramm_max ; i <=  histogramm_max; i -= 1){
				count += histo[i];
				if(count >= fail){
					this->RGB_white_point[pos_comp] = (float)i / (float)histogramm_max;
					break;
				}
			}
			if(RGB_white_point[pos_comp] > RGB_white_point[this->number_components]){
				RGB_white_point[this->number_components] = RGB_white_point[pos_comp];
			}
		}
	}

	void Raw_Processor::create_CameraToXYZ(MPT_Exception *exception)
	{
		unsigned int Ra_whitePoint[6], pos;
		float	colorMatrix1[9], colorMatrix2[9], analogBalance[3], whitePointMatrix[9], XYZtoRGB[9], tranMatrix[9],
				RGBtoCam[9], min, max, factor_m[9];

		this->ifd0->getAsShotNeutral(exception, Ra_whitePoint);
		this->ifd0->getColorMatrix1(exception, colorMatrix1);
		this->ifd0->getColorMatrix2(exception, colorMatrix2);
		this->ifd0->getAnalogBalance(exception, analogBalance);

		for(pos = 0; pos < 9; pos += 1){
			whitePointMatrix[pos]	= 0.0f;
			factor_m[pos]			= 0.0f;
			CamtoXYZ[pos]			= colorMatrix2[pos];
		}

		whitePointMatrix[0] = 1.0f / ((float)Ra_whitePoint[0] / (float)Ra_whitePoint[1]);
		whitePointMatrix[4] = 1.0f / ((float)Ra_whitePoint[2] / (float)Ra_whitePoint[3]);
		whitePointMatrix[8] = 1.0f / ((float)Ra_whitePoint[4] / (float)Ra_whitePoint[5]);

		Mathe::invers_matrix3x3(RGBtoXYZ, XYZtoRGB);
		//Mathe::mul_matrix3x3(colorMatrix1, whitePointMatrix, tranMatrix);
		//Mathe::mul_matrix3x3(XYZtoRGB, tranMatrix, tranMatrix);

		Mathe::mul_matrix3x3(colorMatrix2, RGBtoXYZ, RGBtoCam);
		Mathe::normal_matrix3x3(RGBtoCam, 1.0f, factor);
		Mathe::invers_matrix3x3(RGBtoCam, CamtoRGB);
		Mathe::invers_matrix3x3(RGBtoCam, tranMatrix);
		Mathe::min_max_vector3(factor, &min, &max);

		Mathe::scalar_vector3(factor, 1.0f / min);


		factor_m[0] = factor[0];
		factor_m[4] = factor[1];
		factor_m[8] = factor[2];


		//Mathe::mul_matrix3x3(factor_m, whitePointMatrix, tranMatrix);
		//Mathe::mul_matrix3x3(CamtoRGB, whitePointMatrix, tranMatrix);
		//Mathe::mul_matrix3x3(CamtoRGB, factor_m, tranMatrix);
		//mul_matrix(tranMatrix);
	}

	float *Raw_Processor::get_CamtoXYZ()
	{
		return CamtoXYZ;
	}


/*
	void Raw_Processor::mul_matrix(float matrix[9])
	{
		unsigned int  pos_row, length;
		pixeltype *comp0, *end0, *comp1, *comp2;
		unsigned int red, green, blue; //plus;
		//int ired, igreen, iblue, *icomp0, *icomp1, *icomp2;


		//plus =  65535.0f;
		length = this->length - 1;
		for(pos_row = 1; pos_row < length; pos_row += 1){
			comp0	= &(this->pixel[0][pos_row * this->linestride[0] + this->start_pointer[0] + 2]);
			comp1	= &(this->pixel[1][pos_row * this->linestride[1] + this->start_pointer[1] + 2]);
			comp2	= &(this->pixel[2][pos_row * this->linestride[2] + this->start_pointer[2] + 2]);
			end0	= comp0 + (this->width - 3);
			for(;comp0 < end0; comp0 += 1, comp1 += 1, comp2 += 1){
				red		= *comp0;
				green	= *comp1;
				blue	= *comp2;

				*comp0 =  Mathe::crop(red * matrix[0] + green * matrix[1] + blue * matrix[2]);
				*comp1 =  Mathe::crop(red * matrix[3] + green * matrix[4] + blue * matrix[5]);
				*comp2 =  Mathe::crop(red * matrix[6] + green * matrix[7] + blue * matrix[8]);
			}
		}
	}
*/

	void* Raw_Processor::mul_matrix_02(void *in)
	{
		struct struct_mul_matrix_function_variable_1 *variable = (struct struct_mul_matrix_function_variable_1 *)in;
		unsigned int  pos_row, length;
		pixeltype *comp0, *end0, *comp1, *comp2;
		unsigned int red, green, blue; //plus;
		//int ired, igreen, iblue, *icomp0, *icomp1, *icomp2;
		float *matrix = variable->matrix;

		//plus =  65535.0f;
		length = variable->number_row + variable->start_row;
		for(pos_row = variable->start_row; pos_row < length; pos_row += 1){
			comp0	= &(variable->comp0[pos_row * variable->linestride + variable->start_col + 2]);
			comp1	= &(variable->comp1[pos_row * variable->linestride + variable->start_col + 2]);
			comp2	= &(variable->comp2[pos_row * variable->linestride + variable->start_col + 2]);
			end0	= comp0 + (variable->number_col - 3);
			for(;comp0 < end0; comp0 += 1, comp1 += 1, comp2 += 1){
				red		= *comp0;
				green	= *comp1;
				blue	= *comp2;

				*comp0 =  Mathe::crop(red * matrix[0] + green * matrix[1] + blue * matrix[2]);
				*comp1 =  Mathe::crop(red * matrix[3] + green * matrix[4] + blue * matrix[5]);
				*comp2 =  Mathe::crop(red * matrix[6] + green * matrix[7] + blue * matrix[8]);
			}
		}

		return NULL;
	}

	void Raw_Processor::mul_matrix_03(float matrix[9])
	{
#ifdef __NEON__
		struct struct_mul_matrix_function_variable_asm variable;

		variable.size	= this->width * this->length / 4;
		variable.comp1	= this->pixel[0];
		variable.comp2	= this->pixel[1];
		variable.comp3	= this->pixel[2];
		variable.matrix	= matrix;

		mul_matrix_asm(&variable);
#else

		mul_matrix(matrix);
#endif
	}

	void Raw_Processor::mul_matrix(float matrix[9])
	{
		//mul_matrix_function_variable variable[MAXTHREADS];

		struct struct_mul_matrix_function_variable_1 variable[get_MAXTHREADS()];
		MPT_THREAD_T	thread_id[get_MAXTHREADS()];

		unsigned int 	number_threads 	= get_USETHREADS();
		unsigned int 	thread;
		unsigned int	rows_per_thread	= 0;
		unsigned int 	rest_rows	= 0;
		int start_row			= 0;


		if(this->length > 20){
			rows_per_thread	= this->length / number_threads;
			while(rows_per_thread < 20 && number_threads > 1){
				number_threads	-= 1;
				rows_per_thread	= this->length / number_threads;
			}
			rest_rows	= this->length % number_threads;
		}else{
			number_threads	= 1;
			rest_rows		= 0;
			rows_per_thread	= this->length;
		}

		for(thread = 0, start_row = 0; thread < number_threads; thread += 1){
			variable[thread].matrix		= matrix;
			variable[thread].comp0		= this->pixel[0];
			variable[thread].comp1		= this->pixel[1];
			variable[thread].comp2		= this->pixel[2];
			variable[thread].linestride	= this->linestride[0];
			variable[thread].start_row	= start_row;
			if(thread == 0){
				variable[thread].number_row	= rest_rows + rows_per_thread;
				start_row += rest_rows + rows_per_thread;
			}else{
				variable[thread].number_row	= rows_per_thread;
				start_row +=  rows_per_thread;
			}
			variable[thread].start_col	= 0;
			variable[thread].number_col	= this->width;

			 MPT_THREAD_CREATE(&thread_id[thread], NULL, Raw_Processor::mul_matrix_02, &variable[thread]);
		}

		for(thread = 0, start_row = 0; thread < number_threads; thread += 1){
			MPT_THREAD_JOIN(thread_id[thread], NULL);
		}
	}


	void Raw_Processor::setup_gammacurve(pixeltype white)
	{
		int pos;
		float temp;


		for(pos = 0; pos < (pixeltypeMAX + 1); pos += 1){
			temp = Mathe::gamma( ((float)pos * (float)white) / ((float)pixeltypeMAX * (float)pixeltypeMAX) );
			if(temp < 0.0f){
				this->gamme_curve[pos] = 0;
			}else if(temp >= 1.0f){
				this->gamme_curve[pos] = pixeltypeMAX;
			}else{
				this->gamme_curve[pos] = pixeltypeMAX * temp;
			}
		}
	}

	void Raw_Processor::gamma_correction(MPT_Exception *)
	{

		unsigned int  pos_row, length;
		pixeltype *comp0, *end0, *comp1, *comp2;
		//int ired, igreen, iblue, *icomp0, *icomp1, *icomp2;


		length = this->length - 1;
		for(pos_row = 1; pos_row < length; pos_row += 1){
			comp0	= &(this->pixel[0][pos_row * this->linestride[0] + this->start_pointer[0] + 2]);
			comp1	= &(this->pixel[1][pos_row * this->linestride[1] + this->start_pointer[1] + 2]);
			comp2	= &(this->pixel[2][pos_row * this->linestride[2] + this->start_pointer[2] + 2]);
			end0	= comp0 + (this->width - 3);
			for(;comp0 < end0; comp0 += 1, comp1 += 1, comp2 += 1){


				*comp0 = this->gamme_curve[*comp0];
				*comp1 = this->gamme_curve[*comp1];
				*comp2 = this->gamme_curve[*comp2];
			}
		}
	}

	void Raw_Processor::set_zoom(MPT_Exception *, float zoom, unsigned int size[2])
	{
		int size_i, size_o, i_pos1, i_pos2, i_pos3, i_end1;
		float start1, stop1, zoomsize, rest;

		if(zoom != zoomfactor){
			if(zoom < 5.0f && zoom > 0.05f ){
				this->zoomsize[0] = this->width * zoom;
				this->zoomsize[1] = this->length * zoom;
				zoomfactor = zoom;
			}else{
				this->zoomsize[0] = this->width;
				this->zoomsize[1] = this->length;
				zoomfactor = 1.0f;
			}

			if( width > length){
				size_i = width;
				size_o = this->zoomsize[0];
			}else{
				size_i = length;
				size_o = this->zoomsize[1];
			}

			if(this->zooms != NULL){
				free(this->zooms);
				this->zooms = NULL;
			}

			this->zooms =  (zoomer*)malloc(sizeof(struct zoomer) * size_i * 2);
			this->zoomssize = size_i * 2;

			zoomfactor 	= (float)size_o / (float)size_i;
			zoomsize	= (float)size_i / (float)size_o;
			i_end1		= size_i;
			for(i_pos1 = 0, i_pos3 = 0; i_pos1 < i_end1; i_pos1++){
				i_pos2	= (float)i_pos1 / zoomsize;

				start1	= i_pos1 * zoomfactor;
				stop1	= (i_pos1 + 1) * zoomfactor;

				if(stop1 <= (i_pos2  + 1)){
					this->zooms[i_pos3].from	= i_pos1;
					this->zooms[i_pos3].to		= i_pos2;
					this->zooms[i_pos3].factor	= zoomfactor;
					i_pos3 += 1;
				}else{
					rest = 1.0f - (start1 - i_pos2);
					this->zooms[i_pos3].from	= i_pos1;
					this->zooms[i_pos3].to		= i_pos2;
					this->zooms[i_pos3].factor	= rest; //rest * zoomfactor;
					i_pos3 += 1;

					rest = zoomfactor - rest;
					this->zooms[i_pos3].from	= i_pos1;
					this->zooms[i_pos3].to		= i_pos2 + 1;
					this->zooms[i_pos3].factor	= rest; //rest * zoomfactor;
					i_pos3 += 1;
				}
			}
		}
		if(size != NULL){
			size[0] = this->zoomsize[0];
			size[1] = this->zoomsize[1];
		}
	}

	unsigned int Raw_Processor::get_ZoomWidth(MPT_Exception *)
	{
		return this->zoomsize[0];
	}


	unsigned int Raw_Processor::get_ZoomLength(MPT_Exception *)
	{
		return this->zoomsize[1];
	}

	void Raw_Processor::get_zoom_image(	MPT_Exception *exception,
											unsigned int *data_out,
											unsigned int linestride,
											unsigned int width,
											unsigned int length,
											unsigned int out_x0,
											unsigned int out_y0,
											unsigned int in_x0,
											unsigned int in_y0)
	{
		unsigned int 	*data, *data_end, pos_y, pos_x, pos_x_end, pos, pos_end,
						pos_length_start,pos_length_end, *temp_data,
						pos_width_start, pos_width_end, temp_width, temp_length, pos_length,
						pos_y_from, pos_y_to, pos_y_to_start, temp_pos_y,
						temp_pos_x_start, temp_pos_x_end, pos_x_from_start, pos_x_to_start,
						pos_x_from, pos_x_to, component, *base_to, pos_width, *base_from2;

		float			factor;
		pixeltype		*base_from;

		pos_y = out_y0;
		pos_x_end = out_x0 + width;
		for(pos_x = out_x0; pos_x < pos_x_end; pos_x += 1){
			data		= data_out + (pos_x + pos_y * linestride);
			data_end	= data + length * linestride;
			for(;data < data_end; data += linestride){
				*data = 0x00000000;
			}
		}

		if((in_y0 + length) >= this->zoomsize[1]){
			length = this->zoomsize[1] - in_y0 - 1;
		}

		for(pos_length_start = 0; pos_length_start < zoomssize; pos_length_start += 1){
			if(zooms[pos_length_start].to >= in_y0){
				break;
			}
		}

		for(pos_length_end = 0; pos_length_end < zoomssize; pos_length_end += 1){
			if(zooms[pos_length_end].to >= (in_y0 + length)){
				break;
			}
		}

		if((in_x0 + width) >= this->zoomsize[0]){
			width = this->zoomsize[0] - in_x0 - 1;
		}

		for(pos_width_start = 0; pos_width_start < zoomssize; pos_width_start += 1){
			if(zooms[pos_width_start].to >= in_x0){
				break;
			}
		}

		for(pos_width_end = 0; pos_width_end < zoomssize; pos_width_end += 1){
			if(zooms[pos_width_end].to >= (in_x0 + width)){
				break;
			}
		}

		temp_pos_x_start	= zooms[pos_width_start].from;
		temp_pos_x_end		= zooms[pos_width_end - 1].from + 1;

		temp_width	= temp_pos_x_end - temp_pos_x_start;
		temp_length	= zooms[pos_length_end - 1].to + 1 - zooms[pos_length_start].to;
/*
		if(	temp_width >= zooms[pos_width_end].from &&
			temp_length >= zooms[pos_length_end].to){
*/
			pos_end = temp_width * temp_length;
			temp_data = (unsigned int *)malloc(sizeof(unsigned int) * pos_end);
			if(temp_data == NULL){
				MPT_EXCEPTION("Not enough memory.");
			}
			for(pos = 0; pos < pos_end; pos += 1){
				temp_data[pos] = 0;
			}

			pos_y_to_start = zooms[pos_length_start].to;
			for(pos_length = pos_length_start; pos_length < pos_length_end; pos_length += 1){
				pos_y_from	= zooms[pos_length].from;
 				pos_y_to	= zooms[pos_length].to;
				factor		= zooms[pos_length].factor;
				temp_pos_y	= pos_y_to - pos_y_to_start;


				for(component = 0; component < this->number_components; component++){
					base_from 	= this->pixel[component] + pos_y_from * this->linestride[component] + temp_pos_x_start;
					base_to		= temp_data + (temp_width * temp_pos_y);
					zoom_line(base_from, base_to, temp_width, factor, 16 - component * 8);
				}
			}

			pos_x_to_start		= zooms[pos_width_start].to;
			pos_x_from_start	= zooms[pos_width_start].from;
			for(pos_width = pos_width_start; pos_width < pos_width_end; pos_width += 1){
				pos_x_from	= zooms[pos_width].from;
				pos_x_to	= zooms[pos_width].to;
				factor		= zooms[pos_width].factor;

				base_to		= data_out + pos_x_to -  pos_x_to_start + out_x0 + out_y0 * linestride;
				base_from2	= temp_data + pos_x_from - pos_x_from_start;
				zoom_row(base_from2, base_to, temp_width, linestride, temp_length, factor);

			}

			free(temp_data);
		//}
	}

	void Raw_Processor::get_zoom_image(	MPT_Exception	*exception,
											unsigned short	*data_out,
											unsigned int	linestride,
											unsigned int	width,
											unsigned int	length,
											unsigned int	out_x0,
											unsigned int	out_y0,
											unsigned int	in_x0,
											unsigned int	in_y0)
	{
		struct struct_get_zoom_imager_02 variable[get_MAXTHREADS()];

		variable[0].thiss			= this;
		variable[0].exception		= exception;
		variable[0].data_out		= data_out;
		variable[0].linestride		= linestride;
		variable[0].width			= width;
		variable[0].length			= length;
		variable[0].out_x0			= out_x0;
		variable[0].out_y0			= out_y0;
		variable[0].in_x0			= in_x0;
		variable[0].in_y0			= in_y0;

		get_zoom_image_02((void*) &variable[0]);
	}

	void *Raw_Processor::get_zoom_image_02(void *in)
	{
		struct struct_get_zoom_imager_02 *variable = (struct struct_get_zoom_imager_02*)in;
		unsigned int 	 pos_y, pos_x, pos_x_end, pos, pos_end,
						pos_length_start,pos_length_end, *temp_data,
						pos_width_start, pos_width_end, temp_width, temp_length, pos_length,
						pos_y_from, pos_y_to, pos_y_to_start, temp_pos_y,
						temp_pos_x_start, temp_pos_x_end, pos_x_from_start, pos_x_to_start,
						pos_x_from, pos_x_to, component, *base_to, pos_width, *base_from2;
		unsigned short *data, *data_end, *short_base_to;
		MPT_Exception	*exception	= variable->exception;
		unsigned short	*data_out	= variable->data_out;
		unsigned int	linestride	= variable->linestride;
		unsigned int	width		= variable->width;
		unsigned int	length		= variable->length;
		unsigned int	out_x0		= variable->out_x0;
		unsigned int	out_y0		= variable->out_y0;
		unsigned int	in_x0		= variable->in_x0;
		unsigned int	in_y0		= variable->in_y0;
		Raw_Processor	*thiss		= variable->thiss;

		float			factor;
		pixeltype		*base_from;

		pos_y = out_y0;
		pos_x_end = out_x0 + width;
		for(pos_x = out_x0; pos_x < pos_x_end; pos_x += 1){
			data		= data_out + (pos_x + pos_y * linestride);
			data_end	= data + length * linestride;
			for(;data < data_end; data += linestride){
				*data = 0x0000;
			}
		}

		if((in_y0 + length) >= thiss->zoomsize[1]){
			length = thiss->zoomsize[1] - in_y0 - 1;
		}

		for(pos_length_start = 0; pos_length_start < thiss->zoomssize; pos_length_start += 1){
			if(thiss->zooms[pos_length_start].to >= in_y0){
				break;
			}
		}

		for(pos_length_end = 0; pos_length_end < thiss->zoomssize; pos_length_end += 1){
			if(thiss->zooms[pos_length_end].to >= (in_y0 + length)){
				break;
			}
		}

		if((in_x0 + width) >= thiss->zoomsize[0]){
			width = thiss->zoomsize[0] - in_x0 - 1;
		}

		for(pos_width_start = 0; pos_width_start < thiss->zoomssize; pos_width_start += 1){
			if(thiss->zooms[pos_width_start].to >= in_x0){
				break;
			}
		}

		for(pos_width_end = 0; pos_width_end < thiss->zoomssize; pos_width_end += 1){
			if(thiss->zooms[pos_width_end].to >= (in_x0 + width)){
				break;
			}
		}

		temp_pos_x_start	= thiss->zooms[pos_width_start].from;
		temp_pos_x_end		= thiss->zooms[pos_width_end - 1].from + 1;

		temp_width	= temp_pos_x_end - temp_pos_x_start;
		temp_length	= thiss->zooms[pos_length_end - 1].to + 1 - thiss->zooms[pos_length_start].to;
/*
		if(	temp_width >= zooms[pos_width_end].from &&
			temp_length >= zooms[pos_length_end].to){
*/
			pos_end = temp_width * temp_length;
			temp_data = (unsigned int *)malloc(sizeof(unsigned int) * pos_end);
			if(temp_data == NULL){
				MPT_EXCEPTION("Not enough memory.") NULL;
			}
			for(pos = 0; pos < pos_end; pos += 1){
				temp_data[pos] = 0;
			}

			pos_y_to_start = thiss->zooms[pos_length_start].to;
			for(pos_length = pos_length_start; pos_length < pos_length_end; pos_length += 1){
				pos_y_from	= thiss->zooms[pos_length].from;
 				pos_y_to	= thiss->zooms[pos_length].to;
				factor		= thiss->zooms[pos_length].factor;
				temp_pos_y	= pos_y_to - pos_y_to_start;


				for(component = 0; component < thiss->number_components; component++){
					base_from 	= thiss->pixel[component] + pos_y_from * thiss->linestride[component] + temp_pos_x_start;
					base_to		= temp_data + (temp_width * temp_pos_y);
					zoom_line(base_from, base_to, temp_width, factor, 16 - component * 8);
				}
			}

			pos_x_to_start		= thiss->zooms[pos_width_start].to;
			pos_x_from_start	= thiss->zooms[pos_width_start].from;
			for(pos_width = pos_width_start; pos_width < pos_width_end; pos_width += 1){
				pos_x_from	= thiss->zooms[pos_width].from;
				pos_x_to	= thiss->zooms[pos_width].to;
				factor		= thiss->zooms[pos_width].factor;

				short_base_to	= data_out + pos_x_to -  pos_x_to_start + out_x0 + out_y0 * linestride;
				base_from2		= temp_data + pos_x_from - pos_x_from_start;
				zoom_row(base_from2, short_base_to, temp_width, linestride, temp_length, factor);

			}

			free(temp_data);
		//}
		return NULL;
	}

	void Raw_Processor::zoom_row(unsigned *in, unsigned *out, unsigned linestride_in, unsigned linestride_out, unsigned size, float factor){
		unsigned int *in_pos, *in_end, *out_pos;
		unsigned int MASK1 = 0xFF00FFFF;
		unsigned int MASK2;
		unsigned int shift = 0;
		unsigned int value;
		in_pos	= in;
		in_end	= in_pos + (linestride_in * size);
		out_pos	= out;



		for(;in_pos < in_end; in_pos += linestride_in, out_pos += linestride_out){
			shift = 16;
			MASK2 = (0xFF << shift);
			MASK1 = 0xFFFFFFFF ^ MASK2;
			//f_in = (float)((*out_pos >> shift) & 0xFF) + (float)((*in_pos >> shift)& 0xFF) * factor;
			value = Mathe::round((float)((*in_pos >> shift)& 0xFF) * factor);
			value += ((*out_pos >> shift) & 0xFF);
			(*out_pos) &= MASK1;
			if(value < 256){
				*out_pos |= value << shift;
			}else{
				*out_pos |= MASK2;
			}

			shift = 8;
			MASK2 = (0xFF << shift);
			MASK1 = 0xFFFFFFFF ^ MASK2;
			//f_in = (float)((*out_pos >> shift) & 0xFF) + (float)((*in_pos >> shift)& 0xFF) * factor;
			value = Mathe::round((float)((*in_pos >> shift)& 0xFF) * factor);
			value += ((*out_pos >> shift) & 0xFF);
			*out_pos &= MASK1;
			if(value < 256){
				*out_pos |= value << shift;
			}else{
				*out_pos |= MASK2;
			}

			shift = 0;
			MASK2 = (0xFF << shift);
			MASK1 = 0xFFFFFFFF ^ MASK2;
			//f_in = (float)((*out_pos >> shift) & 0xFF) + (float)((*in_pos >> shift)& 0xFF) * factor;
			value = Mathe::round((float)((*in_pos >> shift)& 0xFF) * factor);
			value += ((*out_pos >> shift) & 0xFF);
			*out_pos &= MASK1;
			if(value < 256){
				*out_pos |= value << shift;
			}else{
				*out_pos |= MASK2;
			}
		}
	}

	void Raw_Processor::zoom_row(unsigned *in, unsigned short *out, unsigned linestride_in, unsigned linestride_out, unsigned size, float factor){
		unsigned int *in_pos, *in_end;
		unsigned short *out_pos;
		unsigned short MASK1 = 0xFFFF;
		unsigned int MASK2;
		unsigned short MASK3;
		unsigned int shift = 0;
		unsigned int shift2 = 0;
		unsigned int value, value2;
		in_pos	= in;
		in_end	= in_pos + (linestride_in * size);
		out_pos	= out;



		for(;in_pos < in_end; in_pos += linestride_in, out_pos += linestride_out){
			shift	= 16;
			shift2	= 8;
			//MASK2 = (0xFF << shift);
			MASK3 = 0xF8;
			MASK1 = 0xFFFF ^ (MASK3 << 8);
			//f_in = (float)((*out_pos >> shift) & 0xFF) + (float)((*in_pos >> shift)& 0xFF) * factor;
			value	= Mathe::round((float)((*in_pos >> shift)& 0xFF) * factor);
			value2	= ((*out_pos >> 11) & 0x1F) * 255;
			value	+= value2 / 0x1F;
			(*out_pos) &= MASK1;
			if(value < 256){
				*out_pos |= ((value & MASK3) << shift2);
			}else{
				*out_pos |= MASK3 << shift2;
			}

			shift 	= 8;
			MASK2 	= (0xFF << shift);
			shift2	= 3;
			MASK3	= 0xFC;
			MASK1 	= 0xFFFF ^ (MASK3 << 3);
			//f_in = (float)((*out_pos >> shift) & 0xFF) + (float)((*in_pos >> shift)& 0xFF) * factor;
			value	= Mathe::round((float)((*in_pos >> shift)& 0xFF) * factor);
			value2	= ((*out_pos >> 5) & 0x3F) * 255;
			value	+= value2 / 0x3f;
			*out_pos &= MASK1;
			if(value < 256){
				*out_pos |= (value & MASK3) << shift2;
			}else{
				*out_pos |= MASK3 << shift2;
			}

			shift 	= 0;
			shift2	= 3;
			MASK2 	= (0xFF << shift);
			MASK3 	= 0xF8;
			MASK1 	= 0xFFFF ^ 0x1F;
			//f_in = (float)((*out_pos >> shift) & 0xFF) + (float)((*in_pos >> shift)& 0xFF) * factor;
			value	= Mathe::round((float)((*in_pos >> shift)& 0xFF) * factor);
			value2	= ((*out_pos ) & 0x1F) * 255;
			value	+= value2 / 0x1F;
			*out_pos &= MASK1;
			if(value < 256){
				*out_pos |= value >> shift2;
			}else{
				*out_pos |= 0x3F;
			}
		}
	}

	void Raw_Processor::zoom_line(pixeltype *in, unsigned *out, unsigned int size, float factor, unsigned int shift){
		unsigned int pos;
		unsigned int value;
		unsigned int MASK = 0xFFFFFFFF ^ (0xFF <<shift);
		for(pos = 0; pos < size; pos += 1){
			value = Mathe::round((float)(in[pos] >> 8) * factor) + ((out[pos] >> shift) & 0xFF);
			out[pos] &= MASK;
			if(value < 256){
				out[pos] |= value << shift;
			}else{
				out[pos] |= 0xFF << shift;
			}
		}
	}

	void Raw_Processor::write_ppm(){

		const char *outName = "testbild.ppm";
		FILE *file;
		unsigned char *data;
		unsigned int posx, posy, basepos;
		unsigned int totalsize = this->length * this->width * 3;

		pixeltype *red, *green, *blue;

		file = fopen (outName, "wb");
		fprintf (file, "P%d\n%d %d\n%d\n", 6, this->width, this->length, 255);
		data = (unsigned char*)malloc (totalsize);
		for(posx = 0; posx < totalsize; posx += 1){
			data[posx] = 0;
		}

		for(posy = 0; posy < this->length; posy += 1){
			basepos = this->width * 3 * posy;
			red		= this->pixel[0] + posy * this->linestride[0];
			green	= this->pixel[1] + posy * this->linestride[1];
			blue	= this->pixel[2] + posy * this->linestride[2];

			for(posx = 0; posx < this->width; posx += 1){

				data[basepos + posx * 3] = (unsigned char) red[posx] >> 8;

				data[basepos + posx * 3 + 1] = (unsigned char) green[posx] >> 8;

				data[basepos + posx * 3 + 2] = (unsigned char) blue[posx] >> 8;
			}
		}

		fwrite(data, 1, totalsize, file);
		free(data);
		fclose(file);
	}

	void Raw_Processor::set_data(unsigned short in)
	{
		unsigned int compo, pos, pos_end;
		unsigned short *data;

		pos_end = this->width * this->length;
		for(compo = 0; compo < this->number_components; compo++){
			data = this->pixel[compo];
			for(pos = 0; pos < pos_end; pos+= 1){
				data[pos] = in;
			}
		}
	}

	unsigned short *Raw_Processor::get_pixel_pointer(int componente, int row)
	{
		unsigned short *back = NULL;
		if(componente >= 0 && (unsigned int)componente < number_components){
			if(row >= 0 && (unsigned int)row < length){
				back = &(pixel[componente][linestride[componente] * row]);
			}
		}


		return back;
	}

	int Raw_Processor::get_pixel_linestride(int componente)
	{
		int back = 0;
		if(componente >= 0 && (unsigned int)componente < number_components){
			back = (int) linestride[componente];
		}

		return back;
	}

	demosaik_AHD_variable::demosaik_AHD_variable()
	{
		int i = 0;

		this->creater					= false;
		this->start_row 				= 0;
		this->number_row				= 0;
		this->block_size				= 0;
		this->filter_extra_size			= 0;
		this->thiss						= NULL;
		this->exception					= NULL;
		this->maximum_read				= NULL;
		this->pos_read					= NULL;
		this->read_write_mutex			= NULL;
		this->start_reading_cond		= NULL;
		this->number_off_threads		= NULL;
		this->arrieved_thread			= NULL;
		this->arrieved_threads_mutex	= NULL;
		this->all_threads_start_cond	= NULL;

		for(i = 0; i < 20; i++){
			buffer[i] = NULL;
		}

		for(i = 0; i < 6; i++){
			i_buffer[i] = NULL;
		}
	}

	demosaik_AHD_variable::~demosaik_AHD_variable()
	{
		int i;

		if(creater){
			if(pos_read != NULL){
				free(pos_read);
				pos_read = NULL;
			}
			if(read_write_mutex != NULL){
				MPT_THREAD_MUTEX_DESTROY(read_write_mutex);
				free(read_write_mutex);
			}

			if(start_reading_cond != NULL){
				MPT_THREAD_COND_DESTROY(start_reading_cond);
				free(start_reading_cond);
			}

			if(arrieved_threads_mutex != NULL){
				MPT_THREAD_MUTEX_DESTROY(arrieved_threads_mutex);
				free(arrieved_threads_mutex);
			}

			if(all_threads_start_cond != NULL){
				MPT_THREAD_COND_DESTROY(all_threads_start_cond);
				free(all_threads_start_cond);
			}

			for(i = 0; i < 20; i++){
				delete this->buffer[i];
			}

			for(i = 0; i < 6; i++){
				delete this->i_buffer[i];
			}

			for(i = 0; i < 2; i++){
				delete this->homo_map[i];

			}

			if(lut != NULL){
				free(lut);
			}

		}
	}

	/*
	void demosaik_AHD_variable::init(	int start_row,
											int number_row,
											int block_size,
											int filter_extra_size,
											Raw_Processor *thiss,
											MPT_Exception  *exception,
											int *maximum_read,
											int *pos_read,
											MPT_THREAD_MUTEX_T	*read_write_mutex,
											MPT_THREAD_COND_T	*start_reading_cond,
											int *number_off_threads,
											int *arrieved_thread,
											MPT_THREAD_MUTEX_T	*arrieved_threads_mutex,
											MPT_THREAD_COND_T	*all_threads_start_cond)
	{
		this->start_row				= start_row;
		this->number_row			= number_row;
		this->block_size			= block_size;
		this->filter_extra_size		= filter_extra_size;
		this->thiss					= thiss;
		this->exception				= exception;
		this->maximum_read			= maximum_read;
		this->pos_read				= pos_read;
		this->read_write_mutex		= read_write_mutex;
		this->start_reading_cond	= start_reading_cond;
		this->number_off_threads	= number_off_threads;
		this->arrieved_thread		= arrieved_thread;
		this->arrieved_threads_mutex	= arrieved_threads_mutex;
		this->all_threads_start_cond	= all_threads_start_cond;

	}
*/
	void demosaik_AHD_variable::init(	int block_size,
											int filter_extra_size,
											int number_off_threads_in,
											Raw_Processor *thiss,
											MPT_Exception  *exception)
	{
		int i;
		float r;
		int width, length, half_width, half_length;

		this->block_size			= block_size;
		this->filter_extra_size		= filter_extra_size;
		this->thiss					= thiss;
		this->exception				= exception;
		this->creater				= true;

		read_write_mutex = (MPT_THREAD_MUTEX_T *)malloc(sizeof(MPT_THREAD_MUTEX_T));
		if(read_write_mutex == NULL)
		{
			MPT_EXCEPTION("No Memory");
		}
		MPT_THREAD_MUTEX_INIT(read_write_mutex, NULL);

		start_reading_cond = (MPT_THREAD_COND_T*)malloc(sizeof(MPT_THREAD_COND_T));
		if(start_reading_cond == NULL)
		{
			MPT_EXCEPTION("No Memory");
		}
		MPT_THREAD_COND_INIT(start_reading_cond, NULL);

		arrieved_threads_mutex = (MPT_THREAD_MUTEX_T *)malloc(sizeof(MPT_THREAD_MUTEX_T));
		if(arrieved_threads_mutex == NULL)
		{
			MPT_EXCEPTION("No Memory");
		}
		MPT_THREAD_MUTEX_INIT(arrieved_threads_mutex, NULL);

		all_threads_start_cond= (MPT_THREAD_COND_T*)malloc(sizeof(MPT_THREAD_COND_T));
		if(all_threads_start_cond == NULL)
		{
			MPT_EXCEPTION("No Memory");
		}
		MPT_THREAD_COND_INIT(all_threads_start_cond, NULL);



		pos_read			= (int*)malloc(sizeof(int) * 4);
		maximum_read		= pos_read + 1;
		number_off_threads	= pos_read + 2;
		arrieved_thread		= pos_read + 3;

		*pos_read			= 0;
		*maximum_read		= 0;
		*number_off_threads	= number_off_threads_in;
		*arrieved_thread	= 0;

		width	= (int)thiss->get_Width(exception);
		length	= (int)thiss->get_Length(exception);

		half_width	= width / 2;
		half_length	= length / 2;

		for(i = 0; i < 20; i++){
			buffer[i] = new short_buffer(exception, half_width, half_length, filter_extra_size);
			MPT_EXCEPTION_TEST;
		}

		for(i = 0; i < 6; i++){
			i_buffer[i] = new int_buffer(exception, width, length, 1);
			MPT_EXCEPTION_TEST;
		}

		for(i = 0; i < 2; i++){
			homo_map[i] = new uchar_buffer(exception, width, length, 1);
			MPT_EXCEPTION_TEST;
		}

		lut = (float*)malloc(sizeof(float) * 0x10000);
		if(lut == NULL){
			MPT_EXCEPTION("No Memory");
		}


		for (i=0; i < 0x10000; i++) {
		    r = i / 65535.0;
		    lut[i] = r > 0.008856 ? pow(r,1/3.0) : 7.787*r + 16/116.0;
		  }
	}

	void demosaik_AHD_variable::init(	demosaik_AHD_variable *variable)
	{
		int i;
		this->block_size				= variable->block_size;
		this->filter_extra_size			= variable->filter_extra_size;
		this->thiss						= variable->thiss;
		this->exception					= variable->exception;
		this->maximum_read				= variable->maximum_read;
		this->pos_read					= variable->pos_read;
		this->read_write_mutex			= variable->read_write_mutex;
		this->start_reading_cond		= variable->start_reading_cond;
		this->number_off_threads		= variable->number_off_threads;
		this->arrieved_thread			= variable->arrieved_thread;
		this->arrieved_threads_mutex	= variable->arrieved_threads_mutex;
		this->all_threads_start_cond	= variable->all_threads_start_cond;
		this->lut						= variable->lut;

		for(i = 0; i < 20; i++){
			this->buffer[i] = variable->buffer[i];
		}

		for(i = 0; i < 6; i++){
			this->i_buffer[i] = variable->i_buffer[i];
		}

		for(i = 0; i < 2; i++){
			this->homo_map[i] = variable->homo_map[i];
		}


	}

	void demosaik_AHD_variable::wait_for_all_threads()
	{
		MPT_THREAD_MUTEX_LOCK(arrieved_threads_mutex);
		*arrieved_thread += 1;
		if(*arrieved_thread == *number_off_threads){
			*arrieved_thread 	= 0;
			*pos_read			= 0;
			MPT_THREAD_COND_BROADCAST(all_threads_start_cond);
		}else{
			MPT_THREAD_COND_WAIT(all_threads_start_cond, arrieved_threads_mutex)
		}

		MPT_THREAD_MUTEX_UNLOCK(arrieved_threads_mutex);
	}

	void demosaik_AHD_variable::set_maximum_read_length(unsigned int maximum)
	{
		MPT_THREAD_MUTEX_LOCK(read_write_mutex);
		*maximum_read = maximum;
		MPT_THREAD_COND_BROADCAST(start_reading_cond);
		MPT_THREAD_MUTEX_UNLOCK(read_write_mutex);
	}

	bool demosaik_AHD_variable::get_next_block()
	{
		bool	has_data_block 	= false;
		bool	no_more_data	= false;
		int length 	= (int)thiss->get_Length(exception);

		MPT_THREAD_MUTEX_LOCK(read_write_mutex);

		while(!has_data_block){


			if(*pos_read + block_size + filter_extra_size < *maximum_read){
				start_row	= *pos_read;
				number_row	= block_size;
				*pos_read += block_size;
				has_data_block = true;

			}else if(*pos_read >= length){
				start_row	= *pos_read;
				number_row	= 0;
				has_data_block	= true;
				no_more_data	= true;

			}else if(*maximum_read >= length){
				start_row		= *pos_read;
				number_row		= length - *pos_read;
				*pos_read 		= length;
				has_data_block 	= true;
			}else{
				MPT_THREAD_COND_WAIT(start_reading_cond, read_write_mutex);
			}
		}

		MPT_THREAD_MUTEX_UNLOCK(read_write_mutex);

		return no_more_data;
	}

	void demosaik_AHD_variable::set_variable_demosaic_AHD(variable_demosaic_AHD *variable)
	{
		int i;

		for(i = 0; i < 20; i++){
			variable->buffer[i]		= buffer[i]->get_Row(start_row / 2);
			variable->linestride[i]	= buffer[i]->linestride;
		}

		for(i = 0; i < 6; i++){
			variable->lab_buffer[i] 	= i_buffer[i]->get_Row(start_row);
			variable->lab_linestride[i]	= i_buffer[i]->linestride;
		}

		for(i = 0; i < 2; i++){
			variable->homo_map[i] 				= homo_map[i]->get_Row(start_row);
			variable->homo_map_linestride[i]	= homo_map[i]->linestride;
		}

		for(i = 0; i < 3; i++){
			variable->componente[i] = thiss->get_pixel_pointer(i, start_row);
			variable->componente_linestride[i] = thiss->get_pixel_linestride(i);
		}

		variable->block_size = number_row;
		variable->width	= thiss->get_Width(exception);

		variable->cam_to_XYZ	= thiss->get_CamtoXYZ();
		variable->lab_lut 		= this->lut;
	}

	void Raw_Processor::demosaik_AHD(MPT_Exception *exception)
	{
		demosaik_AHD_variable *thread_variable = NULL;
		int num_threads 		= get_USETHREADS();
		int pos_thread, pos_row, number_row;
		int block_size			= 8;
		int num_blocks			= 0;
		int blocks_per_thread	= 0;

		num_blocks = length / block_size;
		if(length % block_size != 0){
			num_blocks += 1;
		}

		blocks_per_thread = num_blocks / num_threads;
		thread_variable = new demosaik_AHD_variable[num_threads + 1];
		if(thread_variable == NULL)
		{
			MPT_EXCEPTION("No Memory");
		}

		thread_variable[num_threads].init( 	block_size,
											2,
											num_threads,
											this,
											exception);

		MPT_THREAD_CREATE(	&(thread_variable[num_threads].thread),
							NULL,
							&(Raw_Processor::loadRaw_thread),
							&thread_variable[num_threads]);

		// Only for tests.
		//MPT_THREAD_JOIN(thread_variable[num_threads].thread, NULL);

		for(pos_thread = 0, pos_row = 0; pos_thread < num_threads; pos_thread += 1){
			if(pos_thread == num_threads - 1){
				number_row = length - pos_row;
			}else{
				number_row = blocks_per_thread * block_size;
			}

			thread_variable[pos_thread].init(&(thread_variable[num_threads]));

			pos_row += number_row;
			MPT_THREAD_CREATE(	&(thread_variable[pos_thread].thread),
								NULL,
								&(Raw_Processor::demosaik_AHD_start_thread),
								&(thread_variable[pos_thread]));

		}

		for(pos_thread = 0; pos_thread < num_threads; pos_thread += 1){
			MPT_THREAD_JOIN(thread_variable[pos_thread].thread, NULL);
		}

		delete[] thread_variable;

	}

	void *Raw_Processor::demosaik_AHD_start_thread(void *obj)
	{
		demosaik_AHD_variable *variable = (demosaik_AHD_variable*)obj;
		//Raw_Processor *thiss = variable->thiss;
		variable_demosaic_AHD variable_input;

		while(!variable->get_next_block()){
			variable->set_variable_demosaic_AHD(&variable_input);
			demosaic_AHD_default_green(&variable_input);
			//demosaic_AHD_extended_2_green(&variable_input);
		}

		variable->wait_for_all_threads();

		while(!variable->get_next_block()){
			variable->set_variable_demosaic_AHD(&variable_input);
			demosaic_AHD_default_red_blue(&variable_input);
			//demosaic_AHD_extended_red_blue(&variable_input);
		}

		variable->wait_for_all_threads();

		while(!variable->get_next_block()){
			variable->set_variable_demosaic_AHD(&variable_input);
			demosaic_AHD_default_build_homogeneity_map(&variable_input);
		}

		variable->wait_for_all_threads();

		while(!variable->get_next_block()){
			variable->set_variable_demosaic_AHD(&variable_input);
			demosaic_AHD_default_combine_homogenous_pixels(&variable_input);
		}

		variable->wait_for_all_threads();

		return NULL;
	}


}



