#include "linalg.h"
#include "gauss-jordan.h"
#include "intvector.h"
#include <iostream>
#include <iomanip>
#include "vecrqueue.h"

#ifndef DEBUG_LINALG
#define DEBUG_LINALG 0
#endif

/* Macros for debugging output */
#undef DPRINT
#undef DPRINTLN
#undef DBG
#define DPRINT(x) do { if (DEBUG_LINALG) cout << (x); } while (0) 
#define DPRINTLN(x) DPRINT(string(x) + '\n')
#define DBG(x) do { if (DEBUG_LINALG) { x; } } while (0)

#define VO 1e-6
#define VD 1e-6

static inline int round_to_int(double d)
{
  if (d >= 0)
    return (int) (d + 0.5);
  else 
    return (int) (d - 0.5);
}

/* Always returns a positive integer unless a and b are both 0. */
int positive_gcd(int a, int b)
{
  if (a < 0) a = -a;
  if (b < 0) b = -b;
  if (a > b)
    return positive_gcd(b, a);
  /* 0 <= a <= b */
  if (a == 0)
    return b;
  if (a == 1 || a == b)
    return a;
  return positive_gcd(b % a, a);
}

static double r()
{
  double r1 = random();
  double r2 = 0;
  while (r2 == 0)
    r2 = random();
  return r1 / r2;
}

static double rr() { return (r() + r() + r() + r()) / 16.0; }

double **new_rectangular_matrix(int n, int m)
{
  double **a = new double * [n + 1];
  a[0] = NULL;
  for (int i = 1; i <= n; i++)
    a[i] = new double [m + 1];
  return a;
}

void free_rectangular_matrix(double **a, int n, int m)
{
  for (int i = 1; i <= n; i++)
    delete[] a[i];
  delete[] a;
}

double **new_square_matrix(int n)
{
  return new_rectangular_matrix(n, n);
}

void free_square_matrix(double **a, int n)
{
  free_rectangular_matrix(a, n, n);
}

double **copy_rectangular_matrix(double **a, int n, int m)
{
  double **c = new_rectangular_matrix(n, m);
  for (int i = 1; i <= n; i++)
    for (int j = 1; j <= m; j++)
      c[i][j] = a[i][j];
  return c;
}

double **copy_square_matrix(double **a, int n)
{
  return copy_rectangular_matrix(a, n, n);
}

void zero_matrix(double **a, int n, int m)
{
  for (int i = 1; i <= n; i++)
    for (int j = 1; j <= m; j++)
      a[i][j] = 0;
}

/* Matrix multiplication.  c += a * b, where a is n x m and b is m x p. */
void multiply(double **c, double **a, double **b, int n, int m, int p)
{
  for (int i = 1; i <= n; i++)
    for (int k = 1; k <= p; k++)
      for (int j = 1; j <= m; j++)
	c[i][k] += a[i][j] * b[j][k];
}

void multiply(double **c, double **a, double **b, int n)
{
  multiply(c, a, b, n, n, n);
}

/* Compute product of a and b, n x n matrices.  Return true if all
elements off-diag elements are less than VO in absolute value and all
on diagonal elements are within VD of 1. */
bool verify_inverse(double **a, double **b, int n)
{
  double **c = new_square_matrix(n);
  zero_matrix(c, n, n);
  multiply(c, a, b, n);
  DBG(cout << "Verifying inverse; product is:" << endl);
  DBG(print_matrix(c, n, n));
  bool result = true;
  try {
    for (int i = 1; i <= n; i++)
      for (int j = 1; j <= n; j++)
	if (i == j && fabs(c[i][j] - 1.0) > VD ||
	    i != j && fabs(c[i][j]) > VO)
	  throw SingularMatrix();
  }
  catch (...) {
    result = false;
  }
  free_square_matrix(c, n);
  DBG(cout << "Inverse is " << (result ? "OK" : "bogus") << endl);
  return result;
}


/* Returns whether a[1..n][1..n] is singular.  Works by trying Gauss-Jordan
   elimination and verifying that the resulting inverse is close to a's true 
   inverse.  Should just take the determinant. */
