
#include "ibex.h"

using namespace std;

using namespace ibex;

/*================== data =================*/
const int N=10;                // number of beacons
const int T=3;                 // number of time steps
const double L=10;             // limit of the environment (the robot is in the area [0,L]x[0,L])
const double BEACON_ERROR=0.1; // the uncertainty on each beacon position
const double DIST_ERROR=0.1;   // the uncertainty on each measured distance
const int NB_OUTLIERS=2;       // maximal number of outliers per time units
IntervalMatrix beacons(N,2);   // positions (x,y) of the N beacons
IntervalMatrix dist(T,N);      // dist(t,i)=distance between the position at time t of the robot and the ith beacon
Matrix v(T-1,2);               // v(t)=(dx,dy), the difference vector between the positions at time (t+1) and t.
/*=========================================*/

// init data (simulate measurements)
void init_data() {

	srand(1111);

	cout << "************** beacons **************" << endl;
	for (int b=0; b<N; b++) {
		// set beacons exact positions
		// remark: we don't put uncertainty right now because we
		// need the exact values to calculate the distances.
		beacons[b][0]=((double) rand()/RAND_MAX)*L;
		beacons[b][1]=((double) rand()/RAND_MAX)*L;
		cout << "beacon n°" << b <<  " x=" << beacons[b][0] << " y=" << beacons[b][1] << endl;
	}
	cout << endl;

	// generate the initial position
	// of the robot randomly
	double x1=((double) rand()/RAND_MAX)*L;
	double y1=((double) rand()/RAND_MAX)*L;

	cout << "************** robot **************" << endl;

	for (int t=0; t<T; t++) {

		cout << "x[" << t << "]=" << x1 << " y[" << t << "]=" << y1 << endl ;

		// for each beacon, a measurement is generated from the position
		// of the robot at time t
		for (int b=0; b<N; b++) {
			// compute the "exact" distance
			dist[t][b]=sqrt(pow(beacons[b][0]-x1,2)+pow(beacons[b][1]-y1,2));
			// add the uncertainty to the distances
			dist[t][b]+=DIST_ERROR*Interval(-1,1);
		}

		// generate a movement of length 1
		// we take randomly an angle between 0 and 2*pi
		double alpha=((double) rand()/RAND_MAX)*2*3.14159265359;

		if (t<T-1) {
			v[t][0]=cos(alpha);
			v[t][1]=sin(alpha);
			x1=x1+v[t][0];
			y1=y1+v[t][1];
		}
	}

	// add the uncertainty on the beacons
	for (int i=0; i<N; i++) {
		beacons[i][0] += BEACON_ERROR*Interval(-1,1);
		beacons[i][1] += BEACON_ERROR*Interval(-1,1);
	}
}


/* Contractor for the distance
 * with the beacon n°i at time t. */
class Distance : public Ctc {
public:
	// the contractor expects a box with only 2
	// components coresponding to the positio	n x
	// and y of the robot.
	Distance(int t, int i) : Ctc(2), t(t), i(i) {
		Variable p(2);
		Variable q(2);
		distf = new Function(p,q,sqrt(sqr(p[0]-q[0])+sqr(p[1]-q[1])));
		projx = new Function(p,q,p);
		projy = new Function(p,q,q);
	}

	void contract(IntervalVector& x) {

		// TODO

	}

	// time
	int t;
	// beacon number
	int i;

	Function* distf;
	Function* projx;
	Function* projy;
};

		// TODO

int main() {

	init_data();

	/* =================== test question 1 =================== */
//	Distance d(0,0);
//	IntervalVector x0(2);
//	d.contract(x0);
//	cout << "x0=" << x0 << endl;
//	// with the default values of the constants
//	// you must obtain
//	//           x0=([2.089732641020936, 10.79708888083813] ; [-0.2445307686226208, 8.462825471194575])
	/* ======================================================= */

	// the initial box [0,L]x[0,L]x[0,L]x[0,L]
	IntervalVector box(T*2,Interval(0,L));

	/* =================== test question 2 =================== */
//	OneMove om(0);
//	om.contract(box);
//	cout << "x1=(" << box[2] << " , " << box[3] << ")" << endl;
//	// with the default values of the constants
//	// you must obtain
//	//           x1=([0, 9.433985116047563] , [0.8243950213000487, 10])
	/* ======================================================= */

	/* =================== test question 3 =================== */
//	QInter q(0);
//	q.contract(box);
//	cout << "x0=(" << box[0] << " , " << box[1] << ")" << endl;
//	// with NB_OUTLIERS=1
//	// you must obtain
//	//         x0=([6.774804703102764, 9.0361385155613] , [0.2759856332458481, 1.822368469509435])
//	// with NB_OUTLIERS=2
//	// you must obtain
//	//         x0=([6.704227453452149, 9.204264809102019] , [0.04155481732295302, 2.402844777762428])
	/* ======================================================= */

	// TODO

	// cleanup
	return 0;

}
