/************************************************************************
 * demultiplexing of blocks                                             *
 ************************************************************************/

/*
 *splitframe, get data from TG, find marker in JPEG flow,
 * extract VERBOSErmatios on picture, table and data.
 data from TG : INT1,  frame length = 1 int
 data from TG : INT2,  fixed frame = 10000 bytes
 data to IQ   : OUT1,  block = 64 int
 data to IQ   : OUT2,  size of QTable = 64 int
 data to IQ   : OUT3,  number of block = 1 int
 date to LIBU : OUT4,  picture size = 1 int
 */
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "splitframe.h"

#define my_read(port, ptr, len, p) \
    do {                           \
        Splitframe_State *local1 = (Splitframe_State *)p->local;        \
        memcpy(ptr, local1->fptr, len);      \
        local1->fptr += len;                 \
        local1->fLen -= len;                 \
    } while (0);                               \

#define my_read1(port, ptr, len, local) \
    do {                           \
    memcpy(ptr, local->fptr, len);      \
    local->fptr += len;                 \
    local->fLen -= len;                 \
    } while (0);                               \

int load_huff_tables(const void *Port_p, DOLProcess *p);
int unpack_block2(const void *Port_p0 ,const void *Port_p1, DOLProcess *p, cd_t *comp);
unsigned int get_next_MK2(const void *Port_p ,DOLProcess *p);
int get_bits2(const void *Port_p, DOLProcess *p, int number,
              unsigned long *res);
int get_symbol2(const void *Port_p, DOLProcess *p, int select,
                unsigned char *res);
int get_one_bit2(const void *Port_p, DOLProcess *p, unsigned char *res);
void skip_segment2(const void *Port_p ,DOLProcess *p);
int load_quant_tables2(const void *Port_p0, const void *Port_p1, DOLProcess *p);

unsigned int get_size2(const void *Port_p ,DOLProcess *p) {
    unsigned char aux[2];
    Splitframe_State *local = (Splitframe_State *)p->local;

    my_read1((void*)Port_p, &aux[0], 1*(sizeof(aux[0])), local);
    my_read1((void*)Port_p, &aux[1], 1*(sizeof(aux[1])), local);
    return ((aux[0] << 8) | aux[1]);  /* big endian */
}

/**
 * transform JPEG number format into usual 2's complement format
 */
int reformat(unsigned long s, int good) {
    unsigned int st;
    if (!good)
        return 0;

    st = 1 << (good - 1); //2^(good - 1)
    if (s < st) {
        return (s + 1 + ((-1) << good));
    } else {
        return s;
    }
}


void splitframe_init(DOLProcess *p) {
    Splitframe_State *local = (Splitframe_State *)p->local;
    local->vld.HTable[0] = local->vld.DC_Table0;
    local->vld.HTable[1] = local->vld.DC_Table1;
    local->vld.HTable[2] = local->vld.AC_Table0;
    local->vld.HTable[3] = local->vld.AC_Table1;
    local->num_iter = 0;
}

