/***********************************************************************
	File:	sort_out_samples.c
	A mex file for Matlab (c)
	The calling syntax is:
		  [rr_unique, drr_unique, ix_unique] = sort_out_samples_double(rr_sorted, drr_sorted)
	 rr_sorted = dim x numOfSamples of sorted samples (double)
	 drr_sorted = dim x numOfSamples of samples (derivatives) sorted w.r.t sample value

	 Date:	April 2006

	Copyright (c) 2006 by Mert Rory Sabuncu

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

#include <math.h>
#include <stddef.h>
#include <stdio.h>
#include <stdlib.h>
#include "mex.h"

#define NOT	!
#define AND	&&
#define OR	||
#define EQ	==
#define NE	!=
#define MIN_DISTANCE	1e-5

#define VERBOSE

/*

 * The main routine.  Read in graph, compute derivatives, output it.

 */

void
mexFunction(
			int nlhs, 
			mxArray *plhs[], 
			int nrhs, 
			const mxArray *prhs[])
{

	//Declarations

	const double *	rr_sorted		=	mxGetPr(prhs[0]);
	const double *	drr_sorted		=	mxGetPr(prhs[1]);
	const int		numOfSamples	=   mxGetM(prhs[0]);
	const int		numOfDSamples	=	mxGetM(prhs[1]);
	const int		dim				=	mxGetN(prhs[0]);
	const int		Ddim			=	mxGetN(prhs[1]);

	int i, j, numOfUniqueSamples, cnt, numOfCoincidingSamples, d;
	int * unique_ix;
	double *	rr_unique, *drr_unique, * out_unique_ix; 

	numOfUniqueSamples = 1;

	if (numOfSamples != numOfDSamples)	mexErrMsgTxt("Number of samples doesn't match number of derivative samples!");
	if (dim != 2)	mexErrMsgTxt("Only works with two-dimensional samples!");


	for (i = 0; i < numOfSamples-1; i++){
  	if ((rr_sorted[i] != rr_sorted[i+1]) || (rr_sorted[i + numOfSamples] != rr_sorted[(i+1) + numOfSamples]))

			numOfUniqueSamples ++;
	}

	plhs[0] = mxCreateDoubleMatrix(numOfUniqueSamples, dim, mxREAL);
	plhs[1] = mxCreateDoubleMatrix(numOfUniqueSamples, Ddim, mxREAL);
	plhs[2] = mxCreateDoubleMatrix(numOfSamples, 1, mxREAL);

	rr_unique = mxGetPr(plhs[0]);
	drr_unique = mxGetPr(plhs[1]);
	out_unique_ix = mxGetPr(plhs[2]);


	unique_ix = (int*) malloc((numOfUniqueSamples + 1) * sizeof(int));
	unique_ix[0] = -1;
	cnt = 0;

	for (i = 0; i < numOfSamples - 1; i++){

		out_unique_ix[i] = cnt + 1;
		if ((rr_sorted[i] != rr_sorted[i+1]) || (rr_sorted[i + numOfSamples] != rr_sorted[(i+1) + numOfSamples]))
		{
			unique_ix[cnt + 1] = i;
			rr_unique[cnt] = rr_sorted[i];
			rr_unique[cnt + numOfUniqueSamples] = rr_sorted[i + numOfSamples];
			cnt++;
		}
	}

	out_unique_ix[i] = cnt + 1;
	unique_ix[cnt + 1]	= numOfSamples - 1;
	rr_unique[numOfUniqueSamples-1] = rr_sorted[numOfSamples-1];
	rr_unique[2*numOfUniqueSamples-1] = rr_sorted[2*numOfSamples -1];
	cnt = 0;

	for (i = 0; i < numOfUniqueSamples; i++){
		numOfCoincidingSamples = unique_ix[i+1] - unique_ix[i];
		for (d = 0; d < Ddim; d++){
			drr_unique[i + d*numOfUniqueSamples] = drr_sorted[unique_ix[i] + 1 + d*numOfSamples];
			for (j = unique_ix[i] + 2; j <= unique_ix[i+1]; j++){
			drr_unique[i + d*numOfUniqueSamples] += drr_sorted[j + d*numOfSamples];
			}
  		drr_unique[i + d*numOfUniqueSamples] = drr_unique[i + d*numOfUniqueSamples]/numOfCoincidingSamples;
		}
	}
	free(unique_ix);
	return;
}