// File: /workspace/github_cpp_makefile_star2_O2/DavidLBruce[P]sim3dof/Matrix/build/../../Matrix/src/Matrix.cpp
// Function: Matrix::Inv()
Matrix Matrix::Inv()
{
  //
  // Check to make sure the matrix is square.
  //
  if (rows != cols)
  {
    throw(DIMERR());
  }

  int    col(0);
  double factor(0.0);        // xmult
  int    identElem(0);       //
  int    pivotRow(0);        // lSubk
  int    pivotRowTrial(0);   //
  double *pRhs(NULL);        //
  double *pRowMaxes(NULL);   // sSubi
  int    *pRowOrder(NULL);   // l
  double ratio(0.0);         // r
  double ratioMax(0.0);      // rmax
  int    row(0);             // i1
  double rowMax(0.0);        //
  double sum(0.0);           //
  int    temp(0);            //
  Matrix result(rows, cols); //
  int    rowIdx;             //
  int    rowRatioMax;        // 
  int    rowMaxIdx;          //
  int    scaleRow;           // lSubi
  int    zeroColIdx(0);      // k

  #ifdef debugEn
  FILE*fp = fopen("Debug.txt", "w");
  #endif
  //
  // Forward elimination with scaled pivoting.  This algorithm calculates an
  // upper triangular matrix of coefficients called upTriang.  The resulting
  // system is:
  //
  //   upTriang * x = b'
  //
  // The upper triangular system is easily solved for x by back substitution,
  // starting at the last row.
  //
  // The solution to the upper triangular system x also solves the original 
  // linear system:
  //
  //   Ax = b
  //
  // Later, sucessive columns of the identity matrix will be substituted for
  // b, resulting in successive columns of the inv(A) matrix.
  //
  //   A*inv(A) = I
  //
  // Gaussian elimination takes one equation of a system of linear equations
  // as a pivot row.  For each of the remaining rows a scaled version of the 
  // pivot row is subtracted, such that one element of the selected row is 
  // zeroed.
  //
  // Then a new pivot row is selected and the process is repeated eliminating
  // a new element from the remaining rows.
  //
  // In order to reduce the chance for errors due to finite arithmetic, the
  // sequence of pivot rows is selected such that the initial scale ratio is
  // largest.  Successive pivot rows are selected in order of decreasing 
  // scale ratio.  Rather than reorder the system of equitions, a vector is 
  // created to maintain the order in which the pivot rows are selected. 
  // The order in which the pivot rows are selected is stored in the rowOrder
  // vector.
  //
  pRowOrder = new int[rows];

  //
  // The abs(maximum) of each row is stored in pRowMaxws vector.
  //
  pRowMaxes = new double[rows];

  //
  // Create a copy of the matrix.  The algorithm will convert the new matrix
  // into upper triangular form as it proceeds.
  //
  Matrix upTriang(*this);

  //
  // Find the largest coefficient in each row.
  //
  for (row = 0; row < rows; ++row)
  {
    //
    // Initialize the pivot row order as the initial matrix row order.  This
    // order will be modified as the algorithm progresses.
    //
    pRowOrder[row] = row;

    rowMax = 0.0;
    for (col = 0; col < cols; ++col)
    {
      rowMax = Max(rowMax, fabs(upTriang[row][col]));
    }
    pRowMaxes[row] = rowMax;
  }

  #ifdef debugEn
  fprintf(fp, "p05 pRowMaxes = %7.2f %7.2f %7.2f %7.2f \n", 
               pRowMaxes[0], pRowMaxes[1], pRowMaxes[2], pRowMaxes[3]);
  fprintf(fp, "pRowOrder = %d %d %d %d \n", pRowOrder[0], pRowOrder[1],
                                    pRowOrder[2], pRowOrder[3]);
  #endif

  //
  // Step through the system along the diagonal, selecting the appropriate pivot
  // element for each column.
  //
  for (zeroColIdx = 0; zeroColIdx < cols - 1; ++zeroColIdx)
  {
    #ifdef debugEn
    fprintf(fp, "p10 zeroColIdx (k) = %d \n", zeroColIdx);
    #endif

    //
    // Find the row with the largest scale ratio for each column.  This will be
    // the pivot row for that column.
    //
    ratioMax = 0.0;

    for (rowIdx = zeroColIdx; rowIdx < rows; ++rowIdx)
    {
      //
      // See if the next row should be the pivot row for this pivot index by
      // selecting the highest scale ratio.
      //
      pivotRowTrial = pRowOrder[rowIdx];

      //
      // The denomiator in the ratio calculation below should never be zero
      // since it was chosen to be the max relative to the row.
      //
      if (pRowMaxes[pivotRowTrial] == 0.0)
      {
        throw(SINGERR());
      }

      ratio = fabs(upTriang[pivotRowTrial][zeroColIdx] / 
                                                  pRowMaxes[pivotRowTrial]);

      if (ratio > ratioMax)
      {
        ratioMax = ratio;

        rowRatioMax = rowIdx;
      }
    } // end for (rowIdx = zeroColIdx; rowIdx < rows; ++rowIdx)

    #ifdef debugEn
    fprintf(fp, "p20 ratioMax = %7.2f \n", ratioMax);
    fprintf(fp, "p30 rowRatioMax (j) = %d \n", rowRatioMax); 
    #endif

    //
    // Sort the pivot row list by interchanging the newly found pivot row with
    // the current row pointed to by the pivot index.
    //
    temp = pRowOrder[zeroColIdx];
    pRowOrder[zeroColIdx] = pRowOrder[rowRatioMax];
    pRowOrder[rowRatioMax] = temp;
    
    #ifdef debugEn
    fprintf(fp, "p40: lSubj <--> lSubk \n");
    fprintf(fp, "p50: pRowOrder = %d %d %d %d \n", pRowOrder[0], pRowOrder[1],
                                    pRowOrder[2], pRowOrder[3]);
    #endif

    pivotRow = pRowOrder[zeroColIdx];
    
    #ifdef debugEn
    fprintf(fp, "p30 pivotRow (j) = %d \n", pivotRow); 
    #endif

    for (rowIdx = zeroColIdx + 1; rowIdx < rows; ++rowIdx)
    {
      scaleRow = pRowOrder[rowIdx];
      
      #ifdef debugEn
      fprintf(fp, "p70 scaleRow = row being scaled %d \n", scaleRow);
      #endif
  
      //
      // The denomenator in the factor calculation should never be zero since it
      // was chosen to be the max relative to the row max.
      //
      if (upTriang[pivotRow][zeroColIdx] == 0.0)
      {
        throw(SINGERR());
      }
      factor = upTriang[scaleRow][zeroColIdx] / upTriang[pivotRow][zeroColIdx];
    
      //
      // Store the scaling factor in the upper triangular matrix for use later.
      // This makes the book keeping easy since it can be accessed using the 
      // row and zeroColIdx indeces.  Of course the matrix is no linger strictly 
      // upper traingular.
      //
      upTriang[scaleRow][zeroColIdx] = factor;
    
      //
      // Zero the elements in the pivot column in the remaining rows to create
      // the upper triangular matrix.
      //
      for (col = zeroColIdx + 1; col < cols; ++col)
      {
        upTriang[scaleRow][col] -= factor * upTriang[pivotRow][col];
    
      }

      #ifdef debugEn
      fprintf(fp, "p80 row = %d; factor = %7.2f;", scaleRow, factor);
      fprintf(fp, " upTriang(%d,:) = %7.2f %7.2f %7.2f %7.2f \n", row,
                  upTriang[scaleRow][0], upTriang[scaleRow][1], 
                  upTriang[scaleRow][2], upTriang[scaleRow][3]);
      #endif
    
    } // end for (rowIdx = zeroColIdx + 1; rowIdx < rows; ++rowIdx)
  }  // for (zeroColIdx = 0; zeroColIdx < cols - 1; ++zeroColIdx)

  #ifdef debugEn
  fprintf(fp, "p90 upTriang = \n");
  for (int dbRow = 0; dbRow < 4; ++dbRow)
  {
    for (int dbCol = 0; dbCol < 4; ++dbCol)
    {
      fprintf(fp, "%7.2f ", upTriang[dbRow][dbCol]); 
    }
    fprintf(fp, "\n");
  }
  #endif

  //
  // We now have an upper triangular representation of A in 
  //  A * X = b
  // But we have not operated on b.  Furthermore, what we want is inv(A) as in
  //  A * inv(A) = I
  // We'll brek this problem up into columns of inv(A) and I
  //  A * inv(A)(i) = I(i)
  // So we need to apply forward elimination processing to the columns of I.
  // Then proceed to back substitution
  //
  // The modified right hand side system of equations is stored in pRhs.
  //
  //   A * X = b (rhs)
  //
  pRhs = new double[rows];

  for (identElem = 0; identElem < rows; ++identElem)
  {
    //
    // Extract the appropriate column of the identify matrix for use as 
    // the right hand side of the system of equations.
    //
    for (row = 0; row < rows; ++row)
    {
      pRhs[row] = 0.0;
    }
    pRhs[identElem] = 1.0;

    //
    // Apply the same scaling used on the left hand side of the system
    // to the right hand side.
    //
    for (zeroColIdx = 0; zeroColIdx < rows-1; ++zeroColIdx)
    {
      pivotRow = pRowOrder[zeroColIdx];
      
      for (rowIdx = zeroColIdx + 1; rowIdx < rows; ++rowIdx)
      {
        row = pRowOrder[rowIdx];

        factor = upTriang[row][zeroColIdx];

        pRhs[row] -= factor * pRhs[pivotRow];
      }
    }

    //
    // Back substitution starting a the bottome of the triangular matrix.
    //
    row = pRowOrder[rows - 1];

    if (upTriang[row][rows-1] == 0.0)
    {
      throw(SINGERR());
    }

    result[rows-1][identElem] = pRhs[row] / upTriang[row][rows-1];

    for (rowIdx = rows-2; rowIdx >= 0; --rowIdx)
    {
      row = pRowOrder[rowIdx];
      sum = pRhs[row];
      
      for (col = rowIdx + 1; col < cols; ++col)
      {
        sum = sum - upTriang[row][col] * result[col][identElem];
      }
     
      if (upTriang[row][rowIdx] == 0.0)
      {
      throw(SINGERR());
      }
      result[rowIdx][identElem] = sum / upTriang[row][rowIdx];
    }
  }  // end for (identElem = 0; identElem < rows; ++identElem)

  delete [] pRhs;
  delete [] pRowOrder;
  delete [] pRowMaxes;

  #ifdef debugEn
  fclose(fp);
  #endif

  return(result);
}

