// Filename : mjpeg.cpp
//
// This file was initially generated by SpaceStudio but is meant to be modified
// by the user.
#include "mjpeg.h"
#include "spacecomp_pre_cpp.h"

// Do NOT move macros, functions and includes beginning with "spacecomp".

#if SPACE_SIMULATION
static mjpeg* m_self;
static unsigned long long timer_value() {
    return m_self->get_timer_value();
}
#endif

mjpeg::mjpeg(SPACECOMP_CTOR_PARAMS(mjpeg, INDEX))
	: abstract_module(SPACECOMP_MBASE_ARGS(mjpeg, INDEX))
	, SPACECOMP_START_ILIST(mjpeg, INDEX)
#if defined(SPACE_ARCHITECTURE_IMPLEMENTATION) && SPACE_OS == SPACE_OS_LINUX
	, m_throughput()
#elif SPACE_SIMULATION
    , m_throughput(&timer_value)
#endif
{
	init_values();
	SPACECOMP_THREAD(mjpeg, INDEX, thread);
	request_stack(16 * 1024);
}

void mjpeg::thread(SPACECOMP_THREAD_PARAMS(mjpeg, INDEX, thread)) {
	spacecomp_thread_initialize();
	// Prepare the thread's main loop here if needed. Hardware module instances
	// should not have communications (e.g. DeviceRead()) here because some HLS
	// tools consider what comes before the infinite loop to be simple state
	// reset behavior and therefore refuse synthesizing some/all communications
	// here. Place any initialization communications in the "if" below instead.

	unsigned short temp_mk = 0,  mark = 0;
	bool shouldSkip = true;
	unsigned char  pot_mark = 0;
	unsigned int  found_mk = 0, done = 0, end_nb = 0, currentFrame = 0;
	unsigned int header_size = 0;

	while (1) {
		const bool initializing = spacecomp_thread_loop_start();
		if (initializing) {
			// Perform your own initialization communications here as needed.
			// This happens every time the instance is reset, including on
			// initial startup.
			//
			// You may remove this "if" if you do not use it.
			find_info();
		}

		while(currentFrame < m_nbFrames + 1) {
			shouldSkip = true;

			if (temp_mk != SOI_MK) {
				reset_timer();
			}
			// go to the next "start of image" marker
			while(temp_mk != SOI_MK) {
				temp_mk = get_next_mk();
			}

			// Initialisation
			found_mk = 0;
			done = 0;

			while (!done) {
				// On boucle tant qu'on n'a pas trouvé un marqueur
				if (!found_mk)
					mark = get_next_mk();

				// process the new marker
				switch (mark) {
					// "start of frame" marker
					case SOF_MK:
						currentFrame++;

						header_size = read_16_bits(); // header size
						read_8_bits();                // precision, but we ignore it
						m_height = read_16_bits();      // image height
						m_width = read_16_bits();       // image width
						m_nb_comp = read_8_bits();      // nb of components

						// Set global image attributes and diffuse to others core modules
						m_BLOCKS_W = (ROUND_UP(m_width, BLOCK_WIDTH)/BLOCK_WIDTH);
						m_BLOCKS_H = (ROUND_UP(m_height, BLOCK_WIDTH)/BLOCK_HEIGHT);

						for(unsigned int i = 0; i < m_nb_comp; ++i) {
							read_8_bits(); // ignoring component ID (it's always sorted anyway: 1, 2, 3)
							m_HiVi[i] = read_16_bits(); // m_HiVi (1 byte) + Selector (1 byte)
						}

						header_VLD();
						init_LIBU();
						break;

					// Huffman table marker
					case DHT_MK:
						loadTable_VLD();
						break;

					// Quantization table marker
					case DQT_MK:
						if(m_nbQuant == 1) {
							loadTable_IQZZ();
							m_iqzz_data.comp_cnt=0;
							update_values_IQZZ(m_iqzz_data.comp_cnt);
						} else {
							SpacePrint("MJPEG does not support more than one nbQuant. \n");
						}

						break;

					// RESTART INTERVAL MARKER
					case DRI_MK:
						/* skip sizes */
						read_8_bits();
						read_8_bits();
						read_8_bits();
						read_8_bits();

						break;

					// START OF SCAN
					case SOS_MK:
						if (shouldSkip)
							break;

						data_pipeline();
						++m_frames_processed;
						print_timer_value();
						endOfImage(currentFrame);

						while (!found_mk) {
							temp_mk = read_8_bits();

							if (temp_mk != 0) {
								mark = 0xff00|temp_mk;
								found_mk = 1;
								SpacePrint("MJPEG - Frame %d decoded. \n",currentFrame - 1);
							}
						}
						break;

					// END OF IMAGE MARKER
					case EOI_MK:
						// Indique la fin de l'image
						done = 1;
						break;

					// COMMMENT MARKER
					case COM_MK:
						skip_segment();
						break;

					// Traitement par défaut
					default:
						if ((mark&MK_MSK) == APP_MK) {
							shouldSkip = false;
							skip_segment();	// On passe au prochain segment

						}
						else if (RST_MK(mark)) {
							break;
						}
						else {
							done = 1;
							++end_nb;
							break;
						}
				}
			}
		}
		end_application();
	}
}