int splitframe_fire(DOLProcess *p) {
    char * ptr;
    int done;
    unsigned int aux, mark = 0; // initial 0 OK ??????????????
    unsigned char buf, waste;
    int in_frame; //frame started? current component?
    int found_MK; //if marker found while read data found_MK = 1
    int x_size, y_size;  //picture size in pixel units
    int nblock; //picture size in number of MCUs

    Splitframe_State *local = (Splitframe_State *)p->local;

    cd_t comp;    // descriptors for 3 components
    int leftover; // RST check

    // read frame size and frame
    DOL_read((void*)PORT_SPLITFRAME_IN1, &(local->fLen), sizeof(local->fLen), p);
    DOL_read((void*)PORT_SPLITFRAME_IN2, local->frame, sizeof(local->frame), p);
    local->fptr = local->frame;

    //now process segments as they appear first find the SOI marker
    do {
        aux = get_next_MK2((void *)PORT_SPLITFRAME_IN1, p);
    } while (aux != SOI_MK);

    //dbgprintf(VERBOSE, "SPLITFRAME\tFound the SOI marker !\n");
    //dbgprintf(VERBOSE, "SPLITFRAME\tStart picture\n");

    found_MK = 0; //marker already found
    done = 1;
    while (done) {
        if (found_MK == 0)  {
            mark = get_next_MK2((void *)PORT_SPLITFRAME_IN1, p);
        }
        switch (mark) {
        case SOF_MK:

            //dbgprintf(VERBOSE, "SPLITFRAME\tFound the SOF marker\n");

            in_frame = 1;
            found_MK = 0;
            //header size, don't care
            get_size2((void *)PORT_SPLITFRAME_IN1, p);

            //precision, 8bit, don't care
            my_read((void*)PORT_SPLITFRAME_IN1, &waste, 1*(sizeof(waste)), p);

            //load basic image parameters
            y_size = get_size2((void *)PORT_SPLITFRAME_IN1, p);
            x_size = get_size2((void *)PORT_SPLITFRAME_IN1, p);

            //send y_size & x_size to libu
            DOL_write((void*)PORT_SPLITFRAME_OUT4, &y_size, 1*(sizeof(y_size)), p);
            DOL_write((void*)PORT_SPLITFRAME_OUT4, &x_size, 1*(sizeof(x_size)), p);

            local->vld.x_size = x_size;
            local->vld.y_size = y_size;
            local->vld.mx_size = intceil(x_size, MCU_sx);
            local->vld.my_size = intceil(y_size, MCU_sy);

            //dbgprintf(VERBOSE, "\tVLD\tpicture size: y_size=%d, x_size=%d\n",
                    //y_size,x_size);
            //dbgprintf(VERBOSE, "\tVLD\tpicture size: my_size=%d, mx_size=%d\n",
                   //local->vld.my_size, local->vld.mx_size);

            //total number of MCU in picture
            nblock = (y_size/MCU_sy)*(x_size/MCU_sx);

            //dbgprintf(VERBOSE, "\tSPLITFRAME Number of blocks in picture is %d \n", nblock);
            //dbgprintf(VERBOSE, "\tSPLITFRAME Picture size is %d by %d\n", x_size, y_size);
            //dbgprintf(VERBOSE, "\tSPLITFRAME Monochrome JPEG picture!\n");

            //number of components,don't care
            my_read1((void*)PORT_SPLITFRAME_IN1, &waste, 1*(sizeof(waste)), local);

            //component order
            my_read1((void*)PORT_SPLITFRAME_IN1, &buf, 1*(sizeof(buf)), local);

            //sampling factor, don't care
            my_read1((void*)PORT_SPLITFRAME_IN1, &buf, 1*(sizeof(buf)), local);

            //quantization table index,don't care for jfif
            my_read1((void*)PORT_SPLITFRAME_IN1, &buf, 1*(sizeof(buf)), local);

            //send number of block to iq
            DOL_write((void*)PORT_SPLITFRAME_OUT3, &nblock, sizeof(nblock), p);
            break;

        case DHT_MK:
            //dbgprintf(VERBOSE, "SPLITFRAME\tDefining Huffman Tables\n");
            //VLD: loading Huffman table
            load_huff_tables(NULL, p);

            //remove the rest
            while ((ptr = (char *)memchr(local->fptr,0xFF,local->fLen))
                   != NULL) {
                int offset;
                unsigned char pot_mark = *(ptr+1);
                if ((pot_mark != 0) && (pot_mark != 0xD8)) {
                    offset = ptr - local->fptr + 2;
                    local->fLen -= offset;
                    local->fptr += offset;
                    mark = (0xFF00|(unsigned int)pot_mark);
                    //dbgprintf(VERBOSE, "\tSPLITFRAME\tfound marker while in vld=%x! 2\n",mark);
                    found_MK = 1;
                    break;
                }

                offset = ptr - local->fptr + 2;
                local->fLen -= offset;
                local->fptr += offset;
            }

            break;

        case DQT_MK:
            //dbgprintf(VERBOSE, "SPLITFRAME\tDefining Quantization Tables\n");

            load_quant_tables2((void *)PORT_SPLITFRAME_IN1, (void *)PORT_SPLITFRAME_OUT2, p);
            break;

        case DRI_MK:
            //skip size
            get_size2((void *)PORT_SPLITFRAME_IN1, p);
            get_size2((void *)PORT_SPLITFRAME_IN1, p);
            break;

        case SOS_MK:
            //dbgprintf(VERBOSE, "SPLITFRAME\tFound the SOS marker\n");
            get_size2((void *)PORT_SPLITFRAME_IN1, p); // don't care
            get_size2((void *)PORT_SPLITFRAME_IN1, p); // don't care

            my_read1(PORT_SPLITFRAME_IN1, &buf, sizeof(buf), local);
            comp.DC_HT = first_quad(buf);
            comp.AC_HT = second_quad(buf);

            get_size2((void *)PORT_SPLITFRAME_IN1, p); // don't care
            my_read1(PORT_SPLITFRAME_IN1, &buf, sizeof(buf), local);

            local->vld.bit_count = 0; // initialise vld decoder
            comp.PRED = 0; //initialise vld predictor
            leftover = local->vld.mx_size * local->vld.my_size;

            // process till end of row without restarts
            int i;
            for (i=0; i<leftover ;i++) {
                unpack_block2((void *)PORT_SPLITFRAME_IN1,(void *)PORT_SPLITFRAME_OUT1, p, &comp);
            }

            //if picture end normally, EOI marker is send to VLD
            //get_size2((void *)PORT_IN1, p); // don't care

            //remove the rest
            while ((ptr = (char *)memchr(local->fptr,0xFF,local->fLen))
                   != NULL) {
                int offset;
                unsigned char pot_mark = *(ptr+1);
                if ((pot_mark != 0) && (pot_mark != 0xD8)) {
                    offset = ptr - local->fptr + 2;
                    local->fLen -= offset;
                    local->fptr += offset;
                    mark = (0xFF00|(unsigned int)pot_mark);
                    //dbgprintf(VERBOSE, "\tSPLITFRAME\t found marker in data stream to vld:%x\n", mark);
                    found_MK = 1;
                    break;
                }
                offset = ptr - local->fptr + 2;
                local->fLen -= offset;
                local->fptr += offset;
            }

            in_frame = 0;
            break;


        case EOI_MK:
            //dbgprintf(VERBOSE, "SPLITFRAME\tpicture end\n");
            done = 0;
            break;
        case COM_MK:
            //dbgprintf(VERBOSE, "SPLITFRAME\tSkipping comments\n");
            skip_segment2((void *)PORT_SPLITFRAME_IN1, p);
            break;

        case (unsigned int)EOF:
            //dbgprintf(VERBOSE | CASS, "ERROR SPLITFRAME Ran out of input data !\n");
            exit(0);
        default:
            if ((mark & MK_MSK) == APP_MK) {
                //dbgprintf(VERBOSE, "SPLITFRAME\tSkipping application data\n");
                skip_segment2((void *)PORT_SPLITFRAME_IN1, p);
                break;
            }
            if (RST_MK(mark)) {
                //dbgprintf(VERBOSE, "SPLITFRAME\tfound RST Marker\n");
                break;
            }
            done = 0;
            break;
        }
    }

    //end this process
    local->num_iter++;
    if (local->num_iter == NUMBER_OF_FRAMES) {
        DOL_detach(p);
        return 0;
    }

    return  0;
}