bool is_singular(double **a, int n)
{
  double **b = new_rectangular_matrix(n, 1);
  double **ca = copy_square_matrix(a, n);
  for (int i = 1; i <= n; i++)
    b[i][1] = i;
  DBG(cout << "Checking singularity of" << endl);
  DBG(print_matrix(a, n, n));
  bool result = false;
  try {
    gauss_jordan(ca, n, b, 1);
    return !verify_inverse(ca, a, n);
  } catch (...) {
    result = true;
  }
  free_rectangular_matrix(b, n, 1);
  free_square_matrix(ca, n);
  return result;
}

/* Return whether the vectors in l are linearly independent. */
bool is_linearly_indep(llist<intvector *> *l)
{
  size_t m;
  if (l == NULL || (m = l->size()) == 1)
    return true;
  size_t n = l->front()->size();
  /* m vectors of length n */
  if (m > n)
    return false;

  /* Create an n x n matrix, filling it with the given vectors and
     padding it with random numbers.  Then check if it is singular.
     This is inelegant, but it works. */
  double **a = new_square_matrix(n);
  for (int i = 1; i <= (int) n; i++) {
    if (i <= (int) m)
      assert((*l)[i - 1]->size() == n);
    for (int j = 1; j <= (int) n; j++)
      a[i][j] = (i <= (int) m) ? (*((*l)[i - 1]))[j - 1] : rr();
  }
  bool result = !is_singular(a, n);
  free_square_matrix(a, n);
  return result;
}

/* True iff the normals are linearly indep. */
bool is_linearly_indep(llist<TilingPlane *> *l)
{
  llist<intvector *> *v = NULL;
  while (l != NULL) {
    v = cons((intvector *) l->front()->n(), v);
    l = l->tail();
  }
  bool r = is_linearly_indep(v);
  v->free();
  return r;
}

/* Returns list of all k-tuples of ints where each element of the
   tuple is between lo and hi, inclusive, and each element of the
   tuple is greater than the last.  Should be memoized. */
llist<llist<int> *> *all_increasing_k_tuples_in_range(int k, int lo, int hi)
{
  if (k < 1 || hi - lo < k - 1)
    return NULL;

  /* Either we use lo in the first slot ... */
  llist<llist<int> *> *result1 = NULL;
  if (k == 1)
    result1 = cons(cons(lo));
  else {
    llist<llist<int> *> *x =
      all_increasing_k_tuples_in_range(k - 1, lo + 1, hi);
    for (x = dreverse(x); x != NULL; x = x->tail())
      result1 = cons(cons(lo, x->front()), result1);
  }
  /* ... or we don't */
  llist<llist<int> *> *result2 =
    all_increasing_k_tuples_in_range(k, lo + 1, hi);

  return extend(result1, result2);
}

llist<intvector *> *intlistlist_to_intvectorlist(llist<llist<int> *> *l)
{
  llist<intvector *> *result = NULL;
  for (llist<llist<int> *> *x = l; x != NULL; x = x->tail())
    result = cons(new intvector(x->front()), result);
  return dreverse(result);
}

/* Is v perpendicular to all vectors in l? */
bool is_perp_to_all(intvector *v, llist<intvector *> *l)
{
  while (l != NULL)
    if (l->front()->dot(v) != 0)
      return false;
    else
      l = l->tail();

  return true;
}

