#include "Try_1.h"
#include "listener.h"
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <cmath>
#include <map>
#include <iostream>
#include <chrono>
#include <random>
#include <limits>

//Inicializacion de constantes globales utilizadas
int conexiones = 0;
std::vector<int> id_robot;
std::vector<std::string> robots_names;
std::vector<double> modulo_robots;
std::vector<double> rotation_robots;
std::vector<double> rotation_robots_a;
std::vector<std::string> tipo_robot;
std::vector<int> accion_robot;//accion robot actual
std::vector<int> accion_robot_a;//accion robot anterior
std::vector<double> vec_dif;//actual
std::vector<double> vec_dif_a;//anterior
std::vector<double> vec_sup;//actual
std::vector<double> vec_sup_a;//anterior
std::vector<double> vec_inf;///actual
std::vector<double> vec_inf_a;//anterior
std::vector<double> vec_x;//anterior
std::vector<double> vec_y;//anterior
std::vector<int> vec_Minvc;//anterior
std::vector<int> vec_Maxvl;//anterior
std::vector<int> vec_MinDif;//anterior
std::vector<double> vec_Difa;//anterior
std::vector<int> vec_SS;//anterior
std::vector<int> vec_Acc;
std::vector<int> vec_Accr;
std::vector<double> R;//recompensa actual
std::vector<double> R_a;//recompensa anterior
gazebo::RegistroR registror;
gazebo::Archivo archivo("/home/mrmelkix/Robot/results/simulacion1.csv");
double Q[10][2][10];//Vector Q actual
double Q_a[10][2][10];//Vector Q anterior
int AccRelativa[8][8]={
		{1,2,3,4,5,6,7,8},
		{8,1,2,3,4,5,6,7},
		{7,8,1,2,3,4,5,6},
		{6,7,8,1,2,3,4,5},
		{5,6,7,8,1,2,3,4},
		{4,5,6,7,8,1,2,3},
		{3,4,5,6,7,8,1,2},
		{2,3,4,5,6,7,8,1}
};
std::vector<int> contador; //utilizado para realizar el envio de instruccion
std::vector<int> inicio;
std::vector<std::string> posible_resultado(10);