int unpack_block2(const void *Port_p0 ,const void *Port_p1,
                         DOLProcess *p, cd_t *comp) {
    unsigned long temp;
    unsigned int i, run, cat;
    int value;
    unsigned char symbol = 0; // is the initial value 0 OK ??????
    int T[64];

    memset((void *)T, 0, sizeof(T)); //zeroize block

    //first get the DC coefficient
    get_symbol2(Port_p0, p, HUFF_ID(DC_CLASS,comp->DC_HT),&symbol);
    get_bits2(Port_p0, p, symbol,&temp);

    value = reformat(temp, symbol);
    value += comp->PRED;
    comp->PRED = value;

    //reoganize and unquantify -> move to ZZ and IQ
    T[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 < 64; i++) {
        get_symbol2(Port_p0, p, HUFF_ID(AC_CLASS, comp->AC_HT), &symbol);

        if (symbol == 0x00) break;
        if (symbol == 0xF0) {
            i += 15;
            continue;
        }
        cat = symbol & 15;
        run = (symbol >> 4) & 15;
        i += run;
        get_bits2(Port_p0, p, cat, &temp);
        value = reformat(temp, cat);
        T[i] = value ;

        //63 is to exit without EOB if last coef non-zero
        if (i == 63) break;
    }

    for (i=0; i<NB_SEND; i++) {
        DOL_write((void *)Port_p1, &T[MAX_SEND*i], MAX_SEND*sizeof(T[0]), p);
    }

    return 0;
}