void mjpeg::init_values() {
	m_BufferValid = false;
	m_uiOffset = 0;
	m_nbHuff = 0;
	m_nbQuant = 0;
	m_height = 0;
	m_width = 0;
	m_nb_comp = 0;
	m_BLOCKS_H = 0;
	m_BLOCKS_W = 0;
}

///////////////////////////////////////////////////////////////////////////////
///
/// skip_segment  -- Permet d'ignorer le segment en cours
///
///////////////////////////////////////////////////////////////////////////////
void mjpeg::skip_segment() {
	unsigned short size = 0;

	// On lit la taille du segment (On enlève les 2 bytes pour la taille)
    size = read_16_bits() - 2;

	// On saute
	skip_bytes(size);
}

//////////////////////////////////////////////////////////////////////////////
///
///	Détermine les informations dynamiquement
///
//////////////////////////////////////////////////////////////////////////////
void mjpeg::find_info() {
	//To simulate it faster, we hard-coded these values.
	m_nbFrames = NB_FRAMES;
	m_nbHuff = 1;
	m_nbQuant = 1;

	// On remet le pointeur au début du fichier
	m_uiOffset = 0;
}

// mjpeg::get_next_mk
// ***********************************************************************
///
/// get_next_mk : returns the next marker in the MJPEGRAM (input image file)
///
/// @return =>  one byte of data
///
// ***********************************************************************
unsigned short mjpeg::get_next_mk() {
    unsigned char buffer = 0x00;
    unsigned short returnValue;

    while(buffer != 0xFF) {
    	buffer = read_8_bits();
    }

    buffer = read_8_bits();
    returnValue = 0xFF00 | buffer;

    return returnValue;
}

unsigned char mjpeg::read_8_bits() {
	unsigned char returnValue;
	unsigned char byteAlignment = (m_uiOffset & 0x3);

	// if the address is a multiple of 4 or if the buffer value is not valid,
	// we need to read a new word (4 bytes) from memory
	if (byteAlignment == 0 || !m_BufferValid) {
		eSpaceStatus eStatus = DeviceRead(MJPEGRAM_ID, m_uiOffset & 0xFFFFFFFC, (uint32_t*)m_Buffer);
        if(eStatus == SPACE_OK) {
        	m_BufferValid = true;
        }
        else {
        	SpacePrint("mjpeg::read_8_bits - Error\n");
        	m_BufferValid = false;
        }
	}

	returnValue = m_Buffer[byteAlignment];

	m_uiOffset++; // increase offset to next byte

	return returnValue;
}

// mjpeg::read_16_bits
// ***********************************************************************
///
/// read_8_bits : reads two bytes from the MJPEGRAM (input image file)
///
/// @return =>  two bytes of data
///
// ***********************************************************************
unsigned short mjpeg::read_16_bits() {
	unsigned short returnValue;
	unsigned char buffer_1;
	unsigned char buffer_2;

	buffer_1 = read_8_bits();
	buffer_2 = read_8_bits();

	returnValue = buffer_1 << 8;
	returnValue |= buffer_2;

	return (returnValue);
}

//////////////////////////////////////////////////////////////////////////////
///
///	Saute des octets
///
//////////////////////////////////////////////////////////////////////////////
void mjpeg::skip_bytes(unsigned short NbBytes) {
	m_uiOffset+=NbBytes;
	m_BufferValid = false;
}

//VLD=====================================================================================================================

///////////////////////////////////////////////////////////////////////////////
///
/// header_VLD
///
///////////////////////////////////////////////////////////////////////////////
void mjpeg::header_VLD() {
	m_vld_data.lumHi = first_quad(m_HiVi[0] >> 8);
	m_vld_data.lumVi = second_quad(m_HiVi[0] >> 8);

	m_libu_data.counter_lumVi_lumHi = m_vld_data.lumVi * m_vld_data.lumHi;

	m_vld_data.mx_size = intceil(m_width, BLOCK_WIDTH);
	m_vld_data.my_size = intceil(m_height, BLOCK_HEIGHT);


}

///////////////////////////////////////////////////////////////////////////////
///
/// intceil
///
///////////////////////////////////////////////////////////////////////////////
int mjpeg::intceil(int N, int D) {
    return (N+D-1)/D;
}

