#include <stdio.h>
#include <stdlib.h>
#include "picinfo.h"
#include "frame.c"


void subtract(Frame<short> *rn, Frame<u_char> *f1, Frame<u_char> *f2)
{
	short *rny, *rnu, *rnv;
	u_char *f1y, *f1u, *f1v, *f2y, *f2u, *f2v;

	int width  = rn->GetCols();
	int height = rn->GetRows();

#ifdef BOUNDS_CHECK
	if(! f1->sizecmp(f2))
		goto error;
	if(width != f1->GetCols() ||
	   height != f1->GetRows() )
		goto error;

	if(!width || !height)
		goto error;

#endif

	rny = rn->Getydata();
	rnu = rn->Getudata();
	rnv = rn->Getvdata();

	f1y = f1->Getydata();
	f1u = f1->Getudata();
	f1v = f1->Getvdata();

	f2y = f2->Getydata();
	f2u = f2->Getudata();
	f2v = f2->Getvdata();


	for(int i= (width*height/4); i>0 ; i--) {
		*rny++ = short(*f1y++) - short(*f2y++);
		*rny++ = short(*f1y++) - short(*f2y++);
		*rny++ = short(*f1y++) - short(*f2y++);
		*rny++ = short(*f1y++) - short(*f2y++);

		*rnu++ = short(*f1u++) - short(*f2u++);

		*rnv++ = short(*f1v++) - short(*f2v++);

	}
	return;

#ifdef BOUNDS_CHECK      
 error:
	fprintf(stderr, "%s  %s  bounds check failed\n", 
		__FILE__, __PRETTY_FUNCTION__);

#endif

}



void add(Frame<u_char> *result, Frame<short> *f1, Frame<u_char> *f2)
{

	short *f1y, *f1u, *f1v;
	u_char *resulty, *resultu, *resultv, *f2y, *f2u, *f2v;

	int width  = f1->GetCols();
	int height = f1->GetRows();

#ifdef BOUNDS_CHECK
	if(! result->sizecmp(f2))
		goto error;
	if(width != result->GetCols() ||
	   height != result->GetRows() )
		goto error;

	if(!width || !height)
		goto error;

#endif

	resulty = result->Getydata();
	resultu = result->Getudata();
	resultv = result->Getvdata();

	f1y = f1->Getydata();
	f1u = f1->Getudata();
	f1v = f1->Getvdata();

	f2y = f2->Getydata();
	f2u = f2->Getudata();
	f2v = f2->Getvdata();


	for(int i= (width*height/4); i>0 ; i--) {
		*resulty++ = *f1y++ + *f2y++;
		*resulty++ = *f1y++ + *f2y++;
		*resulty++ = *f1y++ + *f2y++;
		*resulty++ = *f1y++ + *f2y++;

		*resultu++ = *f1u++ + *f2u++;

		*resultv++ = *f1v++ + *f2v++;

	}
	return;

#ifdef BOUNDS_CHECK      
 error:
	fprintf(stderr, "%s  %s  bounds check failed\n", 
		__FILE__, __PRETTY_FUNCTION__);

#endif

}




void copy(Frame<u_char> *dst, Frame<u_char> *src) 
{
	u_char *srcy, *srcu, *srcv;
	u_char *dsty, *dstu, *dstv;

	int width  = src->GetCols();
	int height = src->GetRows();

#ifdef BOUNDS_CHECK

	if(width != dst->GetCols() ||
	   height != dst->GetRows() )
		goto error;

	if(!width || !height)
		goto error;

#endif


	dsty = dst->Getydata();
	dstu = dst->Getudata();
	dstv = dst->Getvdata();

	srcy = src->Getydata();
	srcu = src->Getudata();
	srcv = src->Getvdata();


	memcpy(dsty, srcy, width * height);
	memcpy(dstu, srcu, (width * height) >> 2);
	memcpy(dstv, srcv, (width * height) >> 2);

	return;

#ifdef BOUNDS_CHECK      
 error:
	fprintf(stderr, "%s  %s  bounds check failed\n", 
		__FILE__, __PRETTY_FUNCTION__);

#endif


}





