#include <iostream>
#include <iomanip>
#include <sys/time.h>
#include <fstream>
#include <math.h>
#include <stdio.h>
// #include <list>
#include "placement.h"

////////////////////////////////////////////////////////////////////////////
/////////////////////////// CONSTANTS & GLOBALS /////////////////////////////
/////////////////////////////////////////////////////////////////////////////
const double ERROR = 1.0e-8; // acceptable error
// const int PRECISION = 100;

const int MAX_COORDINATE_VALUE = 5000;

DataContainer* dc = DataContainer::getInstance();
// point_t broker_1_location;

inline int min(int a, int b) {
    return (a < b) ? a : b;
};

ostream& operator<<(ostream& outs, const point_t p) {
//	DataContainer::getInstance()->getDim();
    outs << "(";
    for (int i = 0; i < dc->getDim(); i++) {
        outs << p[i];
		if (i < dc->getDim() - 1)
			outs << " ";
	}
    outs << ")";
};

ostream& operator<<(ostream& outs, const id_set_t& id_set) {
    outs << "{";
	id_set_t::const_iterator iter;
    for (iter = id_set.begin(); iter != id_set.end(); iter++)
        outs << *iter << " ";
    outs << "}";
};

inline bool equal(double a, double b) {
	double dif = a - b;
//	cout << "dif: " << dif << " ";
	if (((dif >= 0) && (dif < ERROR)) || ((dif < 0) && (dif > -ERROR)))
		return true;
	return false;
};

inline double min(double a, double b) {
	return (a > b) ? b : a;
};

struct LongDouble {
	unsigned char mant[8];   // the mantissa is the lower 8 bytes.
	unsigned int exp;        // the exponent is the upper 2 bytes
};

union Real {
  // This union allows us to write in a long double and read out a
  // struct LongDouble.
	struct LongDouble sld;
	long double ld;
};

bool IsNaN (long double x) {
  // This function returns 1 if the parameter is any NaN type.  A
  // float or double may be passed too, and the compiler will
  // promote them to long double so the test will still work.
	union Real real;
	real.ld = x;
	if ((real.sld.exp &0x7FFF) == 0x7FFF)
		return true; // ignore sign of exponent in
					 // high bit of exponent.
	return false;
};
/////////////////////////////////////////////////////////////////////////////
/////////////////////////////////// Timer ///////////////////////////////////
/////////////////////////////////////////////////////////////////////////////
ostream& operator<<(ostream& outs, const Timer& t) {
	outs << "t[" << t.state << ", " << t.start_time.tv_sec << ":" <<
		t.start_time.tv_usec << ", " << t.running_time.tv_sec << ":" <<
		t.running_time.tv_usec << "]";
	return outs;
}

Timer::Timer(void) {
	reset();
};

void Timer::substract(struct timeval& a, struct timeval& b) { // a -= b
	a.tv_sec -= b.tv_sec;
	a.tv_usec -= b.tv_usec;
	if (a.tv_usec < 0) {
		a.tv_sec--;
		a.tv_usec += 1000000;
	}
}

void Timer::add(struct timeval& a, struct timeval& b) { // a += b
	a.tv_sec += b.tv_sec;
	a.tv_usec += b.tv_usec;
	if (a.tv_usec >= 1000000) {
		a.tv_sec++;
		a.tv_usec -= 1000000;
	}
}

void Timer::reset(void) {
	state = STOPPED_STATE;
	running_time.tv_sec = 0;
	running_time.tv_usec = 0;
};

void Timer::start(void) {
	assert(state == STOPPED_STATE);
    gettimeofday(&start_time, NULL);
    state = RUNNING_STATE;
};

void Timer::stop(void) {
	assert(state == RUNNING_STATE);
	struct timeval tv;
	
	gettimeofday(&tv, NULL);
	substract(tv, start_time);
	add(running_time, tv);
	state = STOPPED_STATE;
};

struct timeval Timer::getTime(void) {
	if (state == STOPPED_STATE)
		return running_time;
	else { // state == RUNNING_STATE
		struct timeval tv;

		gettimeofday(&tv, NULL);
		substract(tv, start_time);
		add(tv, running_time);
		return tv;
	}
};

/////////////////////////////////////////////////////////////////////////////
///////////////////////////////// SetMarker /////////////////////////////////
/////////////////////////////////////////////////////////////////////////////
ostream& operator<<(ostream& outs, const SetMarker& sm) {
	outs << "sm[";
	std::set<id_set_t, SetMarker::ltid_set_s>::const_iterator iter;
	iter = sm.marked_sets.begin();
	while (iter != sm.marked_sets.end()) {
//		if (iter != sm.marked_sets.begin())
//			outs << ",";
		outs << "(";
		copy((*iter).begin(), (*iter).end(),
			ostream_iterator<int>(outs, ","));
		outs << ")";
		++iter;
	}
	outs << "]";
    return outs;
};

bool SetMarker::ltid_set_s::operator()(const id_set_t& a, const id_set_t& b) const { // lexicographical comparison

	id_set_t::const_iterator a_iterator, b_iterator;

	a_iterator = a.begin();
	b_iterator = b.begin();

	while ((a_iterator != a.end()) && 
		   (b_iterator != b.end())) {
    	if (*a_iterator < *b_iterator)
			return true;
		else if (*a_iterator > *b_iterator)
			return false;
		++a_iterator;
		++b_iterator;
	}

	if (a_iterator != a.end())
		return false;

	if (b_iterator != b.end())
		return true;

	return false;
};

SetMarker::SetMarker(void) {
//	int ala = 10;
};

void SetMarker::mark(id_set_t id_set_p) {
	marked_sets.insert(id_set_p);
	return;
};

bool SetMarker::marked(id_set_t id_set_p) {
	return (marked_sets.find(id_set_p) != marked_sets.end());
};

/////////////////////////////////////////////////////////////////////////////
////////////////////////////// SetSequencer /////////////////////////////////
/////////////////////////////////////////////////////////////////////////////
ostream& operator<<(ostream& outs, const SetSequencer& ss) {
	outs << "ss[" << ss.max_size << "," << ss.max_id << "]";
    return outs;
};

SetSequencer::SetSequencer(int max_size_p, int max_id_p) {
	max_size = max_size_p;
    max_id = max_id_p;
//	cout << "max_id = " << max_id << " max_size = " << max_size << endl;
    assert(max_id >= max_size);
};

/////////////////////////////////////////////////////////////////////////////
////////////////////////// OrderedSetSequencer //////////////////////////////
/////////////////////////////////////////////////////////////////////////////
ostream& operator<<(ostream& outs, const OrderedSetSequencer& ss) {
	outs << "ss[" << ss.max_size << "," << ss.max_id << ",(";
	copy(ss.previous_set.begin(), ss.previous_set.end(), 
		ostream_iterator<int>(outs, ","));
	outs << ")]";
    return outs;
};

OrderedSetSequencer::OrderedSetSequencer(int max_size_p, int max_id_p) :
	SetSequencer(max_size_p, max_id_p) {
};

id_set_t OrderedSetSequencer::getNext(SetMarker& set_marker_p) {
	do {
		int i;	
		
		if (previous_set.empty()) { // compute the next set
									// previous_set = {1, 2, 3, ..., max_size}
			for (i = 1; i <= max_size; i++) // i is the position in the set
				previous_set.push_back(i);
		} else { // try to keep size of the previous_set
			// find position to shift
			bool found = false;
			int size = previous_set.size();
			i = size - 1;
			while ((i >= 0) && (!found)) {
				if ((previous_set[i] + (size - i)) <= max_id) {
					found = true; // we found the position to shift
					// so let's shift
					int shift = previous_set[i];
					for (int j = i; j < size; j++) {
						previous_set[j] = shift + (j - i + 1);
					}
				}
				i--;
			}
			if (!found) {
				// shift is not possible; we need to decrease the size of the 
				// previous_set, unless the size is 1
				if (size == 1)
					previous_set.clear();
				else {
					previous_set.pop_back();
					for (i = 0; i < (size - 1); i++)
						previous_set[i] = i + 1;
				}
			}
		}
//		cout << "alive4: " << set_marker_p.marked(previous_set) << "\n";
	} while (set_marker_p.marked(previous_set) && !previous_set.empty());
	return previous_set;
};

