#include <stdint.h>
#include <sstream>
#include "elmo.h"

// Elmo Commands

string motorOff("MO=0");
string motorOn("MO=1");

string enableAnalogSumming("RM=1");
string disableAnalogSumming("RM=0");

string motorStop("ST");
string motorStart("BG");

string echoOff("EO=0");
string echoOn("EO=1");

string changeVelocity("JV");

Elmo::~Elmo()
{
	Disconnect();
}

void Elmo::Connect(string& ipAddress, uint16_t portNumber)
{
	m_Socket = new tcp_client_socket(endpoint(ipAddress, portNumber));
}

void Elmo::Command(string& request, string& response)
{
	string command = request + "\r";
	m_ExclusiveAccess.Acquire();
	if (m_Socket->send(command) == -1)
		LostCommunication();
	if (m_Socket->recv(response) == 0)
		LostCommunication();
	m_ExclusiveAccess.Release();
}

void Elmo::Disconnect(void)
{
	if (m_Socket != NULL ) {
		delete m_Socket;
		m_Socket = NULL;
	}
}

void Elmo::Start(void)
{
	string 		response;
	if (m_IsWinch) {
		Command(motorOn, response);
		Command(motorStart, response);
	} else {
		Command(motorOn, response);
		Command(enableAnalogSumming, response);
		Command(motorStart, response);
	}
}

void Elmo::Stop(void)
{
	string 		response;
	if (m_IsWinch) {
		Command(motorStop, response);
		Command(motorOff, response);
	} else {
		Command(motorStop, response);
		Command(disableAnalogSumming, response);
		Command(motorOff, response);
	}
}

void Elmo::JogVelocity(uint32_t velocityInCountsPerSecond)
{
	string response;
	if (m_IsWinch) {
		stringstream ss(changeVelocity);
		ss << "=";
		ss << velocityInCountsPerSecond;
		if (!ss.fail()) {
			string velocityCommand = ss.str();
			Command(velocityCommand, response);
		} else {
			cout << "Illegal Conversion in JogVelocity" << endl;
		}
	}
}

void Elmo::EchoOff(void)
{
	string response;
	Command(echoOff, response);
}

void Elmo::EchoOn(void)
{
	string response;
	Command(echoOn, response);
}

// TO DO:
//     If communication with an ELmo motion communication
//     is lost the profiler should turn off power to Elmo
//     Motion Controller which will engage the brake.
//
//     At this time(5/25) no brake or power switch exists!
//
void Elmo::LostCommunication(void)
{
	if (m_IsWinch)
		cout << "Lost Communication with Winch Elmo"  << endl;
	else
		cout << "Lost COmmunication with Level Wind Elmo" << endl;
}