/* l specifies a matrix L (possibly transposed).  Find a matrix M
whose entries are all integers such that ML = kI for some integer k.
Throws SingularMatrix if it fails.  Returns a 1d array of intvectors.
Sets *ptr_to_k if ptr_to_k is not NULL. */
intvector **multiple_of_inverse(llist<intvector *> *l, bool transpose,
				int *ptr_to_k)
{
  size_t n = l->front()->size();
  assert(l->size() == n);
  intvector **result = new intvector * [n];
  double d, **b = new_rectangular_matrix(n, 1), **a = new_square_matrix(n);
  zero_matrix(a, n, n);
  zero_matrix(b, n, 1);
  for (int i = 1; i <= (int) n; i++)
    for (int j = 1; j <= (int) n; j++)
      a[i][j] = transpose ? (*((*l)[j - 1]))[i - 1] : (*((*l)[i - 1]))[j - 1];
  double **c = copy_square_matrix(a, n);
  int **tmp = new int * [n];
  for (int i = 0; i < (int) n; i++) {
    tmp[i] = new int [n];
    for (int j = 0; j < (int) n; j++)
      tmp[i][j] = 0;
  }

  try {
    DBG(cout << "Inverting A = " << endl);
    DBG(print_matrix(a, n, n));

    gauss_jordan(a, n, b, 1, &d); // may throw SingularMatrix

    DBG(cout << "Result is" << endl);
    DBG(print_matrix(a, n, n));

    for (int i = 1; i <= (int) n; i++) {
      result[i - 1] = zero_vector(n);
      for (int j = 1; j <= (int) n; j++)
	(*(result[i - 1]))[j - 1] = round_to_int(a[i][j] * d);
    }
  
    /* verify the result */
   
    /* matrix multiply */
    for (int i = 1; i <= (int) n; i++)
      for (int k = 1; k <= (int) n; k++)
	for (int j = 1; j <= (int) n; j++)
	  tmp[i - 1][k - 1] +=
	    round_to_int(c[j][k]) * round_to_int(a[i][j] * d);
    DBG(cout << "Product is:" << endl);
    DBG(print_matrix0(tmp, n, n));
    /* verify it is diagonal with same value on along the diagonal */
    int k = tmp[0][0];
    for (int i = 0; i < (int) n; i++)
      for (int j = 0; j < (int) n; j++)
	if (i == j && tmp[i][j] != k ||
	    i != j && tmp[i][j] != 0)
	  throw SingularMatrix();
    if (ptr_to_k != NULL)
      *ptr_to_k = k;
  } catch (...) {
    for (int i = 0; i < (int) n; i++)
      delete result[i];
    delete[] result;
    result = NULL;
  }

  for (int i = 0; i < (int) n; i++)
    delete[] tmp[i];
  delete[] tmp;
  free_square_matrix(a, n);
  free_rectangular_matrix(b, n, 1);
  free_square_matrix(c, n);

  if (result == NULL)
    throw SingularMatrix();
  else
    return result;
}

/* Find a vector that is perpendicular to all vectors in l.  Return
   the zero vector only if no other vector fits the bill.  Assumes all
   vectors in l have the same length.  Assumes number of vectors is
   smaller than the vector length and that they are linearly independent.
   (If exclude is non-negative then ignore that element of l.)
*/
intvector *find_vector_perp_to_all(llist<intvector *> *l, int exclude)
{
  if (exclude >= 0) 
    l = l->copy()->excise(exclude);

  assert(l != NULL);

  intvector *result;
  int m = l->size();
  int n = l->front()->size();
  /* m vectors of length n */

  /* general case */
  /* Construct a system of linear equations and solve it. */
  {
    int k = n - m;
    if (k < 0) throw SingularMatrix();
  
    llist<intvector *> *c = 
      intlistlist_to_intvectorlist(all_increasing_k_tuples_in_range(k, 1, n));

    do {
      double **a = new_square_matrix(n);
      double **b = new_rectangular_matrix(n, 1);
      zero_matrix(a, n, n);
      zero_matrix(b, n, 1);
      for (int i = 1; i <= m; i++)
	for (int j = 1; j <= n; j++)
	  a[i][j] = (*((*l)[i - 1]))[j - 1];

      if (k > 0) {
	/* Make up other constraints */
	intvector *v = c->front();
	c = c->tail();
	int i = m + 1;
	for (int j = 1; j <= n; j++)
	  if (v->contains(j))
	    a[i++][j] = 1;
	for (int i = 1; i <= n; i++)
	  if (i > m)
	    b[i][1] = 1;
      }
    
      try {
	double d;

	DBG(cout << "Solving Ax = b.  A is" << endl);
	DBG(print_matrix(a, n, n));
	DBG(cout << "b is" << endl);
	DBG(print_matrix(b, n, 1));

	gauss_jordan(a, n, b, 1, &d);

	DBG(cout << "result is" << endl);
	DBG(print_matrix(b, n, 1));

	result = new intvector(n);
	for (int i = 1; i <= n; i++)
	  (*result)[i - 1] = round_to_int(b[i][1] * d);
	int g = result->gcd();
	if (g > 1)
	  for (int i = 1; i <= n; i++)
	    (*result)[i - 1] /= g;
	goto done;
      } catch (...) { }
      free_square_matrix(a, n);
      free_rectangular_matrix(b, n, 1);
    } while (c != NULL);
  }

  result = new intvector(n);

 fail:
  for (int i = 1; i <= n; i++)
    result[i - 1] = 0;
  
 done:
  /* Verify the result */
  if (!is_perp_to_all(result, l))
    goto fail;

  /* Display the input and result */
  DBG({
    cout << "Found vector perp to";
    for (int i = 0; i < m; i++)
      cout << ' ' << ((*l)[i])->to_string();
    cout << ": " << result->to_string() << endl;
  });

  if (exclude >= 0)
    free_all(l);
  return result;
}