void mjpeg::loadTable_VLD() {
	/* Load Huffman Tables */
	huff_tables_init(&m_vld_data.huff);
	for(int i = 0; i < m_nbHuff; i++) {
		huff_load_tables(&m_vld_data.huff);

	}
}

///////////////////////////////////////////////////////////////////////////////
///
/// huffman table initialization
///
///////////////////////////////////////////////////////////////////////////////
void mjpeg::huff_tables_init(huff_context *cont) {
    int i, j;

    for(j=0; j<16; ++j) {
        for(i=0; i<4; ++i) {
            cont->MinCode[i][j] = 0;
            cont->MaxCode[i][j] = 0;
            cont->ValPtr[i][j] = 0;
        }
        cont->DC_Table0[j] = 0;
        cont->AC_Table0[j] = 0;
        cont->DC_Table1[j] = 0;
        cont->AC_Table1[j] = 0;
    }
    cont->HTable[0] = cont->DC_Table0;
    cont->HTable[1] = cont->DC_Table1;
    cont->HTable[2] = cont->AC_Table0;
    cont->HTable[3] = cont->AC_Table1;
}

///////////////////////////////////////////////////////////////////////////////
///
/// Loading of Huffman table, with leaves drop ability
///
///////////////////////////////////////////////////////////////////////////////
int mjpeg::huff_load_tables(huff_context *cont) {
	// Variable
	unsigned int aux = 0, buf = 0, nothing = 0;
	int classe = 0, id = 0, max = 0, LeavesN = 0, LeavesT = 0, i = 0, AuxCode = 0, size = 0;

	m_BufferValid = false;

	// Enlève les deux bytes indiquant la taille de la table de Huffman
	size = read_16_bits() - 2;

	// Réception de la table de Huffman
	while (size > 0) {
		// Réception du byte auxilière
		aux = read_8_bits();

		// First quad
		classe = first_quad(aux);

		// Table no
		id = second_quad(aux);

		if (id > 1)
			SpacePrint("ERROR: Bad HTable identity %d!\n", id);

		id = HUFF_ID(classe, id);

		if (id > 3)
			SpacePrint("ERROR: Bad computed HTable identity %d!\n", id);

		// Décrémente la taille restante
		--size;

		// Initialisation
		LeavesT = 0;
		AuxCode = 0;

		// Réception de la table
		for(i=0; i<16; i++) {
			buf = read_8_bits();
			LeavesN = buf;
			cont->ValPtr[id][i] = LeavesT;
			cont->MinCode[id][i] = AuxCode<<1;
			AuxCode = cont->MinCode[id][i] + LeavesN;
			cont->MaxCode[id][i] = (LeavesN) ? (AuxCode - 1) : (-1);
			LeavesT += LeavesN;
		}

		// Décrémente la taille restante
		size -= 16;

		// Vérification
		if (LeavesT > MAX_SIZE(classe))
			max = MAX_SIZE(classe);
		else
			max = LeavesT;

		for(i=0; i<max; ++i) {
			/* get huffman table */
			buf = read_8_bits();
			cont->HTable[id][i] = buf;
		}

		// Skip
		for(i=max; i<LeavesT; ++i)
			nothing = read_8_bits();

		// Décrémente la taille restante
		size -= LeavesT;
	}

	// Retourne
	return 0;
}

unsigned short mjpeg::read_data_VLD() {
	unsigned short temp_mk = 0;

	temp_mk = read_8_bits();

	//Devrait seulement arriver à EOI marker
	if (temp_mk == 0xff) {
		temp_mk = read_8_bits();

		if (temp_mk == 0) {
			temp_mk = 0xff;
			return temp_mk;
		}
		else {
			SpacePrint("Error - No more huff. table data \n");
			sc_stop();
		}
	}
	else { /* buf isn't 0xff */
		return temp_mk;
	}

	// Should never get here ;
	// but at least no more warning.
	return (unsigned short)~0;
}

