// Copyright 2006 Google Inc.
// All Rights Reserved.
// Author: <renn@google.com> (Marius Renn)

// Local includes
#include "debugging.h"
#include "mathfunctions.h"

// C includes
#include <math.h>
#include <stdlib.h>

unsigned helium::SquaredRoot(unsigned x) {
  unsigned k = 0, c = 0;
  for(unsigned o = 1; o <= x; o += k) {
    k += 2;
    c++;
  }
  return c;
}

unsigned helium::Binomi(unsigned n, int k) {
  return ((n == k) || (k <= 1)) ? 1 : Binomi(n-1, k-1) + Binomi(n-1, k);
}

// This comparison function is required for qsort(...), which is used in
// the Median(...) function below.
int float_compare(const void* a, const void* b) {
  const float* f_a = reinterpret_cast<const float*>(a);
  const float* f_b = reinterpret_cast<const float*>(b);
  return (*f_a > *f_b) ? 1 : ((*f_a < *f_b) ? -1 : 0);
}

float helium::Median(float* values, unsigned size) {
  qsort(values, size, sizeof(float), float_compare);
  return values[size / 2];
}

// I feel the need to defend the rare case of using macros here:
// To avoid having to actually swap a row value-by-value in memory, an internal 
// list of n values is stored that map each index to the position of the row. 
// To access an element in the matrix, a look-up must be made to access the
// correct row. These macros should simplify this access, and make the code
// more readable.
#define ELEM_A(ROW, COL) A[rows[ROW] * n + COL]
#define ELEM_B(ROW) b[rows[ROW]]

bool helium::Gauss(float* A, float* b, float* x, unsigned n) {
  float tmp_x[n];
  unsigned rows[n];
  for (unsigned i = 0; i < n; i++) rows[i] = i;
  
  // Diagonalization
  for (unsigned i = 0; i < n; i++) {
    // Find pivot
    float max_val = fabs(ELEM_A(i, i));
    float cur_val = 0.0;
    
    unsigned r = i;
    for (unsigned k = i + 1; k < n; k++) { 
      cur_val = fabs(ELEM_A(k, i));
      if (cur_val > max_val) {
        max_val = cur_val;
        r = k;
      }
    }
    
    // Swap rows to make pivot the current row
    unsigned tmp = rows[r];
    rows[r] = rows[i];
    rows[i] = tmp;
    
    if (max_val == 0.0) return false; 
    
    // Scale row and subtract
    for (unsigned k = i + 1; k < n; k++) {
      float cur_factor =  ELEM_A(k, i) / max_val;
      for (unsigned j = i; j < n; j++) 
        ELEM_A(k, j) = ELEM_A(k, j) - ELEM_A(i, j) * cur_factor; 
      ELEM_B(k) = ELEM_B(k) - ELEM_B(i) * cur_factor;
    }
  }
  
  // Back substitution
  for (int i = n - 1; i >= 0; i--) {
    tmp_x[rows[i]] = ELEM_B(i);
    for (unsigned j = n - 1; j > i; j--) 
      tmp_x[rows[i]] -= tmp_x[rows[j]] * ELEM_A(i, j);
    tmp_x[rows[i]] /= ELEM_A(i, i);
  }
  
  // Swap back values
  for (int i = 0; i < n; i++) x[i] = tmp_x[rows[i]];
  
  return true;
}