/*=================================================================
 * distNzero.c 
 * Calculating nonzero distances of a given coordinate (x1, y1, z1;...;xn, yn, zn)
 * 
 * MEX-file.  NOTE: MATLAB uses 1 based indexing, C uses 0 based indexing.
 *
 * Takes a N-dimensional array of doubles and returns the indices for
 * the non-zero elements in the array. Findnz works differently than
 * the FIND command in MATLAB in that it returns all the indices in
 * one output variable, where the column element contains the index
 * for that dimension.
 *
 * This is a MEX-file for MATLAB.  
 * 2007. 12. 6.
 * Copyright 1984-2006 Byeongdu Lee.
 *============================================================*/

/* $Revision: 1.1 $ */

#include "mex.h"
#include <math.h>
#define RESOL 1000
#define MAXOUT 10000000
/* Output Arguments */

#define	Pr_OUT plhs[1]
#define	r_OUT plhs[0]

double distance(double x1, double y1, double z1, double x2, double y2, double z2)
{
    double d;
    d = floor(sqrt((x1-x2)*(x1-x2) + (y1-y2)*(y1-y2) + (z1-z2)*(z1-z2)));
    return d;
}


void mexFunction(int nlhs,       mxArray *plhs[],
		 int nrhs, const mxArray *prhs[])
{
    /* Declare variables */ 
    mwSize i,j;
    mwSize M1;
    mwSize N1;
    mwSize M2;
    mwSize N2;
    double *pr1, *pr2, *pind;
    
    /*mwSize index, max_index;*/
    double dist;
    int cnt;
    int resolution;
    double atmatm;
    unsigned long index;
    unsigned long max_index = 0;
    double *prr;
    double *r;
    double *respr;
    unsigned long *resr;

    cnt = 0;
    max_index = 0;
    
    /* Check for proper number of input and output arguments */    
    if (nrhs > 2) {
	mexErrMsgTxt("Less than 3 input arguments required.");
    } 
    if (nlhs > 2){
	mexErrMsgTxt("Too many output arguments.");
    }
    
    /* Check data type of input argument */
    if (!(mxIsDouble(prhs[0]))) {
      mexErrMsgTxt("Input array must be of type double.");
    }
    
    /* Get the number of elements in the input argument */
	/*    elements=mxGetNumberOfElements(prhs[0]); */
    M = mxGetM(prhs[0]);
    N = mxGetN(prhs[0]);

    if (nrhs >= 1) {
        pr=(double *)mxGetPr(prhs[0]);
    }
    if (nrhs == 2) {
        atm=(double *)mxGetPr(prhs[1]);
    }
    
    if (N != 3) {
        mexErrMsgTxt("Number of column is not 3.");
    }

    
    respr = malloc(sizeof(double) * MAXOUT);
    resr = malloc(sizeof(unsigned long) * MAXOUT);
    /*respr=mxCreateDoubleMatrix(cnt,1,mxREAL);
    resr=mxCreateDoubleMatrix(cnt,1,mxREAL);*/
    
	
    /* Fill in the indices to return to MATLAB. This loops through the
     * elements and checks for non-zero values. If it finds a non-zero
     * value, it then calculates the corresponding MATLAB indice and
     * assigns them into the output array.  The 1 is added to the
     * calculated indice because MATLAB is 1 based and C is zero
     * based. */
    /*resolution = RESOL;
    
    for (i=0;i<M*N;i++) {
        pr[i] = pr[i]*resolution;
    };*/
    for (i=0;i<MAXOUT;i++) {
        respr[i] = 0.0;
        resr[i] = 0;
    };
    
    for(i=0;i<(M-1);i++) {
        for (j=i+1;j<M;j++) {
            index = (long) distance(pr[i], pr[i+M], pr[i+2*M],pr[j], pr[j+M], pr[j+2*M]);
            if (nrhs == 2) {
                atmatm = atm[i]*atm[j];
            } else {
                atmatm = 1;
            }
            
            if (index>(MAXOUT-1)) {
                mexErrMsgTxt("the number of r larger than MAXOUT. Reduce resolution..");
                break;
            }
            /*index = dist;
            *(respr + index) = *(respr + index) + atmatm;*/
            respr[index] += atmatm;
            *(resr + index) = index;
            /**(resr + index) = index;*/
            if(index > max_index){
                    max_index = index;
            }
        }
    }
    cnt = 0;
    for (i=0;i<max_index+1;i++){
        if (resr[i] > 0){
            cnt++;
        }
    }
    /* Get the number of dimensions in the input argument. Allocate the
       space for the return argument */
    r_OUT=mxCreateDoubleMatrix(cnt,1,mxREAL);
    Pr_OUT=mxCreateDoubleMatrix(cnt,1,mxREAL);
    /* Get the data */
    r=mxGetPr(r_OUT);
    prr=mxGetPr(Pr_OUT);
    j = 0;
    for (i=0;i<max_index+1;i++) {
        if (resr[i] > 0) {
            /*printf("prr value = %d\n", resr[i]);*/
            prr[j] = respr[i];
            r[j] = resr[i];
            /*r[j] = r[j]/resolution;*/
            j++;
        }
    }
    free(respr);
    free(resr);
}