#include "yarp.h"

simulator::simulator(){
	worldPort.open("/sim/world");
	Network::connect("/sim/world", "/icubSim/world");

	world_client.open("/sim/worldrpc");
	world_client.addOutput("/icubSim/world");
	
	// touch senzory
	port_rhand_touch.open("/sim/rhand_touch");
	Network::connect("/icubSim/skin/right_hand", "/sim/rhand_touch");	
	
	// right cam

	cam_right.open("/sim/image/in");  // give the port a name

	initRobotDriver();
	
	objectsPos.x = 0.040;
	objectsPos.y = 0.5;
	objectsPos.z = 0.338;

	objectGap = -0.142;
	middleShift = 0.03;

	objects = gsl_vector_calloc(3);
	gsl_vector_set(objects, 0, SHAPE_CYL);
	gsl_vector_set(objects, 1, SHAPE_SPH);
	gsl_vector_set(objects, 2, SHAPE_BOX);

	colors = gsl_vector_calloc(3);
	gsl_vector_set(colors, 0, COLOR_BLUE);
	gsl_vector_set(colors, 1, COLOR_GREEN);
	gsl_vector_set(colors, 2, COLOR_RED);

	model_sph = -1;
	model_cyl = -1;
	model_count = 0;
}

PolyDriver *simulator::createRobotDriver(int part){
	Property params;

	std::string robotName = params.find("robot").asString().c_str();
	std::string remotePorts = "/";
	remotePorts += "icubSim";
	remotePorts += getPortName(part);

	std::string localPorts = "/sim";
	localPorts += getPortName(part);

	Property options;
	options.put("device", "remote_controlboard");
	options.put("local", localPorts.c_str());   //local port names
	options.put("remote", remotePorts.c_str());         //where we connect to

	// create a device
	PolyDriver *robotDevice = new PolyDriver(options);
	if (!robotDevice->isValid()) {
		printf("Device not available.  Here are the known devices:\n");
		printf("%s", Drivers::factory().toString().c_str());
		return NULL;
	}

	//-------------
	IPositionControl *pos;

	bool ok;
	ok = robotDevice->view(pos);

	if (!ok) {
		printf("Problems acquiring interfaces\n");
		return NULL;
	}

	int nj=0;
	pos->getAxes(&nj);
	Vector tmp;
	tmp.resize(nj);


	for (int i = 0; i < nj; i++) {
		tmp[i] = 20.0;	
	}
	pos->setRefAccelerations(tmp.data());

	for (int i = 0; i < nj; i++) {
		tmp[i] = 20.0;
		pos->setRefSpeed(i, tmp[i]);
	}
/**/
	//pos->setRefSpeeds(tmp.data());
//------------

	return robotDevice;
}

void simulator::initRobotDriver(){
	for(int i = 0; i < 4; i++){
		robotDriver[i] = NULL;
	}

	for(int i = 0; i < 4; i++){
		if(i != PART_LEFT_ARM)
			robotDriver[i] = createRobotDriver(i);
	}
}

void simulator::closeRobotDriver(){
	for(int i = 0; i<4; i++){
		if(robotDriver[i] != NULL){
			robotDriver[i]->close();
		}
	}
}

simulator::~simulator(void){
	//unblockArm();
	closeRobotDriver();
	port_rhand_touch.close();

	gsl_vector_free(colors);
	gsl_vector_free(objects);
}
// permutacie 3 prvkov
gsl_matrix *simulator::get3permutations(){
	gsl_matrix *result = gsl_matrix_calloc(6, 3);
	int row = 0;
	for(int p1 = 0; p1 < 3; p1++){
		for(int p2 = 0; p2 < 3; p2++){
			for(int p3 = 0; p3 < 3; p3++){
				if((p1 != p2) && (p1 != p3) && (p2 != p3)){
					gsl_matrix_set(result, row, 0, p1);
					gsl_matrix_set(result, row, 1, p2);
					gsl_matrix_set(result, row, 2, p3);
					row++;
				}
			}	
		}
	}
	//print_matrix(result);
	return result;
}

gsl_vector *simulator::image2vector(IplImage *image){
	gsl_vector *result = gsl_vector_alloc(image->width * image->height);
	int k = 0;
	for(int i = 0; i < image->height; i++) {
		for(int j = 0; j < image->width; j++){ 
			if((unsigned char)image->imageData[i * image->widthStep + j] > 100){
				gsl_vector_set(result, k, 1.0);
			}else{
				gsl_vector_set(result, k, 0.0);
			}
			k++;
		}
	}
	return result;
}

void simulator::showImage(IplImage *image){
		cvShowImage("Image", image);
		cvWaitKey(2000);
		cvDestroyWindow("Image");
}
IplImage *simulator::getCamImage(){
// len uroby fixny orez
	Network::connect("/icubSim/cam/right", "/sim/image/in");
	ImageOf<PixelRgb> *yarpImage = cam_right.read();  // read an image

	if (yarpImage != NULL) { // check we actually got something

		IplImage *cvImage = cvCreateImage(cvSize(yarpImage->width(), yarpImage->height()), IPL_DEPTH_8U, 3 );
		cvCvtColor((IplImage*)yarpImage->getIplImage(), cvImage, CV_RGB2BGR);

		IplImage* cropped = cvCreateImage(cvSize(270, 135), cvImage->depth, cvImage->nChannels);
		cvSetImageROI(cvImage, cvRect(40, 57, 270, 135 ));
		
		cvCopy(cvImage, cropped);
		cvResetImageROI(cvImage);

		IplImage *resized = cvCreateImage(cvSize(60, 30), cropped->depth, cropped->nChannels);
		cvResize(cropped, resized, CV_INTER_NN);

		IplImage *gray = cvCreateImage(cvSize(resized->width,resized->height), IPL_DEPTH_8U, 1);
		cvCvtColor(resized, gray, CV_BGR2GRAY);

		IplImage *edge = 0;
		edge = cvCreateImage(cvSize(gray->width,gray->height), IPL_DEPTH_8U, 1);

		float threshold = 30;//28
		cvCanny(gray, edge, (float)threshold, (float)threshold*1.6, 3); 
	
		//cvShowImage("hrany", edge);
		//cvShowImage("cvimage", gray);
		//cvSaveImage("file.jpg", edge);

		//cvWaitKey(5000);
		//cvDestroyWindow("cvimage");
		//cvDestroyWindow("hrany");

		cvReleaseImage(&cvImage);
		cvReleaseImage(&cropped);
		cvReleaseImage(&resized);
		cvReleaseImage(&gray);
		//cvReleaseImage(&edge);
		Network::disconnect("/icubSim/cam/right", "/sim/image/in");
		return edge;
	}
	Network::disconnect("/icubSim/cam/right", "/sim/image/in");
	return 0;
}

