/*=========================================================================
 *
 *  Copyright NumFOCUS
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *         https://www.apache.org/licenses/LICENSE-2.0.txt
 *
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 *
 *=========================================================================*/
#include "itkNumericTraits.h"
#include "itpack.h"
#include "itkFEMLinearSystemWrapperItpack.h"

namespace itk
{
namespace fem
{
/**
 * constructor
 */
LinearSystemWrapperItpack::LinearSystemWrapperItpack()
{
  /* fill m_Methods with pointers to solver functions */
  m_Methods[0] = jcg_;
  m_Methods[1] = jsi_;
  m_Methods[2] = sor_;
  m_Methods[3] = ssorcg_;
  m_Methods[4] = ssorsi_;
  m_Methods[5] = rscg_;
  m_Methods[6] = rssi_;
  m_Method = 0; /* set default method to jcg_ */

  /* Set solving parameters */
  dfault_(&(m_IPARM[0]), &(m_RPARM[0]));

  // We don't want the solver routines to
  // overwrite the parameters.
  m_IPARM[2] = 1;

  /* m_IPARM[0] = 500; */ /* number of iterations */
  m_IPARM[1] = -1;        /* no error message output */
  m_IPARM[4] = 1;         /* non-symmetric matrix */

  /* itpack recommended (but not default) value */
#undef min
  m_RPARM[7] = 500.0 * NumericTraits<double>::min();

  m_MaximumNonZeroValues = 0;
  m_Matrices = nullptr;
  m_Vectors = nullptr;
  m_Solutions = nullptr;
}

void
LinearSystemWrapperItpack::InitializeMatrix(unsigned int matrixIndex)
{
  /* error checking */
  if (!m_Order)
  {
    throw FEMExceptionLinearSystem(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::InitializeMatrix", "System order not set");
  }
  if (matrixIndex >= m_NumberOfMatrices)
  {
    throw FEMExceptionLinearSystemBounds(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::InitializeMatrix", "m_Matrices", matrixIndex);
  }
  if (!m_MaximumNonZeroValues)
  {
    throw FEMExceptionLinearSystem(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::InitializeMatrix", "Maximum number of non zeros not set");
  }

  // allocate if necessary
  if (m_Matrices == nullptr)
  {
    m_Matrices = new MatrixHolder(m_NumberOfMatrices);
  }

  /* Set required variables */
  (*m_Matrices)[matrixIndex].Clear();
  (*m_Matrices)[matrixIndex].SetOrder(m_Order);
  (*m_Matrices)[matrixIndex].SetMaxNonZeroValues(m_MaximumNonZeroValues);
}

bool
LinearSystemWrapperItpack::IsMatrixInitialized(unsigned int matrixIndex)
{
  if (!m_Matrices)
  {
    return false;
  }
  if (!(*m_Matrices)[matrixIndex].GetOrder())
  {
    return false;
  }
  if (!(*m_Matrices)[matrixIndex].GetMaxNonZeroValues())
  {
    return false;
  }

  return true;
}

void
LinearSystemWrapperItpack::InitializeVector(unsigned int vectorIndex)
{
  /* error checking */
  if (!m_Order)
  {
    throw FEMExceptionLinearSystem(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::InitializeVector", "System order not set");
  }
  if (vectorIndex >= m_NumberOfVectors)
  {
    throw FEMExceptionLinearSystemBounds(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::InitializeVector", "m_Vectors", vectorIndex);
  }

  /* allocate if necessary */
  if (m_Vectors == nullptr)
  {
    m_Vectors = new VectorHolder(m_NumberOfVectors);
  }

  /* delete old vector */
  delete[](*m_Vectors)[vectorIndex];

  /* insert new vector */
  (*m_Vectors)[vectorIndex] = new doublereal[m_Order];
  /* fill with zeros */
  for (unsigned int i = 0; i < m_Order; ++i)
  {
    (*m_Vectors)[vectorIndex][i] = 0.0;
  }
}

bool
LinearSystemWrapperItpack::IsVectorInitialized(unsigned int vectorIndex)
{
  if (!m_Vectors)
  {
    return false;
  }
  if (!(*m_Vectors)[vectorIndex])
  {
    return false;
  }

  return true;
}

void
LinearSystemWrapperItpack::InitializeSolution(unsigned int solutionIndex)
{
  /* FIX ME: exceptions */
  if (!m_Order)
  {
    throw FEMExceptionLinearSystem(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::InitializeSolution", "System order not set");
  }
  if (solutionIndex >= m_NumberOfSolutions)
  {
    throw FEMExceptionLinearSystemBounds(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::InitializeSolution", "m_Solutions", solutionIndex);
  }

  // allocate if necessary
  if (m_Solutions == nullptr)
  {
    m_Solutions = new VectorHolder(m_NumberOfSolutions);
  }

  /* delete old vector */
  delete[](*m_Solutions)[solutionIndex];

  /* insert new vector */
  (*m_Solutions)[solutionIndex] = new doublereal[m_Order];
  /* fill with zeros */
  for (unsigned int i = 0; i < m_Order; ++i)
  {
    (*m_Solutions)[solutionIndex][i] = 0.0;
  }
}

bool
LinearSystemWrapperItpack::IsSolutionInitialized(unsigned int solutionIndex)
{
  if (!m_Solutions)
  {
    return false;
  }
  if (!(*m_Solutions)[solutionIndex])
  {
    return false;
  }

  return true;
}

void
LinearSystemWrapperItpack::DestroyMatrix(unsigned int matrixIndex)
{
  /* FIX ME: exceptions */
  if (m_Matrices)
  {
    if (matrixIndex >= m_NumberOfMatrices)
    {
      throw FEMExceptionLinearSystemBounds(
        __FILE__, __LINE__, "LinearSystemWrapperItpack::DestroyMatrix", "m_Matrices", matrixIndex);
    }

    (*m_Matrices)[matrixIndex].Clear();
  }
}

void
LinearSystemWrapperItpack::DestroyVector(unsigned int vectorIndex)
{
  /* FIXME: exceptions */
  if (m_Vectors)
  {
    if (vectorIndex >= m_NumberOfVectors)
    {
      throw FEMExceptionLinearSystemBounds(
        __FILE__, __LINE__, "LinearSystemWrapperItpack::DestroyVector", "m_Vectors", vectorIndex);
    }

    /* delete vector */
    delete[](*m_Vectors)[vectorIndex];
    (*m_Vectors)[vectorIndex] = nullptr;
  }
}

void
LinearSystemWrapperItpack::DestroySolution(unsigned int solutionIndex)
{
  // FIXME: exceptions
  if (m_Solutions)
  {
    if (solutionIndex >= m_NumberOfSolutions)
    {
      throw FEMExceptionLinearSystemBounds(
        __FILE__, __LINE__, "LinearSystemWrapperItpack::DestroySolution", "m_Solutions", solutionIndex);
    }

    /* delete vector */
    delete[](*m_Solutions)[solutionIndex];
    (*m_Solutions)[solutionIndex] = nullptr;
  }
}

LinearSystemWrapperItpack::Float
LinearSystemWrapperItpack::GetMatrixValue(unsigned int i, unsigned int j, unsigned int matrixIndex) const
{
  /* error checking */
  if (!m_Matrices)
  {
    throw FEMExceptionLinearSystem(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::GetMatrixValue", "No matrices have been allocated");
  }
  if (matrixIndex >= m_NumberOfMatrices)
  {
    throw FEMExceptionLinearSystemBounds(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::GetMatrixValue", "m_Matrices", matrixIndex);
  }
  if ((i >= m_Order) || (j >= m_Order))
  {
    throw FEMExceptionLinearSystemBounds(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::GetMatrixValue", "m_Matrices[]", i, j);
  }

  /* return value */
  return (*m_Matrices)[matrixIndex].Get(i, j);
}

void
LinearSystemWrapperItpack::SetMatrixValue(unsigned int i, unsigned int j, Float value, unsigned int matrixIndex)
{
  /* error checking */
  if (!m_Matrices)
  {
    throw FEMExceptionLinearSystem(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::SetMatrixValue", "No matrices have been allocated");
  }
  if ((i >= m_Order) || (j >= m_Order))
  {
    throw FEMExceptionLinearSystemBounds(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::SetMatrixValue", "m_Matrices[]", i, j);
  }
  if (matrixIndex >= m_NumberOfMatrices)
  {
    throw FEMExceptionLinearSystemBounds(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::SetMatrixValue", "m_Matrices", matrixIndex);
  }

  /* set value */
  ((*m_Matrices)[matrixIndex]).Set(i, j, value);
}

void
LinearSystemWrapperItpack::AddMatrixValue(unsigned int i, unsigned int j, Float value, unsigned int matrixIndex)
{
  // FIXME: error checking
  if (!m_Matrices)
  {
    throw FEMExceptionLinearSystem(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::AddMatrixValue", "No matrices have been allocated");
  }
  if ((i >= m_Order) || (j >= m_Order))
  {
    throw FEMExceptionLinearSystemBounds(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::AddMatrixValue", "m_Matrices[]", i, j);
  }
  if (matrixIndex >= m_NumberOfMatrices)
  {
    throw FEMExceptionLinearSystemBounds(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::AddMatrixValue", "m_Matrices", matrixIndex);
  }

  ((*m_Matrices)[matrixIndex]).Add(i, j, value);
}

void
LinearSystemWrapperItpack::GetColumnsOfNonZeroMatrixElementsInRow(unsigned int  row,
                                                                  ColumnArray & cols,
                                                                  unsigned int  matrixIndex)
{
  /* FIXME: error checking */
  if (!m_Matrices)
  {
    throw FEMExceptionLinearSystem(__FILE__,
                                   __LINE__,
                                   "LinearSystemWrapperItpack::GetColumnsOfNonZeroMatrixElementsInRow",
                                   "No matrices have been allocated");
  }
  if (row >= this->m_Order)
  {
    throw FEMExceptionLinearSystemBounds(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::GetColumnsOfNonZeroMatrixElementsInRow", "m_Matrices[]", row);
  }
  if (matrixIndex >= m_NumberOfMatrices)
  {
    throw FEMExceptionLinearSystemBounds(__FILE__,
                                         __LINE__,
                                         "LinearSystemWrapperItpack::GetColumnsOfNonZeroMatrixElementsInRow",
                                         "m_Matrices",
                                         matrixIndex);
  }

  cols.clear();

  ItpackSparseMatrix * mat = &(*m_Matrices)[matrixIndex];

  /* Check if matrix is in readable form */
  if (mat->m_MatrixFinalized)
  {
    /* get search bounds in appropriate row */
    unsigned int lower = mat->m_IA[row] - 1;
    unsigned int upper = mat->m_IA[row + 1] - 1;
    for (unsigned int j = lower; j < upper; ++j)
    {
      cols.push_back(mat->m_JA[j] - 1);
    }
  }
  else /* Scan the linked list to obtain the correct indices. */
  {
    int wrk = mat->m_IA[row] - 1;
    while (wrk > 0)
    {
      cols.push_back(mat->m_JA[wrk] - 1);
      wrk = mat->m_IWORK[wrk] - 1;
    }
  }
}

void
LinearSystemWrapperItpack::ScaleMatrix(Float scale, unsigned int matrixIndex)
{
  /* error checking */
  if (!m_Matrices)
  {
    throw FEMExceptionLinearSystem(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::ScaleMatrix", "No matrices have been allocated");
  }
  if (matrixIndex >= m_NumberOfMatrices)
  {
    throw FEMExceptionLinearSystemBounds(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::ScaleMatrix", "m_Matrices", matrixIndex);
  }

  int          i;
  doublereal * values = (*m_Matrices)[matrixIndex].GetA();
  for (i = 0; i < (*m_Matrices)[matrixIndex].GetIA()[this->m_Order] - 1; ++i)
  {
    values[i] = values[i] * scale;
  }
}

LinearSystemWrapperItpack::Float
LinearSystemWrapperItpack::GetVectorValue(unsigned int i, unsigned int vectorIndex) const
{
  /* error checking */
  if (!m_Vectors)
  {
    throw FEMExceptionLinearSystem(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::GetVectorValue", "No vectors have been allocated");
  }
  if (i >= m_Order)
  {
    throw FEMExceptionLinearSystemBounds(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::GetVectorValue", "m_Vectors[]", i);
  }
  if (vectorIndex >= m_NumberOfVectors)
  {
    throw FEMExceptionLinearSystemBounds(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::GetVectorValue", "m_Vectors", vectorIndex);
  }
  if (!(*m_Vectors)[vectorIndex])
  {
    throw FEMExceptionLinearSystem(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::GetVectorValue", "Indexed vector not yet allocated");
  }

  /* return value */
  return (*m_Vectors)[vectorIndex][i];
}

void
LinearSystemWrapperItpack::SetVectorValue(unsigned int i, Float value, unsigned int vectorIndex)
{
  /* error checking */
  if (!m_Vectors)
  {
    throw FEMExceptionLinearSystem(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::SetVectorValue", "No vectors have been allocated");
  }
  if (i >= m_Order)
  {
    throw FEMExceptionLinearSystemBounds(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::SetVectorValue", "m_Vectors[]", i);
  }
  if (vectorIndex >= m_NumberOfVectors)
  {
    throw FEMExceptionLinearSystemBounds(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::SetVectorValue", "m_Vectors", vectorIndex);
  }
  if (!(*m_Vectors)[vectorIndex])
  {
    throw FEMExceptionLinearSystem(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::SetVectorValue", "Indexed vector not yet allocated");
  }

  /* set value */
  (*m_Vectors)[vectorIndex][i] = value;
}

void
LinearSystemWrapperItpack::AddVectorValue(unsigned int i, Float value, unsigned int vectorIndex)
{
  /*( error checking */
  if (!m_Vectors)
  {
    throw FEMExceptionLinearSystem(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::AddVectorValue", "No vectors have been allocated");
  }
  if (i >= m_Order)
  {
    throw FEMExceptionLinearSystemBounds(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::AddVectorValue", "m_Vectors[]", i);
  }
  if (vectorIndex >= m_NumberOfVectors)
  {
    throw FEMExceptionLinearSystemBounds(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::AddVectorValue", "m_Vectors", vectorIndex);
  }
  if (!(*m_Vectors)[vectorIndex])
  {
    throw FEMExceptionLinearSystem(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::AddVectorValue", "Indexed vector not yet allocated");
  }

  /* add value */
  (*m_Vectors)[vectorIndex][i] += value;
}

LinearSystemWrapperItpack::Float
LinearSystemWrapperItpack::GetSolutionValue(unsigned int i, unsigned int solutionIndex) const
{
  // FIXME: error checking
  if ((i >= m_Order) || !m_Solutions || (solutionIndex >= m_NumberOfSolutions) || !(*m_Solutions)[solutionIndex])
  {
    return 0.0;
  }
  return (*m_Solutions)[solutionIndex][i];
}

void
LinearSystemWrapperItpack::SetSolutionValue(unsigned int i, Float value, unsigned int solutionIndex)
{
  /* error checking */
  if (!m_Solutions)
  {
    throw FEMExceptionLinearSystem(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::SetSolutionValue", "No solutions have been allocated");
  }
  if (i >= m_Order)
  {
    throw FEMExceptionLinearSystemBounds(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::SetSolutionValue", "m_Solutions[]", i);
  }
  if (solutionIndex >= m_NumberOfSolutions)
  {
    throw FEMExceptionLinearSystemBounds(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::SetSolutionValue", "m_Solutions", solutionIndex);
  }
  if (!(*m_Solutions)[solutionIndex])
  {
    throw FEMExceptionLinearSystem(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::SetSolutionValue", "Indexed solution not yet allocated");
  }

  /* set value */
  (*m_Solutions)[solutionIndex][i] = value;
}

void
LinearSystemWrapperItpack::AddSolutionValue(unsigned int i, Float value, unsigned int solutionIndex)
{
  /* error checking */
  if (!m_Solutions)
  {
    throw FEMExceptionLinearSystem(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::AddSolutionValue", "No solutions have been allocated");
  }
  if (i >= m_Order)
  {
    throw FEMExceptionLinearSystemBounds(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::AddSolutionValue", "m_Solutions[]", i);
  }
  if (solutionIndex >= m_NumberOfSolutions)
  {
    throw FEMExceptionLinearSystemBounds(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::AddSolutionValue", "m_Solutions", solutionIndex);
  }
  if (!(*m_Solutions)[solutionIndex])
  {
    throw FEMExceptionLinearSystem(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::AddSolutionValue", "Indexed solution not yet allocated");
  }

  (*m_Solutions)[solutionIndex][i] += value;
}

void
LinearSystemWrapperItpack::Solve()
{
  /* error checking */
  if (!m_Order || !m_Matrices || !m_Vectors || !m_Solutions)
  {
    throw FEMExceptionLinearSystem(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::Solve", "Not all necessary data members have been allocated");
  }
  if (!(*m_Matrices)[0].GetOrder())
  {
    throw FEMExceptionLinearSystem(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::AddSolutionValue", "Primary matrix never filled");
  }

  /* itpack variables */
  integer      N;
  integer      NB;
  integer      NW;
  integer      NCG;
  integer *    IWKSP;
  doublereal * WKSP;
  integer      IERR = 0;

  /*********************************************************************
   * FIX ME: itpack does not allow for any non-zero diagonal elements
   * so "very small" numbers are inserted to allow for a solution
   *
   * int i;
   * doublereal fakeZero = 1.0e-16;

   * //insert "fake" zeros
   * for (i=0; i<static_cast<int>(m_Order); ++i)
   *   {
   *   if( (*m_Matrices)[0].Get(i,i) == 0.0)
   *     {
   *     (*m_Matrices)[0].Set(i,i,fakeZero);
   *     }
   *   }
   *   // END FIXME
   *********************************************************************/

  /* Initialize solution values (set to zero) */
  if (!this->IsSolutionInitialized(0))
  {
    this->InitializeSolution(0);
  }

  /* Set up itpack workspace variables
   *
   * N -> Order of system
   *
   * NCG -> temp var
   *
   * NW -> on input: length of wksp, on output: actual amount used
   *  jcg_()    - 4*N + NCG
   *  jsi_()    - 2*N
   *  sor_()    - N
   *  ssorcg_() - 6*N + NCG
   *  ssorsi_() - 5*N
   *  rscg_()   - N + 3*NB + NCG
   *  rssi_()   - N + NB
   *
   * IWKSP -> temp variable used by itpack (integer[3*N])
   * WKSP -> temp variable used by itpack (doublereal[NW])
   */
  N = m_Order;
  if (m_IPARM[4] == 1)
  {
    NCG = 4 * m_IPARM[0];
  }
  else
  {
    NCG = 2 * m_IPARM[0];
  }
  NB = m_IPARM[8] + N; // upper bound of what can be computed in prbndx_

  switch (m_Method)
  {
    case 0:
      NW = 4 * N + NCG;
      break;
    case 1:
      NW = 2 * N;
      break;
    case 2:
      NW = N;
      break;
    case 3:
      NW = 6 * N + NCG;
      break;
    case 4:
      NW = 5 * N;
      break;
    case 5:
      NW = N + 3 * NB + NCG;
      break;
    case 6:
      NW = N + NB;
      break;
    default:
      std::ostringstream msg;
      msg << "m_Method is" << m_Method << " but must be >=0 and <= 6";
      throw FEMExceptionLinearSystem(__FILE__, __LINE__, "LinearSystemWrapperItpack::Solve", msg.str().c_str());
  }
  m_IPARM[7] = NW;
  IWKSP = new integer[3 * N];
  WKSP = new doublereal[NW + 2];

  integer i;
  for (i = 0; i < NW; ++i)
  {
    WKSP[i] = 0.0;
  }
  for (i = 0; i < (3 * N); ++i)
  {
    IWKSP[i] = 0;
  }

  // Save maximum number of iteration, since it will
  // be overwritten.
  int max_num_iterations = m_IPARM[0];

  /* call to itpack solver routine */
  (*m_Methods[m_Method])(&N,
                         (*m_Matrices)[0].GetIA(),
                         (*m_Matrices)[0].GetJA(),
                         (*m_Matrices)[0].GetA(),
                         (*m_Vectors)[0],
                         (*m_Solutions)[0],
                         &(IWKSP[0]),
                         &NW,
                         &(WKSP[0]),
                         &(m_IPARM[0]),
                         &(m_RPARM[0]),
                         &IERR);

  m_IPARM[0] = max_num_iterations;

  /* remove exception throw on convergence failure */
  if (IERR < 100)
  {
    if ((IERR % 10) == 3)
    {
      IERR = 0;
    }
  }

  /* clean up */
  delete[] IWKSP;
  delete[] WKSP;

  /* check for itpack error code */
  if (IERR > 0)
  {
    throw FEMExceptionItpackSolver(__FILE__, __LINE__, "LinearSystemWrapperItpack::Solve", IERR);
  }
}

void
LinearSystemWrapperItpack::SwapMatrices(unsigned int matrixIndex1, unsigned int matrixIndex2)
{
  /* error checking */
  if (!m_Matrices)
  {
    throw FEMExceptionLinearSystem(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::SwapMatrices", "No matrices allocated");
  }
  if (matrixIndex1 >= m_NumberOfMatrices)
  {
    throw FEMExceptionLinearSystemBounds(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::SwapMatrices", "m_Matrices", matrixIndex1);
  }
  if (matrixIndex2 >= m_NumberOfMatrices)
  {
    throw FEMExceptionLinearSystemBounds(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::SwapMatrices", "m_Matrices", matrixIndex2);
  }

  int          n = (*m_Matrices)[matrixIndex2].GetOrder();
  int          nz = (*m_Matrices)[matrixIndex2].GetMaxNonZeroValues();
  integer *    ia = (*m_Matrices)[matrixIndex2].GetIA();
  integer *    ja = (*m_Matrices)[matrixIndex2].GetJA();
  doublereal * a = (*m_Matrices)[matrixIndex2].GetA();

  (*m_Matrices)[matrixIndex2].SetOrder((*m_Matrices)[matrixIndex1].GetOrder());
  (*m_Matrices)[matrixIndex2].SetMaxNonZeroValues((*m_Matrices)[matrixIndex1].GetMaxNonZeroValues());
  (*m_Matrices)[matrixIndex2].SetCompressedRow(
    (*m_Matrices)[matrixIndex1].GetIA(), (*m_Matrices)[matrixIndex1].GetJA(), (*m_Matrices)[matrixIndex1].GetA());

  (*m_Matrices)[matrixIndex1].SetOrder(n);
  (*m_Matrices)[matrixIndex1].SetMaxNonZeroValues(nz);
  (*m_Matrices)[matrixIndex1].SetCompressedRow(ia, ja, a);
}

void
LinearSystemWrapperItpack::SwapVectors(unsigned int vectorIndex1, unsigned int vectorIndex2)
{
  /* error checking */
  if (!m_Vectors)
  {
    throw FEMExceptionLinearSystem(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::SwapVectors", "No vectors allocated");
  }
  if (vectorIndex1 >= m_NumberOfVectors)
  {
    throw FEMExceptionLinearSystemBounds(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::SwapVectors", "m_Vectors", vectorIndex1);
  }
  if (vectorIndex2 >= m_NumberOfVectors)
  {
    throw FEMExceptionLinearSystemBounds(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::SwapVectors", "m_Vectors", vectorIndex2);
  }

  VectorRepresentation temp = (*m_Vectors)[vectorIndex1];

  (*m_Vectors)[vectorIndex1] = (*m_Vectors)[vectorIndex2];
  (*m_Vectors)[vectorIndex2] = temp;
}

void
LinearSystemWrapperItpack::SwapSolutions(unsigned int solutionIndex1, unsigned int solutionIndex2)
{
  /* error checking */
  if (!m_Solutions)
  {
    throw FEMExceptionLinearSystem(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::SwapSolutions", "No solutions allocated");
  }
  if (solutionIndex1 >= m_NumberOfSolutions)
  {
    throw FEMExceptionLinearSystemBounds(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::SwapSolutions", "m_Solutions", solutionIndex1);
  }
  if (solutionIndex2 >= m_NumberOfSolutions)
  {
    throw FEMExceptionLinearSystemBounds(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::SwapSolutions", "m_Solutions", solutionIndex2);
  }

  VectorRepresentation temp = (*m_Solutions)[solutionIndex1];

  (*m_Solutions)[solutionIndex1] = (*m_Solutions)[solutionIndex2];
  (*m_Solutions)[solutionIndex2] = temp;
}

void
LinearSystemWrapperItpack::CopySolution2Vector(unsigned int solutionIndex, unsigned int vectorIndex)
{
  /* error checking */
  if (!m_Vectors)
  {
    throw FEMExceptionLinearSystem(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::CopySolution2Vector", "No vectors allocated");
  }
  if (!m_Solutions)
  {
    throw FEMExceptionLinearSystem(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::CopySolution2Vector", "No solutions allocated");
  }
  if (vectorIndex >= m_NumberOfVectors)
  {
    throw FEMExceptionLinearSystemBounds(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::CopySolution2Vector", "m_Vectors", vectorIndex);
  }
  if (solutionIndex >= m_NumberOfSolutions)
  {
    throw FEMExceptionLinearSystemBounds(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::CopySolution2Vector", "m_Solutions", solutionIndex);
  }

  this->InitializeVector(vectorIndex);
  /* copy values */
  for (unsigned int i = 0; i < m_Order; ++i)
  {
    (*m_Vectors)[vectorIndex][i] = (*m_Solutions)[solutionIndex][i];
  }
}

void
LinearSystemWrapperItpack::CopyVector2Solution(unsigned int vectorIndex, unsigned int solutionIndex)
{
  /* error checking */
  if (!m_Vectors)
  {
    throw FEMExceptionLinearSystem(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::CopySolution2Vector", "No vectors allocated");
  }
  if (!m_Solutions)
  {
    throw FEMExceptionLinearSystem(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::CopySolution2Vector", "No solutions allocated");
  }
  if (vectorIndex >= m_NumberOfVectors)
  {
    throw FEMExceptionLinearSystemBounds(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::CopySolution2Vector", "m_Vectors", vectorIndex);
  }
  if (solutionIndex >= m_NumberOfSolutions)
  {
    throw FEMExceptionLinearSystemBounds(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::CopySolution2Vector", "m_Solutions", solutionIndex);
  }

  this->InitializeSolution(solutionIndex);
  /* copy values */
  for (unsigned int i = 0; i < m_Order; ++i)
  {
    (*m_Solutions)[solutionIndex][i] = (*m_Vectors)[vectorIndex][i];
  }
}

void
LinearSystemWrapperItpack::MultiplyMatrixMatrix(unsigned int resultMatrixIndex,
                                                unsigned int leftMatrixIndex,
                                                unsigned int rightMatrixIndex)
{
  /* error checking */
  if (!m_Matrices)
  {
    throw FEMExceptionLinearSystem(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::MultiplyMatrixMatrix", "No matrices allocated");
  }
  if (resultMatrixIndex >= m_NumberOfMatrices)
  {
    throw FEMExceptionLinearSystemBounds(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::MultiplyMatrixMatrix", "m_Matrices", resultMatrixIndex);
  }
  if (leftMatrixIndex >= m_NumberOfMatrices)
  {
    throw FEMExceptionLinearSystemBounds(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::MultiplyMatrixMatrix", "m_Matrices", leftMatrixIndex);
  }
  if (rightMatrixIndex >= m_NumberOfMatrices)
  {
    throw FEMExceptionLinearSystemBounds(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::MultiplyMatrixMatrix", "m_Matrices", rightMatrixIndex);
  }

  (*m_Matrices)[leftMatrixIndex].mult(&((*m_Matrices)[rightMatrixIndex]), &((*m_Matrices)[resultMatrixIndex]));
}

void
LinearSystemWrapperItpack::MultiplyMatrixVector(unsigned int resultVectorIndex,
                                                unsigned int matrixIndex,
                                                unsigned int vectorIndex)
{
  /* error checking */
  if (!m_Matrices)
  {
    throw FEMExceptionLinearSystem(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::MultiplyMatrixVector", "No matrices allocated");
  }
  if (!m_Vectors)
  {
    throw FEMExceptionLinearSystem(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::MultiplyMatrixVector", "No vectors allocated");
  }
  if (resultVectorIndex >= m_NumberOfVectors)
  {
    throw FEMExceptionLinearSystemBounds(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::MultiplyMatrixVector", "m_Vectors", resultVectorIndex);
  }
  if (matrixIndex >= m_NumberOfMatrices)
  {
    throw FEMExceptionLinearSystemBounds(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::MultiplyMatrixVector", "m_Matrices", matrixIndex);
  }
  if (vectorIndex >= m_NumberOfVectors)
  {
    throw FEMExceptionLinearSystemBounds(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::MultiplyMatrixVector", "m_Vectors", vectorIndex);
  }

  /* perform mult */
  (*m_Matrices)[matrixIndex].mult((*m_Vectors)[vectorIndex], (*m_Vectors)[resultVectorIndex]);
}


void
LinearSystemWrapperItpack::MultiplyMatrixSolution(unsigned int resultVectorIndex,
                                                  unsigned int matrixIndex,
                                                  unsigned int solutionIndex)
{
  /* error checking */
  if (!m_Matrices)
  {
    throw FEMExceptionLinearSystem(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::MultiplyMatrixVector", "No matrices allocated");
  }

  if (!m_Vectors)
  {
    throw FEMExceptionLinearSystem(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::MultiplyMatrixVector", "No vectors allocated");
  }

  if (!m_Solutions)
  {
    throw FEMExceptionLinearSystem(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::MultiplyMatrixVector", "No solutions allocated");
  }

  if (resultVectorIndex >= m_NumberOfVectors)
  {
    throw FEMExceptionLinearSystemBounds(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::MultiplyMatrixVector", "m_Vectors", resultVectorIndex);
  }

  if (matrixIndex >= m_NumberOfMatrices)
  {
    throw FEMExceptionLinearSystemBounds(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::MultiplyMatrixVector", "m_Matrices", matrixIndex);
  }

  if (solutionIndex >= m_NumberOfSolutions)
  {
    throw FEMExceptionLinearSystemBounds(
      __FILE__, __LINE__, "LinearSystemWrapperItpack::MultiplyMatrixVector", "m_Solutions", solutionIndex);
  }

  /* perform multiplication */
  (*m_Matrices)[matrixIndex].mult((*m_Solutions)[solutionIndex], (*m_Vectors)[resultVectorIndex]);
}


LinearSystemWrapperItpack::~LinearSystemWrapperItpack()
{
  delete m_Matrices;

  unsigned int i;
  if (m_Vectors)
  {
    for (i = 0; i < m_NumberOfVectors; ++i)
    {
      delete[](*m_Vectors)[i];
    }
    delete m_Vectors;
  }

  if (m_Solutions)
  {
    for (i = 0; i < m_NumberOfSolutions; ++i)
    {
      delete[](*m_Solutions)[i];
    }
    delete m_Solutions;
  }
}

FEMExceptionItpackSolver::FEMExceptionItpackSolver(const char * file,
                                                   unsigned int lineNumber,
                                                   std::string  location,
                                                   integer      errorCode)
  : FEMException(file, lineNumber)
{
  std::string solverError;

  if (errorCode < 100)
  {
    errorCode = errorCode % 10;
  }

  switch (errorCode)
  {
    case 1:
      solverError = "Invalid order of system";
      break;
    case 2:
      solverError = "Workspace is not large enough";
      break;
    case 3:
      solverError = "Failure to converge before reaching maximum number of iterations";
      break;
    case 4:
      solverError = "Invalid order of black subsystem";
      break;
    case 101:
      solverError = "A diagonal element is not positive";
      break;
    case 102:
      solverError = "No diagonal element in a row";
      break;
    case 201:
      solverError = "Red-black indexing is not possible";
      break;
    case 301:
      solverError = "No entry in a row of the original matrix";
      break;
    case 302:
      solverError = "No entry in a row of the permuted matrix";
      break;
    case 303:
      solverError = "Sorting error in a row of the permuted matrix";
      break;
    case 401:
      solverError = "A diagonal element is not positive";
      break;
    case 402:
      solverError = "No diagonal element in a row";
      break;
    case 501:
      solverError = "Failure to converge before reaching maximum number of iterations";
      break;
    case 502:
      solverError = "Function does not change sign at endpoints";
      break;
    case 601:
      solverError = "Successive iterations are not monotone increasing";
      break;
    default:
      solverError = "Unknown error code returned";
  }

  std::ostringstream buf;
  buf << "Error: " << solverError;

  SetDescription(buf.str().c_str());

  SetLocation(location);
}

} // end namespace fem
} // end namespace itk