namespace gazebo{
	void Try_1::Load(physics::ModelPtr _model,sdf::ElementPtr _sdf){
		this->modelo=_model;
		this->sdf=_sdf;
		cargarUniones();
		gzdbg<<"Iniciacion Correcta de "+modelo->GetName()+"\r\n";
		conexiones += 1;
		//inicializar variables globales con respectivos valores
		id_robot.push_back(modelo->GetId());
		modulo_robots.push_back(0);
		rotation_robots.push_back(0);
		rotation_robots_a.push_back(modelo->GetRelativePose().rot.GetYaw());
		robots_names.push_back(modelo->GetName());
		tipo_robot.push_back(" ");
		accion_robot.push_back(0);//accion robot actual
		accion_robot_a.push_back(0);//accion robot anterior
		contador.push_back(0);
		inicio.push_back(0);
		vec_dif.push_back(0);//actual
		vec_dif_a.push_back(0);//anterior
		vec_sup.push_back(0);//actual
		vec_sup_a.push_back(0);//anterior
		vec_inf.push_back(0);///actual
		vec_inf_a.push_back(0);//anterior
		vec_x.push_back(modelo->GetRelativePose().pos.x);
		vec_y.push_back(modelo->GetRelativePose().pos.y);
		vec_Minvc.push_back(0);
		vec_Maxvl.push_back(0);
		vec_MinDif.push_back(0);
		vec_Difa.push_back(0);
		vec_Acc.push_back(0);
		vec_Accr.push_back(0);
		vec_SS.push_back(0);
		R.push_back(0);//recompensa actual
		R_a.push_back(0);//recompensa anterior
		for(int j = 0;j<2;j++){
			for(int k=0;k<8;k++){
				Q[conexiones][j][k]=0;
				Q_a[conexiones][j][k]=0;
			}

		}
		Q[conexiones][0][9]=-1*std::numeric_limits<int>::infinity();
		Q[conexiones][1][9]=std::numeric_limits<int>::infinity();
		Q_a[conexiones][0][9]=-1*std::numeric_limits<int>::infinity();
		Q_a[conexiones][1][9]=std::numeric_limits<int>::infinity();

		listener = new Listener();
		
		listener->init(this, "/"+((_sdf->HasElement("topic"))?_sdf->GetElement("topic")->GetValue()->GetAsString():_model->GetName()));
		this->estado="iniciado";
		this->conexionUpdate=event::Events::ConnectWorldUpdateBegin(boost::bind(&Try_1::OnUpdate,this,_1));//ojo con el _1  http://osrf-distributions.s3.amazonaws.com/gazebo/api/7.1.0/classgazebo_1_1event_1_1Events.html#a0c8c7e6a33ad46f8be45b6a33d279fdc
	}
	void Try_1::OnUpdate(const common::UpdateInfo & _info){
		//calculo de trayectoria cada n tiempo de ejecucion
		common::Time actual=modelo->GetWorld()->GetSimTime();
		//actualizar pose
		gazebo::math::Pose pose = modelo->GetRelativePose();//http://osrf-distributions.s3.amazonaws.com/gazebo/api/2.2.1/classgazebo_1_1physics_1_1Model.html
		gazebo::math::Vector3 vec = pose.pos;
		//gazebo::math::Quaternion vec2 = pose.rot.GetYaw();
		double vec2 = pose.rot.GetYaw();

		this->estado="Elementos \r\n";
		//Buscar los elementos de Union
		for(auto it=uniones.begin();it!=uniones.end();it++){
			//std::string name = modelo->GetName();
			Union *_union=&it->second;
			this->estado+="\n";
			this->estado+="El numero del modelo principal es:"+std::to_string(modelo->GetId());
			this->estado+= "\n el nombre del model es: "+modelo->GetName();
			this->estado+="\n";
			this->estado+=it->first+": Ang: "+std::to_string(_union->joint->GetAngle(0).Radian());
			this->estado+=" Vel: "+std::to_string(_union->joint->GetVelocity(0));
			this->estado+="\n";
			this->estado+=", x: "+std::to_string(vec.x);
			this->estado+=", y: "+std::to_string(vec.y);
			this->estado+=", yaw[radian]: "+std::to_string(vec2);
			this->estado+=": mod "+std::to_string(sqrt(pow(vec.x,2)+pow(vec.y,2)));
			this->estado+="\n";
			this->estado+=" Rssi Relativa: "+std::to_string(Try_1::calcularRSSI(sqrt(pow(vec.x,2)+pow(vec.y,2)), 1.23))+"[dB]";//gamma Palma = 1.23
			this->estado+="  Accion: "+Try_1::IARobot(sqrt(pow(vec.x,2)+pow(vec.y,2)),vec2, modelo->GetName(),actual.Double(),vec.x,vec.y);//Corre la funcion de inteligencia
			for(int i=2;i<conexiones;i++){
				this->estado+="  Matriz Q de "+robots_names[i]+" es:\n"+
						std::to_string(Q[i][0][0])+" "+std::to_string(Q[i][0][1])+" "+std::to_string(Q[i][0][2])+" "+std::to_string(Q[i][0][3])+" "+std::to_string(Q[i][0][4])+" "+std::to_string(Q[i][0][5])+" "+std::to_string(Q[i][0][6])+" "+std::to_string(Q[i][0][7])+" "+std::to_string(Q[i][0][8])+" "+std::to_string(Q[i][0][9])+" "+"\n"+
						std::to_string(Q[i][1][0])+" "+std::to_string(Q[i][1][1])+" "+std::to_string(Q[i][1][2])+" "+std::to_string(Q[i][1][3])+" "+std::to_string(Q[i][1][4])+" "+std::to_string(Q[i][1][5])+" "+std::to_string(Q[i][1][6])+" "+std::to_string(Q[i][1][7])+" "+std::to_string(Q[i][1][8])+" "+std::to_string(Q[i][1][9])+" "+"\n";
			}
			this->estado+="\r\n";
			//this->estado+=std::to_string(actual.Double());
			if(_union->TiempoAccion>0){
				common::Time TA = _union->TiempoAccion;
			if(_union->moviendo){//solo cambio de angulo al mover "m"
				common::Time ticActual=actual-_union->tiempoActual;
				double error=_union->joint->GetAngle(0).Radian()-_union->anguloFinal;
				double absError=(error>0)?error:-1*error;
				double velocidad=0;
				//if(absError>0.001){
					velocidad=_union->pid.Update(error,ticActual);//(_error,dt)

				/*}else{
					_union->moviendo=false;
				}*/

				_union->velocidad=velocidad;
				_union->TiempoAccion=TA-actual;
			}
			}
			else{
				_union->velocidad=0;
			}

			_union->joint->SetVelocity(0,_union->velocidad);
		}




	}
	void Try_1::mover(std::string laUnion, double valor){
		Union *_union=&uniones[laUnion];
		if(_union){
			_union->anguloFinal=valor;//radianes
			_union->tiempoActual=0;
			_union->moviendo=true;
		}
	}
	void Try_1::parametrizar(std::string laUnion,std::string tipo, double valor, common::Time TA){//TA=TiempoAccion
		Union *_union=&uniones[laUnion];
		if(_union){
			if(tipo=="V"){
				_union->velocidad=valor*2;//metros por segundo, 2 solo para aumentar velocidad de simulacion
				_union->TiempoAccion=TA;
				_union->moviendo=false;
			}else if(tipo=="VM"){
				_union->pid.SetCmdMax(valor);
				_union->pid.SetCmdMin(-1*valor);
			}
		
		}
	}
	void Try_1::pintar(std::string tipo){
		if(tipo=="uniones"){
			this->estado="Elementos \r\n";
			std::map<std::string,physics::BasePtr> uniones=buscar(this->modelo, physics::Entity::EntityType::JOINT);
			for(auto it=uniones.begin();it!=uniones.end();it++){
				this->estado+=it->first+"\r\n";
			}
		}
	}
	std::map<std::string,physics::BasePtr> Try_1::buscar(physics::BasePtr contenedor, const physics::Entity::EntityType &t){
		unsigned int n=contenedor->GetChildCount();
		std::map<std::string,physics::BasePtr> resultado;
		for(int i=0;i<n;i++){
			if(contenedor->GetChild(i)->HasType(t)){
				resultado[contenedor->GetChild(i)->GetName()]=contenedor->GetChild(i);
			}
			std::map<std::string,physics::BasePtr> resultado2=buscar(contenedor->GetChild(i),t);
			resultado.insert(resultado2.begin(),resultado2.end());
		}
		return resultado;
	}
	void Try_1::cargarUniones(){
		std::map<std::string,physics::BasePtr> uniones=buscar(this->modelo, physics::Entity::EntityType::JOINT);
		for(auto it=uniones.begin();it!=uniones.end();it++){
			Union _union;
			_union.joint=boost::static_pointer_cast<physics::Joint>(it->second);
			_union.pid.Init(2, 1, 1, 0, 0, _union.joint->GetVelocityLimit(0), -1*_union.joint->GetVelocityLimit(0));
			//p,i,d,imax,imin,cmdMax,cmdMin http://osrf-distributions.s3.amazonaws.com/gazebo/api/7.1.0/classgazebo_1_1common_1_1PID.html
			_union.tiempoActual=0;
			this->uniones[it->first]=_union;
		}
	}
	double Try_1::calcularRSSI(double modulo, double gamma){

		double constante1 = 0;
		double d0 = 1;			//
		double e = 2.71828;		//Constante exponencial
		double potencia_transmision = 20;// tomando en cuenta el sensor esp8266
		unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
		std::default_random_engine generator (seed);
		std::normal_distribution<double> distribution (0.0,1.0);
		double RuidoGauss = distribution(generator);
		RuidoGauss = 0;
		double perdida = constante1 + 10*gamma * log10(modulo/d0 +1)+ RuidoGauss;//medido en dB
		double rssiRelativo = potencia_transmision - perdida;

		return rssiRelativo;
	}
	int Try_1::Accion(double RSSI_sup , double RSSI_inf, int i){

		int MinDif = 0;
		int Maxvl = 0; //Maximizar el vinculo lejano
		int Minvc = 0;//Minimizar el vinculo cercano
		int SS = 0;

		//Constantes Q-learning

		double alpha = 0.5;
		double lamda = 0.5;

		vec_sup[i]=RSSI_sup;
		vec_inf[i]=RSSI_inf;
		double Difn = RSSI_sup - RSSI_inf;//Diferencia de RSSI
		double Difa = sqrt(pow(RSSI_sup - RSSI_inf,2));//Modulo de diferencia RSSI
		vec_dif[i]=Difa;
		double Difd = Difn/Difa;//Diferencia digital RSSI +1 = mas cercano al superior y -1 = mas cercano al inferior


		//Minimizar Difa
		if(vec_dif[i]-vec_dif_a[i]<0){
			MinDif = 1;
		}else{
			MinDif = 0;
		}
		//Maximizar Vinculo Lejano y Minimizar Vinculo cercano
		if(RSSI_sup>RSSI_inf){
			if(vec_sup[i]-vec_sup_a[i]<0){
				Minvc = 1;
			}else{
				Minvc = 0;
			}
			if(vec_inf[i]-vec_inf_a[i]>0){
				Maxvl=1;
			}else{
				Maxvl=0;
			}
		}else{
			if(vec_inf[i]-vec_inf_a[i]<0){
				Minvc = 1;
			}else{
				Minvc = 0;
			}
			if(vec_sup[i]-vec_sup_a[i]>0){
				Maxvl=1;
			}else{
				Maxvl=0;
			}
		}

		//Recompensa y estado SS segun MinDif Minvc Maxvl
		if(MinDif == 1 && Maxvl==1 && Minvc==1 ){
			R[i] = 1;//SS1
			SS = 0;
		}else{
			R[i] = -1;//SS2
			SS = 1;
		}

		int Acc = 1;
		//accion_robot[i]
		if(R_a[i] == 1 && R[i] ==-1){
			// return A9 es el movimiento contrario.
			if(accion_robot[i] == accion_robot_a[i]){
				accion_robot[i]=rand()%7+1;//aleatorio desde 1 a 8
			}else{
				int contrario = accion_robot[i];
				if(contrario == 1){
					Acc = 5;
				}
				if(contrario == 2){
					Acc = 6;
				}
				if(contrario == 3){
					Acc = 7;
				}
				if(contrario == 4){
					Acc = 8;
				}
				if(contrario == 5){
					Acc = 1;
				}
				if(contrario == 6){
					Acc = 2;
				}
				if(contrario == 7){
					Acc = 3;
				}
				if(contrario == 8){
					Acc = 4;
				}
				accion_robot[i]=Acc;
			}
			//gzdbg<<"Accion Contraria \n";
		}else{
			if(R_a[i] == -1 && R[i] == -1){
				accion_robot[i]=rand()%7+1;//aleatorio desde 1 a 8
				if(accion_robot[i] == accion_robot_a[i]){
					Acc = rand()%7+1;//aleatorio desde 1 a 8
				}else{
					Acc = accion_robot[i];
				}
				//gzdbg<<"Accion aleatoria \n";
			}else{

				//Retornar maximo valor de Q
				int max_Acc_q = -1000;
				for(int j = 1;j<10;j++){
					if(Q[i][SS][j]>max_Acc_q){
						Acc = j;
						max_Acc_q = Q[i][SS][j];
					}

				}
				//Actualizar Estados
				//gzdbg<<"actualicé valores \n";

				Q[i][SS][Acc] = (1-alpha)*Q_a[i][SS][Acc]+alpha*(R[i]+lamda*Q[i][SS][Acc]);

			}
		}

		//Registro de valores anteriores
		accion_robot_a[i]=accion_robot[i];

		R_a[i]=R[i];
		vec_dif_a[i]=Difa;
		vec_sup_a[i]=RSSI_sup;
		vec_inf_a[i]=RSSI_inf;
		vec_Maxvl[i]=Maxvl;
		vec_Minvc[i]=Minvc;
		vec_MinDif[i]=MinDif;
		vec_SS[i]=SS;
		vec_Acc[i]=Acc;

		double yaw2 = rotation_robots[i]*-1;//normalizar la rotacion a la derecha es positivo y a la izquierda es negativo.
		bool direccion;
		if(yaw2>0){
			//Derecha
			direccion = true;
		}else{
			//Izquierda
			direccion=false;
		}
		if(i==2){
		gzdbg<<robots_names[i]+":\n "+"MinDif es: "+std::to_string(MinDif)+
						", Maxvl es: "+std::to_string(Maxvl)+
						", Minvc es: "+std::to_string(Minvc)+" \n "
						", RSSI_sup es: "+std::to_string(RSSI_sup)+
						", RSSI_inf es: "+std::to_string(RSSI_inf)+
						", Difa es: "+std::to_string(Difa)+"\n"+
						",Direccion es: "+std::to_string(direccion)+
						",Acc es: "+std::to_string(Acc);
		Acc = getDeseado(yaw2, Acc-1,direccion);
		gzdbg<<",Acc_relativo es: "+std::to_string(Acc)+"\n";
		}
		vec_Accr[i]=Acc;
		rotation_robots_a[i]= rotation_robots[i];
		return Acc;
	}
	std::string Try_1::IARobot(double modulo,double yaw, std::string robot_name, double timeSim, double x, double y){
		posible_resultado[0]="A0";  //Detener
		posible_resultado[1]="A1";  //Avanzar
		posible_resultado[2]="A2";  //Adelante derecha
		posible_resultado[3]="A3";  //Derecha
		posible_resultado[4]="A4";  //Derecha atras
		posible_resultado[5]="A5";  //Atras
		posible_resultado[6]="A6";  //Atras izquierda
		posible_resultado[7]="A7";  //Izquierda
		posible_resultado[8]="A8";  //Adelante izquierda

		//Variables locales a utilizar
		std::string resultado;
		double Robot_sucesor = 0;
		double Robot_antecesor = 0;

		if(robot_name == "REB"){
			tipo_robot[0] = "no_mover";
			modulo_robots[0]=modulo;
			rotation_robots[0]=yaw;
			vec_x[0]=x;
			vec_y[0]=y;

		}
		if(robot_name == "REA"){
			tipo_robot[1]= "REA_noMover";
			modulo_robots[1]=modulo;
			rotation_robots[1]=yaw;
			vec_x[1]=x;
			vec_y[1]=y;
		}
		if(robot_name == "RRA1"){
			tipo_robot[2]= "RRA";
			modulo_robots[2]=modulo;
			rotation_robots[2]=yaw;
			vec_x[2]=x;
			vec_y[2]=y;
		}
		if(robot_name == "RRA2"){
			tipo_robot[3]= "RRA";
			modulo_robots[3]=modulo;
			rotation_robots[3]=yaw;
			vec_x[3]=x;
			vec_y[3]=y;
		}
		if(robot_name == "RRA3"){
			tipo_robot[4]= "RRA";
			modulo_robots[4]=modulo;
			rotation_robots[4]=yaw;
			vec_x[4]=x;
			vec_y[4]=y;
		}
		if(robot_name == "RRA4"){
			tipo_robot[5]= "RRA";
			modulo_robots[5]=modulo;
			rotation_robots[5]=yaw;
			vec_x[5]=x;
			vec_y[5]=y;
		}
		if(robot_name == "RRA5"){
			tipo_robot[6]= "RRA";
			modulo_robots[6]=modulo;
			rotation_robots[6]=yaw;
			vec_x[6]=x;
			vec_y[6]=y;
		}

		if(Try_1::calcularRSSI(modulo_robots[1],1.23)<14.3){//13
			for(int i = 2;i<conexiones;i++){
				Robot_sucesor = modulo_robots[i-1]-modulo_robots[i];//Rssi superior
				if(Robot_sucesor>1){
					if(inicio[i]==0){
						ros::NodeHandle n;
						ros::Publisher publicador = n.advertise<std_msgs::String>(robots_names[i], 1);//robot_name, 1000);
						Try_1::enviarRos(publicador, "e "+tipo_robot[i]+"/Inicio");
						inicio[i]=1;
					}
				}

				try{
					if(i==conexiones){//ultimo robot
						Robot_antecesor = modulo_robots[i]-modulo_robots[0];
					}else{
					Robot_antecesor = modulo_robots[i]-modulo_robots[i+1];//Rssi inferior
					}

				}catch(...){
					Robot_antecesor=0;
				}
				if(inicio[i]==1){
				contador[i] += 1;
					if(contador[i] ==3000){		//Cantidad de veces que completa un ciclo de ejecucion para tomar decision
						ros::NodeHandle n;
						ros::Publisher publicador = n.advertise<std_msgs::String>(robots_names[i], 1);
						Try_1::enviarRos(publicador, "e "+tipo_robot[i]+"/"+posible_resultado[Try_1::Accion(Try_1::calcularRSSI(Robot_sucesor,1.23), Try_1::calcularRSSI(Robot_antecesor,1.23),i)]);
						contador[i] = 0;

					}
				}
			}

		}else{
			return resultado = "Aun no se necesita un robot";
		}
		/*
		if(timeInicio == -2){
			timeInicio = timeSim;
		}
		if(timeInicio > 0){*/
			registror.t = timeSim;//-timeInicio;
			registror.xe=vec_x[1];
			registror.ye=vec_y[1];
			registror.RR1x=vec_x[2];
			registror.RR1y=vec_y[2];
			registror.RR1sup=vec_sup[2];
			registror.RR1inf=vec_inf[2];
			registror.RR1Difa=vec_dif[2];
			registror.RR1MinDif=vec_MinDif[2];
			registror.RR1Maxvl=vec_Maxvl[2];
			registror.RR1Minvc=vec_Minvc[2];
			registror.RR1SS=vec_SS[2];
			registror.RR1Acc=vec_Acc[2];
			registror.RR1yaw=rotation_robots[2];
			registror.RR1Accr=vec_Accr[2];
			registror.RR1Q01=Q[2][0][0];
			registror.RR1Q02=Q[2][0][1];
			registror.RR1Q03=Q[2][0][2];
			registror.RR1Q04=Q[2][0][3];
			registror.RR1Q05=Q[2][0][4];
			registror.RR1Q06=Q[2][0][5];
			registror.RR1Q07=Q[2][0][6];
			registror.RR1Q08=Q[2][0][7];
			registror.RR1Q09=Q[2][0][8];
			registror.RR1Q11=Q[2][1][0];
			registror.RR1Q12=Q[2][1][1];
			registror.RR1Q13=Q[2][1][2];
			registror.RR1Q14=Q[2][1][3];
			registror.RR1Q15=Q[2][1][4];
			registror.RR1Q16=Q[2][1][5];
			registror.RR1Q17=Q[2][1][6];
			registror.RR1Q18=Q[2][1][7];
			registror.RR1Q19=Q[2][1][8];
			registror.RR2x=vec_x[3];
			registror.RR2y=vec_y[3];
			registror.RR2sup=vec_sup[3];
			registror.RR2inf=vec_inf[3];
			registror.RR2Difa=vec_dif[3];
			registror.RR2MinDif=vec_MinDif[3];
			registror.RR2Maxvl=vec_Maxvl[3];
			registror.RR2Minvc=vec_Minvc[3];
			registror.RR2SS=vec_SS[3];
			registror.RR2Acc=vec_Acc[3];
			registror.RR2yaw=rotation_robots[3];
			registror.RR2Accr=vec_Accr[3];
			registror.RR2Q01=Q[3][0][0];
			registror.RR2Q02=Q[3][0][1];
			registror.RR2Q03=Q[3][0][2];
			registror.RR2Q04=Q[3][0][3];
			registror.RR2Q05=Q[3][0][4];
			registror.RR2Q06=Q[3][0][5];
			registror.RR2Q07=Q[3][0][6];
			registror.RR2Q08=Q[3][0][7];
			registror.RR2Q09=Q[3][0][8];
			registror.RR2Q11=Q[3][1][0];
			registror.RR2Q12=Q[3][1][1];
			registror.RR2Q13=Q[3][1][2];
			registror.RR2Q14=Q[3][1][3];
			registror.RR2Q15=Q[3][1][4];
			registror.RR2Q16=Q[3][1][5];
			registror.RR2Q17=Q[3][1][6];
			registror.RR2Q18=Q[3][1][7];
			registror.RR2Q19=Q[3][1][8];
			registror.RR3x=vec_x[4];
			registror.RR3y=vec_y[4];
			registror.RR3sup=vec_sup[4];
			registror.RR3inf=vec_inf[4];
			registror.RR3Difa=vec_dif[4];
			registror.RR3MinDif=vec_MinDif[4];
			registror.RR3Maxvl=vec_Maxvl[4];
			registror.RR3Minvc=vec_Minvc[4];
			registror.RR3SS=vec_SS[4];
			registror.RR3Acc=vec_Acc[4];
			registror.RR3yaw=rotation_robots[4];
			registror.RR3Accr=vec_Accr[4];
			registror.RR3Q01=Q[4][0][0];
			registror.RR3Q02=Q[4][0][1];
			registror.RR3Q03=Q[4][0][2];
			registror.RR3Q04=Q[4][0][3];
			registror.RR3Q05=Q[4][0][4];
			registror.RR3Q06=Q[4][0][5];
			registror.RR3Q07=Q[4][0][6];
			registror.RR3Q08=Q[4][0][7];
			registror.RR3Q09=Q[4][0][8];
			registror.RR3Q11=Q[4][1][0];
			registror.RR3Q12=Q[4][1][1];
			registror.RR3Q13=Q[4][1][2];
			registror.RR3Q14=Q[4][1][3];
			registror.RR3Q15=Q[4][1][4];
			registror.RR3Q16=Q[4][1][5];
			registror.RR3Q17=Q[4][1][6];
			registror.RR3Q18=Q[4][1][7];
			registror.RR3Q19=Q[4][1][8];

			archivo.setRegistroR(registror);
			archivo.save();
		//}

		resultado = robot_name+"  \n el Robot antecesor esta a :"+
					std::to_string(Robot_antecesor)+"  \n el Robot sucesor esta a :"+
					std::to_string(Robot_sucesor);
		return resultado;
	}
	int Try_1::getDeseado(double yaw, int Acc_d, bool dir){
		//Aproximar yaw a un cuadrante
		int cuadrante;
		if(dir){
		cuadrante = (int) round(abs(yaw)*M_PI/4)+1;//se obtiene un numero de1 a 5
		}else{
			cuadrante = (int) round(abs(yaw)*M_PI/4)+1;
			if(cuadrante == 2){
				cuadrante = 8;
			}
			if(cuadrante == 3){
				cuadrante = 7;
			}
			if(cuadrante == 4){
				cuadrante = 6;
			}
		}
		int Acc_r = AccRelativa[cuadrante-1][Acc_d];
		return Acc_r;
	}
	double Try_1::errorPO(int indice_robot){
		double PO = modulo_robots[1]/(conexiones-2);
		double error = sqrt(pow(PO * (indice_robot-1) - modulo_robots[indice_robot],2));
		return error;
	}
	void Try_1::enviarRos(ros::Publisher publicador, std::string mensaje){
		std_msgs::String msg;
		msg.data = mensaje;
		publicador.publish(msg);


	}

	GZ_REGISTER_MODEL_PLUGIN(Try_1);
}
