#include <utility>
#include "mtl/lu.h"

namespace mtl {



/**
  Swap a bunch of rows in matrix A according to
  the pivot vector.
 */

template <class Matrix, class Pvector>
void
multi_swap(Matrix& A, Pvector& ipvt)
{
  for (int i = 0; i < ipvt.size(); ++i)
    swap(rows(A)[i], rows(A)[ipvt[i] - 1]);
}

template <class Matrix, class Pvector>
void
multi_swap_back(Matrix& A, Pvector& ipvt)
{
  for (int i = ipvt.size() - 1; i >= 0; i--)
    swap(rows(A)[i], rows(A)[ipvt[i] - 1]);
}

#define LU_BF 64

template <class Matrix, class Pvector>
void block_lu(Matrix& A, Pvector& ipvt) MTL_THROW(zero_pivot)
{
  using std::min;
  typedef typename Matrix::value_type PR;

  const int BF = LU_BF; // blocking factor
  const int M = A.nrows();
  const int N = A.ncols();

  if (MTL_MIN(M, N) <= BF || BF == 1)
    lu_factorize(A, ipvt);

  for (int j = 0; j < MTL_MIN(M, N); j += BF) {
    int jb = MTL_MIN(MTL_MIN(M, N) - j, BF);

    // set up the submatrices
    
    int row_sep1[] = { j, j + jb };
    int col_sep1[] = { j };
    Matrix Ap = A.partition(array_to_vec(row_sep1),
                            array_to_vec(col_sep1));  

    int row_sep2[] = { j, j + jb };
    int col_sep2[] = { j, j + jb };
    Matrix As = A.partition(array_to_vec(row_sep2),
                            array_to_vec(col_sep2));

    triangle_view<Matrix, unit_lower>::type L_11(As(1,1));

    Pvector pivots(jb);

    // factor block A_1
    lu_factorize(Ap(1,1), pivots);

    // record the pivot indices in ipvt
    for (int i = j; i < MTL_MIN(M, j + jb); ++i)
      ipvt[i] = pivots[i-j] + j;

    // interchanges the rows in A_0
    if (j > 0)
      multi_swap(Ap(1,0), pivots);

    if (j + jb < M) {
      // interchange the rows in A_2
      multi_swap(Ap(1,2), pivots);
      
      // update block row A_12
      tri_solve(L_11, As(1,2), left_side());

      // update A_22
      if (j + jb < M)
        mult(scaled(As(2,1), PR(-1)), As(1,2), As(2,2));
    }
  }
}

} /* namespace mtl */