#include "qrMatrix.h"

// Constructor
qrMatrix::qrMatrix(Alignment *aln) {

  //printf("   entering qrMatrix(Alignment aln)\n");

  int tempIndex = 0;

  cMi = aln->maximumSequenceLength;
  cMj = 4;
  cMk = aln->nSequences;

  coordMatrix = new float**[cMi];
  for (int i=0; i<cMi; i++) {
    coordMatrix[i] = new float*[cMj];
    for (int j=0; j<cMj; j++) {
      coordMatrix[i][j] = new float[cMk];
      for (int k=0; k<cMk; k++) {
	//printf("    [%d][%d][%d]\n",i,j,k);
	if (j==3) {
	  if (aln->sequences[k].isGap[i]) {
	    coordMatrix[i][j][k] = 1.0;   // THIS SHOULD BE NORMED
	  } else {
	    coordMatrix[i][j][k] = 0.0;
	  }
	}
	else if (aln->sequences[k].isGap[i]) {
	  coordMatrix[i][j][k] = 0.0;
	}
	else {
	  //printf("     here1\n");
	  tempIndex = aln->sequences[k].residuesIndexToResiduesWithoutGapsIndex[i];
	  //printf("     here2\n");
	  coordMatrix[i][j][k] = aln->sequences[k].structure.caCoordinates[tempIndex][j];
	  //printf("     here3\n");
	}
      }
    }
  }

  columnList = new int[cMk];
  for (int k=0; k<cMk; k++) {
    columnList[k] = k;
  }

  //printf("   leaving qrMatrix(Alignment aln)\n");

  return;
}


// Destructor
qrMatrix::~qrMatrix() {

  for (int i=0; i<cMi; i++) {
    for (int j=0; j<cMj; j++) {
      delete coordMatrix[i][j];
    }
    delete coordMatrix[i];
  }

  delete coordMatrix;
  delete columnList;

  return;
}


// qrAlgorithm
//   Loop through the sequences, permuting the most linearly independent
//   sequence (n) to the front of the current submatrix, and perform
//   Householder transformations on the submatrices to zero out the
//   contributions of n
int qrMatrix::qrAlgorithm() {

  int k;  // current column (corresponds to sequence)

  scaleGapData();

  for (k=0; k<cMk; k++) {
    permuteColumns(k);
    householder(k);
  }

  return 0;
}


// printColumns
int qrMatrix::printColumns() {
  
  for (int k=0; k<cMk; k++) {
    printf("%d ", columnList[k]);
  }
  printf("\n");

  return 0;
}


// householder
//
int qrMatrix::householder(int currentColumn) {

  int i,j,k;
  float sign, alpha, beta, gamma;
  float * hhVector;

  // Loop over coordinate dimensions (x,y,z,gap)
  for (j=0; j<cMj; j++) {
    
    // Compute Householder vector for current column
    k = currentColumn;
    alpha = 0;
    for (i=k; i<cMi; i++) {
      alpha += coordMatrix[i][j][columnList[k]] * coordMatrix[i][j][columnList[k]];
    }
    sign = (coordMatrix[k][j][columnList[k]] >= 0) ? 1.0 : -1.0;
    alpha = -sign * sqrt(alpha);
    hhVector = new float[cMi];
    for (i=0; i<k; i++) {
      //hhVector[i] = -alpha;  // REMOVED 8/3
      hhVector[i] = 0;   // ADDED 8/3
    }
    hhVector[k] = coordMatrix[k][j][columnList[k]] - alpha;
    for (i=k+1; i<cMi; i++) {
      //hhVector[i] = coordMatrix[i][j][columnList[k]] - alpha;   // REMOVED 8/3
      // ADDED 8/3 {
      hhVector[i] = coordMatrix[i][j][columnList[k]];
      //if (i==k) {
      //  hhVector[i] -= alpha;
      //}
      // } ADDED 8/3
    }

    // Get inner product of Householder vector with itself
    beta = 0;
    for (i=k; i<cMi; i++) {
      beta += hhVector[i] * hhVector[i];
    }
    
    // Apply transformation to remaining submatrix
    if (beta != 0) {
      //printf("In --- beta: %f\n", beta);
      for (; k<cMk; k++) {
	gamma = 0;
	for (i=0; i<cMi; i++) {
	  gamma += hhVector[i] * coordMatrix[i][j][columnList[k]];
	}
	//printf("gamma: %f, (2*gamma)/beta: %f", gamma, (2*gamma)/beta);
	for (i=currentColumn; i<cMi; i++) {
	  //printf("coordMatrix[%d][%d][%d]: %f\n", i,j,columnList[k], coordMatrix[i][j][columnList[k]]);
	  //printf("((2*gamma)/beta) * hhVector[%d] = %f * %f = %f\n",i, (2*gamma)/beta, hhVector[i], ((2*gamma)/beta) * hhVector[i]);
	  coordMatrix[i][j][columnList[k]] -= ((2*gamma)/beta) * hhVector[i];
	}
	//printf("\n");
      }
    }
  }

  return 0;
}


