#include <stdint.h>
#include <iostream>
#include "socket.h"
#include "user_interface_thread.h"
#include "motor_interogator.h"

using namespace std;

void* UserInterfaceThread::Run(void* parameter)
{
	try {
		tcp_listener telnet(m_TelnetPortNumber);
		telnet.listen();
		for(;;)
			processCommands(telnet.accept()); // Wait for someone to connect
	}
	catch (socket_error_ex& e) {
		cout << "Exception: " << e.what() << endl;
		cout << "Err=" << e.whaterr() << endl;
		cout << "Txt=" << e.whatsystext() << endl;
	}
	catch (socket_error& e) {
		cout << "Exception: " << e.what() << endl;
	}
	return 0;
}

void UserInterfaceThread::processCommands(tcp_socket telnetSocket)
{
	uint32_t	bytesReceived;

	try {
		string 				request;
		string 				response;
		string 				command;
		string::size_type	position;

		telnetSocket.set_input_formatter(new line_input_formatter());
		for (;;) {
			telnetSocket.send("\nEsp Profiler ==> ");
			bytesReceived = telnetSocket.recv(request);
			position = 0;
			command = nextToken(request," \t\r\n", position);
			if (command == "wm") {
				position = position + 1;
				command = nextToken(request, "\r\n", position);
				command += "\r";
				m_Winch.Command(command, response);
				response += "\n";
				if (response.length() >= 0)
					telnetSocket.send(response);
			} else if (command == "lw") {
				position = position + 1;
				command = nextToken(request, "\r\n", position);
				command += "\r";
				m_LevelWind.Command(command, response);
				response += "\n";
				telnetSocket.send(response);
			} else if (command == "wmrpm") {
				position = position + 1;
				command = nextToken(request, "\r\n", position);
				stringstream ss(command);
				double enteredRPM;
				ss >> enteredRPM;
				if (ss.fail()) {
					string s = "Unable to format ";
					s += command;
					s += " as a number!";
					telnetSocket.send(s);
				} else {
					string s = "Setting RPM = " + command;
					m_Controller.SetRPM(enteredRPM);
					telnetSocket.send(s);
				}
			} else if (command == "Start") {
				m_Controller.StartController();
				m_LevelWind.Start();
				m_Winch.Start();
				telnetSocket.send("PID controller started");
			} else if (command == "Stop") {
				m_Controller.StopController();
				m_Winch.Stop();
				m_LevelWind.Stop();
				telnetSocket.send("All Stop");
			} else if (command == "Howdy") {
				telnetSocket.send("Howdy, Partner");
			} else {
				telnetSocket.send("What?");
			}
		}
	}
	catch (socket_error_ex& e) {
		m_Winch.Stop();
		m_LevelWind.Stop();
		cout << "Exception: " << e.what() << endl;
		cout << "Err=" << e.whaterr() << endl;
		cout << "Txt=" << e.whatsystext() << endl;
	}
	catch (socket_error& e) {
		cout << "Exception: " << e.what() << endl;
	}
}

string UserInterfaceThread::nextToken(const string& line,
		const string& delim,
		string::size_type& first)
{
	string::size_type last = line.find_first_of(delim, first);
	string next(line.substr(first, last));
	first = last;
	return next;
}