// Assembly:
pushq %rbp
movq %rsp, %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x88, %rsp
movl 0x8(%rsi), %edx
cmpl (%rsi), %edx
jne 0x13fce
movq %rsi, %r13
movq %rdi, %r14
xorpd %xmm0, %xmm0
movl %edx, %esi
callq 0x12d02
movslq 0x8(%r13), %rbx
leaq (,%rbx,4), %rdi
testq %rbx, %rbx
pushq $-0x1
popq %r15
cmovsq %r15, %rdi
callq 0x3030
movq %rax, -0x40(%rbp)
movq %rbx, %rdi
shlq $0x3, %rdi
testl %ebx, %ebx
cmovsq %r15, %rdi
callq 0x3030
movq %rax, -0x70(%rbp)
leaq -0x90(%rbp), %rdi
movq %r13, %rsi
callq 0x12d48
xorl %r12d, %r12d
leaq -0x90(%rbp), %r15
movslq 0x8(%r13), %rbx
cmpq %rbx, %r12
jge 0x13b43
movq -0x40(%rbp), %rax
movl %r12d, (%rax,%r12,4)
xorpd %xmm0, %xmm0
movsd %xmm0, -0x38(%rbp)
xorl %ebx, %ebx
movslq (%r13), %rax
cmpq %rax, %rbx
jge 0x13b2f
movq %r15, %rdi
movl %r12d, %esi
callq 0x1324e
movsd (%rax,%rbx,8), %xmm0
andpd 0x1a80(%rip), %xmm0     # 0x155a0
maxsd -0x38(%rbp), %xmm0
incq %rbx
movsd %xmm0, -0x38(%rbp)
jmp 0x13aff
movq -0x70(%rbp), %rax
movsd -0x38(%rbp), %xmm0
movsd %xmm0, (%rax,%r12,8)
incq %r12
jmp 0x13ae3
pushq $0x1
popq %rax
movq %rax, -0x78(%rbp)
xorl %edi, %edi
leaq -0x90(%rbp), %r12
movq %r13, -0x38(%rbp)
movslq (%r13), %rax
decq %rax
cmpq %rax, %rdi
jge 0x13d1b
xorpd %xmm0, %xmm0
movsd %xmm0, -0x30(%rbp)
movq %rdi, %r15
movq %rdi, -0x48(%rbp)
movslq %ebx, %rax
cmpq %rax, %r15
jge 0x13beb
movq -0x40(%rbp), %rax
movslq (%rax,%r15,4), %rsi
movq -0x70(%rbp), %rax
movsd (%rax,%rsi,8), %xmm0
movsd %xmm0, -0x60(%rbp)
ucomisd 0x1a2b(%rip), %xmm0     # 0x155c8
jne 0x13ba5
jnp 0x13fa6
movq %r12, %rdi
callq 0x1324e
movq -0x48(%rbp), %rdi
movsd (%rax,%rdi,8), %xmm0
divsd -0x60(%rbp), %xmm0
andpd 0x19dd(%rip), %xmm0     # 0x155a0
movsd -0x30(%rbp), %xmm1
ucomisd %xmm1, %xmm0
movl -0x68(%rbp), %eax
cmoval %r15d, %eax
movl %eax, -0x68(%rbp)
maxsd %xmm1, %xmm0
incq %r15
movq -0x38(%rbp), %rax
movl 0x8(%rax), %ebx
movsd %xmm0, -0x30(%rbp)
jmp 0x13b77
movq -0x40(%rbp), %rsi
movl (%rsi,%rdi,4), %eax
movslq -0x68(%rbp), %rcx
movl (%rsi,%rcx,4), %edx
movl %edx, (%rsi,%rdi,4)
movl %eax, (%rsi,%rcx,4)
movl (%rsi,%rdi,4), %r13d
leaq 0x1(%rdi), %rax
movq %rax, -0xa0(%rbp)
movq -0x78(%rbp), %rcx
movslq %ebx, %rax
cmpq %rax, %rcx
jge 0x13d07
movq -0x40(%rbp), %rax
movq %rcx, -0xa8(%rbp)
movl (%rax,%rcx,4), %ebx
movq %r12, %rdi
movl %r13d, %esi
callq 0x1324e
movq -0x48(%rbp), %rcx
movsd (%rax,%rcx,8), %xmm0
ucomisd 0x1980(%rip), %xmm0     # 0x155c8
jne 0x13c50
jnp 0x13fa6
movq %r12, %rdi
movl %ebx, %esi
callq 0x1324e
movq -0x48(%rbp), %r15
movsd (%rax,%r15,8), %xmm0
movapd %xmm0, -0x60(%rbp)
movq %r12, %rdi
movl %r13d, %esi
callq 0x1324e
movsd (%rax,%r15,8), %xmm0
movsd %xmm0, -0x30(%rbp)
movq %r12, %rdi
movl %ebx, %esi
callq 0x1324e
movapd -0x60(%rbp), %xmm0
divsd -0x30(%rbp), %xmm0
movsd %xmm0, (%rax,%r15,8)
xorpd 0x136f(%rip), %xmm0     # 0x15010
movapd %xmm0, -0x60(%rbp)
movq -0x78(%rbp), %r15
movq -0x38(%rbp), %rax
movslq (%rax), %rax
cmpq %rax, %r15
jge 0x13cf1
movq %r12, %rdi
movl %r13d, %esi
callq 0x1324e
movsd (%rax,%r15,8), %xmm0
movsd %xmm0, -0x30(%rbp)
movq %r12, %rdi
movl %ebx, %esi
callq 0x1324e
movsd -0x30(%rbp), %xmm0
mulsd -0x60(%rbp), %xmm0
addsd (%rax,%r15,8), %xmm0
movsd %xmm0, (%rax,%r15,8)
incq %r15
jmp 0x13caa
movq -0xa8(%rbp), %rcx
incq %rcx
movq -0x38(%rbp), %rax
movl 0x8(%rax), %ebx
jmp 0x13c12
incq -0x78(%rbp)
movq -0xa0(%rbp), %rdi
movq -0x38(%rbp), %r13
jmp 0x13b57
movslq %ebx, %rax
leaq (,%rax,8), %rdi
testl %eax, %eax
pushq $-0x1
popq %rax
cmovsq %rax, %rdi
callq 0x3030
movq %rax, %r13
xorl %r12d, %r12d
movq %r14, -0x98(%rbp)
movslq %ebx, %rax
cmpq %rax, %r12
jge 0x13f6b
movl %ebx, %eax
xorl %ecx, %ecx
cmpq %rcx, %rax
je 0x13d61
andq $0x0, (%r13,%rcx,8)
incq %rcx
jmp 0x13d51
movabsq $0x3ff0000000000000, %rax # imm = 0x3FF0000000000000
movq %rax, (%r13,%r12,8)
xorl %r15d, %r15d
pushq $0x1
popq %r14
movq %r12, -0x68(%rbp)
leal -0x1(%rbx), %eax
cltq
cmpq %rax, %r15
jge 0x13df6
movq -0x40(%rbp), %rax
movslq (%rax,%r15,4), %r12
leaq 0x1(%r15), %rax
movq %rax, -0x60(%rbp)
movq %r14, -0x30(%rbp)
movslq %ebx, %rax
cmpq %rax, %r14
jge 0x13de5
movq -0x40(%rbp), %rax
movl (%rax,%r14,4), %ebx
leaq -0x90(%rbp), %rdi
movl %ebx, %esi
callq 0x1324e
movslq %ebx, %rcx
movsd (%rax,%r15,8), %xmm0
movsd (%r13,%rcx,8), %xmm1
mulsd (%r13,%r12,8), %xmm0
subsd %xmm0, %xmm1
movsd %xmm1, (%r13,%rcx,8)
incq %r14
movq -0x38(%rbp), %rax
movl 0x8(%rax), %ebx
jmp 0x13d99
movq -0x30(%rbp), %r14
incq %r14
movq -0x60(%rbp), %r15
movq -0x68(%rbp), %r12
jmp 0x13d7b
movq -0x40(%rbp), %rcx
movl (%rcx,%rax,4), %ebx
leaq -0x90(%rbp), %rdi
movl %ebx, %esi
movq -0x98(%rbp), %r14
callq 0x1324e
movq -0x38(%rbp), %rcx
movslq 0x8(%rcx), %rcx
movsd -0x8(%rax,%rcx,8), %xmm0
ucomisd 0x17a0(%rip), %xmm0     # 0x155c8
jne 0x13e30
jnp 0x13fa6
movslq %ebx, %rax
movsd (%r13,%rax,8), %xmm0
movsd %xmm0, -0x30(%rbp)
leaq -0x90(%rbp), %rdi
movl %ebx, %esi
callq 0x1324e
movq -0x38(%rbp), %rcx
movslq 0x8(%rcx), %rsi
movsd -0x8(%rax,%rsi,8), %xmm0
movsd %xmm0, -0x60(%rbp)
decq %rsi
movq %r14, %rdi
callq 0x1324e
movsd -0x30(%rbp), %xmm0
divsd -0x60(%rbp), %xmm0
movsd %xmm0, (%rax,%r12,8)
movq -0x38(%rbp), %rax
movl 0x8(%rax), %ecx
addl $-0x2, %ecx
testl %ecx, %ecx
js 0x13f5c
movl %ecx, %ecx
leaq 0x1(%rcx), %rbx
movq -0x40(%rbp), %rax
movq %rcx, -0x48(%rbp)
movslq (%rax,%rcx,4), %r15
movsd (%r13,%r15,8), %xmm0
movsd %xmm0, -0x30(%rbp)
movq -0x38(%rbp), %rax
cmpl %ebx, (%rax)
jle 0x13ef4
leaq -0x90(%rbp), %rdi
movl %r15d, %esi
callq 0x1324e
movsd (%rax,%rbx,8), %xmm0
movsd %xmm0, -0x60(%rbp)
movq %r14, %rdi
movl %ebx, %esi
callq 0x1324e
movsd -0x60(%rbp), %xmm1
mulsd (%rax,%r12,8), %xmm1
movsd -0x30(%rbp), %xmm0
subsd %xmm1, %xmm0
movsd %xmm0, -0x30(%rbp)
incq %rbx
jmp 0x13eab
leaq -0x90(%rbp), %rdi
movl %r15d, %esi
callq 0x1324e
movq -0x48(%rbp), %rcx
movsd (%rax,%rcx,8), %xmm0
ucomisd 0x16b4(%rip), %xmm0     # 0x155c8
jne 0x13f1c
jnp 0x13fa6
leaq -0x90(%rbp), %rdi
movl %r15d, %esi
callq 0x1324e
movq -0x48(%rbp), %rsi
movsd (%rax,%rsi,8), %xmm0
movsd %xmm0, -0x60(%rbp)
movq %r14, %rdi
callq 0x1324e
movsd -0x30(%rbp), %xmm0
divsd -0x60(%rbp), %xmm0
movsd %xmm0, (%rax,%r12,8)
movq -0x48(%rbp), %rcx
decl %ecx
jmp 0x13e85
incq %r12
movq -0x38(%rbp), %rax
movl 0x8(%rax), %ebx
jmp 0x13d41
movq %r13, %rdi
callq 0x3230
movq -0x40(%rbp), %rdi
callq 0x3230
movq -0x70(%rbp), %rdi
callq 0x3230
leaq -0x90(%rbp), %rdi
callq 0x12dd8
movq %r14, %rax
addq $0x88, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
pushq $0x8
popq %rdi
callq 0x30e0
leaq 0x8de3(%rip), %rcx      # 0x1cd98
movq %rcx, (%rax)
leaq 0x8d61(%rip), %rsi      # 0x1cd20
movq 0x901a(%rip), %rdx      # 0x1cfe0
movq %rax, %rdi
callq 0x3300
pushq $0x8
popq %rdi
callq 0x30e0
leaq 0x8d6b(%rip), %rcx      # 0x1cd48
movq %rcx, (%rax)
leaq 0x8d09(%rip), %rsi      # 0x1ccf0
movq 0x8ff2(%rip), %rdx      # 0x1cfe0
movq %rax, %rdi
callq 0x3300
jmp 0x13ff8
movq %rax, %rbx
jmp 0x14026
jmp 0x14017
jmp 0x14017
jmp 0x14017
jmp 0x14017
jmp 0x14017
jmp 0x14017
movq %rax, %rbx
movq -0x98(%rbp), %r14
jmp 0x1401a
jmp 0x14017
movq %rax, %rbx
leaq -0x90(%rbp), %rdi
callq 0x12dd8
movq %r14, %rdi
callq 0x12dd8
movq %rbx, %rdi
callq 0x3320