void char2shortframe(Frame<short> *result, Frame<u_char> *src, 
		     int yoffset, int uvoffset) 
{
	u_char *srcy, *srcu, *srcv;
	short *resulty, *resultu, *resultv;

	int width  = src->GetCols();
	int height = src->GetRows();
	int width_2 = width/2;

#ifdef BOUNDS_CHECK

	if(width != result->GetCols() ||
	   height != result->GetRows() )
		goto error;

	if(!width || !height)
		goto error;

#endif


	resulty = result->Getydata() + yoffset;
	resultu = result->Getudata() + uvoffset;
	resultv = result->Getvdata() + uvoffset;

	srcy = src->Getydata() + yoffset;
	srcu = src->Getudata() + uvoffset;
	srcv = src->Getvdata() + uvoffset;


	for(int i= YBLKELEMS; i>0 ; i--) {
		for(int j = YBLKELEMS; j>0; j--) 
			*resulty++ = *srcy++;
		
		resulty += width  - YBLKELEMS;
		srcy    += width  - YBLKELEMS;
	}

	for(int i = UVBLKELEMS; i>0 ; i--) {
		for(int j = UVBLKELEMS; j>0; j--) {
			*resultu++ = *srcu++;
			*resultv++ = *srcv++;
		}
		resultu += width_2 - UVBLKELEMS;
		srcu    += width_2 - UVBLKELEMS;
		resultv += width_2 - UVBLKELEMS;
		srcv    += width_2 - UVBLKELEMS;
	}

	
	return;


#ifdef BOUNDS_CHECK      
 error:
	fprintf(stderr, "%s  %s  bounds check failed\n", 
		__FILE__, __PRETTY_FUNCTION__);

#endif


}





void subtract(Frame<short> *rn, Frame<u_char> *f1, 
	      Frame<u_char> *f2, int yoffset, int uvoffset)
{
	short *rny, *rnu, *rnv;
	u_char *f1y, *f1u, *f1v, *f2y, *f2u, *f2v;

	int width  = rn->GetCols();
	int height = rn->GetRows();
	int width_2 = width/2;
	
#ifdef BOUNDS_CHECK
	if(! f1->sizecmp(f2))
		goto error;
	if(width != f1->GetCols() ||
	   height != f1->GetRows() )
		goto error;

	if(!width || !height)
		goto error;

#endif

	rny = rn->Getydata() + yoffset;
	rnu = rn->Getudata() + uvoffset;
	rnv = rn->Getvdata() + uvoffset;

	f1y = f1->Getydata() + yoffset;
	f1u = f1->Getudata() + uvoffset;
	f1v = f1->Getvdata() + uvoffset;

	f2y = f2->Getydata() + yoffset;
	f2u = f2->Getudata() + uvoffset;
	f2v = f2->Getvdata() + uvoffset;


	for(int i=YBLKELEMS; i>0; i--) {
		for(int j=YBLKELEMS; j>0; j--)
			*rny++ = short(*f1y++) - short(*f2y++);
		rny += width - YBLKELEMS;
		f1y += width - YBLKELEMS;
		f2y += width - YBLKELEMS;
	}

	for(int i=UVBLKELEMS; i>0; i--) {
		for(int j=UVBLKELEMS; j>0; j--) {
			*rnu++ = short(*f1u++) - short(*f2u++);
			*rnv++ = short(*f1v++) - short(*f2v++);
		}
		rnu += width_2 - UVBLKELEMS;
		f1u += width_2 - UVBLKELEMS;
		f2u += width_2 - UVBLKELEMS;
		rnv += width_2 - UVBLKELEMS;
		f1v += width_2 - UVBLKELEMS;
		f2v += width_2 - UVBLKELEMS;
	}

	return;

#ifdef BOUNDS_CHECK      
 error:
	fprintf(stderr, "%s  %s  bounds check failed\n", 
		__FILE__, __PRETTY_FUNCTION__);

#endif

}




void add(Frame<u_char> *result, Frame<short> *f1, 
	 Frame<u_char> *f2, int yoffset, int uvoffset)
{

	short *f1y, *f1u, *f1v;
	u_char *resulty, *resultu, *resultv, *f2y, *f2u, *f2v;

	int width  = f1->GetCols();
	int height = f1->GetRows();
	int width_2 = width/2;

#ifdef BOUNDS_CHECK
	if(! result->sizecmp(f2))
		goto error;
	if(width != result->GetCols() ||
	   height != result->GetRows() )
		goto error;

	if(!width || !height)
		goto error;

#endif

	resulty = result->Getydata() + yoffset;
	resultu = result->Getudata() + uvoffset;
	resultv = result->Getvdata() + uvoffset;

	f1y = f1->Getydata() + yoffset;
	f1u = f1->Getudata() + uvoffset;
	f1v = f1->Getvdata() + uvoffset;

	f2y = f2->Getydata() + yoffset;
	f2u = f2->Getudata() + uvoffset;
	f2v = f2->Getvdata() + uvoffset;


	for(int i=YBLKELEMS; i>0; i--) {
		for(int j=YBLKELEMS; j>0; j--)
			*resulty++ = *f1y++ + *f2y++;
		resulty += width - YBLKELEMS;
		f1y     += width - YBLKELEMS;
		f2y     += width - YBLKELEMS;
	}

	for(int i=UVBLKELEMS; i>0; i--) {
		for(int j=UVBLKELEMS; j>0; j--) {
			*resultu++ = *f1u++ + *f2u++;
			*resultv++ = *f1v++ + *f2v++;
		}
		resultu += width_2 - UVBLKELEMS;
		f1u     += width_2 - UVBLKELEMS;
		f2u     += width_2 - UVBLKELEMS;
		resultv += width_2 - UVBLKELEMS;
		f1v     += width_2 - UVBLKELEMS;
		f2v     += width_2 - UVBLKELEMS;
	}

	return;

#ifdef BOUNDS_CHECK      
 error:
	fprintf(stderr, "%s  %s  bounds check failed\n", 
		__FILE__, __PRETTY_FUNCTION__);

#endif

}


