/****************************************************************************************/
/*                                                                                      */
/* Clustered-Dot Ordered Dither                                                         */
/*      This function performs the clustered-dot ordered dithering halftoning           */ 
/*      technique with specified screen orientation and dot size.                       */
/*                                                                                      */
/* Synopsis:                                                                            */
/*      Y=cdod(X, M, N)                                                                 */
/*              X=Continuous tone original of type double.                              */
/*              M=Number of pixels between clusters along horizontal axis.              */
/*              N=Number of pixels between clusters along vertical axis.                */
/*                                                                                      */
/*      Y=cdod(X, M, N, A)                                                              */
/*              A=Screen angle                                                          */
/*                                                                                      */
/* Daniel Leo Lau                                                                       */
/* Copyright June 9, 1998                                                               */
/*                                                                                      */
/****************************************************************************************/ 
#include <math.h>
#include "mex.h"
#define pi 3.14159265358979

int image_row;
int image_col;
int image_chn;
int delta_row;
int delta_col;
int offset;

typedef struct {  
        int row;
        int col;
        double rowp;
        double colp;
        double rowc;
        double colc;
        double cost;
        double input;
} input_pixel;

typedef struct {  
        double rowc;
        double colc;
        int num_pixels;
        double gray_sum;
        int *index;
} centroid_pixel;

input_pixel *cluster_list;

/********************************************************************************/
/*                                                                              */
/********************************************************************************/

void add_to_list(centroid_pixel *centroid_list, input_pixel element,
                 int index, double angle)
{
        int Rc, Cc, m;
	double Rcp, Ccp, radius, theta;

	Rcp=element.rowc;
	Ccp=element.colc;

	radius=sqrt(Rcp*Rcp+Ccp*Ccp);
	theta=atan2(Rcp, Ccp);

	Rc=floor(radius*sin(theta+angle)+0.5)+offset;
	Cc=floor(radius*cos(theta+angle)+0.5)+offset;

	if (Rc<0 || Rc>=(image_row+2*offset) || Cc<0 || Cc>=(image_col+2*offset)) return;

	m=Rc+Cc*(image_row+2*offset);
	if (centroid_list[m].num_pixels==0){
	        centroid_list[m].index=(int*)mxCalloc(delta_row*delta_col*2, sizeof(int));
		centroid_list[m].index[0]=index;
		centroid_list[m].num_pixels=1;
		centroid_list[m].rowc=Rcp;
		centroid_list[m].colc=Ccp;
		centroid_list[m].gray_sum=element.input;
	}
	else{
		centroid_list[m].index[centroid_list[m].num_pixels]=index;
		centroid_list[m].num_pixels++;
		centroid_list[m].gray_sum+=element.input;
	}
	return;
}

/********************************************************************************/
/*                                                                              */
/********************************************************************************/

void hpsort(int n, input_pixel *ra)
{
        int i, ir, j, l;
        input_pixel rra;

        if (n<2) return;
        l=(n >> 1)+1;
        ir=n;

        for( ; ; ){
                if (l>1){
                        --l;
                        rra=ra[l-1];
                        }
                else {
                        rra=ra[ir-1];
                        ra[ir-1]=ra[0];
                        if (--ir==1){
                                ra[0]=rra;
                                break;
                                }
                        }
                i=l;
                j=l+l;
                while(j<=ir){
                        if (j < ir && ra[j-1].cost < ra[j].cost)j++;
                        if (rra.cost < ra[j-1].cost){
                                ra[i-1]=ra[j-1];
                                i=j;
                                j<<=1;
                                }
                        else j=ir+1;
                        }
                ra[i-1]=rra;
                }
        return;
}

/********************************************************************************/
/*                                                                              */
/********************************************************************************/