//utility and counter to return the number of bits from file
//right aligned, masked, first bit towards MSB's
int get_bits2(const void *Port_p, DOLProcess *p, int number,
              unsigned long *res) {
    int i, newbit;
    unsigned long result = 0;
    unsigned char aux, wwindow;
    Splitframe_State *local = (Splitframe_State *)p->local;

    *(res) = 0;
    if (!number) return 0;
    for (i = 0; i < number; i++) {
        if (local->vld.bit_count == 0) {
            my_read((void*)Port_p, &wwindow, sizeof(wwindow), p);
            if (wwindow == 0xFF) {
                my_read((void*)Port_p, &aux, sizeof(aux), p);
                local->vld.bit_count = 0;
            }
            local->vld.bit_count = 8;
        } else
            wwindow = local->vld.window;
        newbit = (wwindow >> 7) & 1;
        local->vld.window = wwindow << 1;
        local->vld.bit_count--;
        result = (result << 1) | newbit;
    }
    *(res)= result;
    return 0;
}

/*-----------------------------------*/
/* extract a single symbol from file */
/* using specified huffman table ... */
/*-----------------------------------*/
int get_symbol2(const void *Port_p, DOLProcess *p, int select,
                unsigned char *res) {
    unsigned char temp;
    long code = 0;
    int length;
    int index;
    Splitframe_State *local = (Splitframe_State *)p->local;

    for (length = 0; length < 16; length++) {
        get_one_bit2(Port_p, p, &temp);

        code = (2 * code) | temp;
        if (code <= local->vld.MaxCode[select][length])
            break;
    }
    index = local->vld.ValPtr[select][length] + code -
        local->vld.MinCode[select][length];
    if (index < MAX_SIZE(select / 2)) {
        *(res)=local->vld.HTable[select][index];
        return 0;
    }
#ifndef CASS
    printf("\tWARNING:\tOverflowing symbol table !\n");
#endif
    return 1;
}

int get_one_bit2(const void *Port_p, DOLProcess *p, unsigned char *res) {
    int newbit;
    unsigned char aux, wwindow;
    Splitframe_State *local = (Splitframe_State *)p->local;

    *(res) = 0;
    if (local->vld.bit_count == 0) {
        my_read((void*)Port_p, &wwindow, sizeof(wwindow), p);
        if (wwindow == 0xFF) {
            my_read((void*)Port_p, &aux, sizeof(aux), p);
            local->vld.bit_count = 0;
        }
        local->vld.bit_count = 8;
    } else
        wwindow = local->vld.window;

    newbit = (wwindow >> 7) & 1;
    local->vld.window = wwindow << 1;
    local->vld.bit_count--;
    *(res) = newbit;
    return 0;
}



//------------------------------------------------------------------------
/* utility and counter to return the number of bits from file */
/* right aligned, masked, first bit towards MSB's               */


//skip a segment we don't want
void skip_segment2(const void *Port_p ,DOLProcess *p)
{
    unsigned int size;
    unsigned char tag[5], waste;
    unsigned int i;

    size = get_size2(Port_p, p);
    if (size > 5) {
        for (i = 0; i < 4; i++)
            my_read((void*)Port_p, &tag[i], 1*(sizeof(tag[i])), p);
        tag[4] = 0;
        size -= 4;
    }
    for(i=0; i<(size - 2); i++)
        my_read((void*)Port_p, &waste, 1*(sizeof(waste)), p);
}

