#include "xparameters.h"
#include "stdlib.h"

#include "xbasic_types.h"
#include "shared_mem_if.h"
#include "task_controller_if.h"

#define NUM_TASKS 16
#define SHARED_MEM_BASE_ADDR XPAR_SHARED_MEM_IF_1_BASEADDR
#define TASK_CONTROL_BASE_ADDR XPAR_TASK_CONTROLLER_IF_1_BASEADDR

#define MAX_Y  235L
#define MAX_U  240L
#define MAX_V  240L

#define BLOCK_SIZE 10   // this is the number of pixels that are processed by each task
#define MAX_PIXEL_SIZE 500 //this is the number of pixels in a picture
#define PIC1_YUV_BASE_ADDR 0
#define PIC2_YUV_BASE_ADDR (PIC1_YUV_BASE_ADDR + MAX_PIXEL_SIZE*3)
#define PIC1_RGB_BASE_ADDR (PIC2_YUV_BASE_ADDR + MAX_PIXEL_SIZE*3)
#define PIC2_RGB_BASE_ADDR (PIC1_RGB_BASE_ADDR + MAX_PIXEL_SIZE*3)
#define MERGED_RGB_BASE_ADDR (PIC1_RGB_BASE_ADDR)
#define MERGED_YUV_BASE_ADDR (PIC1_YUV_BASE_ADDR)

/*****************
This function calculates the red byte given Y and V from the YUV color format
****************/
Xuint16 calc_R(Xuint16 Y, Xuint16 V)
{
	Xint32 result;
		
	result = (9535 * ((Xint32)Y - 16) + 13074 * ((Xint32)V - 128)) >> 13;
	
	if (result < 0)
	{
		result = 0;
	}
	else if (result > 255)
	{
		result = 255;
	}
	
	return (Xuint16)result;
}

/*****************
This function calculates the blue byte given Y and U from the YUV color format
****************/
Xuint16 calc_B(Xuint16 Y, Xuint16 U)
{
	Xint32 result;
		
	result = (9535 * ((Xint32)Y - 16) + 16531 * ((Xint32)U - 128)) >> 13;
	
	if (result < 0)
	{
		result = 0;
	}
	else if (result > 255)
	{
		result = 255;
	}
	
	return (Xuint16)result;
}

/*****************
This function calculates the green byte given Y, U, and V from the YUV color format
****************/
Xuint16 calc_G(Xuint16 Y, Xuint16 U, Xuint16 V)
{
	Xint32 result;
		
	result = (9535*((Xint32)Y - 16) - 6660*((Xint32)V-128) - 3203*((Xint32)U - 128)) >> 13;
	
	if (result < 0)
	{
		result = 0;
	}
	else if (result > 255)
	{
		result = 255;
	}
	
	return (Xuint16)result;
}

/*****************
This function calculates the Y of the YUV color format from the RGB color format
****************/
Xuint16 calc_Y(Xuint16 R, Xuint16 G, Xuint16 B)
{
	Xint32 result;
	
	result = ((Xint32)R*2104 + (Xint32)G*4130 + (Xint32)B*802 + 4096 + 131072) >> 13;
	
	if (result > MAX_Y)
	{
		result = MAX_Y;
	}
	
	return (Xuint16)result;
}

/*****************
This function calculates the U of the YUV color format from the RGB color format
****************/
Xuint16 calc_U(Xuint16 R, Xuint16 G, Xuint16 B)
{
	Xint32 result;
	
	result = ((Xint32)R*(-1214) + (Xint32)G*(-2384) + (Xint32)B*3598 + 4096 + 1048576);
	
	if (result < 0)
	{
		result *= -1; //get the absolute value of result
	}
	
	result = result >> 13;
	
	if (result > MAX_U)
	{
		result = MAX_U;
	}
	
	return (Xuint16)result;
}

