/*
 * The copyright in this software is being made available under the 2-clauses 
 * BSD License, included below. This software may be subject to other third 
 * party and contributor rights, including patent rights, and no such rights
 * are granted under this license.
 *
 * Copyright (c) 2008, Jerome Fimes, Communications & Systemes <jerome.fimes@c-s.fr>
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS `AS IS'
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include "opj_includes.h"

/** 
 * LUP decomposition
 */
static OPJ_BOOL opj_lupDecompose(OPJ_FLOAT32 * matrix,
                                 OPJ_UINT32 * permutations, 
                                 OPJ_FLOAT32 * p_swap_area,
                                 OPJ_UINT32 nb_compo);
/** 
 * LUP solving
 */
static void opj_lupSolve(OPJ_FLOAT32 * pResult, 
                         OPJ_FLOAT32* pMatrix, 
                         OPJ_FLOAT32* pVector, 
                         OPJ_UINT32* pPermutations, 
                         OPJ_UINT32 nb_compo,
                         OPJ_FLOAT32 * p_intermediate_data);

/** 
 *LUP inversion (call with the result of lupDecompose)
 */
static void opj_lupInvert ( OPJ_FLOAT32 * pSrcMatrix,
                            OPJ_FLOAT32 * pDestMatrix,
                            OPJ_UINT32 nb_compo,
                            OPJ_UINT32 * pPermutations,
                            OPJ_FLOAT32 * p_src_temp,
                            OPJ_FLOAT32 * p_dest_temp,
                            OPJ_FLOAT32 * p_swap_area);

/*
==========================================================
   Matric inversion interface
==========================================================
*/
/**
 * Matrix inversion.
 */
OPJ_BOOL opj_matrix_inversion_f(OPJ_FLOAT32 * pSrcMatrix,
                                OPJ_FLOAT32 * pDestMatrix, 
                                OPJ_UINT32 nb_compo)
{
	OPJ_BYTE * l_data = 00;
	OPJ_UINT32 l_permutation_size = nb_compo * (OPJ_UINT32)sizeof(OPJ_UINT32);
	OPJ_UINT32 l_swap_size = nb_compo * (OPJ_UINT32)sizeof(OPJ_FLOAT32);
	OPJ_UINT32 l_total_size = l_permutation_size + 3 * l_swap_size;
	OPJ_UINT32 * lPermutations = 00;
	OPJ_FLOAT32 * l_double_data = 00;

	l_data = (OPJ_BYTE *) opj_malloc(l_total_size);
	if (l_data == 0) {
		return OPJ_FALSE;
	}
	lPermutations = (OPJ_UINT32 *) l_data;
	l_double_data = (OPJ_FLOAT32 *) (l_data + l_permutation_size);
	memset(lPermutations,0,l_permutation_size);

	if(! opj_lupDecompose(pSrcMatrix,lPermutations,l_double_data,nb_compo)) {
		opj_free(l_data);
		return OPJ_FALSE;
	}
	
    opj_lupInvert(pSrcMatrix,pDestMatrix,nb_compo,lPermutations,l_double_data,l_double_data + nb_compo,l_double_data + 2*nb_compo);
	opj_free(l_data);
	
    return OPJ_TRUE;
}


/*
==========================================================
   Local functions
==========================================================
*/
OPJ_BOOL opj_lupDecompose(OPJ_FLOAT32 * matrix,OPJ_UINT32 * permutations, 
                          OPJ_FLOAT32 * p_swap_area,
                          OPJ_UINT32 nb_compo) 
{
	OPJ_UINT32 * tmpPermutations = permutations;
	OPJ_UINT32 * dstPermutations;
	OPJ_UINT32 k2=0,t;
	OPJ_FLOAT32 temp;
	OPJ_UINT32 i,j,k;
	OPJ_FLOAT32 p;
	OPJ_UINT32 lLastColum = nb_compo - 1;
	OPJ_UINT32 lSwapSize = nb_compo * (OPJ_UINT32)sizeof(OPJ_FLOAT32);
	OPJ_FLOAT32 * lTmpMatrix = matrix;
	OPJ_FLOAT32 * lColumnMatrix,* lDestMatrix;
	OPJ_UINT32 offset = 1;
	OPJ_UINT32 lStride = nb_compo-1;

	/*initialize permutations */
	for (i = 0; i < nb_compo; ++i) 
	{
    	*tmpPermutations++ = i;
	}
	/* now make a pivot with colum switch */
	tmpPermutations = permutations;
	for (k = 0; k < lLastColum; ++k) {
		p = 0.0;

		/* take the middle element */
		lColumnMatrix = lTmpMatrix + k;
		
		/* make permutation with the biggest value in the column */
        for (i = k; i < nb_compo; ++i) {
			temp = ((*lColumnMatrix > 0) ? *lColumnMatrix : -(*lColumnMatrix));
     		if (temp > p) {
     			p = temp;
     			k2 = i;
     		}
			/* next line */
			lColumnMatrix += nb_compo;
     	}

     	/* a whole rest of 0 -> non singular */
     	if (p == 0.0) {
    		return OPJ_FALSE;
		}

		/* should we permute ? */
		if (k2 != k) {
			/*exchange of line */
     		/* k2 > k */
			dstPermutations = tmpPermutations + k2 - k;
			/* swap indices */
			t = *tmpPermutations;
     		*tmpPermutations = *dstPermutations;
     		*dstPermutations = t;

			/* and swap entire line. */
			lColumnMatrix = lTmpMatrix + (k2 - k) * nb_compo;
			memcpy(p_swap_area,lColumnMatrix,lSwapSize);
			memcpy(lColumnMatrix,lTmpMatrix,lSwapSize);
			memcpy(lTmpMatrix,p_swap_area,lSwapSize);
		}

		/* now update data in the rest of the line and line after */
		lDestMatrix = lTmpMatrix + k;
		lColumnMatrix = lDestMatrix + nb_compo;
		/* take the middle element */
		temp = *(lDestMatrix++);

		/* now compute up data (i.e. coeff up of the diagonal). */
     	for (i = offset; i < nb_compo; ++i)  {
			/*lColumnMatrix; */
			/* divide the lower column elements by the diagonal value */

			/* matrix[i][k] /= matrix[k][k]; */
     		/* p = matrix[i][k] */
			p = *lColumnMatrix / temp;
			*(lColumnMatrix++) = p;
     		
            for (j = /* k + 1 */ offset; j < nb_compo; ++j) {
				/* matrix[i][j] -= matrix[i][k] * matrix[k][j]; */
     			*(lColumnMatrix++) -= p * (*(lDestMatrix++));
			}
			/* come back to the k+1th element */
			lDestMatrix -= lStride;
			/* go to kth element of the next line */
			lColumnMatrix += k;
     	}

		/* offset is now k+2 */
		++offset;
		/* 1 element less for stride */
		--lStride;
		/* next line */
		lTmpMatrix+=nb_compo;
		/* next permutation element */
		++tmpPermutations;
	}
    return OPJ_TRUE;
}
   		
