////////////////////////////////////////////////////////////////////////////////////////
// File   : demux.c  
// Date   : octobre 2015
// author : Alain Greiner
////////////////////////////////////////////////////////////////////////////////////////
// This file define the code of the DEMUX  thread for the MJPEG application.
// This function makes the analysis of the MJPEG stream of bytes: For each 
// compressed image arriving on the <in> input MWMR channel, it dispathch the stream
// on three output MWMR channels:
// - the <out_quanti> channel receive the quantisation table segment.
// - the <out_huff> channel receive the huffman tables.
// - the <out_data> channel receive the compressed bit stream.
// It uses four BUFIO local buffers, connected to the four MWMR channels.
// It search specifically the following 16 bits markers : 
// - the SOI_MK : Start of Image marquer
// - the DQT_MK : Quantization Table marker 
// - the DHT_MK : Huffman Table marker
// - the SOS_MK : Start of Scan marker
// - the EOI_MK : REnd of Image marker
////////////////////////////////////////////////////////////////////////////////////////

#include <stdio.h>
#include <stdint.h>
#include <mwmr_channel.h>
#include "mjpeg.h"

#define SOI_MK	0xFFD8		// start of image
#define APP_MK	0xFFE0		// custom, up to FFEF
#define COM_MK	0xFFFE		// commment segment
#define SOF_MK	0xFFC0		// start of frame
#define SOS_MK	0xFFDA		// start of scan
#define DHT_MK	0xFFC4		// Huffman table
#define DQT_MK	0xFFDB		// Quant. table	
#define EOI_MK	0xFFD9		// end of image		
#define MK_MSK	0xFFF0

// macro to use a shared TTY
#define PRINTF(...)  do { lock_acquire( &tty_lock ); \
                          giet_tty_printf(__VA_ARGS__);  \
                          lock_release( &tty_lock ); } while(0);

///////////////////////////////////////////////////////////
// This function discard all bytes from an input <bufio>, 
// until it found a marker, and returns the marker value.
///////////////////////////////////////////////////////////
uint16_t get_next_marker( mwmr_bufio_t* bufio )
{
    uint16_t   marker;
    uint8_t    byte;
    uint8_t    ff_found = 0;

    do 
    {
        byte = mwmr_bufio_read_byte( bufio );

        if ( ff_found )
        {
            ff_found = 0;
            marker = byte | 0xFF00;

            return marker;
        }
        else if ( byte == 0xFF ) 
        {
            ff_found = 1;
        }
    } while (1);
}
      