/*****************
This function calculates the V of the YUV color format from the RGB color format
****************/
Xuint16 calc_V(Xuint16 R, Xuint16 G, Xuint16 B)
{
	Xint32 result;
	
	result = ((Xint32)R*3598 + (Xint32)G*(-3013) + (Xint32)B*(-585) + 4096 + 1048576);
	
	if (result < 0)
	{
		result *= -1; //get the absolute value of result
	}
	
	result = result >> 13;
	
	if (result > MAX_V)
	{
		result = MAX_V;
	}
	
	return (Xuint16)result;
}

/****************
This function generates a random Y value of the YUV color format.
This simulates receiving a Y value from some input stream.
****************/
Xuint16 generate_Y(void)
{
	Xuint32 value;
	value = (Xuint32)((Xuint16)rand()*(Xuint32)MAX_Y)/(Xuint32)0xFFFF;	
	return value;
}

/****************
This function generates a random U value of the YUV color format.
This simulates receiving a U value from some input stream.
****************/
Xuint16 generate_U(void)
{
	Xuint32 value;
	value = (Xuint32)((Xuint16)rand()*(Xuint32)MAX_U)/(Xuint32)0xFFFF;	
	return value;
}

/****************
This function generates a random V value of the YUV color format.
This simulates receiving a V value from some input stream.
****************/
Xuint16 generate_V(void)
{
	Xuint32 value;
	value = (Xuint32)((Xuint16)rand()*(Xuint32)MAX_V)/(Xuint32)0xFFFF;	
	return value;
}

/*********************************
This function returns 1 if the pixel described by the RGB values is considered green,
and returns 0 otherwise.
**********************************/
Xuint8 is_green(Xuint16 R, Xuint16 G, Xuint16 B) 
{
	Xuint8 retVal = 0;
	
	if ((R < 30) && (B < 30) && (G > 100))
	{
		retVal = 1; // this is a green pixel
	}
	
	return retVal;	
}


/*****************
This function is used just to simulate the time it would take to do some calculations, nothing
is actually accomplished by the function.
****************/
Xuint16 busy_work(void)
{
	Xuint16 i, j;
		
	for (i = 0; i < 1000; i++)
	{
		j = i + i;
	}	
	return j;
}