void short2charframe(Frame<u_char> *result, Frame<short> *src,
		     int yoffset, int uvoffset)
{
	short *srcy, *srcu, *srcv;
	u_char *resulty, *resultu, *resultv;

	int width  = src->GetCols();
	int height = src->GetRows();
	int width_2 = width/2;
#ifdef BOUNDS_CHECK

	if(width != result->GetCols() ||
	   height != result->GetRows() )
		goto error;

	if(!width || !height)
		goto error;

#endif


	resulty = result->Getydata() + yoffset;
	resultu = result->Getudata() + uvoffset;
	resultv = result->Getvdata() + uvoffset;

	srcy = src->Getydata() + yoffset;
	srcu = src->Getudata() + uvoffset;
	srcv = src->Getvdata() + uvoffset;


	for(int i= YBLKELEMS; i>0 ; i--) {
		for(int j=YBLKELEMS; j>0; j--) 
			*resulty++ = (u_char)*srcy++;

		resulty += width - YBLKELEMS;
		srcy    += width - YBLKELEMS;
	}

	for(int i=UVBLKELEMS; i>0; i--) {
		for(int j=UVBLKELEMS; j>0; j--) {
			*resultu++ = (u_char)*srcu++;
			*resultv++ = (u_char)*srcv++;
		}
		resultu += width_2 - UVBLKELEMS;
		srcu    += width_2 - UVBLKELEMS;
		resultv += width_2 - UVBLKELEMS;
		srcv    += width_2 - UVBLKELEMS;
	}

	return;


#ifdef BOUNDS_CHECK      
 error:
	fprintf(stderr, "%s  %s  bounds check failed\n", 
		__FILE__, __PRETTY_FUNCTION__);

#endif


}



void char2shortframe(Frame<short> *result, Frame<u_char> *src) 
{
	u_char *srcy, *srcu, *srcv;
	short *resulty, *resultu, *resultv;

	int width  = src->GetCols();
	int height = src->GetRows();

#ifdef BOUNDS_CHECK

	if(width != result->GetCols() ||
	   height != result->GetRows() )
		goto error;

	if(!width || !height)
		goto error;

#endif


	resulty = result->Getydata();
	resultu = result->Getudata();
	resultv = result->Getvdata();

	srcy = src->Getydata();
	srcu = src->Getudata();
	srcv = src->Getvdata();


	for(int i= (width*height/4); i>0 ; i--) {
		*resulty++ = *srcy++;
		*resulty++ = *srcy++;
		*resulty++ = *srcy++;
		*resulty++ = *srcy++;

		*resultu++ = *srcu++;

		*resultv++ = *srcv++;

	}
	return;


#ifdef BOUNDS_CHECK      
 error:
	fprintf(stderr, "%s  %s  bounds check failed\n", 
		__FILE__, __PRETTY_FUNCTION__);

#endif


}





void short2charframe(Frame<u_char> *result, Frame<short> *src)
{
	short *srcy, *srcu, *srcv;
	u_char *resulty, *resultu, *resultv;

	int width  = src->GetCols();
	int height = src->GetRows();

#ifdef BOUNDS_CHECK

	if(width != result->GetCols() ||
	   height != result->GetRows() )
		goto error;

	if(!width || !height)
		goto error;

#endif


	resulty = result->Getydata();
	resultu = result->Getudata();
	resultv = result->Getvdata();

	srcy = src->Getydata();
	srcu = src->Getudata();
	srcv = src->Getvdata();


	for(int i= (width*height/4); i>0 ; i--) {
		*resulty++ = (u_char)*srcy++;
		*resulty++ = (u_char)*srcy++;
		*resulty++ = (u_char)*srcy++;
		*resulty++ = (u_char)*srcy++;

		*resultu++ = (u_char)*srcu++;

		*resultv++ = (u_char)*srcv++;

	}
	return;


#ifdef BOUNDS_CHECK      
 error:
	fprintf(stderr, "%s  %s  bounds check failed\n", 
		__FILE__, __PRETTY_FUNCTION__);

#endif


}