void mjpeg::data_pipeline() {
	unsigned short temp = 0;
	bitreader_context bits;
	int left = 0;

	// Skip some bytes
	temp = read_data_VLD();
	temp = read_data_VLD();
	temp = read_data_VLD();
	temp = read_data_VLD();

	temp = read_data_VLD();
	m_vld_data.comp.DC_HT = first_quad(temp);
	m_vld_data.comp.AC_HT = second_quad(temp);

	if(m_nb_comp > 1) {
		// Second component
		temp = read_data_VLD();
		temp = read_data_VLD();
		m_vld_data.compCb.DC_HT = first_quad(temp);
		m_vld_data.compCb.AC_HT = second_quad(temp);

		// Third component
		temp = read_data_VLD();
		temp = read_data_VLD();
		m_vld_data.compCr.DC_HT = first_quad(temp);
		m_vld_data.compCr.AC_HT = second_quad(temp);
	}
	// Skip 3 bytes;
	temp = read_data_VLD();
	temp = read_data_VLD();
	temp = read_data_VLD();

	bitreader_init(&bits);
	m_vld_data.comp.PRED = 0;
	m_vld_data.compCb.PRED = 0;
	m_vld_data.compCr.PRED = 0;

	left = m_vld_data.mx_size*m_vld_data.my_size;

	while (left > 0) {
		for(unsigned int vi_cnt = 0; vi_cnt < m_vld_data.lumVi; ++vi_cnt) {
			for(unsigned int hi_cnt = 0; hi_cnt < m_vld_data.lumHi; ++hi_cnt) {
				vld_decode_unpack_block(&bits, 1, &m_vld_data.huff, &m_vld_data.comp);
				--left;
			}
		}

		if(m_nb_comp > 1) {
			vld_decode_unpack_block(&bits, 0, &m_vld_data.huff, &m_vld_data.compCb);
			vld_decode_unpack_block(&bits, 0, &m_vld_data.huff, &m_vld_data.compCr);

		}
	}
}

///////////////////////////////////////////////////////////////////////////////
///
/// bitreader_init
///
///////////////////////////////////////////////////////////////////////////////
void mjpeg::bitreader_init(bitreader_context *cont) {
    cont->available = 0;
    cont->current = 0;
    m_vld_data.nb_consecutive_ff = 0;
}

///////////////////////////////////////////////////////////////////////////////
///
/// check_ff
///
///////////////////////////////////////////////////////////////////////////////
void mjpeg::check_ff(unsigned char value) {
    if (value == 0xff)
    	m_vld_data.nb_consecutive_ff++;
    else
    	m_vld_data.nb_consecutive_ff = 0;
}


///////////////////////////////////////////////////////////////////////////////
///
/// here we unpack a 8x8 DCT block
///
///////////////////////////////////////////////////////////////////////////////
void mjpeg::vld_decode_unpack_block(bitreader_context *bits, char mustOutput, huff_context *huff, cd_t *comp) {
	// Variable
	unsigned char symbol;
	unsigned int temp, i, run, cat;
	int value;
	SPACE_ALIGNED short data[BLOCK_SIZE];

	// Initialisation
    for(i = 0; i < BLOCK_SIZE; i++)
    	data[i] = 0;

    /* first get the DC coefficient */
    symbol = huff_get_symbol(bits, huff, HUFF_ID(DC_CLASS,comp->DC_HT));
    temp = bitreader_get(bits, symbol);
    value = reformat(temp, symbol);
    value += comp->PRED;
    comp->PRED = value;

	 /* reoganize and unquantify -> move to ZZ and IQ  */
    data[0] = value;

	 /* then the AC ones if symbol found is EOB and process not finish, missing values are replaced by zero  */
    for(i=1; i<BLOCK_SIZE; i++) {
        symbol = huff_get_symbol(bits, huff, HUFF_ID(AC_CLASS,comp->AC_HT));

        if (symbol == HUFF_EOB)
            break;

        if (symbol == HUFF_ZRL) {
            i += 15;
            continue;
        }

        cat = symbol & 0xf;
        run = symbol >> 4;
        i += run;
        temp = bitreader_get(bits, cat);
        value = reformat(temp, cat);
        data[i] = value;
    }

    operation_IQZZ(data);
}

//////////////////////////////////////////////////////////////////////////////
///
/// extract a single symbol from file using specified huffman table
///
///////////////////////////////////////////////////////////////////////////////
int mjpeg::huff_get_symbol(bitreader_context *cont, huff_context *huff, int select) {
    long code = 0;
    int length = 0,  index = 0;

    for(length=0; length<16; ++length) {
    	unsigned char temp = bitreader_get_one(cont);
        code = (code<<1) | temp;

        if (code <= huff->MaxCode[select][length])
            break;
    }

    index = huff->ValPtr[select][length] + code - huff->MinCode[select][length];

    if (index < MAX_SIZE(select / 2))
        return huff->HTable[select][index];

    SpacePrint("ERROR: Overflowing symbol table with index 0x%x !\n", index);

    return 0;
}

///////////////////////////////////////////////////////////////////////////////
///
/// bitreader_get_one
///
///////////////////////////////////////////////////////////////////////////////
unsigned char mjpeg::bitreader_get_one(bitreader_context *cont) {
    unsigned int ret = 0, tmp;

    if (cont->available == 0) {
    	unsigned int value = read_data_VLD();

		cont->current = value;
        check_ff(cont->current);
        cont->available = 7;
    } else
        --(cont->available);

    tmp = 1<<(cont->available);
    ret = !!(cont->current&tmp);

    return ret;
}