/////////////////////////////////////////////////////////////////////////////
///////////////////////// RandomizedSetSequencer ////////////////////////////
/////////////////////////////////////////////////////////////////////////////
RandomizedSetSequencer::RandomizedSetSequencer(int max_size_p, int max_id_p) :
	SetSequencer(max_size_p, max_id_p) {
};

id_set_t RandomizedSetSequencer::getNext(SetMarker& set_marker_p) {
	id_set_t next_set;

	do {
		next_set.clear();
		// first select the size of the next_set
		int size = Utilities::randomRange(1, max_size);
		// and then fill in with random elements
		for (int i = 0; i < size; i++) {
			int next_val = -1;
			do {
				next_val = Utilities::randomRange(1, max_id);
				for (int j = 0; j < i; j++)
					if (next_val == next_set[j]) {
						next_val = -1;
						j = i;
					}
			} while (next_val == -1);
			next_set.push_back(next_val);
		}
	} while (set_marker_p.marked(next_set));
	return next_set;
};

/////////////////////////////////////////////////////////////////////////////
/////////////////////// OverloadedFirstSetSequencer /////////////////////////
/////////////////////////////////////////////////////////////////////////////
OverloadedFirstSetSequencer::OverloadedFirstSetSequencer(int max_size_p, 
		int max_id_p) :
	SetSequencer(max_size_p, max_id_p) {
};

id_set_t OverloadedFirstSetSequencer::getNext(SetMarker& set_marker_p) {
	id_set_t next_set;
	int n_overloaded = dc->getNOverloaded();
	bool first = true;

	do {
		next_set.clear();
		// first select the size of the next_set
		int size;
		if (first)
			size = Utilities::randomRange(1, min(max_size, n_overloaded));
		else
			size = Utilities::randomRange(1, max_size);

		// and then fill in with random elements
		for (int i = 0; i < size; i++) {
			int next_val = -1;
			do {
				// try to get only overloaded spheres
				if (first)
					next_val = dc->getOverloaded(
						Utilities::randomRange(0, n_overloaded - 1));
				else
					next_val = Utilities::randomRange(1, max_id);
				for (int j = 0; j < i; j++)
					if (next_val == next_set[j]) {
						next_val = -1;
						j = i;
					}
			} while (next_val == -1);
			if (next_val == 0) {
				// * cout << "Dupa " << n_overloaded << " " << max_id << endl;
				fflush(NULL);
			}
			next_set.push_back(next_val);
		}
		first = false;
	} while (set_marker_p.marked(next_set));
	return next_set;
};
/////////////////////////////////////////////////////////////////////////////
///////////////////////////////// Sphere ////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////
ostream& operator<<(ostream& outs, const Sphere& s) {
    outs << "s[" << s.broker_id << " " << s.weight << " " << s.r << " (";
	for (int i = 0; i < dc->getDim(); i++)
		outs << s.center[i] << ",";
	outs << ")]";
    return outs;
};

Sphere::Sphere(const Sphere& sphere) : broker_id(sphere.broker_id), 
		weight(sphere.weight), r(sphere.r) {
	center = new double[dc->getDim()];
	memcpy(center, sphere.center, sizeof(double) * dc->getDim());
};

Sphere::Sphere(int broker_id_p, double weight_p, double r_p, 
		point_t center_p) : broker_id(broker_id_p), weight(weight_p), 
		r(r_p), center(center_p) {
};

Sphere::~Sphere() {
	delete[] center;
};

point_t Sphere::getCenter() const {
	return center;
};

double Sphere::getRadius() const {
	return r;
};

void Sphere::setRadius(double r_p) {
	r = r_p;
};

int Sphere::getBrokerId() const {
	return broker_id;
};

void Sphere::setBrokerId(int broker_id_p) {
	broker_id = broker_id_p;
};

double Sphere::getWeight() const {
	return weight;
};

int Sphere::contains(const point_t p) {
	double distance = 0;
	for (int i = 0; i < dc->getDim(); i++)
		distance += (center[i] - p[i]) * (center[i] - p[i]);
	double r_2 = r * r;
//	cout << "Error: " << distance - r_2 << " ";
	if (equal(distance, r_2))
		return 0; // p lays on the sphere
	if (distance > r_2)
		return -1; // p lays outside 
	return 1; // p lays inside
};

bool Sphere::intersectsWith(Sphere& sph) {
	double dist = 0;
	for (int i = 0; i < dc->getDim(); i++)
		dist += (center[i] - sph.center[i]) * (center[i] - sph.center[i]);
	double max_dist = (r + sph.r) * (r + sph.r);	
	if ((dist < max_dist) || equal(dist, max_dist))
		return true;
	return false;
};

CharacteristicVector* Sphere::getCharacteristicVector(point_t p) {
	point_t direction = new double[dc->getDim()];
//	point_t anti_direction = new double[dc->getDim()];
	double length = 0;
	int i;
	for (i = 0; i < dc->getDim(); i++) {
		direction[i] = center[i] - p[i];
		length += direction[i] * direction[i];
	}
	length = sqrt(length);
	for (i = 0; i < dc->getDim(); i++) { // scale
		direction[i] /= length;
//		anti_direction[i] = -1.0 * direction[i];
	}
	point_t begin = new double[dc->getDim()];
	memcpy(begin, p, sizeof(double) * dc->getDim());
	return new CharacteristicVector(begin, direction);
}
/////////////////////////////////////////////////////////////////////////////
/////////////////////////////////// Matrix //////////////////////////////////
/////////////////////////////////////////////////////////////////////////////
void Matrix::swapRows(int row, int with_row) {
	assert((row < m) && (with_row < m));
	if (row == with_row)
		return;
	int pos_row = n * row;
	int pos_with_row = n * with_row;

	for (int i = 0; i < n ; i++) {
		double dummy = data[pos_row];
		data[pos_row] = data[pos_with_row];
		data[pos_with_row] = dummy;
		pos_row++;
		pos_with_row++;
	}
};

ostream& operator<<(ostream& outs, const Matrix& m) {
	outs << "m[";
	for (int i = 0; i < m.m; i++) {
		outs << "(";
		int in = i * m.n;
		for (int j = in; j < in + m.n; j++) {
			outs << m.data[j];
			if (j < in + m.n - 1)
				outs << " ";
		}
		outs << ")";
		if (i < m.m - 1)
			outs << " " << endl;
		else
			outs << endl;
	}
	outs << "]";
};

Matrix::Matrix(int m_p, int n_p) : m(m_p), n(n_p) {
	assert((m > 0) && (n > 0));
	data = new double[m * n];
	for (int i = 0; i < m * n; i++)
		data[i] = 0;
};

Matrix::Matrix(int m_p, int n_p, double* data_p) : m(m_p), n(n_p), 
	data(data_p) {
};

Matrix::Matrix(const Matrix& mat) : m(mat.m), n(mat.n) {
	data = new double[m * n];
	memcpy(data, mat.data, sizeof(double) * m * n);
};

Matrix::~Matrix() {
	delete[] data;
};

inline double Matrix::operator()(int row, int col) {
	assert((row < m) && (col < n));
	return data[row * n + col];
};

int Matrix::nRows() const {
	return m;
};

int Matrix::nCols() const {
    return n;
};

void Matrix::copyData(const Matrix& mat) {
	assert((m == mat.m) && (n == mat.n));
	memcpy(data, mat.data, sizeof(double) * m * n);
};

void Matrix::merge(const Matrix& mat) {
	assert(n == mat.n);
	double *new_data = new double[n * (m + mat.m)];
	memcpy(new_data, data, sizeof(double) * m * n);
	memcpy(new_data + m * n, mat.data, 
		sizeof(double) * mat.m * mat.n);
	delete[] data;
	data = new_data;
	m += mat.m;
};

void Matrix::addCols(int a, int b, double coef) {
	assert((a < n) && (b < n));
	int j = b;
	for (int i = a; i < m * n;) {
		data[i] += coef * data[j];
		i += n;
		j += n;
	}
};

void Matrix::cutDown(int rows) {
	assert(rows <= m);
	if (rows <= 0)
		return;
	double *new_data = new double[n * (m - rows)];
	memcpy(new_data, data, sizeof(double) * n * (m - rows));
	delete[] data;
	data = new_data;
	m -= rows;
};