IplImage *simulator::getCamObjectImage(){
// vystrihne objekty zo stola a detekuje hrany
	Network::connect("/icubSim/cam/right", "/sim/image/in");
	ImageOf<PixelRgb> *yarpImage = cam_right.read();  // read an image

	if (yarpImage != NULL) { // check we actually got something

		IplImage *img = cvCreateImage(cvSize(yarpImage->width(), yarpImage->height()), IPL_DEPTH_8U, 3 );
		cvCvtColor((IplImage*)yarpImage->getIplImage(), img, CV_RGB2BGR);

		IplImage *objects = getObjectImage(img);

		//cvDestroyAllWindows();
		cvShowImage("objekty", objects);
		cvWaitKey(1000);
		cvReleaseImage(&img);

		Network::disconnect("/icubSim/cam/right", "/sim/image/in");
		return objects;
	}
	Network::disconnect("/icubSim/cam/right", "/sim/image/in");
	return 0;
}
void simulator::lookAtTable(){
	gsl_vector *v = gsl_vector_calloc(5);
	gsl_vector_set(v, 0, 0);
	gsl_vector_set(v, 1, 0);
	gsl_vector_set(v, 2, 0);
	gsl_vector_set(v, 3, -30);
	gsl_vector_set(v, 4, 8);

	part_set(v, PART_HEAD);

	gsl_vector_free(v);

	v = gsl_vector_alloc(3);
	gsl_vector_set(v, 0, 0);
	gsl_vector_set(v, 1, 0);
	gsl_vector_set(v, 2, 20);
	
	part_set(v, PART_TORSO);

	gsl_vector_free(v);

}



void simulator::createTable(){
	world_mk_sbox(1.2, 0.02, 0.92, -0.1, 0.42, 0.73, 1.0, 1.0, 0.0);
}
void simulator::unblockArm(){
	world_del_all();
	initArmStraight(PART_RIGHT_ARM);
	Sleep(200);
	createTable();
	lookAtTable();
}

void simulator::part_set(gsl_vector* v, int part){
	if((v == NULL) || (v->size <= 0)){
		printf("ERROR: EMPTY VECTOR(part_set)");
		return;
	}

	// get device
	PolyDriver *robotDevice = robotDriver[part];


	IPositionControl *pos;
	IEncoders *encs;

	bool ok;
	ok = robotDevice->view(pos);
	ok = ok && robotDevice->view(encs);

	if (!ok) {
		printf("Problems acquiring interfaces\n");
		return;
	}

	int nj=0;
	pos->getAxes(&nj);
	Vector encoders;
	Vector command;
	Vector tmp;
	encoders.resize(nj);
	tmp.resize(nj);
	command.resize(nj);
/*
	int i;
	for (i = 0; i < nj; i++) {
		tmp[i] = 90.0;
	}
	pos->setRefAccelerations(tmp.data());

	for (i = 0; i < nj; i++) {
		tmp[i] = 90.0;
		pos->setRefSpeed(i, tmp[i]);
	}
*/
	//pos->setRefSpeeds(tmp.data()))

	//fisrst zero all joints

	command=0;

	for (unsigned int i = 0; i < v->size; i++) {
		command[i] = gsl_vector_get(v, i);
	}

	pos->positionMove(command.data());

	bool done=false;

	while(!done)
	{
		pos->checkMotionDone(&done);
		Time::delay(0.1);
	}

//	robotDevice.close();
}

void simulator::arm_set(gsl_vector* v, int part){
	if((v == NULL) || (v->size <= 0)){
		printf("ERROR: EMPTY VECTOR(arm_set)");
		return;
	}

	// get device
	PolyDriver *robotDevice = robotDriver[part];

	IPositionControl *pos;
	IEncoders *encs;

	bool ok;
	ok = robotDevice->view(pos);
	ok = ok && robotDevice->view(encs);

	if (!ok) {
		printf("Problems acquiring interfaces\n");
		return;
	}

	int nj=0;
	pos->getAxes(&nj);
	Vector encoders;
	Vector command;
	Vector prevEncoders;
	Vector tmp;
	encoders.resize(nj);
	prevEncoders.resize(nj);
	tmp.resize(nj);
	command.resize(nj);

/*
	for (int i = 0; i < nj; i++) {
		tmp[i] = 90.0;
	}
	pos->setRefAccelerations(tmp.data());

	for (int i = 0; i < nj; i++) {
		tmp[i] = 90.0;
		pos->setRefSpeed(i, tmp[i]);
	}

	pos->setRefSpeeds(tmp.data());
*/

	//print_vector(v);
	//fisrst zero all joints
	//now then the shoulder to some value
	command=0;
	for (unsigned int i = 0; i < v->size; i++) {
		command[i] = gsl_vector_get(v, i);
	}

	pos->positionMove(command.data());

	bool done = false;

	// wait until done
	while(!done)
	{
		encs->getEncoders(encoders.data());
		memcpy(prevEncoders.data(), encoders.data(), nj * sizeof(double));		
		Time::delay(0.05);
		pos->checkMotionDone(&done);
		encs->getEncoders(encoders.data());
		double diff = 0;
		for(int i = 0; i < nj; i++){
			diff += abs(prevEncoders[i] - encoders[i]);
		}
		//printf("%.30f \n",(diff / (double)nj));
		done = done || ((diff / (double)nj) < 0.001);
	}

}


gsl_vector* simulator::arm_get(int part, int nJoints){
	if(nJoints <= 0){
		printf("ERROR: nJoints <= VECTOR(arm_get)");
		return NULL;
	}

	gsl_vector* v;

	// get device
	PolyDriver *robotDevice = robotDriver[part];


	IPositionControl *pos;
	IEncoders *encs;

	bool ok;
	ok = robotDevice->view(pos);
	ok = ok && robotDevice->view(encs);

	if (!ok) {
		printf("Problems acquiring interfaces\n");
		return 0;
	}

	int nj=0;
	pos->getAxes(&nj);
	Vector encoders;
	encoders.resize(nj);

	if(nJoints == 0){
		nJoints = nj;
	}

	encs->getEncoders(encoders.data());
	
	if(nJoints <= 0){
		printf("ERROR: JOINTS COUNT(arm_get)");
		return NULL;
	}

	v = gsl_vector_alloc(nJoints);
	for(unsigned int i = 0; i < v->size; i++){
		gsl_vector_set(v, i, encoders[i]);
	}

	//robotDevice.close();
	return v;
}

int simulator::rhand_touch(){
    Bottle bot;

	port_rhand_touch.read(bot);
	bot.get(0).asInt();
	//printf("%d", bot.pop().asInt());
	return min(1, bot.get(0).asInt()); //bot.find("rhan").asInt(); //bot.find("rhan").asInt();
}