///////////////////////////////////////////////////////////////////////////////
///
/// utility and counter to return the number of bits from file right aligned, masked, first bit towards MSB's
///
///////////////////////////////////////////////////////////////////////////////
unsigned int mjpeg::bitreader_get(bitreader_context *cont, int number) {
    unsigned int ret = 0;

    if (cont->available)
        cont->current &= (1<<cont->available)-1;

    while (number) {
        if (cont->available == 0) {
	    	unsigned int value = read_data_VLD();

			cont->current = value;
            cont->available = 8;
        }

        if (number == cont->available) {
            cont->available = 0;
            ret = (ret<<number) | cont->current;
            break;
        }

        if (number < cont->available) {
            ret = (ret<<number) | (cont->current>>(cont->available-number));
            cont->available -= number;
            break;
        }

        if (number > cont->available) {
            ret = (ret<<cont->available) | cont->current;
            number -= cont->available;
            cont->available = 0;
        }
    }

    return ret;
}
///////////////////////////////////////////////////////////////////////////////
///
/// transform JPEG number format into usual 2's complement format
///
///////////////////////////////////////////////////////////////////////////////
long mjpeg::reformat(unsigned int S, int good) {
    unsigned int ext, sign;

    if (!good)
        return 0;
    sign = !((1<<(good-1))&(S));
    ext = 0-(sign<<good);
    return (S|ext)+sign;
}

//IQZZ=====================================================================================================================
void mjpeg::loadTable_IQZZ() {
	int nbQuantReal = 0;
	int size = 0;

	// load quantization tables
	if(m_nbQuant == 1) {
		m_BufferValid = false;
		unsigned short size = read_16_bits();
		nbQuantReal = (size - 2) / 65; // check if the tables are concatenated: nbQuant = (totalSize - 2BytesSize) / tableSize

		load_multi_quant_table(m_iqzz_data.quantizationTables, nbQuantReal);
	}
	else {
		SpacePrint("MPJEG does not support nbQuant = %d \n", m_nbQuant);
	}
}

///////////////////////////////////////////////////////////////////////////////
///
/// load_multi_quant_table
///
///////////////////////////////////////////////////////////////////////////////
void mjpeg::load_multi_quant_table(unsigned char QuantizationTables[MAX_Q_TABLE][Q_TABLE_SIZE], int nbQuant) {
	m_BufferValid = false;

	// read the quantization table
	for(int i = 0; i < nbQuant; ++i) {
		// Ignore the element precision & destination identifier
		read_8_bits();

		for(int j = 0; j < Q_TABLE_SIZE; ++j)
			QuantizationTables[i][j] = read_8_bits();
	}
}

void mjpeg::update_values_IQZZ(unsigned int comp_cnt) {
	m_iqzz_data.selector = (m_HiVi[comp_cnt] & 0xFF);
	m_iqzz_data.Hi = first_quad(m_HiVi[m_iqzz_data.comp_cnt] >> 8);
	m_iqzz_data.Vi = second_quad(m_HiVi[m_iqzz_data.comp_cnt] >> 8);
	m_iqzz_data.counter_Hi_Vi = m_iqzz_data.Hi * m_iqzz_data.Vi;

}

void mjpeg::operation_IQZZ(short dataInput[]) {
 	SPACE_ALIGNED short data[BLOCK_SIZE];

	for(unsigned int k = 0; k < BLOCK_SIZE; ++k)
			data[G_ZZ[k]] = dataInput[k] * m_iqzz_data.quantizationTables[m_iqzz_data.selector][k];

	m_iqzz_data.counter_Hi_Vi--;
	if(m_iqzz_data.counter_Hi_Vi == 0){
		m_iqzz_data.comp_cnt ++;
		if(m_iqzz_data.comp_cnt  == 3)
			m_iqzz_data.comp_cnt = 0;

		update_values_IQZZ(m_iqzz_data.comp_cnt);
	}

	operation_IDCT(data);
}

//IDCT==========================================================================================================
void mjpeg::operation_IDCT(short dataInput[]) {
	int Y[BLOCK_SIZE];
	SPACE_ALIGNED unsigned char Idct[BLOCK_SIZE];

	for(int row = 0; row < BLOCK_HEIGHT; row++) {
		for(int column = 0; column < BLOCK_WIDTH; column++)
			Y(row, column) = dataInput[row*BLOCK_WIDTH+column] << S_BITS;
		idct_1d(&Y(row, 0));
		/* Result Y is scaled up by factor sqrt(8)*2^S_BITS. */
	}

	for(int column = 0; column < BLOCK_WIDTH; column++) {
		int Yc[BLOCK_HEIGHT];

		for(int row = 0; row < BLOCK_HEIGHT; row++)
			Yc[row] = Y(row, column);

		idct_1d(Yc);

		for(int row = 0; row < BLOCK_HEIGHT; row++) {
			/* Result is once more scaled up by a factor sqrt(8). */
			int r = 128 + descale(Yc[row], 2*S_BITS);
			/* Clip to 8 bits unsigned: */
			r = r > 0 ? (r < 255 ? r : 255) : 0;
			Idct_fnc(row, column) = r;
		}
	}

	operation_LIBU(Idct);
}

