#define _DEFAULT_SOURCE

#include <stdlib.h>
#include <stdio.h>
#include <stdbool.h>
#include <assert.h>

#include "xutil.h"
#include "sankowski.h"
#include "mod.h"
#include "sm.h"

sankowski_t *sankowski_init(uint32_t n) {
	sankowski_t *s = xmalloc(sizeof(sankowski_t));

	s->n = n;
	s->B = matrix_create(n, n); 
	s->C = matrix_create(n, n); 
	s->rv = matrix_create(1, n); 
	s->cv = matrix_create(1, n);
	s->tc = n;

	matrix_identity(s->C);

	srand48(SEED);

	return s;
}

bool _random_update_for_delete(sankowski_t *s) {
	uint32_t m, n;
	int64_t delta;

	do {
		m = lrand48() % s->n;
		n = lrand48() % s->n;
	} while (m == n || MAT(s->B, m, n) == 0);

	do {
		delta = mod(lrand48(), p);
	} while (delta == 0);

	if (sherman_morrison(s, m, n, delta)) {
		MAT(s->B, m, n) = addmod(MAT(s->B, m, n), delta);
		return true;
	}

	return false;
}

void sankowski_destroy(sankowski_t *s) {
	if (s != NULL) {
		matrix_destroy(s->B);
		matrix_destroy(s->C);
		matrix_destroy(s->rv);
		matrix_destroy(s->cv);
		free(s);
	}
}

void sankowski_insert(sankowski_t *s, uint32_t i, uint32_t j) {
	int64_t delta;

	if (i == j || MAT(s->B, i, j) != 0) {
		return;
	}

	bool notdone = true;

	while (notdone) {
		do {
			delta = mod(lrand48(), p);
		} while (delta == 0);

		MAT(s->B, i, j) = delta;
		notdone = !sherman_morrison(s, i, j, delta);
	}
}

void sankowski_expand(sankowski_t *s) {
	uint32_t n = s->n;

	// Copy B
	matrix_expand(s->B);

	// Copy C
	matrix_expand(s->C);

	// Set the added diagonal entry to 1.
	s->C->data[n*(n+1) + n] = 1;

	// Copy row-vector and column vector
	s->rv->data = realloc(s->rv->data, (n+1)*sizeof(int64_t));
	s->rv->n = n + 1;

	s->cv->data = realloc(s->cv->data, (n+1)*sizeof(int64_t));
	s->cv->n = n + 1;

	s->n = n + 1;
}

void _perform_remove(sankowski_t *s, uint32_t i) {
	uint32_t n = s->n;

	// Copy B
	matrix_remove(s->B, i);

	// Copy C
	matrix_remove(s->C, i);

	// Copy row-vector and column vector
	s->rv->data = realloc(s->rv->data, (n-1)*sizeof(int64_t));
	s->rv->n = n - 1;

	s->cv->data = realloc(s->cv->data, (n-1)*sizeof(int64_t));
	s->cv->n = n - 1;
	s->n = n - 1;
}

void sankowski_remove(sankowski_t *s, uint32_t idx, bool fast) {
	uint32_t i,
			 n = s->n;

	if (!fast) {
		for (i = 0; i < n; i++) {
			if (i == idx) {
				continue;
			}

			if (s->B->data[i * n + idx] != 0) {
				sankowski_delete(s, i, idx);
			}
		}

		for (i = 0; i < n; i++) {
			if (i == idx) {
				continue;
			}

			if (s->B->data[idx * n + i] != 0) {
				sankowski_delete(s, idx, i);
			}
		}
	} else {
		for (i = 0; i < n; i++) {
			s->cv->data[i] = mod( -s->B->data[i * n + idx], p);
			s->rv->data[i] = 0;
		}

		s->rv->data[idx] = 1;
		while (!sherman_morrison_full(s)) {
			while (!_random_update_for_delete(s));
		}

		for (i = 0; i < n; i++) {
			s->rv->data[i] = mod( -s->B->data[idx * n + i], p);
			s->cv->data[i] = 0;
		}

		s->cv->data[idx] = 1;
		while (!sherman_morrison_full(s)) {
			while (!_random_update_for_delete(s));
		}
	}

	_perform_remove(s, idx);
}

void sankowski_delete(sankowski_t *s, uint32_t i, uint32_t j) {
	int64_t delta;

	if (i == j || MAT(s->B, i, j) == 0) {
		return;
	}

	delta = mod(-MAT(s->B, i, j), p);

	while (sherman_morrison(s, i, j, delta) == false) {
		while (!_random_update_for_delete(s));
		delta = mod(-MAT(s->B, i, j), p);
	}

	MAT(s->B, i, j) = 0;
}

uint32_t sankowski_query(sankowski_t *s) {
	return s->tc - s->n;
}