// permutation - 
//   move the column with the max frobenius norm to the front
//   of the current submatrix (currentColumn)
int qrMatrix::permuteColumns(int currentColumn) {
  
  int frontCol = currentColumn;
  int maxCol = 0;
  float *norms = new float[cMk];
  float maxNorm = 0.0;

  for (int k=0; k<cMk; k++) {
    norms[k] = 0.0;
  }

  // Get frobenius norms for remaining matrices
  //for (int k=frontCol; k<cMk; k++) {
  for (int k=0; k<cMk; k++) {
    norms[k] = frobeniusNormSeq(k, frontCol);
    if (norms[k] > maxNorm) {
      maxCol = k;
      maxNorm = norms[k];
    }
  }

  delete norms;

  //printf("frontCol: %d\n",frontCol);
  //printf("maxCol: %d\n",maxCol);

  int tempMaxCol = columnList[maxCol];
  int tempFrontCol = columnList[frontCol];

  //printf(" tempFrontCol: %d\n",tempFrontCol);
  //printf(" tempMaxCol: %d\n",tempMaxCol);

  columnList[frontCol] = tempMaxCol;
  columnList[maxCol] = tempFrontCol;

  //printColumns();

  return 0;
}


// frobeniusNormSeq
//   Get the frobenius norm for the matrix corresponding
//   to the data for one sequence
//   frobeniusNorm(A) = sqrt( sum( all Aij ) );
float qrMatrix::frobeniusNormSeq(int k, int currentRow) {

  float fNorm = 0;

  for (int i=currentRow; i<cMi; i++) {
    for (int j=0; j<cMj; j++) {
      //fNorm += pow(abs(coordMatrix[i][j][k]),2);
      fNorm += coordMatrix[i][j][columnList[k]] * coordMatrix[i][j][columnList[k]];
    }
  }

  //printf("%d,%d: %f\n",k,currentRow,sqrt(fNorm));

  return sqrt(fNorm);
}


// frobeniusNormCoord
//
float qrMatrix::frobeniusNormCoord(int j) {

  float fNorm = 0;

  for (int i=0; i<cMi; i++) {
    for (int k=0; k<cMk; k++) {
      //fNorm += pow(abs(coordMatrix[i][j][k]),2);
      fNorm += coordMatrix[i][j][columnList[k]] * coordMatrix[i][j][columnList[k]];
    }
  }

  return sqrt(fNorm);
}


// scaleGapData
//   Scale the gap matrix elements to appropriate values so that
//   the QR algorithm is not biased towards or against the gaps.
//   scale*fNorm(G) = fNorm(X) + fNorm(Y) + fNorm(Z)
int qrMatrix::scaleGapData() {

  //float coordNorm;
  float scale = 1.0;   // Default for the case where gapNorm==0
  //float scaleConstant = 1.19;   // REMOVED 8/3
  float scaleConstant = 2.0;   // ADDED 8/3
  float gapNorm = frobeniusNormCoord(3);

  if (gapNorm != 0) {
    scale = frobeniusNormCoord(0) + frobeniusNormCoord(1) + frobeniusNormCoord(2);
    scale /= gapNorm;
    scale *= scaleConstant;
    int j=3;
    for (int i=0; i<cMi; i++) {
      for (int k=0; k<cMk; k++) {
	coordMatrix[i][j][columnList[k]] *= scale;
      }
    }
  }

  return 0;
}