#include "rotate.h"

/* Try to get a 32-bit unsigned integer type. */
#ifdef TYPE_32BIT
typedef unsigned TYPE_32BIT __uint32;
#else
#	error "Failed to find a 32-bit integer type."
#endif

/* The code below is copied (with modification) from bits/byteswap.h. It 
 * provides a macro/function named __bswap_32 that swaps the bytes in a 32-bit 
 * integer. If autoconf found support for the bswap assembler instruction, it
 * will be used for optimal performance.
 */

/* Swap bytes in 32 bit value. This is used as a fallback and for constants.  */
#define __bswap_constant_32(x) \
	((((x) & 0xff000000) >> 24) | (((x) & 0x00ff0000) >>  8) | \
	 (((x) & 0x0000ff00) <<  8) | (((x) & 0x000000ff) << 24))

#ifdef __GNUC__
#	if (__GNUC__ >= 2) && (i386 || __i386 || __i386__)
/* We're on an Intel-compatible platform, so we can use inline Intel assembler 
 * for the swapping. 
 */
#		ifndef HAVE_BSWAP
/* Bswap is not available, we have to use three instructions instead. */
#			define __bswap_32(x)						\
				(__extension__						\
				({ register unsigned int __v, __x = (x);		\
				if (__builtin_constant_p (__x))				\
					__v = __bswap_constant_32 (__x);		\
				else							\
					__asm__ ("rorw $8, %w0;"			\
							"rorl $16, %0;"			\
							"rorw $8, %w0"			\
							: "=r" (__v)			\
							: "0" (__x)			\
							: "cc");			\
				__v; }))
#		else
#			define __bswap_32(x)						\
				(__extension__						\
				({ register unsigned int __v, __x = (x);		\
				if (__builtin_constant_p (__x))				\
					__v = __bswap_constant_32 (__x);		\
				else							\
					__asm__ ("bswap %0" : "=r" (__v) : "0" (__x));	\
				__v; }))
#		endif
#	else
/* Non-Intel platform or too old version of gcc. */
#		define __bswap_32(x)							\
			(__extension__							\
			({ register unsigned int __x = (x);				\
			__bswap_constant_32 (__x); }))
#	endif
#else
/* Not a GNU compiler. */
static inline unsigned int __bswap_32 (unsigned int __bsx) {
	return __bswap_constant_32 (__bsx);
}
#endif

/* Reverses a block of memory in-place, 4 bytes at a time. This function
 * requires the __uint32 type, which is 32 bits wide.
 */
void reverse_inplace_quad(char *src, int size) {
	__uint32 *nsrc = (__uint32 *)src;
	__uint32 *ndst = (__uint32 *)(src + size - 4);
	register __uint32 tmp;
	while(nsrc < ndst) {
		tmp = __bswap_32(*ndst);
		*ndst-- = __bswap_32(*nsrc);
		*nsrc++ = tmp;
	}
}

/* Reverses a block of memory in-place, 1 byte at a time. This function
 * is slower than reverse_inplace_quad, and should only be used when the
 * size of the memory block isn't divisible by 4.
 */
void reverse_inplace_single(char *src, int size) {
	register char tmp;
	char *dst = src + size - 1;
	while(src < dst) {
		tmp = *dst;
		*dst-- = *src;
		*src++ = tmp;
	}
}

/* Performs a 90 degrees clockwise rotation of the memory block pointed to 
 * by src. The rotation is not performed in-place; dst must point to a 
 * receiving memory block the same size as src. Parameters width and height
 * are the dimensions of the original block.
 */
inline void rot90cw(char *src, register char *dst, int size, int width, int height) {
	char *endp;
	register char *base;
	int j;

	endp = src + size;
	for(base = endp - width; base < endp; base++) {
		src = base;
		for(j = 0; j < height; j++, src -= width) {
			*dst++ = *src;
		}
	}
}

/* Performs a 90 degrees counterclockwise rotation of the memory block pointed
 * to by src. The rotation is not performed in-place; dst must point to a 
 * receiving memory block the same size as src. Parameters width and height
 * are the dimensions of the original block.
 */
inline void rot90ccw(char *src, register char *dst, int size, int width, int height) {
	char *endp;
	register char *base;
	int j;

	endp = src + size;
	dst = dst + size - 1;
	for(base = endp - width; base < endp; base++) {
		src = base;
		for(j = 0; j < height; j++, src -= width) {
			*dst-- = *src;
		}
	}
}

/* Initializes rotation data - allocates memory and determines which function
 * to use for 180 degrees rotation.
 */