//////////////////////////////////////////////////////////////
__attribute__ ((constructor)) void demux( uint32_t index )
//////////////////////////////////////////////////////////////
{
    // get platform parameters
    uint32_t  x_size;
    uint32_t  y_size;
    uint32_t  nprocs;
    giet_procs_number( &x_size , &y_size , &nprocs );

    // get processor coordinates
    uint32_t x, y, p;
    giet_proc_xyp( &x , &y , &p );
 
    // private TTY allocation
    // giet_tty_alloc( 0 );

    PRINTF("\n[MJPEG] thread DEMUX[%d] starts on P[%d,%d,%d] / trdid = %x\n", 
           index , x , y , p, (uint32_t)trdid_demux[index] );

    // initialise BUFIO for MWMR channel <in>
    mwmr_channel_t*   mwmr_in = main_2_demux[index];
    mwmr_bufio_t      bufio_in;
    uint8_t           in_buffer[64];
    mwmr_bufio_init( &bufio_in , in_buffer , 64 , 1 , mwmr_in );

#if (DEBUG_DEMUX > 1)
if ( (index == DEBUG_CLUSTER_INDEX) || (DEBUG_CLUSTER_INDEX == 0XFFFFFFFF) )
PRINTF("\nDEMUX[%d] <in> : &mwmr = %x / &bufio = %x\n", 
         index , mwmr_in , &bufio_in );
#endif

    // initialise BUFIO for MWMR channel <out_quanti>
    mwmr_channel_t*   mwmr_out_quanti = demux_2_iqzz[index];
    mwmr_bufio_t      bufio_out_quanti;
    uint8_t           out_quanti_buffer[64];
    mwmr_bufio_init( &bufio_out_quanti , out_quanti_buffer , 64 , 0 , mwmr_out_quanti );

#if (DEBUG_DEMUX > 1)
if ( (index == DEBUG_CLUSTER_INDEX) || (DEBUG_CLUSTER_INDEX == 0XFFFFFFFF) )
PRINTF("\nDEMUX[%d] : <out_quanti> : mwmr = %x / &bufio = %x\n",
         index , mwmr_out_quanti , &bufio_out_quanti );
#endif

    // initialise BUFIO for MWMR channel <out_huff>
    mwmr_channel_t*   mwmr_out_huff = demux_2_vld_huff[index];
    mwmr_bufio_t      bufio_out_huff;
    uint8_t           out_huff_buffer[64];
    mwmr_bufio_init( &bufio_out_huff , out_huff_buffer , 64 , 0 , mwmr_out_huff );

#if (DEBUG_DEMUX > 1)
if ( (index == DEBUG_CLUSTER_INDEX) || (DEBUG_CLUSTER_INDEX == 0XFFFFFFFF) )
PRINTF("\nDEMUX[%d] : <out_huff> : mwmr = %x / &bufio = %x\n",
         index , mwmr_out_huff , &bufio_out_huff );
#endif

    // initialise BUFIO for MWMR channel <out_data>
    mwmr_channel_t*   mwmr_out_data = demux_2_vld_data[index];
    mwmr_bufio_t      bufio_out_data;
    uint8_t           out_data_buffer[64];
    mwmr_bufio_init( &bufio_out_data , out_data_buffer , 64 , 0 , mwmr_out_data );

#if (DEBUG_DEMUX > 1)
if ( (index == DEBUG_CLUSTER_INDEX) || (DEBUG_CLUSTER_INDEX == 0XFFFFFFFF) )
PRINTF("\nDEMUX[%d] : <out_data> : mwmr = %x / &bufio = %x\n",
         index , mwmr_out_data , &bufio_out_data );
#endif

    uint32_t   found_marker;
    uint32_t   image_done;

    uint16_t   marker;          // 16 bits marker read from bufio_in
    uint32_t   word;            // 32 bits word read from bufio_in
    uint8_t    byte;            // one byte read from bufio_in
    uint32_t   byte_count;      // byte counter in a given compressed image
    
    uint32_t   image = index;

    // infinite loop : one image per iteration
    while ( image < MAX_IMAGES ) 
    {
        // search start of image marker
        do 
        {
            marker = get_next_marker( &bufio_in );

        } 
        while ( marker != SOI_MK );

#if (DEBUG_DEMUX > 1)
if ( (index == DEBUG_CLUSTER_INDEX) || (DEBUG_CLUSTER_INDEX == 0XFFFFFFFF) )
PRINTF("\nDEMUX[%x] found Start of Image marker\n", index );
#endif

        found_marker = 0;
        image_done   = 0;

        // analyse image
        while ( !image_done ) 
        {
            // search next marker if required
            if ( !found_marker ) 
            {
                marker = get_next_marker( &bufio_in );
            }

            ////////////////////////////////////////////
            if ( marker == SOF_MK )   // start of frame
            {

#if (DEBUG_DEMUX > 1)
if ( (index == DEBUG_CLUSTER_INDEX) || (DEBUG_CLUSTER_INDEX == 0XFFFFFFFF) )
PRINTF("\nDEMUX[%x] found Start of Frame marker\n", index );
#endif
                // Only one component per image is supported
                // we check the image size (must fit Frame Buffer),
                // and skip the fixed length frame header

                // skip 3 bytes (Lf , P) from bufio_in
                mwmr_bufio_skip( &bufio_in , 3 );

                // read width & height from bufio_in
                uint32_t height = (uint32_t)mwmr_bufio_read_byte( &bufio_in );
                height = (height<<8) | mwmr_bufio_read_byte( &bufio_in );

                uint32_t width  = (uint32_t)mwmr_bufio_read_byte( &bufio_in );
                width  = (width<<8)  | mwmr_bufio_read_byte( &bufio_in );

                giet_pthread_assert( (fbf_width == width) && (fbf_height == height) ,
                "ERROR in demux() : image size doesn't fit frame buffer size" );

                // read one byte (number of components) from bufio_in
                uint8_t nf  = mwmr_bufio_read_byte( &bufio_in );

                giet_pthread_assert( (nf==1) ,
                "ERROR in demux() : only one component supported" );

                // skip 3 bytes (C0,H0,V0,TQ0)
                mwmr_bufio_skip( &bufio_in , 3 );

                found_marker = 0;
            }
            ///////////////////////////////////////////////////////////
            else if ( marker == DQT_MK )   // quantization table marker
            {

#if (DEBUG_DEMUX > 1)
if ( (index == DEBUG_CLUSTER_INDEX) || (DEBUG_CLUSTER_INDEX == 0XFFFFFFFF) )
PRINTF("\nDEMUX[%x] found Quantization Table marker\n", index );
#endif
                // The  quantisation table segment being fixed length, 
                // we skip the header and write the 64 coefs to out_quanti channel.

                uint32_t  n;

                // skip three first bytes (Lq , PqTq) from bufio_in
                mwmr_bufio_skip( &bufio_in , 3 );

                // write 64 coefs (one byte each) 
                for ( n = 0 ; n < 64 ; n++ )
                {
                    uint8_t byte = mwmr_bufio_read_byte( &bufio_in );
                    mwmr_bufio_write_byte( &bufio_out_quanti , byte );
                }

                // flush out_quanti buffer
                mwmr_bufio_flush( &bufio_out_quanti );

                found_marker = 0;
            }
            ///////////////////////////////////////////////////////
            else if ( marker == DHT_MK )   // Huffman table marker
            {

#if (DEBUG_DEMUX > 1)
if ( (index == DEBUG_CLUSTER_INDEX) || (DEBUG_CLUSTER_INDEX == 0XFFFFFFFF) )
PRINTF("\nDEMUX[%x] found Huffman Table marker\n", index );
#endif
                // The Huffman Table segment being variable length, 
                // we search the next marker, and transfer the complete
                // segment (without marker) to the out_huff channel.

                found_marker = 0;

                while ( !found_marker ) 
                {
                    // read one byte from bufio_in
                    byte = mwmr_bufio_read_byte( &bufio_in );

                    if ( byte == 0xFF )  // potential marker
                    {
                        // read another byte from bufio_in
                        byte = mwmr_bufio_read_byte( &bufio_in );

                        if (byte == 0)  // not a real marker
                        {
                            // write one byte to bufio_out_huff
                            mwmr_bufio_write_byte( &bufio_out_huff, 0xFF );
                        }
                        else            // it's a marker 
                        {
                            marker       = 0xFF00 | byte;
                            found_marker = 1;
                        }
                    }
                    else                // normal byte 
                    {
                        // write one byte to bufio_out_huff
                        mwmr_bufio_write_byte( &bufio_out_huff , byte );
                    }
                }

                // flush out_huff bufio
                mwmr_bufio_flush( &bufio_out_huff );
            } 
            ///////////////////////////////////////////////////////
            else if ( marker == SOS_MK )    // start of scan marker 
            {

#if (DEBUG_DEMUX > 1)
if ( (index == DEBUG_CLUSTER_INDEX) || (DEBUG_CLUSTER_INDEX == 0XFFFFFFFF) )
PRINTF("\nDEMUX[%x] found Start of Scan marker\n", index );
#endif
                // The scan segment has a variable length: 
                // we skip the header, and search the next marker,
                // to transfer the segment (without header) to out_data channel.

                // read 2 bytes (scan header length) from bufio_in
                uint32_t length = (uint32_t)mwmr_bufio_read_byte( &bufio_in );
                length = (length<<8) | mwmr_bufio_read_byte( &bufio_in );

                // skip scan segment header 
                mwmr_bufio_skip( &bufio_in , length-2 );

                found_marker = 0;
                 
                while ( !found_marker ) 
                {
                    // read one byte from bufio_in
                    byte = mwmr_bufio_read_byte( &bufio_in );

                    if ( byte == 0xFF )  // potential marker
                    {
                        // read another byte from bufio_in
                        byte = mwmr_bufio_read_byte( &bufio_in );

                        if ( byte == 0x00 )   // not a real marker
                        {
                            // write one byte to bufio_out_data
                            mwmr_bufio_write_byte( &bufio_out_data, 0xFF);
                        }
                        else                  // it is a marker
                        {
                            marker       = 0xFF00 | byte;
                            found_marker = 1;
                        }
                    }
                    else   // normal byte
                    {
                        // write one byte to bufio_out_data
                        mwmr_bufio_write_byte( &bufio_out_data , byte );
                    }
                }

                // flush out_data bufio
                mwmr_bufio_flush( &bufio_out_data );
            }
            //////////////////////////////////////////////////////
            else if ( marker == EOI_MK )    // end of image marker
            {

#if (DEBUG_DEMUX > 1)
if ( (index == DEBUG_CLUSTER_INDEX) || (DEBUG_CLUSTER_INDEX == 0XFFFFFFFF) )
PRINTF("\nDEMUX[%x] found End of Image marker\n", index );
#endif
                mwmr_bufio_flush( &bufio_out_data );
                image_done = 1;
            }
            ///////////////////////////////////////////////////////////////////////////
            else if ( ((marker & MK_MSK) == APP_MK) ||   // application specific marker 
                      (marker == COM_MK)            )    // comment marker
            {

#if (DEBUG_DEMUX > 1)
if ( (index == DEBUG_CLUSTER_INDEX) || (DEBUG_CLUSTER_INDEX == 0XFFFFFFFF) )
PRINTF("\nDEMUX[%x] found Comment or Application marker\n", index );
#endif
                // read segment length from bufio_in
                uint32_t length = (uint32_t)mwmr_bufio_read_byte( &bufio_in );
                length = (length<<8) | mwmr_bufio_read_byte( &bufio_in );

                // skip segment from bufio_in
                mwmr_bufio_skip( &bufio_in , length - 2 );

                found_marker = 0;
            }
            /////////////////////////////////////////////////////////////////////
            else if ( marker & 0xFFF0 == 0xFFC0 )  // other Start of Frame marker
            {
                giet_pthread_assert( 0 ,
                "ERROR in demux() : only baseline DCT supported");
            }        
            ///////////////////////////
            else   // any other marker
            {
                giet_pthread_assert( 0 ,
                "ERROR in demux() : unsupported marker in MJPEG stream");
            }
        }  // end while ( image_done )

#if DEBUG_DEMUX
if ( (index == DEBUG_CLUSTER_INDEX) || (DEBUG_CLUSTER_INDEX == 0XFFFFFFFF) )
PRINTF("\nDEMUX[%d] completes image %d at cycle %d\n", index , image , giet_proctime() );
#endif
        image = image + x_size * y_size;

    }  // end while on images

    giet_pthread_exit( "DEMUX completed" );

}  // end demux()