///////////////////////////////////////////////////////////////////////////////
///
/// rot
///
///////////////////////////////////////////////////////////////////////////////
void mjpeg::rot(int f, int k, int x, int y, int* rx, int* ry) {
#define Cos(k)  COS[f][k]
#define Sin(k)  Cos(8-k)
    *rx = (Cos(k) * x - Sin(k) * y) >> C_BITS;
    *ry = (Sin(k) * x + Cos(k) * y) >> C_BITS;
#undef Cos
#undef Sin
}


///////////////////////////////////////////////////////////////////////////////
///
/// idct_1d
///
///////////////////////////////////////////////////////////////////////////////
void mjpeg::idct_1d(int* Y) {
	// Variable
    int z1[8], z2[8], z3[8];

    /* Stage 1: */
    but(Y[0], Y[4], z1[1], z1[0]);
    rot(1, 6, Y[2], Y[6], &z1[2], &z1[3]);
    but(Y[1], Y[7], z1[4], z1[7]);
    z1[5] = (sqrt2 * Y[3]) >> C_BITS;
    z1[6] = (sqrt2 * Y[5]) >> C_BITS;

    /* Stage 2: */
    but(z1[0], z1[3], z2[3], z2[0]);
    but(z1[1], z1[2], z2[2], z2[1]);
    but(z1[4], z1[6], z2[6], z2[4]);
    but(z1[7], z1[5], z2[5], z2[7]);

    /* Stage 3: */
    z3[0] = z2[0];
    z3[1] = z2[1];
    z3[2] = z2[2];
    z3[3] = z2[3];
    rot(0, 3, z2[4], z2[7], &z3[4], &z3[7]);
    rot(0, 1, z2[5], z2[6], &z3[5], &z3[6]);

    /* Final stage 4: */
    but(z3[0], z3[7], Y[7], Y[0]);
    but(z3[1], z3[6], Y[6], Y[1]);
    but(z3[2], z3[5], Y[5], Y[2]);
    but(z3[3], z3[4], Y[4], Y[3]);
}

//LIBU==========================================================================================================
void mjpeg::operation_LIBU_readY(unsigned char dataInput[]) {
	for(int i = 0; i < BLOCK_SIZE; i++) {
		m_libu_data.Y[(m_libu_data.vi_cnt * m_vld_data.lumVi) + m_libu_data.hi_cnt][i] = dataInput[i];
	}
	m_libu_data.hi_cnt++;
	if(m_libu_data.hi_cnt == m_vld_data.lumHi) {
		m_libu_data.hi_cnt = 0;
		m_libu_data.vi_cnt++;
	}
	if(m_libu_data.vi_cnt == m_vld_data.lumVi) {
		m_libu_data.vi_cnt = 0;
	}
}

void mjpeg::operation_LIBU_readCb(unsigned char dataInput[]) {
	for(int i = 0; i < BLOCK_SIZE; i++) {
		m_libu_data.Cb[i] = dataInput[i];
	}
}

void mjpeg::operation_LIBU_readCr(unsigned char dataInput[]) {
	for(int i = 0; i < BLOCK_SIZE; i++){
		m_libu_data.Cr[i] = dataInput[i];
	}
}

///////////////////////////////////////////////////////////////////////////////
///
/// header_VLD
///
///////////////////////////////////////////////////////////////////////////////
void mjpeg::init_LIBU() {
	m_libu_data.counter_lumVi_lumHi = m_vld_data.lumVi * m_vld_data.lumHi;
	m_libu_data.hi_cnt = 0;
	m_libu_data.vi_cnt = 0;
	m_libu_data.minR = 255;
	m_libu_data.minG = 255;
	m_libu_data.minB = 255;
	m_libu_data.minA = 255;
	m_libu_data.maxR = 0;
	m_libu_data.maxG = 0;
	m_libu_data.maxB = 0;
	m_libu_data.maxA = 0;
	m_libu_data.sumR = 0;
	m_libu_data.sumG = 0;
	m_libu_data.sumB = 0;
	m_libu_data.sumA = 0;
	m_libu_data.nbPixels = 0;
	m_libu_data.macroColumn=0;
	m_libu_data.macroLine=0;
	m_libu_data.frameCnt=0;
}

#define Y_LIBU(x, i, j) m_libu_data.Y[x][i * BLOCK_WIDTH + j]