/*********************************
This function executes all of the tasks.  The task number and iteration is passed
to the function.  The particular tasks associated with the task number are executed, and
the addresses of data for the task are dependent on the iteration number.
**********************************/
void task(Xuint16 iteration, Xuint16 task_num)
{
	Xuint16 value1, value2, value3, value4;
	Xuint16 addr1, addr2, addr3;
	Xuint16 i;
	
	//each entry in the switch-case structure represents a particular task in the dataflow graph
	switch (task_num)
	{
	   //task 0 generates Y values for picture 1 and writes it to memory 
		case 0:
			addr1 = PIC1_YUV_BASE_ADDR + iteration * 3 * BLOCK_SIZE;
			for (i = 0; i < BLOCK_SIZE; i++)
			{
				busy_work(); //add a delay here
				value1 = generate_Y(); //generate a Y value
				write_to_shared_mem(SHARED_MEM_BASE_ADDR, addr1, value1);	
				addr1 += 3; //increment by three because Y, U, and V values are stored together				
			}
			break;
		
		//task 1 generates U values for picture 1 and writes it to memory
		case 1:
			addr1 = PIC1_YUV_BASE_ADDR + iteration * 3 * BLOCK_SIZE + 1;
			for (i = 0; i < BLOCK_SIZE; i++)
			{
				busy_work(); //add a delay here
				value1 = generate_U(); //generate a U value
				write_to_shared_mem(SHARED_MEM_BASE_ADDR, addr1, value1);	
				addr1 += 3; //increment by three because Y, U, and V values are stored together				
			}
			break;
			
		//task 2 generates V values for picture 1 and writes it to memory
		case 2:
			addr1 = PIC1_YUV_BASE_ADDR + iteration * 3 * BLOCK_SIZE + 2;
			for (i = 0; i < BLOCK_SIZE; i++)
			{
				busy_work(); //add a delay here
				value1 = generate_V(); //generate a V value
				write_to_shared_mem(SHARED_MEM_BASE_ADDR, addr1, value1);	
				addr1 += 3; //increment by three because Y, U, and V values are stored together				
			}
			break;
			
		//task 3 generates Y values for picture 2 and writes it to memory 
		case 3:
			addr1 = PIC2_YUV_BASE_ADDR + iteration * BLOCK_SIZE * 3;
			for (i = 0; i < BLOCK_SIZE; i++)
			{
				busy_work(); //add a delay here
				value1 = generate_Y(); //generate a Y value
				write_to_shared_mem(SHARED_MEM_BASE_ADDR, addr1, value1);	
				addr1 += 3; //increment by three because Y, U, and V values are stored together				
			}
			break;
		
		//task 4 generates U values for picture 2 and writes it to memory
		case 4:
			addr1 = PIC2_YUV_BASE_ADDR + iteration * 3 * BLOCK_SIZE + 1;
			for (i = 0; i < BLOCK_SIZE; i++)
			{
				busy_work(); //add a delay here
				value1 = generate_U(); //generate a U value
				write_to_shared_mem(SHARED_MEM_BASE_ADDR, addr1, value1);	
				addr1 += 3; //increment by three because Y, U, and V values are stored together				
			}
			break;
			
		//task 5 generates V values for picture 2 and writes it to memory
		case 5:
			addr1 = PIC2_YUV_BASE_ADDR + iteration * 3 * BLOCK_SIZE + 2;
			for (i = 0; i < BLOCK_SIZE; i++)
			{
				busy_work(); //add a delay here
				value1 = generate_V(); //generate a V value
				write_to_shared_mem(SHARED_MEM_BASE_ADDR, addr1, value1);	
				addr1 += 3; //increment by three because Y, U, and V values are stored together				
			}
			break;
			
		//task 6 calculates B values for pixels in picture 1 and writes them to memory
		case 6:
			addr1 = PIC1_YUV_BASE_ADDR + iteration * 3 * BLOCK_SIZE; //this is the address of the first Y value of the block
			addr2 = PIC1_RGB_BASE_ADDR + iteration * 3 * BLOCK_SIZE + 2; //this is the address of where the first B value of the block will be stored
			
			for (i = 0; i < BLOCK_SIZE; i++)
			{
				busy_work(); //add a delay here
				value1 = read_from_shared_mem(SHARED_MEM_BASE_ADDR, addr1); //get Y value
				value2 = read_from_shared_mem(SHARED_MEM_BASE_ADDR, addr1+1); //get U value
				value3 = calc_B(value1, value2); //calculate the B value
				
				write_to_shared_mem(SHARED_MEM_BASE_ADDR, addr2, value3);	
				addr1 += 3; //increment by three because Y, U, and V values are stored together	
				addr2 += 3; //increment by three because R, G, and B values are stored together		
			}			
			break;
		
		//task 7 calculates G values for pixels in picture 1 and writes them to memory
		case 7:
			addr1 = PIC1_YUV_BASE_ADDR + iteration * 3 * BLOCK_SIZE; //this is the address of the first Y value of the block
			addr2 = PIC1_RGB_BASE_ADDR + iteration * 3 * BLOCK_SIZE + 1; //this is the address of where the first G value of the block will be stored
			
			for (i = 0; i < BLOCK_SIZE; i++)
			{
				busy_work(); //add a delay here
				value1 = read_from_shared_mem(SHARED_MEM_BASE_ADDR, addr1); //get Y value
				value2 = read_from_shared_mem(SHARED_MEM_BASE_ADDR, addr1+1); //get U value
				value3 = read_from_shared_mem(SHARED_MEM_BASE_ADDR, addr1+2); //get V value
				
				value4 = calc_G(value1, value2, value3); //calculate the G value
				
				write_to_shared_mem(SHARED_MEM_BASE_ADDR, addr2, value4);	
				addr1 += 3; //increment by three because Y, U, and V values are stored together	
				addr2 += 3; //increment by three because R, G, and B values are stored together		
			}		
			break;
			
		//task 8 calculates R values for pixels in picture 1 and writes them to memory
		case 8:
			addr1 = PIC1_YUV_BASE_ADDR + iteration * 3 * BLOCK_SIZE; //this is the address of the first Y value of the block
			addr2 = PIC1_RGB_BASE_ADDR + iteration * 3 * BLOCK_SIZE; //this is the address of where the first R value of the block will be stored
			
			for (i = 0; i < BLOCK_SIZE; i++)
			{
				busy_work(); //add a delay here
				value1 = read_from_shared_mem(SHARED_MEM_BASE_ADDR, addr1); //get Y value
				value2 = read_from_shared_mem(SHARED_MEM_BASE_ADDR, addr1+2); //get V value
				
				value3 = calc_R(value1, value2); //calculate the R value
				
				write_to_shared_mem(SHARED_MEM_BASE_ADDR, addr2, value3);	
				addr1 += 3; //increment by three because Y, U, and V values are stored together	
				addr2 += 3; //increment by three because R, G, and B values are stored together		
			}		
			break;
			
		//task 9 calculates B values for pixels in picture 2 and writes them to memory
		case 9:
			addr1 = PIC2_YUV_BASE_ADDR + iteration * 3 * BLOCK_SIZE; //this is the address of the first Y value of the block
			addr2 = PIC2_RGB_BASE_ADDR + iteration * 3 * BLOCK_SIZE + 2; //this is the address of where the first B value of the block will be stored
			
			for (i = 0; i < BLOCK_SIZE; i++)
			{
				busy_work(); //add a delay here
				value1 = read_from_shared_mem(SHARED_MEM_BASE_ADDR, addr1); //get Y value
				value2 = read_from_shared_mem(SHARED_MEM_BASE_ADDR, addr1+1); //get U value
				value3 = calc_B(value1, value2); //calculate the B value
				
				write_to_shared_mem(SHARED_MEM_BASE_ADDR, addr2, value3);	
				addr1 += 3; //increment by three because Y, U, and V values are stored together	
				addr2 += 3; //increment by three because R, G, and B values are stored together		
			}			
			break;
		
		//task 10 calculates G values for pixels in picture 2 and writes them to memory
		case 10:
			addr1 = PIC2_YUV_BASE_ADDR + iteration * 3 * BLOCK_SIZE; //this is the address of the first Y value of the block
			addr2 = PIC2_RGB_BASE_ADDR + iteration * 3 * BLOCK_SIZE + 1; //this is the address of where the first G value of the block will be stored
			
			for (i = 0; i < BLOCK_SIZE; i++)
			{
				busy_work(); //add a delay here
				value1 = read_from_shared_mem(SHARED_MEM_BASE_ADDR, addr1); //get Y value
				value2 = read_from_shared_mem(SHARED_MEM_BASE_ADDR, addr1+1); //get U value
				value3 = read_from_shared_mem(SHARED_MEM_BASE_ADDR, addr1+2); //get V value
				
				value4 = calc_G(value1, value2, value3); //calculate the G value
				
				write_to_shared_mem(SHARED_MEM_BASE_ADDR, addr2, value4);	
				addr1 += 3; //increment by three because Y, U, and V values are stored together	
				addr2 += 3; //increment by three because R, G, and B values are stored together		
			}		
			break;
			
		//task 11 calculates R values for pixels in picture 2 and writes them to memory
		case 11:
			addr1 = PIC2_YUV_BASE_ADDR + iteration * 3 * BLOCK_SIZE; //this is the address of the first Y value of the block
			addr2 = PIC2_RGB_BASE_ADDR + iteration * 3 * BLOCK_SIZE; //this is the address of where the first R value of the block will be stored
			
			for (i = 0; i < BLOCK_SIZE; i++)
			{
				busy_work(); //add a delay here
				value1 = read_from_shared_mem(SHARED_MEM_BASE_ADDR, addr1); //get Y value
				value2 = read_from_shared_mem(SHARED_MEM_BASE_ADDR, addr1+2); //get V value
				
				value3 = calc_R(value1, value2); //calculate the R value
				
				write_to_shared_mem(SHARED_MEM_BASE_ADDR, addr2, value3);	
				addr1 += 3; //increment by three because Y, U, and V values are stored together	
				addr2 += 3; //increment by three because R, G, and B values are stored together		
			}		
			break;
		
		//task 12 determines if a pixel in picture 1 is green, and if it is it is replaced by the pixel in picture 2	
		case 12:
			addr1 = PIC1_RGB_BASE_ADDR + iteration * 3 * BLOCK_SIZE; //this is the address of where an R value of pic1 is stored
			addr2 = PIC2_RGB_BASE_ADDR + iteration * 3 * BLOCK_SIZE; //this is the address of where an R value of pic2 is stored
			addr3 = MERGED_RGB_BASE_ADDR + iteration * 3 * BLOCK_SIZE; //this is the address of where an R value of merged pic is stored
			
			for (i = 0; i < BLOCK_SIZE; i++)
			{
				busy_work(); //add a delay here
				value1 = read_from_shared_mem(SHARED_MEM_BASE_ADDR, addr1); //get R value
				value2 = read_from_shared_mem(SHARED_MEM_BASE_ADDR, addr1+1); //get G value
				value3 = read_from_shared_mem(SHARED_MEM_BASE_ADDR, addr1+2); //get B value
				
				//check if the pixel is a green pixel
				if (is_green(value1, value2, value3))
				{
				   //if the pixel is green then read in the corresponding pixel from pic2
					value1 = read_from_shared_mem(SHARED_MEM_BASE_ADDR, addr2); //get R value
					value2 = read_from_shared_mem(SHARED_MEM_BASE_ADDR, addr2+1); //get G value
					value3 = read_from_shared_mem(SHARED_MEM_BASE_ADDR, addr2+2); //get B value
				}
				else
				{
					//this accomplishes nothing, but is done to maintain a consistant number of memory
					//accesses so that a different number of processors in the system can be evaluated
					//for speed-up without the number of memory accesses changing
					read_from_shared_mem(SHARED_MEM_BASE_ADDR, addr2); //get R value
					read_from_shared_mem(SHARED_MEM_BASE_ADDR, addr2+1); //get G value
					read_from_shared_mem(SHARED_MEM_BASE_ADDR, addr2+2); //get B value
				}
				
				//now write the chosen pixel to the merged RGB picture
				write_to_shared_mem(SHARED_MEM_BASE_ADDR, addr3, value1);
				write_to_shared_mem(SHARED_MEM_BASE_ADDR, addr3+1, value2);				
				write_to_shared_mem(SHARED_MEM_BASE_ADDR, addr3+2, value3);
				
				addr1 += 3; //increment by three because R, G, and B values are stored together	
				addr2 += 3; //increment by three because R, G, and B values are stored together
				addr3 += 3; //increment by three because R, G, and B values are stored together				
			}		
			break;
			
		//task 13 calculates Y values for pixels in the merged picture and writes them to memory
		case 13:
			addr1 = MERGED_RGB_BASE_ADDR + iteration * 3 * BLOCK_SIZE; //this is the address of the first R value of the block
			addr2 = MERGED_YUV_BASE_ADDR + iteration * 3 * BLOCK_SIZE; //this is the address of where the first Y value of the block will be stored
			
			for (i = 0; i < BLOCK_SIZE; i++)
			{
				busy_work(); //add a delay here
				value1 = read_from_shared_mem(SHARED_MEM_BASE_ADDR, addr1); //get R value
				value2 = read_from_shared_mem(SHARED_MEM_BASE_ADDR, addr1+1); //get G value
				value3 = read_from_shared_mem(SHARED_MEM_BASE_ADDR, addr1+2); //get B value
				
				value4 = calc_Y(value1, value2, value3); //calculate the Y value
				
				write_to_shared_mem(SHARED_MEM_BASE_ADDR, addr2, value4);	
				
				addr1 += 3; //increment by three because R, G, and B values are stored together		
				addr2 += 3; //increment by three because Y, U, and V values are stored together	
			}	
			break;
		
		//task 14 calculates U values for pixels in the merged picture and writes them to memory
		case 14:
			addr1 = MERGED_RGB_BASE_ADDR + iteration * 3 * BLOCK_SIZE; //this is the address of the first R value of the block
			addr2 = MERGED_YUV_BASE_ADDR + iteration * 3 * BLOCK_SIZE + 1; //this is the address of where the first U value of the block will be stored
			
			for (i = 0; i < BLOCK_SIZE; i++)
			{
				busy_work(); //add a delay here
				value1 = read_from_shared_mem(SHARED_MEM_BASE_ADDR, addr1); //get R value
				value2 = read_from_shared_mem(SHARED_MEM_BASE_ADDR, addr1+1); //get G value
				value3 = read_from_shared_mem(SHARED_MEM_BASE_ADDR, addr1+2); //get B value
				
				value4 = calc_U(value1, value2, value3); //calculate the U value
				
				write_to_shared_mem(SHARED_MEM_BASE_ADDR, addr2, value4);	
				
				addr1 += 3; //increment by three because R, G, and B values are stored together		
				addr2 += 3; //increment by three because Y, U, and V values are stored together	
			}	
			break;
		
		//task 15 calculates V values for pixels in the merged picture and writes them to memory
		case 15:
			addr1 = MERGED_RGB_BASE_ADDR + iteration * 3 * BLOCK_SIZE; //this is the address of the first R value of the block
			addr2 = MERGED_YUV_BASE_ADDR + iteration * 3 * BLOCK_SIZE + 2; //this is the address of where the first V value of the block will be stored
			
			for (i = 0; i < BLOCK_SIZE; i++)
			{
				busy_work(); //add a delay here
				value1 = read_from_shared_mem(SHARED_MEM_BASE_ADDR, addr1); //get R value
				value2 = read_from_shared_mem(SHARED_MEM_BASE_ADDR, addr1+1); //get G value
				value3 = read_from_shared_mem(SHARED_MEM_BASE_ADDR, addr1+2); //get B value
				
				value4 = calc_V(value1, value2, value3); //calculate the V value
				
				write_to_shared_mem(SHARED_MEM_BASE_ADDR, addr2, value4);	
				
				addr1 += 3; //increment by three because R, G, and B values are stored together		
				addr2 += 3; //increment by three because Y, U, and V values are stored together	
			}	
			break;	
	}	
}

int main (void) {

	Xuint16 task_num;
	Xuint16 iteration;
      
	init_shared_mem(SHARED_MEM_BASE_ADDR);	
	init_task_controller(TASK_CONTROL_BASE_ADDR);
	set_executing_flag(TASK_CONTROL_BASE_ADDR, 0);	
		
	task_num = wait_for_next_task(TASK_CONTROL_BASE_ADDR);
	
	while (task_num != TASKS_DONE_FLAG)
	{
		iteration = get_current_iteration(TASK_CONTROL_BASE_ADDR);
		set_executing_flag(TASK_CONTROL_BASE_ADDR, 1);
		task(iteration, task_num);
		set_executing_flag(TASK_CONTROL_BASE_ADDR, 0);
		task_num = wait_for_next_task(TASK_CONTROL_BASE_ADDR);
	}
	
	while (all_tasks_done(TASK_CONTROL_BASE_ADDR) == 0);	
	
   return 0;
}