#ifndef MIROBOT
#define MIROBOT
#include <stdio.h>
#include "listener.h"
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>

namespace gazebo{
	struct Union{
		gazebo::physics::JointPtr joint;
		gazebo::common::PID pid;
		gazebo::common::Time tiempoActual;
		
		double velocidad;
		double anguloFinal;
		common::Time TiempoAccion;
		
		bool moviendo=false;
		
	};
	class RegistroR{

	public:
		double t;
		double xe;
		double ye;
		double RR1x;
		double RR1y;
		double RR1sup;
		double RR1inf;
		double RR1Difa;
		int RR1MinDif;
		int RR1Maxvl;
		int RR1Minvc;
		int RR1SS;
		int RR1Acc;
		double RR1yaw;
		int RR1Accr;
		double RR1Q01;
		double RR1Q02;
		double RR1Q03;
		double RR1Q04;
		double RR1Q05;
		double RR1Q06;
		double RR1Q07;
		double RR1Q08;
		double RR1Q09;
		double RR1Q11;
		double RR1Q12;
		double RR1Q13;
		double RR1Q14;
		double RR1Q15;
		double RR1Q16;
		double RR1Q17;
		double RR1Q18;
		double RR1Q19;

		double RR2x;
		double RR2y;
		double RR2sup;
		double RR2inf;
		double RR2Difa;
		int RR2MinDif;
		int RR2Maxvl;
		int RR2Minvc;
		int RR2SS;
		int RR2Acc;
		double RR2yaw;
		int RR2Accr;
		double RR2Q01;
		double RR2Q02;
		double RR2Q03;
		double RR2Q04;
		double RR2Q05;
		double RR2Q06;
		double RR2Q07;
		double RR2Q08;
		double RR2Q09;
		double RR2Q11;
		double RR2Q12;
		double RR2Q13;
		double RR2Q14;
		double RR2Q15;
		double RR2Q16;
		double RR2Q17;
		double RR2Q18;
		double RR2Q19;

		double RR3x;
		double RR3y;
		double RR3sup;
		double RR3inf;
		double RR3Difa;
		int RR3MinDif;
		int RR3Maxvl;
		int RR3Minvc;
		int RR3SS;
		int RR3Acc;
		double RR3yaw;
		int RR3Accr;
		double RR3Q01;
		double RR3Q02;
		double RR3Q03;
		double RR3Q04;
		double RR3Q05;
		double RR3Q06;
		double RR3Q07;
		double RR3Q08;
		double RR3Q09;
		double RR3Q11;
		double RR3Q12;
		double RR3Q13;
		double RR3Q14;
		double RR3Q15;
		double RR3Q16;
		double RR3Q17;
		double RR3Q18;
		double RR3Q19;
	};

	class Archivo{
	private:
		FILE *arch;
		RegistroR registror;

		void abrir(char *filename){
		  arch=fopen(filename,"w");
		  fprintf(arch,"t,xe,ye,RR1x,RR1y,RR1sup,RR1inf,RR1Difa,RR1MinDif,RR1Maxvl,RR1Minvc,RR1SS,RR1Acc,RR1yaw,RR1Accr,RR1Q01,RR1Q02,RR1Q03,RR1Q04,RR1Q05,RR1Q06,RR1Q07,RR1Q08,RR1Q09,RR1Q11,RR1Q12,RR1Q13,RR1Q14,RR1Q15,RR1Q16,RR1Q17,RR1Q18,RR1Q19,RR2x,RR2y,RR2sup,RR2inf,RR2Difa,RR2MinDif,RR2Maxvl,RR2Minvc,RR2SS,RR2Acc,RR2yaw,RR2Accr,RR2Q01,RR2Q02,RR2Q03,RR2Q04,RR2Q05,RR2Q06,RR2Q07,RR2Q08,RR2Q09,RR2Q11,RR2Q12,RR2Q13,RR2Q14,RR2Q15,RR2Q16,RR2Q17,RR2Q18,RR2Q19,RR3x,RR3y,RR3sup,RR3inf,RR3Difa,RR3MinDif,RR3Maxvl,RR3Minvc,RR3SS,RR3Acc,RR3yaw,RR3Accr,RR3Q01,RR3Q02,RR3Q03,RR3Q04,RR3Q05,RR3Q06,RR3Q07,RR3Q08,RR3Q09,RR3Q11,RR3Q12,RR3Q13,RR3Q14,RR3Q15,RR3Q16,RR3Q17,RR3Q18,RR3Q19\n");
		}

