#pragma once
#include "functions.h"
#include <stdio.h>
/*
#include <yarp/os/Network.h>
#include <yarp/os/Port.h>
#include <yarp/os/BufferedPort.h>
#include <yarp/os/Bottle.h>
#include <yarp/os/Time.h>
*/
#include <stdio.h>

#include <yarp/dev/ControlBoardInterfaces.h>
#include <yarp/dev/PolyDriver.h>
#include <yarp/sig/Vector.h>
#include <string>

#include <iostream>
//#include "cv.h"
//#include "highgui.h"


#include <yarp/os/all.h>

#include <yarp/sig/all.h>
using namespace yarp::dev;
using namespace yarp::sig;
using namespace yarp::sig::draw;
using namespace yarp::sig::file;
using namespace yarp::os;

using namespace yarp;
using namespace yarp::sig::draw;
using namespace yarp::sig::file;

class simulator
{
private:
	Network yarp;
	Port worldPort;
	RpcClient world_client;
	PolyDriver *robotDriver[4];
	Port port_rhand_touch;
	BufferedPort<ImageOf<PixelRgb>> cam_right;  // make a port for reading images

	position objectsPos;
	double objectGap;
	double middleShift;

	gsl_vector *objects;
	gsl_vector *colors;
	//initial positions for created objects
	position pos_cyl, pos_sph, pos_box;
	// cislo modelov pre dany tvar
	int model_sph;
	int model_cyl;
	// pocet importovanych 3d objektov
	int model_count;

public:
	simulator();
	~simulator(void);
	
	void initRobotDriver();
	void closeRobotDriver();
	PolyDriver *createRobotDriver(int port);

	void showImage(IplImage *image);
	IplImage *getCamImage();
	IplImage *getCamObjectImage();

	void lookAtTable();
	void moveDefaultObjectsAway();
	void world_set(char *type, double box_id, double xpos, double ypos, double zpos);
	void world_mk_box(double sx, double sy, double sz, double px, double py, double pz, double r, double g, double b);
	void world_mk_sph(double sr, double px, double py, double pz, double r, double g, double b);
	void world_mk_cyl(double sr, double sl, double px, double py, double pz, double r, double g, double b);
	void world_mk_sbox(double sx, double sy, double sz, double px, double py, double pz, double r, double g, double b);
	
	void world_mk_model_sph(double sr, double px, double py, double pz, double r, double g, double b) ;
	void world_mk_model_cyl(double sr, double sl, double px, double py, double pz, double r, double g, double b);

	void world_rot_cyl(int no, double anglex = 90, double angley=0, double anglez=0);
	void wait_object_drop(char *object, int no = 1);
	void world_del_all();

	void createTable();
	void unblockArm();

	void generateDataSet(gsl_matrix **inputSet, gsl_matrix **outputSet);
	void generateDataSet_target(gsl_matrix **input, gsl_matrix **output);
	void generateDataSet_target_double(gsl_matrix **input, gsl_matrix **output);
	void generateDataSet_mult(gsl_matrix **input, gsl_matrix **output);
	void generate_combination_set(gsl_matrix **input, gsl_matrix **output);

	void createcombinationFiles();

	bool wasMoved(int shape);
	bool movedTarget(gsl_vector *target);
	bool movedNotTarget(gsl_vector *target);

	int getNearestShape(gsl_vector *target);
	int isTargetNearest(gsl_vector *target);
	
	int getTargetShape(gsl_vector *target);
	position getObjectPosition(gsl_vector *target);
	position getObjectStartPosition(gsl_vector *target);
	gsl_vector *getObjectsFromCommand(char *command);
	gsl_vector *getColorsFromCommand(char *command);
	gsl_matrix *setObjects(char *command, int target, int action);
	void createObjects(gsl_vector *objects = NULL, gsl_vector *colors = NULL);

	gsl_vector *image2vector(IplImage *image);
	gsl_matrix *get3permutations();
	void createDatasetFiles();

	int rhand_touch();
	position world_get_rhand();
	position world_get_lhand();
	position world_get(char *type, int box_id);

	void part_set(gsl_vector* v, int part);
	void arm_set(gsl_vector* v, int part);
	gsl_vector* arm_get(int part, int nJoints = 0);
	void initArm(int part);
	void initArmStraight(int part);
	void initArmStraight_rand(int part);

	void scale_arm(gsl_vector *arm);
	void scale_back_arm(gsl_vector *arm);
	void write_command_desc(FILE *f, gsl_vector *objects, gsl_vector *colors, int target);
};

int test();