/*
 * \file	imageProcessor.c
 * \author	Megyesi, Balazs Karoly
 * \date	19.06.2015.
 *
 * \brief	Different image processor functions.
 */


/* ------------------------------------------------------------ */
/*				Include File Definitions						*/
/* ------------------------------------------------------------ */

#include <limits.h>

#include "arm_neon.h"

#include "xil_types.h"
#include "xil_cache.h"

#include "imageProcessor.h"
#include "vga_modes.h"

/* ------------------------------------------------------------ */
/*				Global constants								*/
/* ------------------------------------------------------------ */

static const s8 filterEdge[3][3] =
{
	{-1, -1, -1},
	{-1,  8, -1},
	{-1, -1, -1}
};

static const s8 filterEmboss[3][3] =
{
	{-2, -1,  0},
	{-1,  1,  1},
	{ 0,  1,  2}
};

/* ------------------------------------------------------------ */
/*				Local function declarations						*/
/* ------------------------------------------------------------ */

// Unused attribute suppresses warning if function is not used
static void BypassImage(rgbType* frameInput, rgbType* frameOutput, int brightness) __attribute__((unused));
static void BypassImageNeon(rgbType* frameInput, rgbType* frameOutput, int brightness) __attribute__((unused));

static void SampleImageProcessing(rgbType* frameInput, rgbType* frameOutput, int contrast) __attribute__((unused));
static void SampleImageProcessingNeon(rgbType* frameInput, rgbType* frameOutput) __attribute__((unused));

/* ------------------------------------------------------------ */
/*				Function implementations						*/
/* ------------------------------------------------------------ */



// 1. feladat -  vilagositas beallitasa


int saturation(int value,  int add)
{

		if((value +add) < 0) return 0;
		else if((value + add) > 255) return 255;
		else{return (value+ add);}

}


/*
 * Wrapper function for different image processing algorithms
 */
void RunImageProcessing(rgbType* frameInput, rgbType* frameOutput)
{
	//BypassImage(frameInput, frameOutput, 1);
	//BypassImageNeon(frameInput, frameOutput, -150);
	//SampleImageProcessing(frameInput, frameOutput, 8);
	SampleImageProcessingNeon(frameInput, frameOutput );
}


/*
 * Bypasses incoming images.
 *
 * Runtime (-O3, -fno-tree-vectorize, -fno-prefetch-loop-arrays): 3.87ms
 */
static void BypassImage(rgbType* frameInput, rgbType* frameOutput, int brightness)
{
	u32 i;

	for (i = 0; i < VMODE_640x480.size; i++)
	{	// Plain pixel copy

		frameOutput[i].red = saturation(frameInput[i].red, brightness);
		frameOutput[i].green = saturation(frameInput[i].green, brightness);
		frameOutput[i].blue = saturation(frameInput[i].blue, brightness);
	}
}


/*
 * Bypasses incoming images with NEON instructions.
 *
 * Runtime (-O3): 2.85ms
 */
static void BypassImageNeon(rgbType* frameInput, rgbType* frameOutput, int brightness)
{
	// Vector variable
	uint8x16x4_t pixelVectorArray;

	u32 i;

	for (i = 0; i < VMODE_640x480.size; i += 16)
	{
		// Load pixel vector
		pixelVectorArray = vld4q_u8((u8*)&frameInput[i]);


		if(brightness > 0)
		{
			pixelVectorArray.val[0] = vqaddq_u8( pixelVectorArray.val[0], vdupq_n_u8(brightness));
			pixelVectorArray.val[1] = vqaddq_u8( pixelVectorArray.val[1], vdupq_n_u8(brightness));
			pixelVectorArray.val[2] = vqaddq_u8( pixelVectorArray.val[2], vdupq_n_u8(brightness));

		}
		else
		{
			pixelVectorArray.val[0] = vqsubq_u8( pixelVectorArray.val[0], vdupq_n_u8(-brightness));
			pixelVectorArray.val[1] = vqsubq_u8( pixelVectorArray.val[1], vdupq_n_u8(-brightness));
			pixelVectorArray.val[2] = vqsubq_u8( pixelVectorArray.val[2], vdupq_n_u8(-brightness));


		}

		// Copy the result back to DDRAM
		vst4q_u8((u8*)&frameOutput[i], pixelVectorArray);
	}
}


/*
 * Sample image processing code. It eliminates green channel.
 *
 * Runtime (-O3, -fno-tree-vectorize, -fno-prefetch-loop-arrays): 4.80ms
 */
static void SampleImageProcessing(rgbType* frameInput, rgbType* frameOutput, int contrast)
{
	u32 i;

	for (i = 0; i < VMODE_640x480.size; i++)
	{


		s16 red = (((frameInput[i].red - 128) * contrast) >> 4) + 128;
		s16 green = (((frameInput[i].green - 128) * contrast) >> 4) + 128;
		s16 blue = (((frameInput[i].blue - 128) * contrast) >> 4) + 128;

		frameOutput[i].red = saturation(red, 0);
		frameOutput[i].green = saturation(green, 0);
		frameOutput[i].blue = saturation(blue, 0);

	}
}


/*
 * Sample image processing code with NEON instructions. It eliminates green channel.
 *
 * Runtime (-O3): 2.91ms
 */





static void SampleImageProcessingNeon(rgbType* frameInput, rgbType* frameOutput)
{




	// Vector variable
	uint8x8x4_t pixelVectorArray;
	int16x8x4_t pixelVectorArray_new;

	u32 i;

	for (i = 0; i < VMODE_640x480.size; i += 8)
	{
		// Load pixel vector
		pixelVectorArray = vld4_u8((u8*)&frameInput[i]);

		// Vector containing green channels is set to zero
		s16 shift = 128;
		s8 contrast = 7;

		for(int i = 0; i < 3; i++)
		{
			pixelVectorArray_new.val[i]  = (int16x8_t)vmovl_u8(pixelVectorArray.val[i]);
			pixelVectorArray_new.val[i] 	= vqsubq_s16(pixelVectorArray_new.val[i],  vdupq_n_s16(shift));
			pixelVectorArray_new.val[i]	= vmulq_n_s16(pixelVectorArray_new.val[i], contrast);
			pixelVectorArray_new.val[i]	= vshrq_n_s16(pixelVectorArray_new.val[i], 4);
			pixelVectorArray_new.val[i] 	= vqaddq_s16(pixelVectorArray_new.val[i], vdupq_n_s16(shift));
			pixelVectorArray.val[i]  	= vqmovun_s16(pixelVectorArray_new.val[i]);


		}

		// Copy the result back to DDRAM
		vst4_u8((u8*)&frameOutput[i], pixelVectorArray);
	}
}