void process_list(centroid_pixel *centroid_list, input_pixel *image_list, unsigned char *output_image)
{
        int m, n, num_off_pixels;
 
	for (n=0; n<(image_row+2*offset)*(image_col+2*offset); n++){
	  if (centroid_list[n].num_pixels>0){
	    for (m=0; m<centroid_list[n].num_pixels; m++){
	      cluster_list[m]=image_list[((centroid_list[n]).index)[m]];
	    }
	    hpsort(centroid_list[n].num_pixels, cluster_list);
	    num_off_pixels=floor(centroid_list[n].gray_sum+0.5);
	    for (m=0; m<num_off_pixels; m++){
	      output_image[cluster_list[m].row+cluster_list[m].col*image_row]=0;
	    }
	  }
	}
	return;
}

/********************************************************************************/
/*                                                                              */
/********************************************************************************/

void print_list(centroid_pixel *centroid_list)
{
        int m;

	for (m=0; m<(image_row+2*offset)*(image_col+2*offset); m++){
	  if (centroid_list[m].num_pixels!=0){
	    mexPrintf("ROW: %5f COL: %5f\n", centroid_list[m].rowc, centroid_list[m].colc);
	    mexPrintf("NUM PIXELS: %d\n\n", centroid_list[m].num_pixels);
	  }
	}
	return;
}

/********************************************************************************/
/*                                                                              */
/********************************************************************************/

void cdod(unsigned char *output_image, double *input_image, double angle)
{
        int m,n;
	double radius, theta;
	centroid_pixel *centroid_list;
	input_pixel *image_list;

	cluster_list=(input_pixel*)mxCalloc(delta_row*delta_col*2, sizeof(input_pixel));
	image_list=(input_pixel*)mxCalloc(image_row*image_col, sizeof(input_pixel));
	centroid_list=(centroid_pixel*)mxCalloc((image_row+2*offset)*(image_col+2*offset), sizeof(centroid_pixel));
	for (m=0; m<image_row; m++){
	  for (n=0; n<image_col; n++){
	    image_list[m+n*image_row].row=m;
	    image_list[m+n*image_row].col=n;

	    radius=sqrt(m*m+n*n);
	    theta=atan2(m,n);
	    image_list[m+n*image_row].rowp=sin(theta-angle)*radius;
	    image_list[m+n*image_row].colp=cos(theta-angle)*radius;

	    image_list[m+n*image_row].rowc=delta_row*floor(image_list[m+n*image_row].rowp/delta_row+0.5);
	    image_list[m+n*image_row].colc=delta_col*floor(image_list[m+n*image_row].colp/delta_col+0.5);

	    image_list[m+n*image_row].cost=sqrt((image_list[m+n*image_row].rowp-
						 image_list[m+n*image_row].rowc)*
                                                (image_list[m+n*image_row].rowp-
						 image_list[m+n*image_row].rowc)/
						(delta_row*delta_row)+
                                                (image_list[m+n*image_row].colp-
						 image_list[m+n*image_row].colc)*
                                                (image_list[m+n*image_row].colp-
						 image_list[m+n*image_row].colc)/
						(delta_col*delta_col));

	    image_list[m+n*image_row].input=1.0-input_image[m+n*image_row];
	  }
	}
	
	for (m=0; m<image_row*image_col; m++){
	  add_to_list(centroid_list, image_list[m], m, angle);
	}
	/*print_list(centroid_list);*/
	
	process_list(centroid_list, image_list, output_image);
	mxFree(cluster_list);
	mxFree(image_list);
	mxFree(centroid_list);
	return;
}