void simulator::world_set(char *type, double box_id, double xpos, double ypos, double zpos) {
	Bottle objectBot;  
	objectBot.clear();
	objectBot.addString("world");
	objectBot.addString("set");
	objectBot.addString(type);
	if(box_id != 0)
		objectBot.addDouble(box_id);
	objectBot.addDouble(xpos);
	objectBot.addDouble(ypos);
	objectBot.addDouble(zpos);  
	worldPort.write(objectBot);
} 

void simulator::world_mk_box(double sx, double sy, double sz, double px, double py, double pz, double r, double g, double b) {
	Bottle objectBot;  
	objectBot.clear();
	objectBot.addString("world");
	objectBot.addString("mk");
	objectBot.addString("box");

	objectBot.addDouble(sx);
	objectBot.addDouble(sy);
	objectBot.addDouble(sz); 

	objectBot.addDouble(px);
	objectBot.addDouble(py);
	objectBot.addDouble(pz); 

	objectBot.addDouble(r);
	objectBot.addDouble(g);
	objectBot.addDouble(b);  

	worldPort.write(objectBot);
} 

void simulator::world_mk_sbox(double sx, double sy, double sz, double px, double py, double pz, double r, double g, double b) {
	Bottle objectBot;  
	objectBot.clear();
	objectBot.addString("world");
	objectBot.addString("mk");
	objectBot.addString("sbox");

	objectBot.addDouble(sx);
	objectBot.addDouble(sy);
	objectBot.addDouble(sz); 

	objectBot.addDouble(px);
	objectBot.addDouble(py);
	objectBot.addDouble(pz); 

	objectBot.addDouble(r);
	objectBot.addDouble(g);
	objectBot.addDouble(b);  

	worldPort.write(objectBot);
} 
void simulator::world_mk_sph(double sr, double px, double py, double pz, double r, double g, double b) {
	Bottle objectBot;  
	objectBot.clear();
	objectBot.addString("world");
	objectBot.addString("mk");
	objectBot.addString("sph");

	objectBot.addDouble(sr);

	objectBot.addDouble(px);
	objectBot.addDouble(py);
	objectBot.addDouble(pz); 

	objectBot.addDouble(r);
	objectBot.addDouble(g);
	objectBot.addDouble(b);  

	worldPort.write(objectBot);
} 

void simulator::world_mk_cyl(double sr, double sl, double px, double py, double pz, double r, double g, double b) {
	Bottle objectBot;  
	objectBot.clear();
	objectBot.addString("world");
	objectBot.addString("mk");
	objectBot.addString("cyl");

	objectBot.addDouble(sr);
	objectBot.addDouble(sl);

	objectBot.addDouble(px);
	objectBot.addDouble(py);
	objectBot.addDouble(pz); 

	objectBot.addDouble(r);
	objectBot.addDouble(g);
	objectBot.addDouble(b);  

	worldPort.write(objectBot);
} 
void simulator::world_mk_model_cyl(double sr, double sl, double px, double py, double pz, double r, double g, double b) {
Bottle objectBot;  
	objectBot.clear();
	objectBot.addString("world");
	objectBot.addString("mk");
	objectBot.addString("model");
	objectBot.addString("cyl.x");

	if(r == 1){
		objectBot.addString("red.bmp");
	}
	if(g == 1){
		objectBot.addString("green.bmp");
	}
	if(b == 1){
		objectBot.addString("blue.bmp");
	}	

	objectBot.addDouble(px);
	objectBot.addDouble(py);
	objectBot.addDouble(pz); 
/*
	printf(objectBot.toString());
	printf("\n");
*/
	worldPort.write(objectBot);

	model_count++;
	model_cyl = model_count;
} 
void simulator::world_mk_model_sph(double sr, double px, double py, double pz, double r, double g, double b) {
	Bottle objectBot;  
	objectBot.clear();
	objectBot.addString("world");
	objectBot.addString("mk");
	objectBot.addString("model");
	objectBot.addString("ball.x");

	if(r == 1){
		objectBot.addString("red.bmp");
	}
	if(g == 1){
		objectBot.addString("green.bmp");
	}
	if(b == 1){
		objectBot.addString("blue.bmp");
	}	

	objectBot.addDouble(px);
	objectBot.addDouble(py);
	objectBot.addDouble(pz); 
/*
	printf(objectBot.toString());
	printf("\n");
*/
	worldPort.write(objectBot);

	model_count++;
	model_sph = model_count;
} 




void simulator::world_rot_cyl(int no, double anglex, double angley, double anglez){
	Bottle objectBot;  
	objectBot.clear();
	objectBot.addString("world");
	objectBot.addString("rot");
	objectBot.addString("cyl");

	objectBot.addInt(no);
	objectBot.addDouble(anglex);
	objectBot.addDouble(angley);
	objectBot.addDouble(anglez);

	worldPort.write(objectBot);
} 

void simulator::world_del_all() {
	Bottle objectBot;  
	objectBot.clear();
	objectBot.addString("world");
	objectBot.addString("del");
	objectBot.addString("all");
	worldPort.write(objectBot);
	model_count = 0;
} 

void simulator::moveDefaultObjectsAway(){
	world_set("cube", 0, 2.0, 0.2, 2.0);
	world_set("ball", 0, 2.0, 1.0, 2.0);
}


void simulator::generateDataSet(gsl_matrix **input, gsl_matrix **output){
	gsl_matrix *inputSet = *input = gsl_matrix_calloc(1200 + 9 + 6 + 1, 216); //vstupne data ukladane do stlpcov
	gsl_matrix *outputSet = *output = gsl_matrix_calloc(3, 216); //vystupne data ukladane do stlpcov
	gsl_matrix *objects = get3permutations(); // permutacie objektov
	gsl_matrix *colors = get3permutations(); // permutacie farieb

	print_matrix(colors);
	int column = 0;
	for(unsigned int obj = 0; obj < objects->size1; obj++){
		for(unsigned int color = 0; color < colors->size1; color++){
			unblockArm();			
			//rozmiestni objekty
			//nastavim stavove premenne farieb a objektov
			gsl_matrix_get_row(this->colors, colors, color);
			gsl_matrix_get_row(this->objects, objects, obj);
			//vyrobim objekty na scene
			createObjects();
			
			IplImage *image = getCamObjectImage();

			gsl_vector *imagev = image2vector(image);

			// mame rozmiestnene objekty, vygenerujeme jazykovu instrukciu
			//for(unsigned int action = 0; action < 3; action++){ // aku akciu chceme vykonat
				for(unsigned int target = 0; target < 6; target++){ // ktory objekt je cielovy
					//skopirujeme obrazovu informaciu
					for(unsigned int i = 0; i < imagev->size; i++)
						gsl_matrix_set(inputSet, i, column, gsl_vector_get(imagev, i));
					
					//setting colors
					gsl_matrix_set(inputSet, imagev->size + 3 * 0 + (int)gsl_matrix_get(colors, color, 0), column, 1.0); //farba prvej pozicie
					gsl_matrix_set(inputSet, imagev->size + 3 * 1 + (int)gsl_matrix_get(colors, color, 1), column, 1.0); //farba druhej pozicie
					gsl_matrix_set(inputSet, imagev->size + 3 * 2 + (int)gsl_matrix_get(colors, color, 2), column, 1.0); //farba tretej pozicie
					//setting action
					//gsl_matrix_set(inputSet, imagev->size + 9 + action, column, 1.0); 
					//setting target
					gsl_matrix_set(inputSet, imagev->size + 9 + target, column, 1.0); 
					//bias input
					gsl_matrix_set(inputSet, imagev->size + 15, column, -1.0); 

					// vypocet pozadovaneho vystupu

					if(target < 3){
						for(int j = 0; j < 3; j++){
							if(gsl_matrix_get(objects, obj, j) == target){
								gsl_matrix_set(outputSet, j, column, 1.0);
							}
						}
					} else {
						for(int j = 0; j < 3; j++){
							if(gsl_matrix_get(colors, color, j) == (target - 3)){
								gsl_matrix_set(outputSet, j, column, 1.0);
							}
						}
					}

					column++; // dalsi vstup/stlpec
				}
			//}

			gsl_vector_free(imagev);
			cvReleaseImage(&image);
		}
	}
	
	gsl_matrix_free(colors);
	gsl_matrix_free(objects);
}


