#include <R.h>
#include <Rinternals.h>
#include <R_ext/Rdynload.h>

#include <stdio.h>
#include <stdlib.h>

void weightedTau(double * x, double * y, double * w, int * len, double * tau);

R_CMethodDef cMethods[] = {
    {"weightedTau", &weightedTau, 5,
	{ REALSXP, REALSXP, REALSXP, INTSXP, REALSXP }},
    {NULL, NULL, 0}
};

void R_init_myLib(DllInfo *info)
{
    R_registerRoutines(info, cMethods, NULL, NULL, NULL);
}

// places to store a pointer to a list of x and y coordiantes for
// sorting a list of indexes by x then y in sortTauPtByX()
double * weightedTauX;
double * weightedTauY;

int sortTauPtByX(int * item1, int * item2)
{   return(weightedTauX[*item1] != weightedTauX[*item2] ?
	       weightedTauX[*item1] > weightedTauX[*item2] :
	       weightedTauY[*item1] > weightedTauY[*item2]);
}

void weightedTau(double * x, double * y, double * w, int * len, double * tau)
{   int psize=0, i=0, j=0, a=0, aend=0, b=0, bend=0;
    int s_minus=0, s_plus=0, T=0, U=0, V=0;
    int firstSameX=0, firstSameY=0, firstSameXY=0;
    double totalWeight=0, totalDis=0;

    int * fromArray = malloc(sizeof(int) * *len);
    int * toArray   = malloc(sizeof(int) * *len);
    int * ties      = calloc(*len, sizeof(int));  // initialize the ties 
    int * disPairs  = calloc(*len, sizeof(int));  // and disPairs to zero
    int * tmpArray  = NULL;

    // store x and y arrays for access by sortTauPtByX
    weightedTauX = x;
    weightedTauY = y;

    // make sure we got the requested memory
    if(!fromArray || !toArray || !ties || !disPairs)
    {   perror("unable to allocate memory");
	exit(1);
    }

    // set the fromArray to the input order
    for(i = 0; i < *len; ++i)
        fromArray[i] = i;

    // sort fromArray by x
    qsort(fromArray, *len, sizeof(int), sortTauPtByX);

    /* dump the array
    for(i = 0; i < *len; ++i)
        printf("(x:%.2f y:%.2f w:%.2f s:%d t:%d) ",
	       x[fromArray[i]], y[fromArray[i]], w[fromArray[i]],
	       disPairs[fromArray[i]], ties[fromArray[i]]);
    printf("\n"); //*/

    for(i = 1; i < *len; ++i)
    {   if(x[fromArray[firstSameX]] != x[fromArray[i]])
	{   for(j = firstSameX; j < i; ++j)
		ties[j] += i - firstSameX - 1;
	    for(j = firstSameXY; j < i; ++j)
		ties[j] -= i - firstSameXY - 1;
	    firstSameXY = firstSameX = i;
	} else if(y[fromArray[firstSameXY]] != y[fromArray[i]])
	{   for(j = firstSameXY; j < i; ++j)
		ties[j] -= i - firstSameXY - 1;
	    firstSameXY = i;
	}
    }
    for(j = firstSameX; j < *len; ++j)
	ties[j] += *len - firstSameX - 1;
    for(j = firstSameXY; j < *len; ++j)
	ties[j] -= *len - firstSameXY - 1;

    /* dump the array
    for(i = 0; i < *len; ++i)
        printf("(x:%.2f y:%.2f w:%.2f s:%d t:%d) ",
	       x[fromArray[i]], y[fromArray[i]], w[fromArray[i]],
	       disPairs[fromArray[i]], ties[fromArray[i]]);
    printf("\n"); //*/

    // compute s_minus via merge sort (the number of discordant pairs)
    for(psize = 1; psize < *len; psize *= 2)
    {   bend = 0; // we lie about the last bend to make the loop start at a=0

	while(bend < *len)
	{   a = bend;  // [a, aend) will be merged with [b, bend)
	    b = aend = a + psize;
	    bend = (b + psize < *len ? b + psize : *len);

	    for(i = a; i < bend; ++i)
	    {   if(b >= bend || a < aend && y[fromArray[a]] <= y[fromArray[b]])
		{   toArray[i] = fromArray[a];
		    disPairs[toArray[i]] += i - a++;
		} else 
		{   toArray[i] = fromArray[b];
		    disPairs[toArray[i]] -= i - b++;
		}
	    }
	}

	tmpArray = fromArray;
	fromArray = toArray;
	toArray = tmpArray;
    }

    for(i = 1; i < *len; ++i)
    {   if(y[fromArray[firstSameY]] != y[fromArray[i]])
	{   for(j = firstSameY; j < i; ++j)
		ties[j] += i - firstSameY - 1;
	    firstSameY = i;
	}
    }
    for(j = firstSameY; j < *len; ++j)
	ties[j] += *len - firstSameY - 1;

    /* dump the array
    for(i = 0; i < *len; ++i)
        printf("(x:%.2f y:%.2f w:%.2f s:%d t:%d) ",
	       x[fromArray[i]], y[fromArray[i]], w[fromArray[i]],
	       disPairs[fromArray[i]], ties[fromArray[i]]);
    printf("\n"); //*/

    /* dump the array
    for(i = 0; i < *len; ++i)
        printf("(x:%.2f y:%.2f w:%.2f s:%d t:%d) ",
	       x[i], y[i], w[i], disPairs[i], ties[i]);
    printf("\n"); //*/

    for(i = 0; i < *len; ++i)
    {   totalWeight += w[i];
	totalDis    += w[i] * disPairs[i] + 0.5 * w[i] * ties[i];
    }
    totalWeight *= *len - 1;

    //printf("totalDis:%.1f totalWeight:%.1f\n", totalDis, totalWeight);

    tau[0] = 1 - 2 * totalDis / totalWeight;

    // free the memory resources
    free(fromArray);
    free(toArray);
    free(disPairs);
    free(ties);
}

/* compile with:
 *  % R CMD SHLIB weightedTau.c
 *
 * run with:
 * > dyn.load("tau.so")
 * > .C("tau", ...)$tau
 */