intvector *find_vector_perp_to_all(llist<TilingPlane *> *l, int exclude)
{
  llist<intvector *> *v = NULL;
  int i = 0;
  while (l != NULL) {
    if (i++ != exclude)
      v = cons((intvector *) l->front()->n(), v);
    l = l->tail();
  }
  intvector *r = find_vector_perp_to_all(v);
  free_all(v);
  return r;
}

/* Return whether x is parallel to anything in l. */
bool any_parallel(intvector *x, llist<intvector *> *l)
{
  while (l != NULL)
    if (x->is_parallel(l->front()))
      return true;
    else
      l = l->tail();
  return false;
}

/* Find a positive integer i such that i * v has roughly the given length.
   Return i * v. */
intvector *multiple_with_approximate_length(const intvector *v, int len)
{
  int i = round_to_int(len / v->norm());
  if (i < 1)
    i = 1;
  return v->times(i);
}

/* Return a vector with length approximately len that is in roughly
   the same direction as v.  The result will always be non-zero, even
   in len is very small.  If exclude is non-empty, avoid returning any
   vector that is not linearly independent to that set. */
intvector *approximate_vector_with_length(const intvector *v, int len,
					  llist<intvector *> *exclude)
{
  intvector *result;
  if (exclude == NULL) {
    llist<int> *l = NULL;
    double n = v->norm();
    assert(n > 0);
    for (int i = v->size() - 1; i >= 0; i--)
      l = cons(round_to_int((*v)[i] * len / n), l);
    result = new intvector(l);
    if (result->is_zero()) {
      delete result;
      result = v->approximate_with_unit_vector();
    }
  } else {
    intvector *r = approximate_vector_with_length(v, len, NULL);
    if (!is_linearly_indep(cons(r, exclude))) {
      rqueue<intvector *> *offsets = taxi_upfrom(1, v->size());
      while (true) {
	result = r->plus(offsets->pop());
	if (result->dot(v) > 0 && is_linearly_indep(cons(result, exclude)))
	  break;
	delete result;
      }
      delete offsets;
    } else
      result = r;
  }
  DBG(cout << "approximate_vector_with_length(" << v->to_string() <<
      ", " << len << ") = " << result->to_string() << endl);
  return result;
}

/* Print a[1..n][1..m] to cout. */
void print_matrix(double **a, int n, int m)
{
  for (int i = 1; i <= n; i++) {
    cout << "[";
    for (int j = 1; j <= m; j++)
      cout << ' ' << setw(12) << a[i][j];
    cout << " ]" << endl; 
  } 
}

/* Print a[1..n][1..m] to cout. */
void print_matrix(int **a, int n, int m)
{
  for (int i = 1; i <= n; i++) {
    cout << "[";
    for (int j = 1; j <= m; j++)
      cout << ' ' << setw(12) << a[i][j];
    cout << " ]" << endl; 
  } 
}

/* Print a[0..n-1][0..m-1] to cout. */
void print_matrix0(int **a, int n, int m)
{
  for (int i = 0; i < n; i++) {
    cout << "[";
    for (int j = 0; j < m; j++)
      cout << ' ' << setw(12) << a[i][j];
    cout << " ]" << endl; 
  } 
}

/////////////////////////////////////////////////////////////////////////////
// Test routines
/////////////////////////////////////////////////////////////////////////////