void opj_lupSolve (OPJ_FLOAT32 * pResult, 
                   OPJ_FLOAT32 * pMatrix, 
                   OPJ_FLOAT32 * pVector, 
                   OPJ_UINT32* pPermutations, 
                   OPJ_UINT32 nb_compo,OPJ_FLOAT32 * p_intermediate_data) 
{
	OPJ_INT32 k;
    OPJ_UINT32 i,j;
	OPJ_FLOAT32 sum;
	OPJ_FLOAT32 u;
    OPJ_UINT32 lStride = nb_compo+1;
	OPJ_FLOAT32 * lCurrentPtr;
	OPJ_FLOAT32 * lIntermediatePtr;
	OPJ_FLOAT32 * lDestPtr;
	OPJ_FLOAT32 * lTmpMatrix;
	OPJ_FLOAT32 * lLineMatrix = pMatrix;
	OPJ_FLOAT32 * lBeginPtr = pResult + nb_compo - 1;
	OPJ_FLOAT32 * lGeneratedData;
	OPJ_UINT32 * lCurrentPermutationPtr = pPermutations;

	
	lIntermediatePtr = p_intermediate_data;
	lGeneratedData = p_intermediate_data + nb_compo - 1;
	
    for (i = 0; i < nb_compo; ++i) {
       	sum = 0.0;
		lCurrentPtr = p_intermediate_data;
		lTmpMatrix = lLineMatrix;
        for (j = 1; j <= i; ++j) 
		{
			/* sum += matrix[i][j-1] * y[j-1]; */
        	sum += (*(lTmpMatrix++)) * (*(lCurrentPtr++));
        }
		/*y[i] = pVector[pPermutations[i]] - sum; */
        *(lIntermediatePtr++) = pVector[*(lCurrentPermutationPtr++)] - sum;
		lLineMatrix += nb_compo;
	}

	/* we take the last point of the matrix */
	lLineMatrix = pMatrix + nb_compo*nb_compo - 1;

	/* and we take after the last point of the destination vector */
	lDestPtr = pResult + nb_compo;


    assert(nb_compo != 0);
	for (k = (OPJ_INT32)nb_compo - 1; k != -1 ; --k) {
		sum = 0.0;
		lTmpMatrix = lLineMatrix;
        u = *(lTmpMatrix++);
		lCurrentPtr = lDestPtr--;
        for (j = (OPJ_UINT32)(k + 1); j < nb_compo; ++j) {
			/* sum += matrix[k][j] * x[j] */
        	sum += (*(lTmpMatrix++)) * (*(lCurrentPtr++));
		}
		/*x[k] = (y[k] - sum) / u; */
        *(lBeginPtr--) = (*(lGeneratedData--) - sum) / u;
		lLineMatrix -= lStride;
	}
}
    

void opj_lupInvert (OPJ_FLOAT32 * pSrcMatrix,
                    OPJ_FLOAT32 * pDestMatrix,
                    OPJ_UINT32 nb_compo,
                    OPJ_UINT32 * pPermutations,
                    OPJ_FLOAT32 * p_src_temp,
                    OPJ_FLOAT32 * p_dest_temp,
                    OPJ_FLOAT32 * p_swap_area )
{
	OPJ_UINT32 j,i;
	OPJ_FLOAT32 * lCurrentPtr;
	OPJ_FLOAT32 * lLineMatrix = pDestMatrix;
	OPJ_UINT32 lSwapSize = nb_compo * (OPJ_UINT32)sizeof(OPJ_FLOAT32);

	for (j = 0; j < nb_compo; ++j) {
		lCurrentPtr = lLineMatrix++;
        memset(p_src_temp,0,lSwapSize);
    	p_src_temp[j] = 1.0;
		opj_lupSolve(p_dest_temp,pSrcMatrix,p_src_temp, pPermutations, nb_compo , p_swap_area);

		for (i = 0; i < nb_compo; ++i) {
    		*(lCurrentPtr) = p_dest_temp[i];
			lCurrentPtr+=nb_compo;
    	}
    }
}