void Matrix::cutDownZeroRows() {
	int rows = 0; // how many rows to cut
	
	for (int r = m - 1; r >= 0; r--) {
		bool zeros = true;
		int rn = r * n;
		for (int c = 0; c < n; c++) 
			if (data[rn + c] != 0) {
				zeros = false;
				c = n;
			}
		if (zeros)
			rows = m - r;
	}
	cutDown(rows);
};

bool* Matrix::transformToR() {
//	int nIter = min(rows, min(n, m));
	int i, j;
	int c = 0;
	int r = 0;
	int mn = m * n;
	bool* ignore_cols = new bool[n - 1];
	while ((r < m) && (c < n - 1)) { // c < n ????????
		int rn = r * n;
		// find row >= r with maximal value in column r
		int c_max = rn + c;
		for (i = rn + c + n; i < mn; i += n)
			if (fabs(data[i]) > fabs(data[c_max]))
				c_max = i;
		if (data[c_max] == 0)
			ignore_cols[c] = true;
		else {
			ignore_cols[c] = false;
			swapRows(r, (c_max - c) / n);

			// perform elimination with row r
			for (int in = rn + n; in < mn; in += n) {
				double coef = data[in + c] / data[rn + c];
				for (j = c + 1; j < n; j++)
					data[in + j] -= coef * data[rn + j];
				data[in + c] = 0;
			}
			// scale row r
			for (j = rn + c + 1; j < rn + n; j++)
				data[j] = data[j] / data[rn + c];
			data[rn + c] = 1;
			r++;
		}
		c++;
	}
	for (i = c; i < n - 1; i++)
		ignore_cols[i] = true;
	return ignore_cols;
};

point_t Matrix::solve(const bool* ignore_cols) {
	point_t x = new double[n - 1];
	int diagonal = m * n - 2;
	int b_index = diagonal + 1;

	for (int c = n - 2; c >= 0; c--) {
		if (ignore_cols[c]) {
			x[c] = 0;
			diagonal -= 1;
		} else {
			double b = data[b_index]; // set the values
			int a_index = b_index - 1;
			for (int j = n - 2; j > c; j--, a_index--)
				if (!ignore_cols[j])
					b -= data[a_index] * x[j];
			if (data[a_index] == 0) // no solution
				return NULL;
			x[c] = b / data[a_index];
			diagonal -= n + 1;
			b_index -= n;
		}
	}
	return x;
};

/////////////////////////////////////////////////////////////////////////////
/////////////////////////////////// Plane ///////////////////////////////////
/////////////////////////////////////////////////////////////////////////////
set<point_t> Plane::getBase() {
	bool* ignore_cols = equations.transformToR();
	equations.cutDownZeroRows();
	int n = equations.nCols();
	set<point_t> base; // don't forget to free the memory
	int i;
	point_t x;
	point_t ref_point = NULL; // reference point
	Matrix dummy(equations);

//	for (i = 0; i < n - 1; i++)
//		cout << ignore_cols[i] << " ";
//	cout << endl;
//	fflush(NULL);

	for (i = 0; i < n - 1; i++)
		if (ignore_cols[i]) {
//			cout << "i = " << i << endl;
//			fflush(NULL);
			equations.addCols(n - 1, i, -1);
			x = equations.solve(ignore_cols);
			x[i] = 1;
			if (ref_point == NULL)
				ref_point = x;
			else {
				for (int j = 0; j < n - 1; j++)
					x[j] -= ref_point[j];
				base.insert(x);
			}
			equations.copyData(dummy); // reset content
		}

	assert(ref_point != NULL); // plane dimention > 0

	for (i = 0; i < n - 1; i++)
		if (ignore_cols[i])
			equations.addCols(n - 1, i, -2);
	x = equations.solve(ignore_cols);
	for (i = 0; i < n - 1; i++) {
		if (ignore_cols[i])
			x[i] = 2;
		x[i] -= ref_point[i];
	}
	equations.copyData(dummy);
	base.insert(x);
	delete[] ignore_cols;
	delete[] ref_point;
	
	return base;
};

ostream& operator<<(ostream& outs, const Plane& p) {
	outs << "p[" << p.equations << "]";
};

Plane::Plane() : equations(1, dc->getDim() + 1) {
};

Plane::Plane(Matrix& equations_p) : equations(equations_p) {
};

void Plane::intersectWith(Plane& p) {
	equations.merge(p.equations);
//	equations.transformToR();
};

// bool Plane::isEmpty() {	
// };

point_t* Plane::project(point_t p) {
	set<point_t> base = getBase();
	set<point_t>::iterator iter;
	int n = equations.nCols();

//	cout << "Base: " << endl;
//	for (iter = base.begin(); iter != base.end(); iter++)
//		cout << (*iter);
//	fflush(NULL);

	if (base.empty())
		return NULL;
	double* data = new double[n * base.size()];
	int i = 0;
	for (iter = base.begin(); iter != base.end(); iter++) {
		point_t base_vector = *iter;
		double b = 0;

		for (int j = 0; j < n - 1; j++) {
			data[i++] = base_vector[j];
			b += p[j] * base_vector[j];
		}
		data[i++] = b;
		if (iter != base.begin())
			delete[] base_vector; // not needed any more
	}

	Matrix m(base.size(), n, data);
	// cout << m << endl << equations << endl;
	m.merge(equations);
	// cout << m << endl;
	m.transformToR();
//	cout << m << endl;
	m.cutDownZeroRows();
//	cout << m << endl;
	bool ignore_cols[n - 1];
	for (int i = 0; i < n - 1; i++)
		ignore_cols[i] = false;

	point_t *result = new point_t[2];
//	point_t x = m.solve(ignore_cols);
//	print_point(x);
	result[0] = m.solve(ignore_cols);
	result[1] = *(base.begin());
//	cout << "Zyje" << m << endl;
//	fflush(NULL);
	return result;
};

/////////////////////////////////////////////////////////////////////////////
///////////////////////////// SphereIntersection ////////////////////////////
/////////////////////////////////////////////////////////////////////////////
ostream& operator<<(ostream& outs, const SphereIntersection& si) {
	outs << "si[" << si.sphere << ", " << si.plane << "]";
};

SphereIntersection::SphereIntersection(const Sphere& s) : sphere(s) {
};

// SphereIntersection::SphereIntersection(const Sphere& a, const Sphere& b) :
//	sphere(a) {
//	// cout << a << endl << sphere << endl;
//	this->intersect(b);
//};

void SphereIntersection::intersect(const Sphere& s) {
	point_t center = sphere.getCenter();
	point_t s_center = s.getCenter();
	int dim = dc->getDim();
	double *data = new double[dim + 1];
	double b = (sphere.getRadius() * sphere.getRadius() - 
		s.getRadius() * s.getRadius()) / 2.0;
	

	for (int i = 0; i < dim; i++) {
		data[i] = s_center[i] - center[i];
		b -= (center[i] * center[i] - s_center[i] * s_center[i]) / 2.0;
	}
	data[dim] = b;
	Matrix m(1, dim + 1, data);
	// cout << "---------------" << endl << sphere << endl << s << endl << m << 
	//	"---------------" << endl;
	Plane p(m);
	plane.intersectWith(p);
	// cout << plane << endl;
};

point_t* SphereIntersection::getPoints() {
	point_t center = sphere.getCenter();

//	cout << sphere << endl << plane << endl;
//	print_point(center);
//	fflush(NULL);

	point_t* projection = plane.project(center);
//	cout << projection[0];
//	fflush(NULL);
	if (projection == NULL)
		return NULL;
	point_t x = projection[0];
	point_t base_vector = projection[1];
	int dim = dc->getDim();

//	cout << x << " " << base_vector << endl;

	double dist = 0;
	for (int i = 0; i < dim; i++)
		dist += (center[i] - x[i]) * (center[i] - x[i]);

	// compute the desired length^2 of the base vector
	double r = sphere.getRadius() * sphere.getRadius();
	r -= dist;

	if (r < 0) { // plane does not intersect with the sphere
		delete[] x;
		delete[] base_vector;
		return NULL;
	}

	// compute the real length^2 of the base vector
	double len = 0;
	for (int i = 0; i < dim; i++)
		len += base_vector[i] * base_vector[i];
//	cout << base_vector;
	// compute the result points
	point_t p1 = new double[dim];
	point_t p2 = new double[dim];
	double factor = sqrt(r / len);
//	cout << "r=" << r << " len=" << len << "factor=" << factor << endl;
	for (int i = 0; i < dim; i++) {
		p1[i] = x[i] + base_vector[i] * factor;
		p2[i] = x[i] - base_vector[i] * factor;
	}

	delete[] x;
	delete[] base_vector;

	// resuse variable projection
	projection[0] = p1;
	projection[1] = p2;

	return projection;
};

