#include <stdio.h>
#include <stdlib.h>
#include "alloc.h"
#include "ivseti.h"

#define DEBUG_ONE 1
#define DEBUG_TWO 1
#define DEBUG_THREE 1
#define DEBUG_FOUR 1

#if DEBUG_ONE
#define DEBUG1(x) x
#else
#define DEBUG1(x) 
#endif

#if DEBUG_TWO
#define DEBUG2(x) x
#else
#define DEBUG2(x) 
#endif

#if DEBUG_THREE
#define DEBUG3(x) x
#else
#define DEBUG3(x) 
#endif

#if DEBUG_FOUR
#define DEBUG4(x) x
#else
#define DEBUG4(x) 
#endif

static ivseti *findem2(int n, int m, int **v, int **inv, int k, int **r);

char *str(int *v, int n)
{
  int i;
  char *s = (char *) malloc(n * 20);
  sprintf(s, "[%d", v[0]);
  for (i = 1; i < n; i++)
    sprintf(s + strlen(s), ", %d", v[i]);
  sprintf(s + strlen(s), "]");
  return s;
}

/* Add y * p to the rectangle r in n-space. */
static void moverect(int *p, int y, int **r, int n)
{
  int i, j;
  for (i = 0; i < 2; i++)
    for (j = 0; j < n; j++)
      r[i][j] += y * p[j];
}

/* Return all p s.t. lo <= k * p <= hi. */
static iseti findem_interval(int k, int lo, int hi)
{
  if (k == 0)
    return (lo <= 0 && 0 <= hi) ? iseti_universe() : iseti_emptyset();
  printf("findem_interval(%d, %d, %d): ", k, lo, hi); 
  lo = divide_int_rounding_to_plus_inf(lo, k);
  hi = divide_int_rounding_to_minus_inf(hi, k);
  printf("%d - %d\n", lo, hi); 
  return iseti_single_interval(make_iinterval(lo, hi));
}

/* r specifies a rectangle in n-space.  Put the selected corner of it in q. */
static void pickcorner(int **r, int corner, int *q, int n)
{
  int i;
  for (i = 0; i < n; i++)
    q[i] = r[(corner & (1 << i)) ? 1 : 0][i];
}

/* Multiply nxn matrix A by vector x and store the result in b. */
static void mult(int **A, int *x, int *b, int n)
{
  int j, i;
  for (i = 0; i < n; i++) {
    int sum = 0;
    for (j = 0; j < n; j++)
      sum += A[i][j] * x[j];
    b[i] = sum;
  }
}

/* Multiply the transpose of nxn matrix A by x and store the result in b. */
static void tmult(int **A, int *x, int *b, int n)
{
  int j, i;
  for (i = 0; i < n; i++) {
    int sum = 0;
    for (j = 0; j < n; j++)
      sum += A[j][i] * x[j];
    b[i] = sum;
  }
}

/* n is the number of dimensions. v is m vectors in n-space.  r is a
   rectangle (low point and high point) in n-space.  Search for lists of
   integers s.t. the linear combination of the vectors 
   specified by the list lies in the rectangle.  range is m intervals;
   range[i] specifies the range of possibilities that we might multiply
   v[i] by when searching for linear combinations.
*/
static ivseti *findem3(int n, int m, int **v, int **r, iinterval *range)
{
  printf("findem3 n=%d m=%d v[0]=%s r=[%s : %s]\n", n, m,
	 str(v[0], n), str(r[0], n), str(r[1], n));
  if (m == 1)
    return findem2(n, 1, v, NULL, 0, r);
  else {
    int lo = range[0].lo, hi = range[0].hi, i;
    ivseti *result = ivseti_emptyset();
    for (i = lo; i <= hi; i++) {
      ivseti *subresult;
      printf("%*sChecking %d\n", 10 - 2 * m, "", i);
      moverect(v[0], -i, r, n);
      subresult = findem3(n, m - 1, v + 1, r, range + 1);
      if (!ivseti_is_empty(subresult))
	result = ivseti_union(result, ivseti_cons(i, subresult));
      moverect(v[0], i, r, n);
    }
    return result;
  }
}

/* n is the number of dimensions. v is m vectors in n-space.  r is a
   rectangle (low point and high point) in n-space.  Find every list
   of integers s.t. the linear combination of the vectors 
   specified by the list lies in the rectangle. m should be n or 1.
*/
static ivseti *findem2(int n, int m, int **v, int **inv, int k, int **r)
{
  if (m == 1) {
    int i;
    iseti result;
    printf("findem2 n=%d m=%d v[0]=%s r=[%s : %s]\n", n, m,
	   str(v[0], n), str(r[0], n), str(r[1], n));
    /* Intersect all 1d solutions */
    for (i = 0; i < n; i++) {
      iseti o = findem_interval(v[0][i], r[0][i], r[1][i]);
      result = (i == 0) ? o : iseti_intersection(result, o);
    }
    return ivseti_flat(result);
  } else {
    int i, corners = 1 << n, corner,
      *c = ALLOC(int, n), *q = ALLOC(int, n);
    iinterval *range = ALLOC(iinterval, n);
    for (i = 0; i < n; i++) {
      range[i].lo = 0;
      range[i].hi = -1;
    }
    for (corner = 0; corner < corners; corner++) {
      pickcorner(r, corner, q, n);
      tmult(inv, q, c, n);
      printf("corner %d (%s) maps to %s\n", corner, str(q, n), str(c, n));
      for (i = 0; i < n; i++)
	include_in_iinterval(&(range[i]), c[i]);
    }
    for (i = 0; i < n; i++) {
      divide_iinterval(&(range[i]), k);
      printf("intervals for i=%d: %d to %d\n", i, range[i].lo, range[i].hi);
    }
    return findem3(n, m, v, r, range);
  }
}

