#include "ModbusServiceWorker.h"
#include "libdiff.h"
#include "globals.h"

#include <iostream>

ModbusServiceWorker::ModbusServiceWorker(int socket, LogFile *log)
    : socket(socket), log(log)
{
    s.Init(socket);
}

void ModbusServiceWorker::Run()
{
    log->LogString("Started ModbusServiceWorker");
    try {
        while (true)
        {
			string header = s.Receive(7);
			string transaction_id = header.substr(0, 2);
			string protocol_id = header.substr(2, 2);
			int length = ((int)(unsigned char)header[5]) | (((int)(unsigned char)header[4]) << 8);
			string unit_identifier = header.substr(6, 1);
			string data = s.Receive(length - 1);
			string answer_data = "";
			int function = data[0];
			if (function == 3)
			{
				LockMutex(register_mutex);
				int address = GetInt(data.substr(1, 2));
				int amount = GetInt(data.substr(3, 2));
				string result = "";
				for (int a = address; a < address + amount; a++)
				{
					int val = 0;
					if (a >= 0 && a <= 65535) val = registers[a];
					result += (char)((val >> 8) & 0xFF);
					result += (char)(val & 0xFF);
				}
				ReleaseMutex(register_mutex);
				answer_data += (char)3;
				answer_data += (char)result.length();
				answer_data += result;
			}
			else if (function == 6)
			{
				LockMutex(register_mutex);
				int address = GetInt(data.substr(1, 2));
				int value = GetInt(data.substr(3, 2));
				if (address >= 0 && address <= 65535)
				{
					registers[address] = value;
				}
				ReleaseMutex(register_mutex);
				answer_data = data;
			}
			else throw (string)("Unknown function " + ToString(function));
			answer_data = unit_identifier + answer_data;
			string length_str = "";
			length_str += (char)((answer_data.length() >> 8) & 0xFF);
			length_str += (char)((answer_data.length()) & 0xFF);
			s.Send(transaction_id + protocol_id + length_str + answer_data);
        }
    }
    catch (string s)
    {
        log->LogString("Exception in ModbusServiceWorker: " + s);
    }
    catch (std::exception e)
    {
        log->LogString("Exception in ModbusServiceWorker: " + ToString(e.what()));
    }
    catch (...)
    {
        log->LogString("Unknown exception in ModbusServiceWorker");
        // непонятно, что случилось, закрываем сокет и все дела
    }
    s.Disconnect();
}

int ModbusServiceWorker::GetInt(const string &s)
{
	int res = 0;
	for (int i = 0; i < (int)s.length(); i++)
	{
		res <<= 8;
		res += (int)(unsigned char)s[i];
	}
	return res;
}

ModbusServiceWorker::~ModbusServiceWorker()
{
    s.Disconnect();
}