/////////////////////////////////////////////////////////////////////////////
//////////////////////////// CharacteristicVector ///////////////////////////
/////////////////////////////////////////////////////////////////////////////
ostream& operator<<(ostream& outs, const CharacteristicVector& cv) {
	outs << "cv[" << cv.begin << "," << cv.direction << "]";
};

CharacteristicVector::CharacteristicVector(point_t begin_p, 
	point_t direction_p) : 
	begin(begin_p), direction(direction_p) {
};

CharacteristicVector::CharacteristicVector(const CharacteristicVector& cv) {
	begin = new double[dc->getDim()];
	memcpy(begin, cv.begin, sizeof(double) * dc->getDim());
	direction = new double[dc->getDim()];
	memcpy(direction, cv.direction, sizeof(double) * dc->getDim());
};

CharacteristicVector::~CharacteristicVector() {
	delete[] begin;
	delete[] direction;
};

point_t CharacteristicVector::getBegin() {
	return begin;
};

point_t CharacteristicVector::getDirection() {
	return direction;
};

bool CharacteristicVector::isZeroVector() {
	bool zeros = true;
	for (int i = 0; i < dc->getDim(); i++)
		if (direction[i] != 0) {
			zeros = false;
			i = dc->getDim();
		}
	return zeros;
};

int CharacteristicVector::angle(CharacteristicVector& cv) {
	double length = 0;

	for (int i = 0; i < dc->getDim(); i++) {
		double a = direction[i] + cv.direction[i];
		length += a * a;
	}

	if ((length > 4.0) || equal(length, 4.0)) // == 4.0
		return 0; // angle == 0
	if (equal(length, 2.0)) // ~ 2.0
		return 2; // angle == pi / 2
	if (length > 2.0)
		return 1; // 0 < angle < pi / 2
	if (equal(length, 0.0)) // angle == pi
		return 4;
	return 3; // pi / 2 < angle < pi
};

void CharacteristicVector::add(CharacteristicVector& cv) {
	double length = 0;
	int i;
//	cout << "add(" << *this << "," << cv << ") : ";
	for (i = 0; i < dc->getDim(); i++) {
		direction[i] += cv.direction[i];
		length += direction[i] * direction[i];
	}
	// and then scale
	length = sqrt(length);
	if (length != 0)
		for (i = 0; i < dc->getDim(); i++)
			direction[i] /= length;
//	cout << *this << endl;
};

CharacteristicVector* CharacteristicVector::getAntiVector() {
	double *anti_begin = new double[dc->getDim()];
	double *anti_direction = new double[dc->getDim()];

	memcpy(anti_begin, begin, sizeof(double) * dc->getDim());
	memcpy(anti_direction, direction, sizeof(double) * dc->getDim());
	for (int i = 0; i < dc->getDim(); i++)
		anti_direction[i] *= -1.0;
	return new CharacteristicVector(anti_begin, anti_direction);
};

/////////////////////////////////////////////////////////////////////////////
///////////////////////////// CharacteristicPoint ///////////////////////////
/////////////////////////////////////////////////////////////////////////////
ostream& operator<<(ostream& outs, const CharacteristicPoint& cp) {
	outs << "cp[" << cp.point << "," << cp.spheres << "]";
};

CharacteristicPoint::CharacteristicPoint(point_t point_p, id_set_t spheres_p) :
	point(point_p), spheres(spheres_p) {
};

CharacteristicPoint::~CharacteristicPoint() {
	delete[] point;
};

CharacteristicVector *CharacteristicPoint::checkRegions() {
	map<int, Sphere*> all_spheres = dc->getAllSpheres();
	map<int, Sphere*>::const_iterator s_iter;
	set<Sphere*> on_border;
	Sphere *sphere;
	CharacteristicVector *cv;
	double weight = 0;
	double anti_weight = 0;
	int n_inside = 0;

	// STEP 1: limit number of spheres
	for (s_iter = all_spheres.begin(); s_iter != all_spheres.end(); s_iter++) {
		sphere = (*s_iter).second;
		int loc = sphere->contains(point);
		if (loc == 0) // on the sphere
			on_border.insert(sphere);
		else if (loc == 1) { // inside
			n_inside++;
			if (sphere->getBrokerId() == 1) // broker to underload
				weight += sphere->getWeight();
			else // another broker
				anti_weight += sphere->getWeight();
		}
	}

	// STEP 2: create base characteristic vectors
	id_set_t::iterator id_iter;
	map<int, CharacteristicVector*> base_cvs;
	int vector_id = 0;
	for (id_iter = spheres.begin(); id_iter != spheres.end(); id_iter++) {
		sphere = dc->getSphere(*id_iter);
		// needed because of inaccuracies in computing Sphere::contains
		on_border.insert(sphere);

		// construct base characteristic vectors
		cv = sphere->getCharacteristicVector(point);
		base_cvs[++vector_id] = cv;
		base_cvs[++vector_id] = cv->getAntiVector();
	}

	// cout << "on_border: ";
	// for (set<Sphere*>::iterator sph_iter1 = on_border.begin(); 
	//	sph_iter1 != on_border.end(); sph_iter1++) {
	//	cout << **sph_iter1 << " ";
	// }
	// cout << endl;
//	cout << "point: " << point << endl << "weight: " << weight << 
//		" anti_weight: " << anti_weight << " on_border.size() = " << 
//		on_border.size() << endl;

	// cout << "base_cvs: ";
	// for (map<int, CharacteristicVector*>::iterator cvs_iter = 
	//		base_cvs.begin(); cvs_iter != base_cvs.end(); cvs_iter++)
	//	cout << *((*cvs_iter).second) << " ";
	// cout << endl;

	// STEP 3: check regions
	SetMarker sm;
	OrderedSetSequencer ss(spheres.size(), 2 * spheres.size());
	bool found = false;
	id_set_t base_ids = ss.getNext(sm);
	sm.mark(base_ids);
	do {
//		cout << "base_ids: " << base_ids << endl;
		// construct characteristic vector
		id_set_t::iterator id_iter = base_ids.begin();
		cv = new CharacteristicVector(*(base_cvs[*id_iter]));
		id_iter++;
//		cout << "base_cvs: " << *cv << endl;
		for (; id_iter != base_ids.end(); id_iter++) {
//			cout << "base_cvs: " << *(base_cvs[*id_iter]) << endl;
			cv->add(*(base_cvs[*id_iter]));
		}
//		cout << "cv: " << *cv << endl;
		if (!cv->isZeroVector()) {
			// check region described by this characteristic vector
			double l_weight = weight;
			double l_anti_weight = anti_weight;
			set<Sphere*>::iterator sph_iter;
			for (sph_iter = on_border.begin(); sph_iter != on_border.end(); 
					sph_iter++) {
				sphere = *sph_iter;
				CharacteristicVector *s_cv = 
					sphere->getCharacteristicVector(point);
				if (cv->angle(*s_cv) < 2) { // sphere contains cv
					if (sphere->getBrokerId() == 1)
						l_weight += sphere->getWeight();
					else
						l_anti_weight += sphere->getWeight();
				}
				delete s_cv;
			}
			// * cout << "point: " << point << endl << "weight: " << l_weight << 
			// * " anti_weight: " << l_anti_weight << " on_border.size() = " << 
			// *	on_border.size() << endl;
	
			if ((l_weight >= dc->getOverload()) && 
				((l_weight + l_anti_weight) <= dc->getMaxLoad())) {
				found = true;
//				cout << "Found: weight = " << l_weight << " anti_weight = " <<
//					l_anti_weight << endl;
			} else 
				delete cv;
		}
		base_ids = ss.getNext(sm);
		sm.mark(base_ids);
	} while (!found && (base_ids.size() == spheres.size()));
	map<int, CharacteristicVector*>::iterator cv_iter;
	for (cv_iter = base_cvs.begin(); cv_iter != base_cvs.end(); cv_iter++)
		delete (*cv_iter).second;
	if (found)
		return cv;
	else 
		return NULL;
};