void simulator::write_command_desc(FILE *f, gsl_vector *objects, gsl_vector *colors, int target){
	for(int i = 0; i < objects->size; i++){
		switch((int)objects->data[i]){
			case SHAPE_BOX: fprintf(f, "box "); printf("box ");
				break;
			case SHAPE_CYL: fprintf(f, "cyl "); printf("cyl ");
				break;
			case SHAPE_SPH: fprintf(f, "sph "); printf("sph ");
				break;
			default: 
				fprintf(f, "unk "); printf("unk ");
				break;
		}
	}

	for(int i = 0; i < colors->size; i++){
		switch((int)colors->data[i]){
			case COLOR_RED: fprintf(f, "red "); printf("red ");
				break;
			case COLOR_GREEN: fprintf(f, "green "); printf("green ");
				break;
			case COLOR_BLUE: fprintf(f, "blue "); printf("blue ");
				break;
			default:
				fprintf(f, "unk "); printf("unk ");
				break;
		}
	}

	if(target < 3){
		//ask shape
		switch(target){
		case SHAPE_BOX: fprintf(f, "q_box"); printf("q_box ");
			break;
		case SHAPE_CYL: fprintf(f, "q_cyl"); printf("q_cyl ");
			break;
		case SHAPE_SPH: fprintf(f, "q_sph"); printf("q_sph ");
			break;	
		default: fprintf(f, "q_unk"); printf("q_unk ");
			break;
		}
	}else{
	//ask color
		switch(target - 3){
		case COLOR_RED: fprintf(f, "q_red"); printf("q_red ");
			break;
		case COLOR_GREEN: fprintf(f, "q_green"); printf("q_green ");
			break;
		case COLOR_BLUE: fprintf(f, "q_blue"); printf("q_blue ");
			break;	
		default: fprintf(f, "q_unk"); printf("q_unk");
			break;
		}
	}

	fprintf(f, "\n"); printf("\n");
}
void simulator::generateDataSet_target(gsl_matrix **input, gsl_matrix **output){
	FILE *f;
	fopen_s(&f, "command_desc.txt", "w");
	

	int mult = 3; //pocet zopakovania target
	gsl_matrix *inputSet = *input = gsl_matrix_calloc(1200 + 9 + 6 * mult + 1, 216); //vstupne data ukladane do stlpcov
	gsl_matrix *outputSet = *output = gsl_matrix_calloc(3, 216); //vystupne data ukladane do stlpcov
	gsl_matrix *objects = get3permutations(); // permutacie objektov
	gsl_matrix *colors = get3permutations(); // permutacie farieb

	gsl_vector *row_color = gsl_vector_alloc(3);
	gsl_vector *row_object = gsl_vector_alloc(3);

	print_matrix(colors);
	int column = 0;
	for(unsigned int obj = 0; obj < objects->size1; obj++){
		for(unsigned int color = 0; color < colors->size1; color++){
			unblockArm();			
			//rozmiestni objekty
			//nastavim stavove premenne farieb a objektov
			gsl_matrix_get_row(this->colors, colors, color);
			gsl_matrix_get_row(this->objects, objects, obj);
			//vyrobim objekty na scene
			createObjects();
			
			IplImage *image = getCamObjectImage();
			cvReleaseImage(&image);
			image = getCamObjectImage();

			gsl_vector *imagev = image2vector(image);

			// mame rozmiestnene objekty, vygenerujeme jazykovu instrukciu
			//for(unsigned int action = 0; action < 3; action++){ // aku akciu chceme vykonat
				for(unsigned int target = 0; target < 6; target++){ // ktory objekt je cielovy
					//skopirujeme obrazovu informaciu
					for(unsigned int i = 0; i < imagev->size; i++)
						gsl_matrix_set(inputSet, i, column, gsl_vector_get(imagev, i));
					
					//setting colors
					gsl_matrix_set(inputSet, imagev->size + 3 * 0 + (int)gsl_matrix_get(colors, color, 0), column, 1.0); //farba prvej pozicie
					gsl_matrix_set(inputSet, imagev->size + 3 * 1 + (int)gsl_matrix_get(colors, color, 1), column, 1.0); //farba druhej pozicie
					gsl_matrix_set(inputSet, imagev->size + 3 * 2 + (int)gsl_matrix_get(colors, color, 2), column, 1.0); //farba tretej pozicie
					//setting action
					//gsl_matrix_set(inputSet, imagev->size + 9 + action, column, 1.0); 
					//setting target
					for(int j = 0; j < mult; j++){
						gsl_matrix_set(inputSet, imagev->size + 9 + target * mult + j, column, 1.0); 
					}
					

					//bias input
					gsl_matrix_set(inputSet, inputSet->size1 - 1, column, -1.0); 
					
					// vypocet pozadovaneho vystupu

					if(target < 3){
						for(int j = 0; j < 3; j++){
							if(gsl_matrix_get(objects, obj, j) == target){
								gsl_matrix_set(outputSet, j, column, 1.0);
							}
						}
					} else {
						for(int j = 0; j < 3; j++){
							if(gsl_matrix_get(colors, color, j) == (target - 3)){
								gsl_matrix_set(outputSet, j, column, 1.0);
							}
						}
					}
					gsl_matrix_get_row(row_color, colors, color);
					gsl_matrix_get_row(row_object, objects, obj);
					
					write_command_desc(f, row_object, row_color, target);
					column++; // dalsi vstup/stlpec
				}
			//}

			gsl_vector_free(imagev);
			cvReleaseImage(&image);
		}
	}
	
	gsl_matrix_free(colors);
	gsl_matrix_free(objects);
	fclose(f);
	
	gsl_vector_free(row_object);
	gsl_vector_free(row_color);

}


