#include "util.h"

template<typename ComplexOrRealType>
void den_kron_submatrix(const int nrow_A,
                        const int ncol_A,
                        const PsimagLite::Matrix<ComplexOrRealType>& a_,
                        const int nrow_B,
                        const int ncol_B,
                        const PsimagLite::Matrix<ComplexOrRealType>& b_,
                        const int nrindex,
                        const int ncindex,
                        const PsimagLite::Vector<int>::Type& rindex,
                        const PsimagLite::Vector<int>::Type& cindex,
                        PsimagLite::Matrix<ComplexOrRealType>& e_ )
{
/*
 * -------------------------------------------------
 * extract a submatrix out of kronecker product
 * equivalent to
 * C = kron(A,B), then E = C( rindex(:), cindex(:) )
 *
 * assume A, B are in dense matrix format
 * -------------------------------------------------
 */
	const int nrow_E = nrindex;
	const int ncol_E = ncindex;

	int ie = 0;
	int je = 0;

	/*
  * ------------------------
  * check rindex[], cindex[]
  * ------------------------
  */
#ifndef NDEBUG

	const int nrow_C = nrow_A * nrow_B;
	const int ncol_C = ncol_A * ncol_B;

	for(ie=0; ie < nrindex; ie++) {
		int ic = rindex[ie];
		int isok = (0 <= ic) && (ic < nrow_C);
		assert( isok );
	};

	for(je=0; je < ncindex; je++) {
		int jc = cindex[je];
		int isok = (0 <= jc) && (jc < ncol_C);
		assert( isok );
	};
#endif


	/*
  * --------------------------------
  * fill entries in E(ie,je) matrix
  * --------------------------------
  */

	for(je=0; je < ncol_E; je++) {
		for(ie=0; ie < nrow_E; ie++) {
			int ic = rindex[ie];
			int jc = cindex[je];
			/*
	 * --------------------------
	 * ic = [ib,ia] or
	 * ic =  ib + ia * nrow_B
	 *
	 * jc = [jb,ja] or
	 * jc =  jb + ja * ncol_B
	 * --------------------------
	 */
			int ib = (ic % nrow_B);
			int ia = (ic - ib)/nrow_B;


			int jb = (jc % ncol_B);
			int ja = (jc - jb)/ncol_B;



			assert((0 <= ia) && (ia < nrow_A) &&
			       (0 <= ib) && (ib < nrow_B) &&
			       ((ib + ia*nrow_B) == ic ));


			assert((0 <= ja) && (ja < ncol_A) &&
			       (0 <= jb) && (jb < ncol_B) &&
			       ((jb + ja*ncol_B) == jc));

			ComplexOrRealType cij = a_(ia,ja)*b_(ib,jb);

			e_(ie,je) = cij;
		};
	};
}
#undef A
#undef B
#undef E