/*******************************************************************************/ 
/* mexFUNCTION                                                                 */
/* Gateway routine for use with MATLAB.                                        */
/*******************************************************************************/ 
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
	unsigned char *output_data;
	double *input_image, *angle, *angle_data;
	int m, n, output_dims[3], number_of_dims;
	int num_angle;
	const int *dim_array;

    /****** Check for errors in user's call to function. ***********************/
    if (nrhs<3 || nrhs>4)
            mexErrMsgTxt("CDOD requires three or four input arguments!");
    else if (nlhs!=1)
            mexErrMsgTxt("CDOD returns exactly one output argument!");
    else if (!mxIsNumeric(prhs[0]) ||
              mxIsComplex(prhs[0]) ||
              mxIsSparse(prhs[0])  ||
             !mxIsDouble(prhs[0]))
            mexErrMsgTxt("Input X must be a real matrix of type double!");
    else if (!mxIsNumeric(prhs[1]) ||
              mxIsComplex(prhs[1]) ||
              mxIsSparse(prhs[1])  ||
             !mxIsDouble(prhs[1])  ||
	         (mxGetM(prhs[1])!=1)  ||
	         (mxGetN(prhs[1])!=1))
            mexErrMsgTxt("Input dM must be a real scalar of type double!");
    else if (!mxIsNumeric(prhs[2]) ||
              mxIsComplex(prhs[2]) ||
              mxIsSparse(prhs[2])  ||
             !mxIsDouble(prhs[2])  ||
	         (mxGetM(prhs[2])!=1)  ||
	         (mxGetN(prhs[2])!=1))
            mexErrMsgTxt("Input dN must be a real scalar of type double!");

    /****** Get row and column sizes for input image ***************************/
    number_of_dims=mxGetNumberOfDimensions(prhs[0]);
    if (number_of_dims<2 || number_of_dims>3)
            mexErrMsgTxt("Input image X must be 2 or 3 dimensional!");
    else if (number_of_dims==2){
            image_row=mxGetM(prhs[0]);
            image_col=mxGetN(prhs[0]);
            image_chn=1;
            }
    else{
            dim_array=mxGetDimensions(prhs[0]);
            image_row=dim_array[0];
            image_col=dim_array[1];
            image_chn=dim_array[2];
            }
    input_image=mxGetPr(prhs[0]);
 
    /****** Get row and column sizes for clusters. *****************************/
	delta_row=mxGetScalar(prhs[1]);
	delta_col=mxGetScalar(prhs[2]);
	offset=delta_row+delta_col;

    /****** Get angles. ********************************************************/
	angle=(double*)mxCalloc(image_chn, sizeof(double));
    if ((nrhs==4) && (!mxIsNumeric(prhs[3]) ||
                       mxIsComplex(prhs[3]) ||
                       mxIsSparse(prhs[3])  ||
                      !mxIsDouble(prhs[3])  ||
                      (mxGetM(prhs[3])*mxGetN(prhs[3])!=1 && 
                       mxGetM(prhs[3])*mxGetN(prhs[3])!=image_chn)))	                 
        mexErrMsgTxt("Input A must be a real scalar of type double!");
	else if ((nrhs==4) && (mxGetM(prhs[3])*mxGetN(prhs[3])==1)){
	    for (m=0; m<image_chn; m++){
	        angle[m]=mxGetScalar(prhs[3])*pi/180.0;
	        }
	    }
	else if ((nrhs==4) && (mxGetM(prhs[3])*mxGetN(prhs[3])==image_chn)){
        angle_data=mxGetPr(prhs[3]);
        for (m=0; m<image_chn; m++){
			angle[m]=angle_data[m]*pi/180.0;
			}
        }
	else {
        for (m=0; m<image_chn; m++){
			angle[m]=pi/4.0;
			}
        }

    /****** Extract pointers to input data. ************************************/
	input_image=mxGetPr(prhs[0]);

    /****** Create output variables and extract pointers to data. **************/
	output_dims[0]=image_row;
	output_dims[1]=image_col;
	output_dims[2]=image_chn;

	plhs[0]=mxCreateLogicalArray(2, output_dims);
    output_data=mxGetLogicals(plhs[0]);
	
    for (m=0; m<image_row*image_col*image_chn; m++){
	  output_data[m]=1;
	}
	for (m=0; m<image_chn; m++){
	     cdod(&output_data[m*(image_row*image_col)],
                  &input_image[m*(image_row*image_col)],
                  angle[m]);
	  }
        return;
}