void simulator::generateDataSet_mult(gsl_matrix **input, gsl_matrix **output){
	// s viacnasobnymi farbami a cielom
	FILE *f;
	fopen_s(&f, "command_desc.txt", "w");
	

	int mult_target = 20; //pocet zopakovania target
	int mult_color = 20; //pocet zopakovania farby

	gsl_matrix *inputSet = *input = gsl_matrix_calloc(1200 + 9 * mult_color + 6 * mult_target + 1, 216); //vstupne data ukladane do stlpcov
	gsl_matrix *outputSet = *output = gsl_matrix_calloc(3, 216); //vystupne data ukladane do stlpcov
	gsl_matrix *objects = get3permutations(); // permutacie objektov
	gsl_matrix *colors = get3permutations(); // permutacie farieb

	gsl_vector *row_color = gsl_vector_alloc(3);
	gsl_vector *row_object = gsl_vector_alloc(3);

	print_matrix(colors);
	int column = 0;
	for(unsigned int obj = 0; obj < objects->size1; obj++){
		for(unsigned int color = 0; color < colors->size1; color++){
			unblockArm();			
			//rozmiestni objekty
			//nastavim stavove premenne farieb a objektov
			gsl_matrix_get_row(this->colors, colors, color);
			gsl_matrix_get_row(this->objects, objects, obj);
			//vyrobim objekty na scene
			createObjects();
			
			IplImage *image = getCamObjectImage();
			cvReleaseImage(&image);
			image = getCamObjectImage();

			gsl_vector *imagev = image2vector(image);

			// mame rozmiestnene objekty, vygenerujeme jazykovu instrukciu
			//for(unsigned int action = 0; action < 3; action++){ // aku akciu chceme vykonat
				for(unsigned int target = 0; target < 6; target++){ // ktory objekt je cielovy
					//skopirujeme obrazovu informaciu
					for(unsigned int i = 0; i < imagev->size; i++)
						gsl_matrix_set(inputSet, i, column, gsl_vector_get(imagev, i));
					
					//setting colors
					for(int j = 0; j < mult_color; j++){
						gsl_matrix_set(inputSet, imagev->size + 3 * mult_color * 0 + (int)gsl_matrix_get(colors, color, 0) * mult_color + j, column, 1.0); //farba prvej pozicie
					}
					for(int j = 0; j < mult_color; j++){
						gsl_matrix_set(inputSet, imagev->size + 3 * mult_color * 1 + (int)gsl_matrix_get(colors, color, 1) * mult_color + j, column, 1.0); //farba druhej pozicie
					}
					for(int j = 0; j < mult_color; j++){
						gsl_matrix_set(inputSet, imagev->size + 3 * mult_color * 2 + (int)gsl_matrix_get(colors, color, 2) * mult_color + j, column, 1.0); //farba tretej pozicie
					}

					//setting action
					//gsl_matrix_set(inputSet, imagev->size + 9 + action, column, 1.0); 
					//setting target
					for(int j = 0; j < mult_target; j++){
						gsl_matrix_set(inputSet, imagev->size + 9 * mult_color + target * mult_target + j, column, 1.0); 
					}
					

					//bias input
					gsl_matrix_set(inputSet, inputSet->size1 - 1, column, -1.0); 
					
					// vypocet pozadovaneho vystupu

					if(target < 3){
						for(int j = 0; j < 3; j++){
							if(gsl_matrix_get(objects, obj, j) == target){
								gsl_matrix_set(outputSet, j, column, 1.0);
							}
						}
					} else {
						for(int j = 0; j < 3; j++){
							if(gsl_matrix_get(colors, color, j) == (target - 3)){
								gsl_matrix_set(outputSet, j, column, 1.0);
							}
						}
					}
					gsl_matrix_get_row(row_color, colors, color);
					gsl_matrix_get_row(row_object, objects, obj);
					
					write_command_desc(f, row_object, row_color, target);
					column++; // dalsi vstup/stlpec
				}
			//}

			gsl_vector_free(imagev);
			cvReleaseImage(&image);
		}
	}
	
	gsl_matrix_free(colors);
	gsl_matrix_free(objects);
	fclose(f);
	
	gsl_vector_free(row_object);
	gsl_vector_free(row_color);

}


void simulator::generateDataSet_target_double(gsl_matrix **input, gsl_matrix **output){
	// so zdvojenym vystupom
	
	FILE *f;
	fopen_s(&f, "command_desc.txt", "w");


	int mult = 3; //pocet zopakovania target
	gsl_matrix *inputSet = *input = gsl_matrix_calloc(1200 + 9 + 6 * mult + 1, 216); //vstupne data ukladane do stlpcov
	gsl_matrix *outputSet = *output = gsl_matrix_calloc(4, 216); //vystupne data ukladane do stlpcov
	gsl_matrix *objects = get3permutations(); // permutacie objektov
	gsl_matrix *colors = get3permutations(); // permutacie farieb

	gsl_vector *row_color = gsl_vector_alloc(3);
	gsl_vector *row_object = gsl_vector_alloc(3);

	print_matrix(colors);
	int column = 0;
	for(unsigned int obj = 0; obj < objects->size1; obj++){
		for(unsigned int color = 0; color < colors->size1; color++){
			unblockArm();			
			//rozmiestni objekty
			//nastavim stavove premenne farieb a objektov
			gsl_matrix_get_row(this->colors, colors, color);
			gsl_matrix_get_row(this->objects, objects, obj);
			//vyrobim objekty na scene
			createObjects();
			
			IplImage *image = getCamObjectImage();

			gsl_vector *imagev = image2vector(image);

			// mame rozmiestnene objekty, vygenerujeme jazykovu instrukciu
			//for(unsigned int action = 0; action < 3; action++){ // aku akciu chceme vykonat
				for(unsigned int target = 0; target < 6; target++){ // ktory objekt je cielovy
					//skopirujeme obrazovu informaciu
					for(unsigned int i = 0; i < imagev->size; i++)
						gsl_matrix_set(inputSet, i, column, gsl_vector_get(imagev, i));
					
					//setting colors
					gsl_matrix_set(inputSet, imagev->size + 3 * 0 + (int)gsl_matrix_get(colors, color, 0), column, 1.0); //farba prvej pozicie
					gsl_matrix_set(inputSet, imagev->size + 3 * 1 + (int)gsl_matrix_get(colors, color, 1), column, 1.0); //farba druhej pozicie
					gsl_matrix_set(inputSet, imagev->size + 3 * 2 + (int)gsl_matrix_get(colors, color, 2), column, 1.0); //farba tretej pozicie
					//setting action
					//gsl_matrix_set(inputSet, imagev->size + 9 + action, column, 1.0); 
					//setting target
					for(int j = 0; j < mult; j++){
						gsl_matrix_set(inputSet, imagev->size + 9 + target * mult + j, column, 1.0); 
					}
					

					//bias input
					gsl_matrix_set(inputSet, inputSet->size1 - 1, column, -1.0); 
					
					// vypocet pozadovaneho vystupu

					if(target < 3){
						for(int j = 0; j < 3; j++){
							if(gsl_matrix_get(objects, obj, j) == target){
								gsl_matrix_set(outputSet, j, column, 1.0);
								gsl_matrix_set(outputSet, j+1, column, 1.0);
							}
						}
					} else {
						for(int j = 0; j < 3; j++){
							if(gsl_matrix_get(colors, color, j) == (target - 3)){
								gsl_matrix_set(outputSet, j, column, 1.0);
								gsl_matrix_set(outputSet, j+1, column, 1.0);
							}
						}
					}
					gsl_matrix_get_row(row_color, colors, color);
					gsl_matrix_get_row(row_object, objects, obj);
					
					write_command_desc(f, row_object, row_color, target);
					column++; // dalsi vstup/stlpec
				}
			//}

			gsl_vector_free(imagev);
			cvReleaseImage(&image);
		}
	}
	
	gsl_matrix_free(colors);
	gsl_matrix_free(objects);
	fclose(f);
	
	gsl_vector_free(row_object);
	gsl_vector_free(row_color);

}

