/*

 * 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;

      }

    }

}