/* solveqn.c -- Solve a linear equation.   2003.12.04
   This was copied with very little change from FastMech.java .
*/

// x1 = x1f * x1 + x2f * x2
void scaled_vector_add( double x1f, double *x1, double x2f, double *x2,
			int size) {
  while( --size >= 0 )  {
    x1[ size ] = x1f * x1[ size ]  +  x2f * x2[ size ];
  }
}


// row i = fi*(row i) + fj*(row j)
void scaled_row_add( double **A, double *b, 
		     double fi, int i, double fj, int j, int size )  {
  scaled_vector_add( fi, A[ i ], fj, A[ j ], size );
  b[ i ] = fi * b[ i ] + fj * b[ j ];
}


// Naive substitution solver with no pivoting.
// Find x such that A * x + b = 0.  Modifies A and b.
void solve( double **A, double *x, double *b, int size ) {
  int i, j;
  double f, *row, t;

  // Substitute rows into lower rows:
  for( i = 0;  i < size - 1;  i++ ) {
    f = -1.0 / A[i][i];
    for( j = i + 1;  j < size;  j++ ) {
      if( A[ j ][ i ] != 0.0 ) {
	scaled_row_add( A, b, 1.0, j, f * A[ j ][ i ], i, size );
	// A[j][i] = A[j][i] - (A[j][i]/A[i][i]) * A[i][i],
	// so it should be pretty close to zero,
	A[ j ][ i ] = 0.0;  // but make sure.
      }
    }
  }
  // Now the matrix is "upper triangular," yay!
  // Each row expresses a var in terms of lower vars,
  // and the bottom row is trivial.  So go bottom-up:
  for( i = size - 1;  i >= 0;  i-- ) {
    t = b[ i ];
    row = A[ i ];
    for( j = i + 1;  j < size;  j++ ) {
      if( row[j] != 0.0 )  t += row[ j ] * x[ j ];
    }
    x[ i ] = -t / row[ i ];
  }
}