void simulator::generate_combination_set(gsl_matrix **input, gsl_matrix **output){

	gsl_matrix *inputSet = *input = gsl_matrix_calloc(9 + 9 + 6 + 1, 216); //vstupne data ukladane do stlpcov
	gsl_matrix *outputSet = *output = gsl_matrix_calloc(3, 216); //vystupne data ukladane do stlpcov
	gsl_matrix *objects = get3permutations(); // permutacie objektov
	gsl_matrix *colors = get3permutations(); // permutacie farieb

	gsl_vector *row_color = gsl_vector_alloc(3);
	gsl_vector *row_object = gsl_vector_alloc(3);

	print_matrix(colors);
	int column = 0;
	for(unsigned int obj = 0; obj < objects->size1; obj++){
		for(unsigned int color = 0; color < colors->size1; color++){
			
			//nastavim stavove premenne farieb a objektov
			gsl_matrix_get_row(this->colors, colors, color);
			gsl_matrix_get_row(this->objects, objects, obj);


			// mame rozmiestnene objekty, vygenerujeme jazykovu instrukciu

				for(unsigned int target = 0; target < 6; target++){ // ktory objekt je cielovy
					//skopirujeme obrazovu informaciu
					gsl_matrix_set(inputSet, 3 * 0 + (int)gsl_matrix_get(objects, obj, 0), column, 1.0); //farba prvej pozicie
					gsl_matrix_set(inputSet, 3 * 1 + (int)gsl_matrix_get(objects, obj, 1), column, 1.0); //farba druhej pozicie
					gsl_matrix_set(inputSet, 3 * 2 + (int)gsl_matrix_get(objects, obj, 2), column, 1.0); //farba tretej pozicie					
					//setting colors
					gsl_matrix_set(inputSet, 9 + 3 * 0 + (int)gsl_matrix_get(colors, color, 0), column, 1.0); //farba prvej pozicie
					gsl_matrix_set(inputSet, 9 + 3 * 1 + (int)gsl_matrix_get(colors, color, 1), column, 1.0); //farba druhej pozicie
					gsl_matrix_set(inputSet, 9 + 3 * 2 + (int)gsl_matrix_get(colors, color, 2), column, 1.0); //farba tretej pozicie
					//setting action
					//gsl_matrix_set(inputSet, imagev->size + 9 + action, column, 1.0); 
					//setting target
					
					gsl_matrix_set(inputSet, 9 + 9 + target, column, 1.0); 

					//bias input
					gsl_matrix_set(inputSet, inputSet->size1 - 1, column, -1.0); 
					
					// vypocet pozadovaneho vystupu

					if(target < 3){
						for(int j = 0; j < 3; j++){
							if(gsl_matrix_get(objects, obj, j) == target){
								gsl_matrix_set(outputSet, j, column, 1.0);
								//gsl_matrix_set(outputSet, j+1, column, 1.0);
							}
						}
					} else {
						for(int j = 0; j < 3; j++){
							if(gsl_matrix_get(colors, color, j) == (target - 3)){
								gsl_matrix_set(outputSet, j, column, 1.0);
								//gsl_matrix_set(outputSet, j+1, column, 1.0);
							}
						}
					}

					column++; // dalsi vstup/stlpec
				}

		}
	}
	
	gsl_matrix_free(colors);
	gsl_matrix_free(objects);

}


gsl_vector *simulator::getObjectsFromCommand(char *command){
	gsl_vector *result = gsl_vector_alloc(3);

	for(unsigned int i = 0; i < result->size; i++){
		if(command[i] == 'b'){
			gsl_vector_set(result, i, 0.0);
		}
		if(command[i] == 'c'){
			gsl_vector_set(result, i, 1.0);
		}
		if(command[i] == 's'){
			gsl_vector_set(result, i, 2.0);
		}
	}

	return result;
}

gsl_vector *simulator::getColorsFromCommand(char *command){
	gsl_vector *result = gsl_vector_alloc(3);

	for(unsigned int i = 0 ; i < result->size; i++){
		if(command[i + 3] == 'r'){
			gsl_vector_set(result, i, 0.0);
		}
		if(command[i + 3] == 'g'){
			gsl_vector_set(result, i, 1.0);
		}
		if(command[i + 3] == 'b'){
			gsl_vector_set(result, i, 2.0);
		}
	}

	return result;
}

position simulator::getObjectPosition(gsl_vector *target){
	int pos = 0;
	for(int i = 0; i < 3; i++){
		if(target->data[i] == 1.0){
			pos = i;
		}
	}

	switch((int)objects->data[pos]){
		case SHAPE_BOX: return world_get("box", 1);
		case SHAPE_CYL: return world_get("model", model_cyl);
		case SHAPE_SPH: return world_get("model", model_sph);
	}
}

position simulator::getObjectStartPosition(gsl_vector *target){
	int pos = 0;
	for(int i = 0; i < 3; i++){
		if(target->data[i] == 1.0){
			pos = i;
		}
	}

	switch((int)objects->data[pos]){
		case SHAPE_BOX: return pos_box;
		case SHAPE_CYL: return pos_cyl;
		case SHAPE_SPH: return pos_sph;
	}
}