/////////////////////////////////////////////////////////////////////////////
/////////////////////////////// DataContainer ///////////////////////////////
/////////////////////////////////////////////////////////////////////////////
DataContainer* DataContainer::dc = NULL;

DataContainer* DataContainer::getInstance() {
	// dc = *(new DataContainer());
	if (dc == NULL)
		dc = new DataContainer();
//	cout << "Zyje" << endl;
	return dc;
};

ostream& operator<<(ostream& outs, const DataContainer& dc) {
    outs << "dc[" << dc.dim << " {";
	map<int, point_t>::const_iterator b_iter;
	for (b_iter = dc.broker_locations.begin(); 
		b_iter != dc.broker_locations.end(); b_iter++) {
		outs << b_iter->first << ":(";
		for (int i = 0; i < dc.dim; i++)
			outs << b_iter->second[i] << ",";
		outs << ")";
	}
	outs << "} {";
	
	map<int, Sphere*>::const_iterator s_iter;
//	s_iter = dc->spheres.begin();
    for (s_iter = dc.spheres.begin(); s_iter != dc.spheres.end(); s_iter++)
        outs << s_iter->first << ":" << *(s_iter->second);
    outs << "}]";
    return outs;
};

DataContainer::DataContainer() {
};

DataContainer::~DataContainer() {
	map<int, Sphere*>::const_iterator s_iter;
	for (s_iter = spheres.begin(); s_iter != spheres.end(); s_iter++)
		delete (*s_iter).second;

    map<int, point_t>::const_iterator b_iter;
    for (b_iter = broker_locations.begin(); 
			b_iter != broker_locations.end(); b_iter++)
        delete[] (*b_iter).second;
};

void DataContainer::loadFromFile(const char* file_name) {
	int i, j;
	ifstream input(file_name, ios::in);
	if ((input.rdstate() & ifstream::failbit ) != 0) {
		cerr << "Error oppening file: " << file_name << endl;
		return;
	}
	input >> setprecision(PRECISION);
	input >> dim; // first line - dimention

	int n_brokers; 
	input >> n_brokers; // second line - number of brokers
	for (i = 0; i < n_brokers; i++) { // succesive n_brokers lines - 
									  // locations of brokers
		point_t location = new double[dim];
		for (j = 0; j < dim; j++)
			input >> location[j];
		broker_locations[i + 1] = location;
	}

	int n_spheres;
	input >> n_spheres; // following line - number of brokers
	for (i = 0; i < n_spheres; i++) { // succesive n_spheres lines - 
									  // descriptions of the spheres
		int broker_id; 
		input >> broker_id; // fist position in the line - broker id
		double weight;
		input >> weight; // second position in the line - weight
		double r; 
		input >> r; // third position - radius
		point_t center = new double[dim];
		for (j = 0; j < dim; j++) // successive dim positions - coordinates
								  // of the center
			input >> center[j];
		spheres[i + 1] = new Sphere(broker_id, weight, r, center);
		if (broker_id == 1)
			overloaded_spheres.push_back(i + 1);
//		delete[] center;
	}
	input >> overload; input >> max_load; // last line - overload max_load
	overload /= 2.0;
	max_load /= 2.0;
// *	cout << "n_brokers == " << n_brokers << " n_spheres == " << n_spheres << 
// *		endl;

//	assert(input.eof());
	input.close();
};

void DataContainer::saveToFile(const char* file_name) {
	int i, j;
	
	ofstream output(file_name, ios::out);
	if ((output.rdstate() & ofstream::failbit ) != 0) {
		cerr << "Error oppening file: " << file_name << endl;
		return;
	}
	output << dim << endl; // first line - dimention

	int n_brokers = broker_locations.size();
	output << n_brokers << endl; // second line - number of brokers

	// find the highest loaded broker
	map<int, Sphere*>::const_iterator s_iter;
	double *loads = new double[n_brokers];
	Sphere *sphere;
	for (i = 0; i < n_brokers; i++)
		loads[i] = 0;
	for (s_iter = spheres.begin(); s_iter != spheres.end(); s_iter++) {
		sphere = (*s_iter).second;
		loads[sphere->getBrokerId() - 1] += sphere->getWeight();
	}
	int first_broker = 0;
	for (i = 0; i < n_brokers; i++)
		if (loads[i] > loads[first_broker])
			first_broker = i;
	first_broker++;
	for (i = 1; i <= n_brokers; i++) { // succesive n_brokers lines - 
									  // locations of brokers
		if (i == 1)
			i = first_broker;
		else if (i == first_broker)
			i = 1;
		for (j = 0; j < dim; j++)
			output << broker_locations[i][j] << " ";
		output << endl;
		if (i == 1)
			i = first_broker;
		else if (i == first_broker)
			i = 1;
	}

	int n_spheres = spheres.size();
	output << n_spheres << endl; // following line - number of brokers
	for (s_iter = spheres.begin(); s_iter != spheres.end(); s_iter++) { 
			// succesive n_spheres lines - 
			// descriptions of the spheres
		sphere = (*s_iter).second;
		int broker_id = sphere->getBrokerId();
		if (broker_id == 1)
			broker_id = first_broker;
		else if (broker_id == first_broker)
			broker_id = 1;
		output << broker_id << " "; // fist position in the line - 
									// broker id
		output << sphere->getWeight() << " " ; // second position in the line - 
										       // weigh 
		output << setprecision(PRECISION);
		output << sphere->getRadius(); // third position - radius
		output << setprecision(6);
		point_t center = sphere->getCenter();
		for (j = 0; j < dim; j++) // successive dim positions - coordinates
								  // of the center
			output << " " << center[j];
		output << endl;
//		delete[] center;
	}
	output << loads[first_broker - 1] / 4.0 << " "; // overload
	output << Utilities::randomRange((int) (loads[first_broker - 1] / 3.8) + 1,
		(int) (loads[first_broker - 1] / 2.0)) << endl; // max_load
// *	cout << "n_brokers == " << n_brokers << " n_spheres == " << n_spheres << 
// *		endl;

//	assert(input.eof());
	delete [] loads;
	output.close();
};

void DataContainer::limitNumberOfSpheres() {
	vector<Sphere*> sph_of_broker_1;
	map<int, Sphere*>::iterator all_iter;
	Sphere *sph;
	map<int, Sphere*> new_spheres;
	vector<int> new_overloaded_spheres;

	// select spheres of broker 1
	for (all_iter = spheres.begin(); all_iter != spheres.end(); all_iter++) {
		sph = (*all_iter).second;
		if (sph->getBrokerId() == 1)
			sph_of_broker_1.push_back(sph);
	}
	cout << "sph_of_broker_1.size() = " << sph_of_broker_1.size() << endl;
	// delete all the spheres with empty intersection with all spheres from 
	// sph_of_broker_1
	int n_erased = 0;
	int next_index = 1;
	for (all_iter = spheres.begin(); all_iter != spheres.end(); all_iter++) {
		sph = (*all_iter).second;
		bool intersects;
		if (sph->getBrokerId() == 1) {
			intersects = true;
			new_overloaded_spheres.push_back(next_index);
		} else {
			intersects = false;
			vector<Sphere*>::iterator iter;
			for (iter = sph_of_broker_1.begin(); iter != sph_of_broker_1.end();
					) {
				if (!sph->intersectsWith(**iter)) {
					iter++;
				} else {
					intersects = true;
					iter = sph_of_broker_1.end();
				}
			}
		}
		if (intersects) {
			new_spheres[next_index++] = sph;
		} else {
			delete sph;
			// spheres.erase(all_iter);
			n_erased++;
		}
	}
	if (n_erased != 0) {
		spheres.swap(new_spheres);
		overloaded_spheres.swap(new_overloaded_spheres);
	}
	cout << "n_erased = " << n_erased << " spheres.size() = " << 
		spheres.size() <<  endl;
};

bool DataContainer::testConsistency() {
	for (int i = 1; i < spheres.size(); i++)
		if (spheres.find(i) == spheres.end())
			return false;
	return true;
};