void test_linalg()
{
  {
    llist<intvector *> *m =
      cons(new intvector(cons(1, cons(7))),
	   cons(new intvector(cons(11, cons(4)))));

    try {
      multiple_of_inverse(m);
    }
    catch (...) {
      cout << "unexpected exception while doing multiple_of_inverse(): fail" <<
	endl;
      exit(1);
    }
  }
  llist<intvector *> *c = 
    intlistlist_to_intvectorlist(all_increasing_k_tuples_in_range(3, 1, 5));
  cout << "All increasing 3-tuples in range 1 to 5:" << endl;
  while (c != NULL) {
    cout << c->front()->to_string() << ' ';
    c = c->tail();
  }
  cout << endl;

  find_vector_perp_to_all(cons(new intvector(cons(1, cons(2)))));
  find_vector_perp_to_all(cons(new intvector(cons(1, cons(2, cons(3))))));
  find_vector_perp_to_all(cons(new intvector(cons(1, cons(2, cons(3)))),
			       cons(new intvector(cons(2,
						       cons(-2, cons(7)))))));

  {
    double **m = new_square_matrix(2);
    m[1][1] = 1;
    m[1][2] = 2;
    m[2][1] = 3;
    m[2][2] = -1;

    double **b = new_rectangular_matrix(2, 1);
    b[1][1] = 7;
    b[2][1] = 35;

    cout << "Solving Ax = b.  A is" << endl;
    print_matrix(m, 2, 2);
    cout << "b is" << endl;
    print_matrix(b, 2, 1);

    try {
      gauss_jordan(m, 2, b, 1);
    }
    catch (...) {
      cout << "unexpected exception while doing gauss_jordan(): fail" << endl;
      exit(1);
    }

    cout << "After Gauss-Jordan elimination, A is" << endl;
    print_matrix(m, 2, 2);
    cout << "b is" << endl;
    print_matrix(b, 2, 1);
    free_square_matrix(m, 2);
    free_rectangular_matrix(b, 2, 1);
  }
  {
    double **m = new_square_matrix(3);
    m[1][1] = 1;
    m[1][2] = 0;
    m[1][3] = 0;
    m[2][1] = 0;
    m[2][2] = 1;
    m[2][3] = 0;
    m[3][1] = 0;
    m[3][2] = 0;
    m[3][3] = 1;

    double **b = new_rectangular_matrix(3, 1);
    b[1][1] = 7;
    b[2][1] = -2.3;
    b[3][1] = 18;

    cout << "Solving Ax = b.  A is" << endl;
    print_matrix(m, 3, 3);
    cout << "b is" << endl;
    print_matrix(b, 3, 1);

    try {
      gauss_jordan(m, 3, b, 1);
    }
    catch (...) {
      cout << "unexpected exception while doing gauss_jordan(): fail" << endl;
      exit(1);
    }

    cout << "After Gauss-Jordan elimination, A is" << endl;
    print_matrix(m, 3, 3);
    cout << "b is" << endl;
    print_matrix(b, 3, 1);
    free_square_matrix(m, 3);
    free_rectangular_matrix(b, 3, 1);
  }
  {
    double **m = new_square_matrix(3);
    m[1][1] = 1;
    m[1][2] = 0;
    m[1][3] = 0;
    m[2][1] = 0;
    m[2][2] = 0;
    m[2][3] = 0;
    m[3][1] = 0;
    m[3][2] = 0;
    m[3][3] = 1;

    double **b = new_rectangular_matrix(3, 1);
    b[1][1] = 7;
    b[2][1] = -2.3;
    b[3][1] = 18;

    cout << "Solving Ax = b.  A is" << endl;
    print_matrix(m, 3, 3);
    cout << "b is" << endl;
    print_matrix(b, 3, 1);

    try {
      gauss_jordan(m, 3, b, 1);
    }
    catch (SingularMatrix) {
      cout << "Correctly diagnosed singular matrix." << endl;
    }
    catch (...) {
      cout << "Unknown exception in Gauss-Jordan: fail" << endl;
      exit(1);
    }
    free_square_matrix(m, 3);
    free_rectangular_matrix(b, 3, 1);
  }
}