bool simulator::wasMoved(int shape){
	position original;
	position actual;
	switch(shape){
		case SHAPE_BOX: 
			original = pos_box;
			actual = world_get("box", 1);
			break;
		case SHAPE_CYL: 
			original = pos_cyl;
			actual = world_get("model", model_cyl);
			break;
		case SHAPE_SPH: 
			original = pos_sph;
			actual = world_get("model", model_sph);
			break;
	}
	// ak je posunuty o viac ako limit
	return distance_euclidean(original, actual) > 0.01;
}

bool simulator::movedTarget(gsl_vector *target){
	return wasMoved(getTargetShape(target));
}

bool simulator::movedNotTarget(gsl_vector *target){
	int targetShape = getTargetShape(target);
	if((SHAPE_BOX != targetShape) && wasMoved(SHAPE_BOX)){
		return TRUE;
	}
	if((SHAPE_CYL != targetShape) && wasMoved(SHAPE_CYL)){
		return TRUE;
	}
	if((SHAPE_SPH != targetShape) && wasMoved(SHAPE_SPH)){
		return TRUE;
	}
	return FALSE;
}

int simulator::getTargetShape(gsl_vector *target){
	int pos = 0;
	for(int i = 0; i < 3; i++){
		if(target->data[i] == 1.0){
			pos = i;
		}
	}

	return (int)objects->data[pos];
}
int simulator::getNearestShape(gsl_vector *target){
	position hand = world_get_rhand();
	int nearestShape = SHAPE_BOX;
	double nearest = distance_euclidean(world_get("box", 1), hand);

	if(distance_euclidean(world_get("model", model_sph), hand) < nearest){
		nearest = distance_euclidean(world_get("model", model_sph), hand);
		nearestShape = SHAPE_SPH;
	}

	if(distance_euclidean(world_get("model", model_cyl), hand) < nearest){
		nearest = distance_euclidean(world_get("model", model_cyl), hand);
		nearestShape = SHAPE_CYL;
	}

	return nearestShape;
}
int simulator::isTargetNearest(gsl_vector *target){
	return (getNearestShape(target) == getTargetShape(target));
}
void simulator::createObjects(gsl_vector *objects, gsl_vector *colors){
	if(objects != NULL){
		gsl_vector_memcpy(this->objects, objects);
	}
	if(colors != NULL){
		gsl_vector_memcpy(this->colors, colors);
	}

	for(int i = 0; i < 3; i++){
		double pos = objectsPos.x + i * objectGap;
		double iobj = gsl_vector_get(this->objects, i);
		double r = 0, g = 0, b = 0;
		//konvertuje farby

		if(gsl_vector_get(this->colors, i) == COLOR_RED)
			r = 1;
		if(gsl_vector_get(this->colors, i) == COLOR_GREEN)
			g = 1;
		if(gsl_vector_get(this->colors, i) == COLOR_BLUE)
			b = 1;

		double shift = 0;
		if(i == 1){
			shift = middleShift;
		}


		if (iobj == SHAPE_BOX){
			world_mk_box(0.065, 0.095, 0.065, pos, objectsPos.y, objectsPos.z + shift, r, g, b);
		}

		if (iobj == SHAPE_CYL){
			world_mk_model_cyl(0.035, 0.10, pos, objectsPos.y, objectsPos.z + shift, r, g, b);
			//world_rot_cyl(1, 90.0, 0.0, 0.0);
		}

		if (iobj == SHAPE_SPH){
			world_mk_model_sph(0.0425, pos, objectsPos.y, objectsPos.z + shift, r, g, b);

		}
	}	
		wait_object_drop("model", model_sph);

		pos_sph = world_get("model", model_sph);
		pos_box = world_get("box", 1);
		pos_cyl = world_get("model", model_cyl);

}
void simulator::wait_object_drop(char *object, int no){
	position actual, prev;
	prev = world_get(object, no);
	Sleep(200);
	actual = world_get(object, no);

	while(abs(actual.y - prev.y) > 0.0001){
		Sleep(200);
		prev = actual;
		actual = world_get(object, no);
	}
}

gsl_matrix *simulator::setObjects(char *command, int target, int action){
	gsl_matrix *inputSet = gsl_matrix_calloc(1200 + 9 + 9 + 1, 1); //vstupne data ukladane do stlpcov

	gsl_vector *objects = getObjectsFromCommand(command); // permutacie objektov
	gsl_vector *colors = getColorsFromCommand(command); // permutacie farieb

	int column = 0;
	unblockArm();			
	//rozmiestni objekty
	createObjects(objects, colors);

	IplImage *image = getCamObjectImage();
	gsl_vector *imagev = image2vector(image);

	// mame rozmiestnene objekty, vygenerujeme jazykovu instrukciu

	//skopirujeme obrazovu informaciu
	for(unsigned int i = 0; i < imagev->size; i++)
		gsl_matrix_set(inputSet, i, column, gsl_vector_get(imagev, i));

	//setting colors
	gsl_matrix_set(inputSet, imagev->size + 3 * 0 + (int)gsl_vector_get(colors, 0), column, 1.0); //farba prvej pozicie
	gsl_matrix_set(inputSet, imagev->size + 3 * 1 + (int)gsl_vector_get(colors, 1), column, 1.0); //farba druhej pozicie
	gsl_matrix_set(inputSet, imagev->size + 3 * 2 + (int)gsl_vector_get(colors, 2), column, 1.0); //farba tretej pozicie
	//setting action
	gsl_matrix_set(inputSet, imagev->size + 9 + action, column, 1.0); 
	//setting target
	gsl_matrix_set(inputSet, imagev->size + 12 + target, column, 1.0); 
	//bias input
	gsl_matrix_set(inputSet, imagev->size + 18, column, -1.0); 

	gsl_vector_free(imagev);
	cvReleaseImage(&image);

	gsl_vector_free(colors);
	gsl_vector_free(objects);
	
	return inputSet;
}

position simulator::world_get_rhand(){

		position result;
		Bottle cmd;
		Bottle response;

		cmd.addString("world");
		cmd.addString("get");
		cmd.addString("rhand");

		world_client.write(cmd, response);

		result.z = response.pop().asDouble();
		result.y = response.pop().asDouble();
		result.x = response.pop().asDouble();

		//printf("Got reply: %f %f %f \n", result.x, result.y, result.z);

		return result;
}

position simulator::world_get_lhand(){
	
		position result;
		Bottle cmd;
		Bottle response;

		cmd.addString("world");
		cmd.addString("get");
		cmd.addString("lhand");

		world_client.write(cmd, response);

		result.z = response.pop().asDouble();
		result.y = response.pop().asDouble();
		result.x = response.pop().asDouble();

		//printf("Got reply: %f %f %f \n", x.asDouble(), y.asDouble(), z.asDouble());

		return result;
}