		void close(){
			fclose(arch);
		}

	public:
		Archivo(char *filename){
			abrir(filename);
		}
		~Archivo(){
			close();
		}

		void setRegistroR(RegistroR r){
			registror=r;
		}

		void save(){
			fprintf(arch,"%f,%f,%f,%f,%f,%f,%f,%f,%d,%d,%d,%d,%d,%f,%d,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%d,%d,%d,%d,%d,%f,%d,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%d,%d,%d,%d,%d,%f,%d,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f \n",
						registror.t,registror.xe,registror.ye,
						registror.RR1x, registror.RR1y,registror.RR1sup,registror.RR1inf,registror.RR1Difa,registror.RR1MinDif,registror.RR1Maxvl,registror.RR1Minvc,registror.RR1SS,registror.RR1Acc,	registror.RR1yaw,registror.RR1Accr,registror.RR1Q01,registror.RR1Q02,registror.RR1Q03,	registror.RR1Q04,registror.RR1Q05,registror.RR1Q06,registror.RR1Q07,registror.RR1Q08,registror.RR1Q09,registror.RR1Q11,registror.RR1Q12,registror.RR1Q13,registror.RR1Q14,registror.RR1Q15,registror.RR1Q16,registror.RR1Q17,registror.RR1Q18,registror.RR1Q19,
						registror.RR2x,registror.RR2y,registror.RR2sup,registror.RR2inf,registror.RR2Difa,registror.RR2MinDif,registror.RR2Maxvl,registror.RR2Minvc,registror.RR2SS,registror.RR2Acc,registror.RR2yaw,registror.RR2Accr,registror.RR2Q01,registror.RR2Q02,registror.RR2Q03,registror.RR2Q04,registror.RR2Q05,registror.RR2Q06,registror.RR2Q07,registror.RR2Q08,registror.RR2Q09,registror.RR2Q11,registror.RR2Q12,registror.RR2Q13,registror.RR2Q14,registror.RR2Q15,registror.RR2Q16,registror.RR2Q17,registror.RR2Q18,registror.RR2Q19,
						registror.RR3x,registror.RR3y,registror.RR3sup,registror.RR3inf,registror.RR3Difa,registror.RR3MinDif,registror.RR3Maxvl,registror.RR3Minvc,registror.RR3SS,registror.RR3Acc,registror.RR3yaw,registror.RR3Accr,registror.RR3Q01,registror.RR3Q02,registror.RR3Q03,registror.RR3Q04,registror.RR3Q05,registror.RR3Q06,registror.RR3Q07,registror.RR3Q08,registror.RR3Q09,registror.RR3Q11,registror.RR3Q12,registror.RR3Q13,registror.RR3Q14,registror.RR3Q15,registror.RR3Q16,registror.RR3Q17,registror.RR3Q18,registror.RR3Q19
						);
		}
	};

	class Listener;
	class Try_1: public ModelPlugin{
		Listener *listener;
		physics::ModelPtr modelo;
		sdf::ElementPtr sdf;
		std::map<std::string,Union> uniones;
		event::ConnectionPtr conexionUpdate; //http://osrf-distributions.s3.amazonaws.com/gazebo/api/7.1.0/group__gazebo__event.html
		
		std::string estado;
		std::map<std::string,physics::BasePtr> buscar(physics::BasePtr contenedor, const physics::Entity::EntityType &);
		math::Pose pose;//gazebo::math::Pose pose = this->model->gazebo::physics::ModelState::GetPose();


		public:
		void Load(physics::ModelPtr 	_model,sdf::ElementPtr 	_sdf);
		void OnUpdate(const common::UpdateInfo & _info);
		const std::string& getEstado() const {
			return estado;
		}
		void mover(std::string laUnion, double valor);
		void parametrizar(std::string laUnion,std::string tipo, double valor, common::Time TA);
		void pintar(std::string);
		void cargarUniones();
		double calcularRSSI(double modulo, double gamma);
		std::string IARobot(double modulo,double yaw, std::string Robot_name, double timeSim, double x, double y);
		void enviarRos(ros::Publisher publicador, std::string mensaje);
		int getDeseado(double yaw, int Acc_d, bool dir);
		double errorPO(int indice_robot);
		int Accion(double RSSI_sup , double RSSI_inf, int i);
	};
}
#endif
