/* permutation/permute_source.c
 * 
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Brian Gough
 * 
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

/* In-place Permutations 

   permute:    OUT[i]       = IN[perm[i]]     i = 0 .. N-1
   invpermute: OUT[perm[i]] = IN[i]           i = 0 .. N-1

   PERM is an index map, i.e. a vector which contains a permutation of
   the integers 0 .. N-1.

   From Knuth "Sorting and Searching", Volume 3 (3rd ed), Section 5.2
   Exercise 10 (answers), p 617

   FIXME: these have not been fully tested.
*/

int
TYPE(gsl_permute) (const size_t * p, ATOMIC * data, const size_t stride,
		   const size_t n) {
    size_t i, k, pk;

    for (i = 0; i < n; i++) {
	k = p[i];

	while (k > i)
	    k = p[k];

	if (k < i)
	    continue;

	/* Now have k == i, i.e the least in its cycle */

	pk = p[k];

	if (pk == i)
	    continue;

	/* shuffle the elements of the cycle */

	{
	    unsigned int a;

	    ATOMIC t[MULTIPLICITY];

	    for (a = 0; a < MULTIPLICITY; a++)
		t[a] = data[i * stride * MULTIPLICITY + a];

	    while (pk != i) {
		for (a = 0; a < MULTIPLICITY; a++) {
		    ATOMIC r1 = data[pk * stride * MULTIPLICITY + a];
		    data[k * stride * MULTIPLICITY + a] = r1;
		}
		k = pk;
		pk = p[k];
	    };

	    for (a = 0; a < MULTIPLICITY; a++)
		data[k * stride * MULTIPLICITY + a] = t[a];
	}
    }

    return GSL_SUCCESS;
}

int
FUNCTION(gsl_permute, inverse) (const size_t * p, ATOMIC * data,
				const size_t stride, const size_t n) {
    size_t i, k, pk;

    for (i = 0; i < n; i++) {
	k = p[i];

	while (k > i)
	    k = p[k];

	if (k < i)
	    continue;

	/* Now have k == i, i.e the least in its cycle */

	pk = p[k];

	if (pk == i)
	    continue;

	/* shuffle the elements of the cycle in the inverse direction */

	{
	    unsigned int a;

	    ATOMIC t[MULTIPLICITY];

	    for (a = 0; a < MULTIPLICITY; a++)
		t[a] = data[k * stride * MULTIPLICITY + a];

	    while (pk != i) {
		for (a = 0; a < MULTIPLICITY; a++) {
		    ATOMIC r1 = data[pk * stride * MULTIPLICITY + a];
		    data[pk * stride * MULTIPLICITY + a] = t[a];
		    t[a] = r1;
		}

		k = pk;
		pk = p[k];
	    };

	    for (a = 0; a < MULTIPLICITY; a++)
		data[pk * stride * MULTIPLICITY + a] = t[a];
	}
    }

    return GSL_SUCCESS;
}


int
TYPE(gsl_permute_vector) (const gsl_permutation * p, TYPE(gsl_vector) * v)
{
    if (v->size != p->size) {
	GSL_ERROR("vector and permutation must be the same length",
		  GSL_EBADLEN);
    }

    TYPE(gsl_permute) (p->data, v->data, v->stride, v->size);

    return GSL_SUCCESS;
}

int
FUNCTION(gsl_permute_vector, inverse) (const gsl_permutation * p,
				       TYPE(gsl_vector) * v) {
    if (v->size != p->size) {
	GSL_ERROR("vector and permutation must be the same length",
		  GSL_EBADLEN);
    }

    FUNCTION(gsl_permute, inverse) (p->data, v->data, v->stride, v->size);

    return GSL_SUCCESS;
}