Sphere* DataContainer::getSphere(int id) {
    return spheres[id];
};

point_t DataContainer::getBrokerLocation(int id) {
	return broker_locations[id];
};

map<int, Sphere*>& DataContainer::getAllSpheres() {
    return spheres;
};

int DataContainer::getNOverloaded() {
	return overloaded_spheres.size();
};

int DataContainer::getOverloaded(int i) {
	return overloaded_spheres[i];
};

int DataContainer::getDim() {
    return dim;
};

double DataContainer::getOverload() {
    return overload;
};

double DataContainer::getMaxLoad() {
    return max_load;
};

void DataContainer::addBroker(point_t broker_location) {
	map<int, Sphere*>::const_iterator s_iter;
	int broker_id = broker_locations.size() + 1;
	for (s_iter = spheres.begin(); s_iter != spheres.end(); s_iter++) {
		Sphere *sphere = (*s_iter).second;
		point_t center = sphere->getCenter();
		double dist = 0;
		for (int d = 0; d < dim; d++) {
			dist += (broker_location[d] - center[d]) * 
				(broker_location[d] - center[d]);
		}
		double dist_2 = sqrt(dist);
		if (dist_2 < sphere->getRadius()) {
			sphere->setRadius(dist_2);
			sphere->setBrokerId(broker_id);
		}
	}
	point_t new_broker_location = new double[dim];
	memcpy(new_broker_location, broker_location, sizeof(double) * dim);
	broker_locations[broker_id] = new_broker_location;
};

void DataContainer::addSphere(int weight, point_t center) {
	map<int, point_t>::const_iterator b_iter;
	double r = -1;
	int broker_id;
	for (b_iter = broker_locations.begin(); b_iter != broker_locations.end(); 
			b_iter++) {
		point_t broker_location = (*b_iter).second;
		double dist = 0;
		for (int d = 0; d < dim; d++)
			dist += (center[d] - broker_location[d]) * 
				(center[d] - broker_location[d]);
		if ((r == -1) || (r > dist)) {
			r = dist;
			broker_id = (*b_iter).first;
		}
	}
	int sphere_id = spheres.size() + 1;
	r = sqrt(r);
	Sphere *s = new Sphere(broker_id, weight, r, center);
	spheres[sphere_id] = s;
};

bool DataContainer::testData() {
	map<int, Sphere*>::const_iterator s_iter;
	bool result = true;

	for (s_iter = spheres.begin(); s_iter != spheres.end(); s_iter++) {
		Sphere *sphere = (*s_iter).second;
		point_t center = sphere->getCenter();
		point_t broker_location = getBrokerLocation(sphere->getBrokerId());
		double dist = 0;
		for (int d = 0; d < dim; d++)
			dist += (broker_location[d] - center[d]) * 
				(broker_location[d] - center[d]);
		double r = sphere->getRadius();
		r *= r;
		double error = (dist > r) ? (dist - r) : (r - dist);
		if (error > 10.0) {
		  	result = false;
			// cout << "dupa" << endl;
		}
	}
	int n_errors = 0;
	double max_error = 0.0;
    for (s_iter = spheres.begin(); s_iter != spheres.end(); s_iter++) {
		Sphere *sphere = (*s_iter).second;
		int broker_id = sphere->getBrokerId();
		point_t broker_loc = getBrokerLocation(broker_id);
		point_t center = sphere->getCenter();
		double error = 0;
		double distance = 0;
		double r = sphere->getRadius();
		r *= r;
		for (int i = 0; i < dim; i++)
			distance += (center[i] - broker_loc[i]) *
				(center[i] - broker_loc[i]);
			error = r - distance;
			if (!equal(r, distance))
				n_errors++;
			if (error < 0)
				error = -error;
			if (error > max_error)
				max_error = error;
			if (error > 10.0)
				cout << "DUPA" << endl;
																					}
	cout << "max_error = " << max_error << endl;
	return result;
};

/////////////////////////////////////////////////////////////////////////////
//////////////////////////////// Utilities //////////////////////////////////
/////////////////////////////////////////////////////////////////////////////
int Utilities::randomRange(int lowest_number, int highest_number) {
    if(lowest_number > highest_number) {
        swap(lowest_number, highest_number);
    }

    int range = highest_number - lowest_number + 1;
//	cout << range << " " << range * (rand()/(RAND_MAX + 1.0)) << endl;
    return lowest_number + int(range * ((double) rand()/(RAND_MAX + 1.0)));
};

double Utilities::randn() {
	double u1 = (rand() + 1.0) / (RAND_MAX + 1.0);
	double u2 = rand() / (RAND_MAX + 1.0);

	return sqrt(-2.0*log(u1))*cos(2 * M_PI * u2);
};

int Utilities::randomNormalRange(int lowest_number, int highest_number, 
		double esp, double var) {
	if(lowest_number > highest_number) {
		swap(lowest_number, highest_number);
	}

	double z = randn();

//  int var = highest_number - lowest_number + 1;
//	double esp = lowest_number + range / 2.0;
	double result = esp + var * z;
	if ((result > highest_number) || (result < lowest_number))
		result = randomRange(lowest_number, highest_number);
	return int(result);
};

CharacteristicVector* Utilities::solve() {
	SetMarker sm;
	OverloadedFirstSetSequencer ss(dc->getDim(), dc->getAllSpheres().size());
	CharacteristicVector *solution = NULL;
	id_set_t id_set = ss.getNext(sm);
	sm.mark(id_set);
	int n_iterations = 0;

	do {
		n_iterations++;
		// * cout << id_set << endl;
		id_set_t::iterator iter;
		iter = id_set.begin();
		SphereIntersection *si = 
			new SphereIntersection(*(dc->getSphere(*iter)));
		iter++;
		for (; iter != id_set.end(); iter++)
			si->intersect(*(dc->getSphere(*iter)));

		point_t *points = si->getPoints();
		// cout << dc->getSphere(5)->contains(points[0]) << " " << 
		//	dc->getSphere(5)->contains(points[1]) << endl;

		if (points != NULL) {
			// * cout << "p1 = " << points[0] << " p2 = " << points[1] << endl;
			if (IsNaN(points[0][0]))
				exit(-1);

			for (int i = 0; (i < 2) && (solution == NULL); i++) {
				// Optimization 1: check whether it is a broker location
				int j;
				bool is_broker_1_location = true;
				point_t broker_1_location = dc->getBrokerLocation(1);
				for (j = 0; (j < dc->getDim()) && broker_1_location; j++)
					if (!equal(broker_1_location[j], points[i][j]))
						is_broker_1_location = false;
				if (is_broker_1_location)
					memcpy(points[i], broker_1_location, 
						sizeof(double) * dc->getDim());

				CharacteristicPoint *cp = new CharacteristicPoint(points[i], 
					id_set);
				solution = cp->checkRegions();
				delete cp; // deletes also points[i]
//				delete[] points[i];
			}
			delete[] points;
		}
		delete si;
		id_set = ss.getNext(sm);
		sm.mark(id_set);
	} while ((solution == NULL) && !id_set.empty());
	if (solution != NULL)
		cout << "Found solution: " << *solution << " in " << n_iterations <<  
			" iterations" << endl;
	return solution;
};

