#include <math.h>
#include "mex.h"

#define Int16 short
#define Uint16 unsigned short

/*
 * Read image data from an array of unsigned 8-bit integers.
 */

static void
geread_i(double *img, double *pbytes,
	 const unsigned char *data, const int Ndata, 
	 const Uint16 width, const Uint16 height, 
	 const Uint16 depth, const Uint16 compress,
	 const double *map_left, const double *map_wide)
{
	unsigned row;
	unsigned bytes=0;
	/*
	 *  For strict compatibility with imagefaq, last_pixel should be Uint16,
	 * but some of the images I have seen seem to include negative pixels.
	 * Uint16 last_pixel=0;
	 */
	int last_pixel=0;
	for (row=0; row<height; ++row) {
		unsigned j;
		unsigned start;
		unsigned end;

		if (compress == 2 || compress == 4) { // packed/compacked
			start=map_left[row];
			end=start+map_wide[row];
		}
		else {
			start=0;
			end=width;
		}
		// Pad the first "empty" part of the line ...
		for (j=0; j<start; j++) img[j*height+row] = (double)0;

		if (compress == 3 || compress == 4) { // compressed/compacked
			while (start<end) {
				unsigned char byte;
				short inc;
				if (bytes >= Ndata) return;
				byte = data[bytes++];
				if (byte & 0x80) {
					unsigned char byte2;
					if (bytes >= Ndata) return;
					byte2 = data[bytes++];
					if (byte & 0x40) {      // next word
						if (bytes >= Ndata) return;
						byte = data[bytes++];
						last_pixel=
						    (((Uint16)byte2<<8)+byte);
					}
					else {                  // 14 bit delta
						if (byte & 0x20) byte|=0xe0;
						else byte&=0x1f;
						inc = (((Int16)byte<<8)+byte2);
						last_pixel += inc;
					}
				}
				else {                          // 7 bit delta
					if (byte & 0x40) byte|=0xc0;
					last_pixel+=(signed char)byte;
				}
				img[row + start*height] = (double)last_pixel;
				++start;
			}
		}
		else {
			while (start<end) {
				unsigned char byte2, byte;
				if (bytes >= Ndata) return;
				byte2 = data[bytes++];
				if (bytes >= Ndata) return;
				byte = data[bytes++];
				last_pixel=(((Uint16)byte2<<8)+byte);
				img[row + start*height] = (double)last_pixel;
				++start;
			}
		}

		// Pad the last "empty" part of the line ...
		for (j=end; j<width; j++) img[j*height+row]=(double)0;
	}

	// Finally, before we return, write the number of bytes to output storage
	*pbytes = bytes;
}


/*
 * This function (classFullPr) does a strict check
 * to see if the variable being called is a non-sparse matrix of the specified data class.
 * If so, it returns a void* to the real portion of the matrix (must be re-cast to the correct type by caller).
 * If not, it calls an error message containing the variable name, *varname.
 */
void *classFullPr(const mxArray *M, const int m, const int n, const char *class, const char *varname)
{
  static char buf[1024];

  if (!mxIsClass(M, class) || mxIsSparse(M) || mxGetM(M) != m || mxGetN(M) != n ) {
    sprintf(buf, "%s must be of type %s, size at least %d by %d\n", varname, class, m, n);
    mexErrMsgTxt(buf);
  }
  return(mxGetData(M));
}



/*
 * Entry function mexFunction:
 * Read and check the input and output variables, 
 * and define, read, and check the global variables.
 * function IMG = GEREAD_I( DATA, SIZE, COMPRESS, LEFT, WIDE); 
 *   data is a vector of type uint8
 *   all others are of type double
 */

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
    double *img, *pbytes, *left, *wide;
    unsigned char *data;
    Uint16 width, height, depth, compress;
    int Ndata;
    mxArray *tmpbytes;

    if (nrhs < 6) { mexErrMsgTxt("geread_i requires at least six input arguments."); }
    if (nlhs < 1) { mexErrMsgTxt("geread_i requires at least 1 output argument"); }

    /* 
     * Get input variables: data, length, height, compress, left, wide
     */
    Ndata = mxGetM(prhs[0]);
    data = (unsigned char *) classFullPr(prhs[0], Ndata, 1, "uint8", "First input variable (DATA)");
    Ndata = (int) mxGetScalar(prhs[1]);
    width = (Uint16) mxGetScalar(prhs[2]);
    height = (Uint16) mxGetScalar(prhs[3]);
    depth = (Uint16) mxGetScalar(prhs[4]);
    compress = (Uint16) mxGetScalar(prhs[5]);

    if (nrhs > 6) {
      left = (double *) classFullPr(prhs[6], 1, (int)height, "double", "Seventh input variable (LEFT)");
      wide = (double *) classFullPr(prhs[7], 1, (int)height, "double", "Eighth input variable (WIDE)");
    } else {
      left = (double *) NULL;
      wide = (double *) NULL;
    }

    /* 
     * Create the output variables; if nlhs<2, the second output arg is a temp
     */
    plhs[0] = mxCreateDoubleMatrix(height, width, mxREAL);
    img = mxGetPr(plhs[0]);
    if (nlhs > 1) {
      plhs[1] = mxCreateDoubleMatrix(1, 1, mxREAL);
      pbytes = mxGetPr(plhs[1]);
    }
    else {
      tmpbytes = mxCreateDoubleMatrix(1, 1, mxREAL);
      pbytes = mxGetPr(tmpbytes);
    }

    /*
     * Call the function geread_i
     */
    geread_i(img, pbytes, data, Ndata, width, height, depth, compress, left, wide);
}