#if DEBUG_THREE
/* Iterate b through the rectangle [lo : hi].  Return false and set b to lo 
   if b >= hi.  Otherwise move b to the (lexicographically) next point 
   and return true. */
static inline bool bump(int *b, int *lo, int *hi, int n)
{
  int i = n - 1;
  do
    if (++b[i] <= hi[i])
      return true;
    else
      b[i] = lo[i];
  while (--i >= 0);

  return false;
}

static void verify(int n, int *p, int **v, int **r, ivseti *c)
{
  if (ivseti_is_empty(c))
    return;
  printf("Verifying: c=");
  ivseti_print(c);
  puts("");
  assert(ivseti_dim(c) == n);
  {
    int i, j, *x = ALLOC(int, n), *lo = ALLOC(int, n),
      *hi = ALLOC(int, n), *z = ALLOC(int, n); 
    for (i = 0; i < n ; i++) {
      lo[i] = x[i] = ivseti_min(c, i);
      hi[i] = ivseti_max(c, i);
    }
    /* x iterates over the bounding box of c. */ 
    do 
      if (ivseti_contains(c, x)) {
	/* set z to p plus a linear combination of the v's */
	for (i = 0; i < n; i++)
	  for (z[i] = p[i], j = 0; j < n; j++)
	    z[i] += x[j] * v[j][i];
	/* test it */
	for (i = 0; i < n; i++)
	  if (z[i] < r[0][i] || z[i] > r[1][i]) {
	    printf("ERROR! Rectangle [%s : %s] does not contain %s",
		   str(r[0], n), str(r[1], n), str(p, n));
	    for (j = 0; j < n; j++)
	      printf(" + %d * %s", x[j], str(v[j], n));
	    printf(" = %s\n", str(z, n));
	    abort();
	  }
	DEBUG4({
	  printf("[%s : %s] does contain %s",
		 str(r[0], n), str(r[1], n), str(p, n));
	  for (j = 0; j < n; j++)
	    printf(" + %d * %s", x[j], str(v[j], n));
	  printf(" = %s\n", str(z, n));
	})
      }
    while (bump(x, lo, hi, n));
    FREE(x);
    FREE(hi);
    FREE(lo);
    FREE(z);
  }
}
#endif /* DEBUG_THREE */

/* n is the number of dimensions. p is a point in n-space. v is n
   vectors in n-space.  r is a rectangle (low point and high point) in
   n-space.  Find all linear combinations of the vectors in v s.t. the
   sum of p and the linear combination lies in the rectangle. 
   The product of matrices inv and v should equal k times I.
*/
static ivseti *findem(int n, int *p, int **v, int **inv, int k, int **r)
{
  ivseti *result;
  /* Subtract off p from the rectangle, then solve the p = 0 case. */
  moverect(p, -1, r, n);
  result = findem2(n, n, v, inv, k, r);
  /* Restore r to its original value. */
  moverect(p, 1, r, n);
  DEBUG3(verify(n, p, v, r, result));
  return result;
}

main()
{
  puts("*** 1 ***");
  {
    int point[] = {1, 2};
    int a[] = {-1, 3};
    int b[] = {2, 2};
    int *v[] = {a, b};
    int lo[] = {10, 10};
    int hi[] = {30, 20};
    int *rect[] = {lo, hi};
    int inv0[] = {-2, 3}, inv1[] = {2, 1};
    int *inv[] = {inv0, inv1};
  
    ivseti_print(findem(2, point, v, inv, 8, rect));
    putchar('\n');
  }
  puts("*** 2 ***");
  {
    int point[] = {0, 0};
    int a[] = {-1, 3};
    int b[] = {2, 2};
    int *v[] = {a, b};
    int lo[] = {9, 8};
    int hi[] = {29, 18};
    int *rect[] = {lo, hi};
    int inv0[] = {-2, 3}, inv1[] = {2, 1};
    int *inv[] = {inv0, inv1};
  
    ivseti_print(findem(2, point, v, inv, 8, rect));
    putchar('\n');
  }
  puts("*** 3 ***");
  {
    int point[] = {0, 0};
    int a[] = {-1, 2};
    int b[] = {3, 2};
    int *v[] = {a, b};
    int lo[] = {5, 10};
    int hi[] = {10, 15};
    int *rect[] = {lo, hi};
    int inv0[] = {-2, 2}, inv1[] = {3, 1};
    int *inv[] = {inv0, inv1};
  
    ivseti_print(findem(2, point, v, inv, 8, rect));
    putchar('\n');
  }
  puts("*** 4 ***");
  {
    int point[] = {0, 0};
    int a[] = {1, 0};
    int b[] = {0, 1};
    int *v[] = {a, b};
    int *inv[] = {a, b};
    int lo[] = {2, 5};
    int hi[] = {3, 11};
    int *rect[] = {lo, hi};
  
    ivseti_print(findem(2, point, v, inv, 1, rect));
    putchar('\n');
  }
}