CharacteristicVector* Utilities::solveRandom () {
//	const int MAX_COORDINATE_VALUE = 1000;
	const int AFTER_DECIMAL_POINT = int(1.0 / ERROR);
	const int SEED = 8216;
	CharacteristicVector *solution = NULL;
	int n_iterations = 0;
	map<int, Sphere*> all_spheres = dc->getAllSpheres();
	map<int, Sphere*>::const_iterator s_iter;
	point_t broker_1_loc = dc->getBrokerLocation(1);

	srand(SEED);

	do {
		n_iterations++;
		int i;
		point_t random_point = new double[dc->getDim()];
		for (i = 0; i < dc->getDim(); i++) {
			double esp = broker_1_loc[i];
			double var = min(MAX_COORDINATE_VALUE - broker_1_loc[i], 
				broker_1_loc[i] + MAX_COORDINATE_VALUE) / 70.0;

			random_point[i] = (double) randomNormalRange(
					-MAX_COORDINATE_VALUE + 1, 
					MAX_COORDINATE_VALUE - 1, esp, var) + 
				(double) randomRange(-AFTER_DECIMAL_POINT, 
					AFTER_DECIMAL_POINT) / (double) AFTER_DECIMAL_POINT;
		}
		double weight = 0;
		double anti_weight = 0;
		for (s_iter = all_spheres.begin(); s_iter != all_spheres.end(); 
				s_iter++) {
			Sphere *sphere = (*s_iter).second;
			int loc = sphere->contains(random_point);
			if (loc == 1) { // inside
				if (sphere->getBrokerId() == 1) // broker to underload
					weight += sphere->getWeight();
				else // another broker
					anti_weight += sphere->getWeight();
			}
		}
// *		cout << "random_point = " << random_point << " weight = " << weight <<
// *			" anti_weight = " << anti_weight << endl;
		if ((weight < dc->getOverload()) ||
                ((weight + anti_weight) > dc->getMaxLoad()))
			delete[] random_point;
		else {
			point_t zero_vector = new double[dc->getDim()];
			for (i = 0; i < dc->getDim(); i++)
				zero_vector[i] = 0;
			solution = new CharacteristicVector(random_point, zero_vector);
		}
	} while(solution == NULL);
	cout << "Found solution: " << *solution << " in " << n_iterations <<
		" iterations" << endl;

	return solution;
};

bool Utilities::testSolution(CharacteristicVector& solution) {
	map<int, Sphere*> all_spheres = dc->getAllSpheres();
	map<int, Sphere*>::iterator s_iter;
	double weight = 0;
	double anti_weight = 0;
	point_t begin = solution.getBegin();
	point_t direction = solution.getDirection();
	int n_val1 = 0;
	int n_val2 = 0;
	int n_b_1_spheres = 0;

	// * cout << "begin = " << begin << endl;

	for (s_iter = all_spheres.begin(); s_iter != all_spheres.end(); s_iter++) {
		Sphere *sphere = (*s_iter).second;
		point_t center = sphere->getCenter();
		double r = sphere->getRadius();
		
		if (sphere->getBrokerId() == 1)
			n_b_1_spheres++;
		// check begin of the solution
		double length = 0;
		double r_2 = r * r;
		int i;
		for (i = 0; i < dc->getDim(); i++)
			length += (center[i] - begin[i]) * (center[i] - begin[i]);
//		if (length < r_2) { // inside
//			n_val1++;
//			if (sphere->getBrokerId() == 1)
//				weight += sphere->getWeight();
//			else 
//				anti_weight += sphere->getWeight();
		bool inside = false;
		if (equal(length, r_2)) {
			n_val2++;
			CharacteristicVector *cv =
				sphere->getCharacteristicVector(begin);
			if (solution.angle(*cv) < 2) { // inside
				inside = true;
//				n_val2++;
			} 
			delete cv;
		} else if (length < r_2) { // inside
			inside = true;
			n_val1++;
		}
		if (inside) {
			if (sphere->getBrokerId() == 1)
				weight += sphere->getWeight();
			else 
				anti_weight += sphere->getWeight();
		}
 
	}
	cout << "Test: weight = " << weight << " anti_weight = " << anti_weight <<
		" n_val1 = " << n_val1 << " n_val2 = " << n_val2 << 
		" n_b_1_spheres = " << n_b_1_spheres << endl;
	if (weight >= dc->getOverload() && (weight + anti_weight) <= dc->getMaxLoad())
		return true;
	else
		return false;
};

bool Utilities::testTransSolution(CharacteristicVector& solution) {
	const double SCALE = 0.0001;
	point_t begin = solution.getBegin();
	point_t direction = solution.getDirection();
	point_t trans_begin = new double[dc->getDim()];
	point_t zero_vector = new double[dc->getDim()];

	for (int i = 0; i < dc->getDim(); i++) {
		trans_begin[i] = begin[i] + direction[i] * SCALE;
		zero_vector[i] = 0;
	}
	CharacteristicVector trans_solution(trans_begin, zero_vector);

	return testSolution(trans_solution);
};