/*----------------------------------------------------------------*/
/* find next marker of any type, returns it, positions just after */
/* EOF instead of marker if end of file met while searching ...   */
/*----------------------------------------------------------------*/
unsigned int get_next_MK2(const void *Port_p ,DOLProcess *p) {
    unsigned char bufp;
    unsigned int c;
    int ffmet = 0;
    int locpassed = -1;

    do {
        my_read((void*)Port_p, &bufp, sizeof(bufp), p);
        c = (unsigned int)bufp;
        switch (c) {
        case 0xFF:
            ffmet = 1;
            break;
        case 0x00:
            ffmet = 0;
            break;
        default:
            if (ffmet){
                //dbgprintf(VERBOSE, "\tSPLITFRAME\tfound marker %x\n",c);
                return (0xFF00 | c);
            }
            ffmet = 0;
            break;
        }
        locpassed++;
    } while (c!= (unsigned int)EOF);
    return (unsigned int)EOF;
}

/*----------------------------------------------------------*/
/* loading and allocating of quantization table */
/* table elements are in ZZ order (same as unpack output) */
/*----------------------------------------------------------*/
int load_quant_tables2(const void *Port_p0, const void *Port_p1, DOLProcess *p)
{
    unsigned char QTable[64]; //quantization tables
    unsigned char aux;
    unsigned int size, n, i;
    Splitframe_State *local = (Splitframe_State *)p->local;

    size = get_size2(Port_p0, p); //this is the table's size
    n = (size - 2) / 65;
    for (i = 0; i < n; i++) {
        my_read((void*)Port_p0, &aux, 1*(sizeof(aux)), p);
        DOL_write((void*)Port_p1, local->fptr, sizeof(QTable), p);
        local->fptr += sizeof(QTable);
    }
    return 0;
}


/*----------------------------------------------------------*/
/* Loading of Huffman table, with leaves drop ability       */
/*----------------------------------------------------------*/
int load_huff_tables(const void *Port_p, DOLProcess *p) {
    unsigned char aux, buf, waste;
    int size, Mclass, id, max;
    int LeavesN, LeavesT, i;
    int AuxCode;
    Splitframe_State *local = (Splitframe_State *)p->local;

    size = get_size2(Port_p, p);/* this is the tables' size */

    size -= 2;
    while ((size > 0))  {
        my_read((void *)Port_p, &aux, 1*(sizeof(aux)), p);

        Mclass = first_quad(aux);  /* AC or DC */
        id = second_quad(aux);    /* table no */

        if (id > 1) {
            //dbgprintf(INFO, "\tERROR:\tBad HTable identity %d!\n", id);
        }

        id = HUFF_ID(Mclass, id);

        //dbgprintf(VERBOSE, "\tVLD\tLoading Table %d\n", id);

        size--;
        LeavesT = 0;
        AuxCode = 0;
        for (i = 0; i < 16; i++) {
            my_read((void *)Port_p, &buf, 1*(sizeof(buf)), p);

            LeavesN = buf;
            local->vld.ValPtr[id][i] = LeavesT;
            local->vld.MinCode[id][i] = AuxCode * 2;
            AuxCode = local->vld.MinCode[id][i] + LeavesN;
            local->vld.MaxCode[id][i] = (LeavesN) ? (AuxCode - 1) : (-1);
            LeavesT += LeavesN;
        }

        size -= 16;
        if (LeavesT > MAX_SIZE(Mclass)) {
            max = MAX_SIZE(Mclass);
            printf("\tWARNING:\tTruncating Table by %d symbols\n",
                   LeavesT - max);
        } else
            max = LeavesT;

        for (i = 0; i < max; i++) { /* get huffman table */
            my_read((void *)Port_p, &buf, 1*(sizeof(buf)), p);
            local->vld.HTable[id][i] = buf;  /* load in raw order */
        }

        for (i = max; i < LeavesT; i++) {
            my_read((void *)Port_p, &waste, 1*(sizeof(waste)), p);
        }
        size -= LeavesT;
        //dbgprintf(VERBOSE, "\tVLD:\tUsing %d words of table memory\n", LeavesT);
    }
    return 0;
}