void rotate_init(struct context *cnt) {
	int size, multiple;
	
	/* Make sure temp_buf isn't freed if it hasn't been allocated. */
	cnt->rotate_data.temp_buf = NULL;

	/* Assign the value in conf.rotate_deg to rotate_data.degrees. This way,
	 * we have a value that is safe from changes caused by motion-control.
	 */
	if((cnt->conf.rotate_deg % 90) > 0) {
		cnt->rotate_data.degrees = 0; /* rotate_deg is invalid, so ignore it */
		syslog(LOG_ERR, "Config option \"rotate\" is invalid: %d",
			cnt->conf.rotate_deg);
		return;
	}
	else
		cnt->rotate_data.degrees = cnt->conf.rotate_deg % 360; /* range: 0..359 */

	/* Upon entrance to this function, imgs.width and imgs.height contain the
	 * capture dimensions (as set in the configuration file, or read from a 
	 * netcam source). 
	 *
	 * If rotating 90 or 270 degrees, the capture dimensions and output dimensions
	 * are not the same. Capture dimensions will be contained in cap_width and
	 * cap_height in cnt->rotate_data, while output dimensions will be contained
	 * in imgs.width and imgs.height.
	 */
	/* 1. Transfer capture dimensions into cap_width and cap_height. */
	cnt->rotate_data.cap_width = cnt->imgs.width;
	cnt->rotate_data.cap_height = cnt->imgs.height;
	if((cnt->rotate_data.degrees == 90) || (cnt->rotate_data.degrees == 270)) {
		/* 2. "Swap" imgs.width and imgs.height. */
		cnt->imgs.width = cnt->rotate_data.cap_height;
		cnt->imgs.height = cnt->rotate_data.cap_width;
	}

	/* If we're not rotating, let's exit once we have setup the capture dimensions
	 * and output dimensions properly.
	 */
	if(cnt->rotate_data.degrees == 0)
		return;

	switch(cnt->imgs.type)
	{
	case VIDEO_PALETTE_YUV420P:
		/* For YUV 4:2:0 planar, the memory block used for 90/270 degrees
		 * rotation needs to be width x height x 1.5 bytes large. Also, 
		 * width x height needs to be divisible by 16 for reverse_inplace_quad 
		 * to be used (because the U and V planes must be divisible by 4, and 
		 * they are each four times smaller than the Y plane, which is width x
		 * height bytes in size).
		 */
		size = cnt->imgs.width * cnt->imgs.height * 3 / 2;
		multiple = 16;
		break;
	case VIDEO_PALETTE_GREY:
		/* For greyscale, the memory block used for 90/270 degrees rotation
		 * needs to be width x height bytes large. Also, width x height needs
		 * to be divisible by 4 for reverse_inplace_quad to be used.
		 */
		size = cnt->imgs.width * cnt->imgs.height;
		multiple = 4;
		break;
	default:
		cnt->rotate_data.degrees = 0;
		syslog(LOG_ERR, "Unsupported palette (%d), rotation is disabled.",
			cnt->imgs.type);
		return;
	}

	/* Set the rot80 pointer to point to the appropriate reverse function. */
	if((cnt->imgs.width * cnt->imgs.height) % multiple > 0)
		cnt->rotate_data.rotate_180 = &reverse_inplace_single;
	else
		cnt->rotate_data.rotate_180 = &reverse_inplace_quad;

	/* Allocate memory if rotating 90 or 270 degrees, because those rotations 
	 * cannot be performed in-place (they can, but it would be too slow).
	 */
	if((cnt->rotate_data.degrees == 90) || (cnt->rotate_data.degrees == 270))
		cnt->rotate_data.temp_buf = mymalloc(size);
}

/* Frees resources previously allocated by rotate_init. */
void rotate_deinit(struct context *cnt) {
	if(cnt->rotate_data.temp_buf)
		free(cnt->rotate_data.temp_buf);
}

/* Main entry point for rotation. This is the function that is called from
 * video.c/video_freebsd.c to perform the rotation.
 */
int rotate_map(char *map, struct context *cnt)
{
	/* The image format is either YUV 4:2:0 planar, in which case the pixel 
	 * data is divided in three parts:
	 *	Y - width x height bytes
	 *	U - width x height / 4 bytes
	 *	V - as U
	 * or, it is in greyscale, in which case the pixel data simply consists 
	 * of width x height bytes.
	 */
	int wh, wh4 = 0, w2 = 0, h2 = 0;
	int size, deg;
	int width, height;

	deg = cnt->rotate_data.degrees;
	width = cnt->rotate_data.cap_width;
	height = cnt->rotate_data.cap_height;

	/* Pre-calculate some stuff:
	 *  wh   - size of the Y plane, or the entire greyscale image
	 *  size - size of the entire memory block
	 *  wh4  - size of the U plane, and the V plane
	 *  w2   - width of the U plane, and the V plane
	 *  h2   - as w2, but height instead
	 */
	wh = width * height;
	if(cnt->imgs.type == VIDEO_PALETTE_YUV420P) {
		size = wh * 3 / 2;
		wh4 = wh / 4;
		w2 = width / 2;
		h2 = height / 2;
	}
	else { /* VIDEO_PALETTE_GREY */
		size = wh;
	}

	switch(deg) {
	case 90:
		/* first do the Y part */
		rot90cw(map, cnt->rotate_data.temp_buf, wh, width, height);
		if(cnt->imgs.type == VIDEO_PALETTE_YUV420P) {
			/* then do U and V */
			rot90cw(map + wh, cnt->rotate_data.temp_buf + wh, wh4, w2, h2);
			rot90cw(map + wh + wh4, cnt->rotate_data.temp_buf + wh + wh4, wh4, w2, h2);
		}
		
		memcpy(map, cnt->rotate_data.temp_buf, size);

		break;
		
	case 180:
		/* 180 degrees is easy - just reverse the data within
		 * Y, U and V.
		 */
		(*cnt->rotate_data.rotate_180)(map, wh);
		if(cnt->imgs.type == VIDEO_PALETTE_YUV420P)
		{
			(*cnt->rotate_data.rotate_180)(map + wh, wh4);
			(*cnt->rotate_data.rotate_180)(map + wh + wh4, wh4);
		}
		break;

	case 270:

		/* first do the Y part */
		rot90ccw(map, cnt->rotate_data.temp_buf, wh, width, height);
		if(cnt->imgs.type == VIDEO_PALETTE_YUV420P) {
			/* then do U and V */
			rot90ccw(map + wh, cnt->rotate_data.temp_buf + wh, wh4, w2, h2);
			rot90ccw(map + wh + wh4, cnt->rotate_data.temp_buf + wh + wh4, wh4, w2, h2);
		}
		
		memcpy(map, cnt->rotate_data.temp_buf, size);
		break;
		
	default:
		/* invalid */
		return 1;
		;
	}
	
	return 0;
}