#include "Comandos.h"
#include <stdio.h>
#include "std_msgs/String.h"
#include <vector>
#include "Try_1.h"

namespace gazebo{
	bool Comandos::procesar(std::string &comando, Try_1 * robot){

		std::vector<std::string> partes=split(comando, ' ');   //separacion entre comandos
		
		if(partes.size()==0){
			//gzerr<<"No se ha indicado un comando valido."<<"\r\n";
		}else{
			//gzdbg<<partes[0]<<" "<<partes[1]<<"\r\n";
			switch(partes[0][0]){
				case 'd':
					if(partes.size()>1){
						robot->pintar(partes[1]);
					}
					break;
				case 'p':
					//p union tipo valor TiempoAccion
					if(partes.size()>4){
						double temp=atof(partes[4].c_str());
						robot->parametrizar(partes[1],partes[2],std::stod(partes[3]),common::Time(temp));
					}
					break;
				case 'm':
					//m union posicion_radianes
					if(partes.size()>2){
						robot->mover(partes[1],std::stod(partes[2]));
					}
					break;
				case 's':


										//ROS_INFO("durmiendo");
										usleep(atof(partes[1].c_str())*1000);
										//ROS_INFO("despierto");


													break;
				case 'e':
					//e ficheros .. recordar que inicia en la carpeta ros ../Robot/src/Try_1/instrucciones/*.txt
					std::string archivo = "../Robot/src/Try_1/instrucciones/"+partes[1];
					/*if(partes[1]=="REA/A0"){
						timeInicio = -1;
					}*/
					std::ifstream stream(archivo);

					if(stream.good()){
						std::string linea;
						while(!stream.eof()){
							linea.clear();
							std::getline(stream, linea);
							Comandos::procesar(linea, robot);
						}
						stream.close();
					}
					break;


			}
		}
		return false;
	
	}
	
}

std::vector<std::string> split(const std::string &c, char d){
	std::vector<std::string> resultado;
	std::stringstream cs(c);
	std::string parte;
	while(std::getline(cs, parte, d)){
		resultado.push_back(parte);
	}
	
	return resultado;
}