void mjpeg::calculate_output_pixel(bool grayScale) {
	int offset = 0;
	unsigned int outputPixel;

	for(unsigned int vi_cnt = 0; vi_cnt < m_vld_data.lumVi; ++vi_cnt) {
		for(unsigned int hi_cnt = 0; hi_cnt < m_vld_data.lumHi; ++hi_cnt) {
			for(int i = 0; i < BLOCK_HEIGHT; ++i) {
				for(int j = 0; j < BLOCK_WIDTH; ++j) {
					if(!grayScale) {
						offset = 4 * (((m_vld_data.lumVi * m_libu_data.macroLine + vi_cnt) * BLOCK_HEIGHT * m_width) + (i * m_width) + (m_vld_data.lumHi * m_libu_data.macroColumn * BLOCK_WIDTH) + (hi_cnt * BLOCK_WIDTH) + j);
						if (WIDTH_VGA != m_width)
							offset += 4 * ((int)(offset / 4 / m_width))*(WIDTH_VGA-m_width); // padding to change line

						outputPixel = ycbcr2rgba(Y_LIBU((vi_cnt * m_vld_data.lumVi) + hi_cnt, i, j), m_libu_data.Cb(((vi_cnt * BLOCK_HEIGHT + i) / m_vld_data.lumVi), ((hi_cnt * BLOCK_WIDTH + j) / m_vld_data.lumHi)), m_libu_data.Cr(((vi_cnt * BLOCK_HEIGHT + i) / m_vld_data.lumVi), ((hi_cnt * BLOCK_WIDTH + j) / m_vld_data.lumHi)));
#ifdef SPACE_SIMULATION
						DeviceWrite(VGA_CONTROLLER0_ID, offset, &outputPixel, 1);
#endif
						int R = (outputPixel >> 0) & 0xFF;
						int G = (outputPixel >> 8) & 0xFF;
						int B = (outputPixel >> 16) & 0xFF;
						int A = (outputPixel >> 24) & 0xFF;

						m_libu_data.sumR += R;
						m_libu_data.sumG += G;
						m_libu_data.sumB += B;
						m_libu_data.sumA += A;

						if(R < m_libu_data.minR) m_libu_data.minR = R;
						if(G < m_libu_data.minG) m_libu_data.minG = G;
						if(B < m_libu_data.minB) m_libu_data.minB = B;
						if(A < m_libu_data.minA) m_libu_data.minA = A;

						if(R > m_libu_data.maxR) m_libu_data.maxR = R;
						if(G > m_libu_data.maxG) m_libu_data.maxG = G;
						if(B > m_libu_data.maxB) m_libu_data.maxB = B;
						if(A > m_libu_data.maxA) m_libu_data.maxA = A;

						m_libu_data.nbPixels++;
					} else {
							offset = 4 * (((m_vld_data.lumVi * m_libu_data.macroLine + vi_cnt) * BLOCK_HEIGHT * m_width) + (i * m_width) + (m_vld_data.lumHi * m_libu_data.macroColumn * BLOCK_WIDTH) + (hi_cnt * BLOCK_WIDTH) + j);

							if (WIDTH_VGA != m_width)
								offset += 4 * ((int)(offset / 4 / m_width))*(WIDTH_VGA-m_width); // padding to change line

							outputPixel = grayscale2rgba(Y_LIBU((vi_cnt *  m_vld_data.lumVi) + hi_cnt, i, j));
#ifdef SPACE_SIMULATION
							DeviceWrite(VGA_CONTROLLER0_ID, offset, &outputPixel, 1);
#endif
					}
				}
			}
		}
	}
}

void mjpeg::operation_LIBU_pixel(unsigned char dataInput[]) {
	// Load Ys, Cb and Cr tables
	static bool readCb = true;

	if(m_libu_data.counter_lumVi_lumHi > 0) {
		operation_LIBU_readY(dataInput);
		m_libu_data.counter_lumVi_lumHi --;
	}  else {
		if(m_nb_comp > 1) {
			if(readCb) {
				operation_LIBU_readCb(dataInput);
				readCb = false;
			} else {
				operation_LIBU_readCr(dataInput);
				readCb = true;

				calculate_output_pixel(false);
				m_libu_data.counter_lumVi_lumHi = m_vld_data.lumVi * m_vld_data.lumHi;
				m_libu_data.macroColumn++;
			}
		} else {
			calculate_output_pixel(true);
		}
	}
}

void mjpeg::operation_LIBU(unsigned char dataInput[]) {
	if(m_libu_data.macroColumn == (m_BLOCKS_W / m_vld_data.lumHi)){
		m_libu_data.macroColumn = 0;
		m_libu_data.macroLine++;
	}
	operation_LIBU_pixel(dataInput);
}