void Utilities::generateRandomData(int n_spheres, int n_brokers, const char 
		*file_name) {
//	const int MAX_COORDINATE_VALUE = 1000000;
	const int MIN_LOAD = 1; // minimal load produced by a server
	const int MAX_LOAD = 10; // maximal load produced by a server
	const int SEED = 8763;
	const int d = 6;
	const double overload = 100.0;
	const double max_load = 500.0;
//	const int n_brokers = 100;
//	const int n_spheres = 50000;

	// * cout << n_spheres << " " << n_brokers << endl;
	fflush(NULL);

	ofstream output(file_name, ios::out);
    if ((output.rdstate() & ofstream::failbit ) != 0) {
        cerr << "Error oppening file: " << file_name << endl;
        return;
    }
//	output << setprecision(PRECISION);
	output << d << endl;
	output << overload << " " << max_load << endl;
	srand(SEED);

	int i, j, k, l;
	bool passed;
	// generate broker locations
	output << n_brokers << endl;
	double broker_locations[n_brokers][d];
	for (i = 0; i < n_brokers;) {
		for (j = 0; j < d; j++)
			broker_locations[i][j] = randomRange(-MAX_COORDINATE_VALUE, 
				MAX_COORDINATE_VALUE);

		// for (j = 0; j < d; j++)
		// 	cout << broker_locations[i][j] << " ";
		// cout << endl;

		// check if such point already exists
		if (i == 0)
			passed = true;
		else {
			passed = false;
			for (k = 0; k < i; k++)
				for (j = 0; j < d; j++)
					if (broker_locations[i][j] != broker_locations[k][j]) {
						j = d; // break
						k = i;
						passed = true;
					}
		}
		if (passed) {
			for (l = 0; l < d; l++)
				output << broker_locations[i][l] << " ";
			output << endl;
			i++;
		}
	}

	// generate spheres
	double loads[n_brokers]; // loads of the brokers
	int n_b_spheres[n_brokers];
	for (i = 0; i < n_brokers; i++) {
		loads[i] = 0;
		n_b_spheres[i] = 0;
	}
	output << n_spheres << endl;
	double *centers = new double[n_spheres * d];
	for (i = 0; i < n_spheres;) {
		for (j = 0; j < d; j++)
			centers[i * d + j] = randomRange(-MAX_COORDINATE_VALUE, 
				MAX_COORDINATE_VALUE);
		// check if the sphere is distinct
		if (i == 0)
			passed = true;
		else {
			passed = false;
			for (k = 0; k < i; k++)
				for (j = 0; j < d; j++)
					if (centers[i * d + j] != 
							centers[k * d + j]) {
						// find broker
						passed = true;
						j = d; // break
						k = i;
					}
		}
		if (passed) { // find the closest broker
			double min_dist = -1;
			int min_index;
			for (k = 0; k < n_brokers; k++) {
				double dist = 0;
				for (j = 0; j < d; j++)
					dist += (centers[i * d + j] - 
						broker_locations[k][j]) * 
						(centers[i * d + j] - broker_locations[k][j]);
				if ((min_dist == -1) || (min_dist > dist)) {
					min_dist = dist;
					min_index = k;
				}
			}
			output << min_index + 1 << " ";
			int load = randomRange(MIN_LOAD, MAX_LOAD);
			output << load << " ";
			loads[min_index] += load;
			n_b_spheres[min_index]++;
			output << sqrt(min_dist) << " ";
			for (j = 0; j < d; j++)
				output << centers[i * d + j] << " ";
			output << endl;
			i++;
		}
	}
	for (i = 0; i < n_brokers; i++) 
		cout << "broker " << i + 1 << " " << n_b_spheres[i] << " : " << 
			loads[i] << endl;
	output.close();
};
/*
/////////////////////////////////////////////////////////////////////////////
////////////////////////////////// nain /////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////
void setMarkerTest(void) {
	const int N = 6;
	const int a[N] = {1, 2, 3, 4, 5, 6};
	
	id_set_t id1 = id_set_t(a, a + N);
	id_set_t id2 = id_set_t(a, a + N);
	
	id_set_t::iterator iter;

	for (iter = id1.begin(); iter != id1.end(); ++iter)
		cout << *iter << " ";
	cout << "\n";
	SetMarker sm; 
	sm.mark(id1);
	id2 = id1;
	cout << sm.marked(id1) << "\n";
	id2[5] = 10;
	cout << sm.marked(id1) << "\n";
	cout << sm.marked(id2) << "\n";

}

void setSequencerTest(void) {
	SetMarker sm;
	RandomizedSetSequencer ss(6, 10000);

	for (int i = 0; i < 10; i++) {
		id_set_t id = ss.getNext(sm);
		cout << id << endl;
//		id_set_t::iterator iter;

//		for (iter = id.begin(); iter != id.end(); ++iter)
//        	cout << *iter << " ";
//		cout << "\n";
		sm.mark(id);
	}
	cout << sm << endl << ss << endl;
}

void dataContainerTest(const char* data_file_name_p) {
	dc->loadFromFile(data_file_name_p);
	dc->limitNumberOfSpheres();
	cout << dc << endl;
}

void matrixTest() {
	double data[3 * 4] = {112, 11, 23, 111,
						  1231, 22, 11, 1123, 
						  1232, 11, 31, 1};
	Matrix mat(3, 4, data);
	cout << mat;
	bool *ignore_cols = mat.transformToR();
	cout << mat;
	point_t x = mat.solve(ignore_cols);
	cout << "x = (";
	for (int i = 0; i < 3; i++)
		cout << x[i] << " ";
	cout << ")" << endl;
	delete[] ignore_cols;
	delete[] x;
};

void planeTest() {
	double d[4] = {1, 1, 0, 0};
	double *data = new double[4];
	memcpy(data, d, sizeof(double) * 4);

	Matrix mat(1, 4, data);	
	Plane p(mat);
	cout << p << endl;
	point_t pt = new double[3];
	pt[0] = 0;
	pt[1] = 1;
	pt[2] = 0;
	point_t* projection = p.project(pt);
	if (projection == NULL)
		cout << "no solution" << endl;
	else {
		point_t x = projection[0];
		point_t base_vector = projection[1];
		cout << "x = (";
		for (int i = 0; i < 3; i++)
			cout << x[i] << " ";
		cout << ")" << endl;
		cout << "b_v = (";
		for (int i = 0; i < 3; i++)
			cout << base_vector[i] << " ";
		cout << ")" << endl;
		
		delete[] x;
		delete[] base_vector;
		delete[] projection;
	}
	delete[] pt;
};

void sphereIntersectionTest() {
	Sphere* s1 = dc->getSphere(1);
	Sphere* s2 = dc->getSphere(2);
	Sphere* s3 = dc->getSphere(3);

	cout << *s1 << endl << *s2 << endl << *s3 << endl;

	SphereIntersection si(*s1);
	si.intersect(*s2);
	si.intersect(*s3);
//	cout << "dupa" << endl;
//	fflush(NULL);
//	si.intersect(*s3);
	point_t *points = si.getPoints();
//	cout << "dupa" << endl;
//	fflush(NULL);
	cout << points[0];
	cout << points[1];
	cout << s1->contains(points[0]) << " " << s1->contains(points[1]) << endl;
	cout << s2->contains(points[0]) << " " << s2->contains(points[1]) << endl;
	cout << s3->contains(points[0]) << " " << s3->contains(points[1]) << endl;
	delete[] points[0];
	delete[] points[1];
	delete[] points;
};

double estimateError() {
	double max_error = 0.0;
	map<int, Sphere*> all_spheres = dc->getAllSpheres();
	map<int, Sphere*>::iterator iter;
	int n_errors = 0;

	for (iter = all_spheres.begin(); iter != all_spheres.end(); iter++) {
		Sphere *sphere = (*iter).second;
		int broker_id = sphere->getBrokerId();
		point_t broker_loc = dc->getBrokerLocation(broker_id);
		point_t center = sphere->getCenter();
		double error = 0;
		double distance = 0;
		double r = sphere->getRadius();

		r *= r;

		for (int i = 0; i < dc->getDim(); i++)
			distance += (center[i] - broker_loc[i]) * 
				(center[i] - broker_loc[i]);
		error = r - distance;
		if (!equal(r, distance))
			n_errors++;
		if (error < 0)
			error = -error;
		if (error > max_error)
			max_error = error;
	}
	cout << "Max error = " << max_error << " n_errors = " << n_errors << endl;
};

int nain(int argc, char *argv[]) {
//	double ala = 10.12312313;
	cout << setprecision(PRECISION);
//	cout << ala << endl;
//	cout << ala << endl;
//	exit(-1);

//	for (int i = 0; i < 100; i++)
//		cout << Utilities::randomNormalRange(-10, 10, 0, 5) << endl;
//	return -1;

	if (((argc != 4) && (argc != 5)) ||
		(strcmp("-g", argv[1]) * strcmp("-s", argv[1]) != 0) ||
		((argc == 4) && (strcmp("-s", argv[1]) != 0)) ||
		((argc == 5) && (strcmp("-g", argv[1]) != 0)) ||
		((argc == 4) && (strcmp("-d", argv[2]) * strcmp("-r", argv[2]) != 0))){
		cerr << "usage: " << argv[0] << 
			" [-g <n_spheres> <n_brokers>|-s [-d|-r]] <data_file>\n";
		exit(-1);
	}

//	srand(static_cast<unsigned>(time(0)));
//	cout << rand() << " " << rand() << " " << 
//		Utilities::randomRange(-10, 10) << endl;
//	return 1;

	Timer t;
	struct timeval tv;
	t.start();
//	setSequencerTest();

	if (strcmp("-g", argv[1]) == 0) { // generate
		Utilities::generateRandomData(atoi(argv[2]), atoi(argv[3]), argv[4]);
	} else  { // solve
		Timer t1;
		t1.start();
		dc->loadFromFile(argv[3]);
//		broker_1_location = dc->getBrokerLocation(1);
		tv = t1.getTime();
    	cout << "DATA LOADING TIME: " << tv.tv_sec << " s " << tv.tv_usec << 
			" ms\n";
		t1.reset();
		t1.start();
		dc->limitNumberOfSpheres();
		tv = t1.getTime();
    	cout << "PREPROCESSING TIME: " << tv.tv_sec << " s " << tv.tv_usec << 
			" ms\n";
		estimateError();

		double *p1 = new double[dc->getDim()];
		double *d1 = new double[dc->getDim()];
		int i;
		for (i = 0; i < dc->getDim(); i++) {
			p1[i] = dc->getBrokerLocation(1)[i];
			d1[i] = 0;
		}
		CharacteristicVector cv1(p1, d1);
		cout << "Test: " << Utilities::testSolution(cv1) << endl;
//		cout << "Test: " << Utilities::testSolution(cv1) << endl;

		if (dc->testConsistency())
			cout << "Consistency test passed" << endl;
		else {
			cout << "Consistency test failed" << endl;
			exit(-1);
		}
//		cout << dc;i
		t1.reset();
		t1.start();
		CharacteristicVector *solution;
		if (strcmp("-d", argv[2]) == 0)
			solution = Utilities::solve();
		else
			solution = Utilities::solveRandom();
		tv = t1.getTime();
    	cout << "SOLVING TIME: " << tv.tv_sec << " s " << tv.tv_usec << 
			" ms\n";
		cout << "Test: " << Utilities::testTransSolution(*solution) << endl;
//		for (i = 0; i < dc->getDim(); i++) {
//			if ((solution->getBegin())[i] != p1[i])
//				cout << (solution->getBegin())[i] << " != " << p1[i] << 
//					" difference = " << 
//					equal((solution->getBegin())[i], p1[i]) << endl;
//		}
		delete solution;	
		// cout << "Test: " << Utilities::testSolution(cv1) << endl;
//		cout << dc << endl;
	}

//	setSequencerTest();
//	dataContainerTest(argv[1]);
//	matrixTest();
//	planeTest();
//	sphereIntersectionTest();

//   list<int> L;
//   L.push_back(0);              // Insert a new element at the end
//   L.push_front(0);             // Insert a new element at the beginning
//   L.insert(++L.begin(),2);     // Insert "2" before position of first argument
                                // (Place before second argument)
//   L.push_back(5);
//   L.push_back(6);

//   list<int>::iterator i;

//   for(i=L.begin(); i != L.end(); ++i) cout << *i << " ";
//   cout << endl;
   
	tv = t.getTime();
	cout << "TOTAL TIME: " << tv.tv_sec << " s " << tv.tv_usec << " ms\n";
	return 0;
}
*/