#include "motor_interogator.h"
#include "string_to_number.h"
#include <iomanip>
#include <string>

// Elmo Commands

string getActiveCurrent("IQ");
string getReactiveCurrent("ID");
string getPosition("PX");
string getVelocity("VX");
string getVelocityError("VE");
string getPositionError("PE");
string getTemperature("TI[1]");

stringstream& operator<< (stringstream& out, const MotorInterogator& motor)
{
	out.clear();
	cout << "foo1 " << out.str();
	out << "$EP" << motor.m_MotorId						<< ",";
	out << setprecision(5) << motor.m_ReactiveCurrent	<< ",";
	out << setprecision(5) << motor.m_ActiveCurrent		<< ",";
	out << motor.m_Position								<< ",";
	out << motor.m_Velocity								<< ",";
	out << motor.m_PositionError						<< ",";
	out << motor.m_VelocityError						<< ",";
	out << setprecision(2) << motor.m_Temperature		<< ",;";
	out << "\r\n";
	cout << "foo2 " << out.str();
	return(out);
}

void MotorInterogator::Query(void)
{
	string	response;

	m_Elmo.Command(getReactiveCurrent, response);
	m_ReactiveCurrent = StringToNumber<double>(response);

	m_Elmo.Command(getActiveCurrent, response);
	m_ActiveCurrent	= StringToNumber<double>(response);

	m_Elmo.Command(getPosition, response);
	m_Position = StringToNumber<int32_t>(response);

	m_Elmo.Command(getVelocity, response);
	m_Velocity = StringToNumber<int32_t>(response);

	m_Elmo.Command(getPositionError, response);
	m_PositionError = StringToNumber<int32_t>(response);

	m_Elmo.Command(getVelocityError, response);
	m_VelocityError = StringToNumber<int32_t>(response);

	m_Elmo.Command(getTemperature, response);
	m_Temperature = StringToNumber<uint32_t>(response);
}