void mjpeg::endOfImage(unsigned int currentFrame){
	unsigned long ready = 0;

#ifdef SPACE_SIMULATION
	DeviceWrite(VGA_CONTROLLER0_ID, FRAME_DONE, &currentFrame);
#endif

	// Wait for the VGAController to be ready for the next frame
	do {
#ifdef SPACE_SIMULATION
		DeviceRead(VGA_CONTROLLER0_ID, 0x00000000, &ready);
#endif
	}
	while(ready == 0);
}

//  LIBU::ycbcr2rgba
//////////////////////////////////////////////////////////////////////////////
///
/// This function converts YCbCr values within a valid range of 0-255 to RGBA
/// values within a valid range of 0-255.
///
/// @param  =>  y : luma component
/// @param  =>  cb : blue-difference chroma components
/// @param  =>  cr : red-difference chroma components
///
/// @return =>  RGBA color conversion
//////////////////////////////////////////////////////////////////////////////
unsigned int mjpeg::ycbcr2rgba(unsigned char y, unsigned char cb, unsigned char cr) {
	unsigned int RGBAWord;
	int R, G, B, A;

	R = (((y << 10) + 1436 * (cr - 128)) >> 10);
	G = (((y << 10) - 352  * (cb - 128) - 732 * (cr - 128)) >> 10);
	B = (((y << 10) + 1815 * (cb - 128)) >> 10);
	A = 255;

	if (R < 0)
		R = 0;
	else if (R > 255)
		R = 255;

	if (G < 0)
		G = 0;
	else if (G > 255)
		G = 255;

	if (B < 0)
		B = 0;
	else if (B > 255)
		B = 255;

	//
	// pixels are stored as such : 0xAABBGGRR
	//
	RGBAWord = A;
	RGBAWord <<= 8;
	RGBAWord |= B;
	RGBAWord <<= 8;
	RGBAWord |= G;
	RGBAWord <<= 8;
	RGBAWord |= R;

	return RGBAWord;
}


//
//  LIBU::grayscale2rgba
//////////////////////////////////////////////////////////////////////////////
///
/// This function converts YCbCr values within a valid range of 0-255 to RGBA
/// values within a valid range of 0-255.
///
/// @param  =>  y : luma component
///
/// @return =>  RGBA grayscale conversion
//////////////////////////////////////////////////////////////////////////////
unsigned int mjpeg::grayscale2rgba(unsigned char y) {
	unsigned int RGBAWord;
	int R, G, B, A;

	R = y;
	G = y;
	B = y;
	A = 255;

	//
	// pixels are stored as such : 0xAABBGGRR
	//
	RGBAWord = A;
	RGBAWord <<= 8;
	RGBAWord |= B;
	RGBAWord <<= 8;
	RGBAWord |= G;
	RGBAWord <<= 8;
	RGBAWord |= R;

	return RGBAWord;
}

void mjpeg::reset_timer() {
#if defined(SPACE_SIMULATION) || defined(SPACE_ARCHITECTURE_IMPLEMENTATION) && SPACE_OS == SPACE_OS_LINUX
	m_throughput.reset();
#endif
}

void mjpeg::print_timer_value() {
#if defined(SPACE_SIMULATION) || defined(SPACE_ARCHITECTURE_IMPLEMENTATION) && SPACE_OS == SPACE_OS_LINUX
    throughput_stat stat = m_throughput.get_throughput();
    double delta = stat.delta_time;
    m_total_caculation_delta += delta;

    int whole, thousandths;
	whole = stat.throughput;
	thousandths = (stat.throughput - whole) * 1000;
	SpacePrint("%d.%3d fps\n", whole, thousandths);

    if ((m_frames_processed % 10) == 0) {
    	double fps = m_frames_processed / m_total_caculation_delta;
    	whole = fps;
    	thousandths = (fps - whole) * 1000;
        SpacePrint("%d frames processed so far. Average fps: %d.%3d\n", m_frames_processed, whole, thousandths);
    }
#endif
}

#if SPACE_SIMULATION
unsigned long long mjpeg::get_timer_value() {
#if MAPPING == HW_MAPPING
    return sc_core::sc_time_stamp().value();
#else
    unsigned long low, high, high2;
    unsigned long long llow, lhigh;

    do {
        DeviceRead(SIMULATION_TIMER0_ID, SIMULATION_TIME_HIGH, &high);
        DeviceRead(SIMULATION_TIMER0_ID, SIMULATION_TIME_LOW, &low);
        DeviceRead(SIMULATION_TIMER0_ID, SIMULATION_TIME_HIGH, &high2);
    } while(high2 != high);

    llow = low;
    lhigh = high;
    return lhigh << 32 | llow;
#endif
}
#endif

void mjpeg::end_application() {
	sc_core::sc_stop();
	//exit(0);
}

#include "spacecomp_post_cpp.h"