position simulator::world_get(char *type, int box_id){
		
		position result;
		Bottle cmd;
		Bottle response;

		cmd.addString("world");
		cmd.addString("get");
		cmd.addString(type);
		if(box_id != 0)
			cmd.addInt(box_id);
		
		world_client.write(cmd, response);

		result.z = response.pop().asDouble();
		result.y = response.pop().asDouble();
		result.x = response.pop().asDouble();

		//printf("Got reply: %f %f %f \n", x.asDouble(), y.asDouble(), z.asDouble());

		return result;
}

void simulator::createDatasetFiles(){
	//moveDefaultObjectsAway();

	unblockArm();

	gsl_matrix *input;
	gsl_matrix *output;

	generateDataSet_mult(&input, &output);

	print_matrix_to_file(input, "input.txt");
	print_matrix_to_file(output, "desired.txt");

	gsl_matrix_free(input);
	gsl_matrix_free(output);

}

void simulator::createcombinationFiles(){

	gsl_matrix *input;
	gsl_matrix *output;

	generate_combination_set(&input, &output);
	print_matrix_to_file(input, "./experimental/input.txt");
	print_matrix_to_file(output, "./experimental/desired.txt");

	gsl_matrix_free(input);
	gsl_matrix_free(output);

}

void simulator::initArm(int part){
	gsl_vector *v = gsl_vector_alloc(7);
	gsl_vector_set(v, 0, -40);
	gsl_vector_set(v, 1, 125);
	gsl_vector_set(v, 2, 0);
	gsl_vector_set(v, 3, 6);
	gsl_vector_set(v, 4, 0);
	gsl_vector_set(v, 5, 0);
	gsl_vector_set(v, 6, 0);
	
	arm_set(v, part);

	gsl_vector_set(v, 0, -95);
	gsl_vector_set(v, 1, 10);
	gsl_vector_set(v, 2, 0);
	gsl_vector_set(v, 3, 6);
	gsl_vector_set(v, 4, 0);
	gsl_vector_set(v, 5, 0);
	gsl_vector_set(v, 6, 0);
	
	arm_set(v, part);

	gsl_vector_free(v);
}

void simulator::initArmStraight(int part){
	gsl_vector *v = gsl_vector_alloc(7);

	gsl_vector_set(v, 0, 25); //lebo inac zavadza v kamere + (rand() % 60) - 30
	gsl_vector_set(v, 1, 10);
	gsl_vector_set(v, 2, -12);
	gsl_vector_set(v, 3, 106);
	gsl_vector_set(v, 4, 0);
	gsl_vector_set(v, 5, 0);
	gsl_vector_set(v, 6, 0);

	/*
		gsl_vector_set(v, 0, -95);
	gsl_vector_set(v, 1, 10);
	gsl_vector_set(v, 2, 0);
	gsl_vector_set(v, 3, 6);
	gsl_vector_set(v, 4, 0);
	gsl_vector_set(v, 5, 0);
	gsl_vector_set(v, 6, 0);
	*/
	
	arm_set(v, part);

	gsl_vector_free(v);
}

void simulator::initArmStraight_rand(int part){
	gsl_vector *v = gsl_vector_alloc(7);

	gsl_vector_set(v, 0, 25+ (rand() % 60) - 30); //lebo inac zavadza v kamere + (rand() % 60) - 30
	gsl_vector_set(v, 1, 10+ (rand() % 60) - 30);
	gsl_vector_set(v, 2, -12+ (rand() % 60) - 30);
	gsl_vector_set(v, 3, 106+ (rand() % 60) - 30);
	gsl_vector_set(v, 4, 0);
	gsl_vector_set(v, 5, 0);
	gsl_vector_set(v, 6, 0);
	
	arm_set(v, part);

	gsl_vector_free(v);
}

void simulator::scale_arm(gsl_vector *arm){
	
	if(arm->size > 0)
		gsl_vector_set(arm, 0, 2 * (gsl_vector_get(arm, 0) + 95) / (90 + 95) - 1);
	if(arm->size > 1)
		gsl_vector_set(arm, 1, 2 * (gsl_vector_get(arm, 1) + 0) / (0 + 161) - 1);
	if(arm->size > 2)
		gsl_vector_set(arm, 2, 2 * (gsl_vector_get(arm, 2) + 37) / (37 + 100) - 1);
	if(arm->size > 3)
		gsl_vector_set(arm, 3, 2 * (gsl_vector_get(arm, 3) + 6) / (6 + 106) - 1);
	if(arm->size > 4)
		gsl_vector_set(arm, 4, 2 * (gsl_vector_get(arm, 4) + 90) / (90 + 90) - 1);
	if(arm->size > 5)
		gsl_vector_set(arm, 5, 2 * (gsl_vector_get(arm, 5) + 90) / (90 + 10) - 1);
	if(arm->size > 6)
		gsl_vector_set(arm, 6, 2 * (gsl_vector_get(arm, 6) + 20) / (20 + 40) - 1);
}

void simulator::scale_back_arm(gsl_vector *arm){
	if(arm->size > 0)
		gsl_vector_set(arm, 0, ((gsl_vector_get(arm, 0) + 1) * (90 + 95) / 2.0 - 95));
	if(arm->size > 1)
		gsl_vector_set(arm, 1, ((gsl_vector_get(arm, 1) + 1) * (0 + 161) / 2.0 - 0));
	if(arm->size > 2)
		gsl_vector_set(arm, 2, ((gsl_vector_get(arm, 2) + 1) * (37 + 100) / 2.0 - 37));
	if(arm->size > 3)
		gsl_vector_set(arm, 3, ((gsl_vector_get(arm, 3) + 1) * (6 + 106) / 2.0 - 6));
	if(arm->size > 4)
		gsl_vector_set(arm, 4, ((gsl_vector_get(arm, 4) + 1) * (90 + 90) / 2.0 - 90));
	if(arm->size > 5)
		gsl_vector_set(arm, 5, ((gsl_vector_get(arm, 5) + 1) * (90 + 10) / 2.0 - 90));
	if(arm->size > 6)
		gsl_vector_set(arm, 6, ((gsl_vector_get(arm, 6) + 1) * (20 + 40) / 2.0 - 20));
	
}

int test(){

	simulator *sim = new simulator();

	//sim->createDatasetFiles();
    
	const char *name = "/clientrpc";

	RpcClient port;
	port.open(name);
	port.addOutput("/icubSim/world");

	while(true){
		sim->world_mk_model_sph(0.0425, 0.0, 0.7, 0.35, 1, 1, 1);
		position p = sim->world_get("model", 1);
		printf("Got reply: %f %f %f \n", p.x, p.y, p.z);
	}

	delete sim;

	return 0;
}

