void IncomingMessageQueue::deserializeAndNotify(unsigned short cur_pos) {
	boost::mutex::scoped_lock lock(_mutex);

	Header* header = new Header(_messageQueue[cur_pos].header);
	Bitstream* bitstream = new Bitstream(_messageQueue[cur_pos].bitstream);

	_lastSeqNumCompleted->at(_lastSeqNumQueueIdx) = header->getSeqNum();
	++_lastSeqNumQueueIdx;
	if (_lastSeqNumQueueIdx == _lastSeqNumQueueLength) {
		_lastSeqNumQueueIdx = 0;
	}

	Message* msg = NULL;

	if (_DEBUG)
		cout
				<< "IncomingMessageQueue::deserializeAndNotify: bitstream->size() = "
				<< bitstream->size() << endl;

	msg = MessageParser::parseMessage(header, bitstream);

	if (msg != NULL) {
		double rxTime = (_messageQueue[cur_pos].endTime
				- _messageQueue[cur_pos].startTime) / cv::getTickFrequency();

		switch (_messageQueue[cur_pos].header.getMsgType()) {
		case MESSAGETYPE_DATA_CTA: {
			((DataCTAMsg*) (msg))->setTxTime(rxTime);
			break;
		}
		case MESSAGETYPE_DATA_ATC: {
			((DataATCMsg*) (msg))->setTxTime(rxTime);
			break;
		}
		default:
			break;
		}

		_nodeNetworkSystem->telosMessageHandler(msg);

	}

//erase the entry from the message queue once notified
	if (_DEBUG)
		cout << "IncomingMessageQueue::deserializeAndNotify: erasing entry"
				<< endl;
	_messageQueue.erase(_messageQueue.begin() + cur_pos);
	if (_DEBUG)
		cout << "IncomingMessageQueue::deserializeAndNotify: entry erased"
				<< endl;

}
void test_poke_normal_case(const Bitstream& original,
                           const Bitstream& changed,
                           const int exit_code,
                           const uint64_t value,
                           const size_t start,
                           const size_t length)
{

    Bitstream max_value(length);

    max_value.set();

    if (start + length > original.size())
    {
        assert(original == changed);
        assert(exit_code == -1);
    }
    else if(value > max_value.to_ulong())
    {
        assert(original == changed);
        assert(exit_code == -2);
    }
    else
    {
        Bitstream sequence(length, value);

        Bitstream cpy = original;

        for(size_t i = 0; i < sequence.size(); i++)
        {
            size_t pos = (cpy.size() - start - length) + i;
            cpy[pos] = sequence[i];
        }

        assert(cpy == changed);
        assert(exit_code == 0);

        for(size_t  i = cpy.size()-start;  i < cpy.size(); i++)
            assert(original[i] == cpy[i]);

        for(size_t  i = cpy.size()-start-length; i < cpy.size()-start; i++)
            assert(changed[i] == cpy[i]);

        for(size_t  i = 0;  i < cpy.size()-start-length; i++)
            assert(original[i] == cpy[i]);
    }
}
void Session::_sendMessageThread() {
	if (_DEBUG)
		cout << "Session::_sendMessageThread" << endl;

	Message* msg;
	double time;

	while (1) {
		_messagesQueue.wait_and_pop(msg);
		if (_deleted) {
			if (_DEBUG)
				cout << "Session::_sendMessageThread: deleted thread" << endl;
			break;
		}

		if (_DEBUG)
			cout << "Session::_sendMessageThread: Message: " << *msg << endl;

		/*
		 * If Camera set DataATC and DataCTA txTime from previous measurements
		 */
		switch (NetworkNode::getMyself()->getType()) {
		case NODETYPE_CAMERA: {
			switch (msg->getType()) {
			case MESSAGETYPE_DATA_ATC: {
				if (_DEBUG)
					cout
							<< "Session::_sendMessageThread: setting ATC send time: "
							<< _txTimeATC << endl;
				((DataATCMsg*) msg)->setTxTime(_txTimeATC);
				break;
			}
			case MESSAGETYPE_DATA_CTA: {
				if (_DEBUG)
					cout
							<< "Session::_sendMessageThread: setting CTA send time: "
							<< _txTimeCTA << endl;
				((DataCTAMsg*) msg)->setTxTime(_txTimeCTA);
				break;
			}
			default:
				break;
			}
			break;
		}
		default:
			break;
		}

		time = cv::getCPUTickCount();
		Bitstream* msgBitstream = msg->getBitStream();
		if (_DEBUG > 2)
			cout << "Session::_sendMessageThread: message serialization time: "
					<< (cv::getCPUTickCount() - time) / cv::getTickFrequency()
					<< " - message bitstream length: " << msgBitstream->size()
					<< " byte" << endl;

		Header header(msg->getSrc(), msg->getDst(), msg->getSeqNum(), 1, 0,
				msg->getType(), msg->getLinkType(), msgBitstream->size());

		bool notifySendEnd = msg->getType() == MESSAGETYPE_DATA_ATC
				|| msg->getType() == MESSAGETYPE_DATA_CTA;

		delete msg;

		time = cv::getCPUTickCount();
		Bitstream* headerBitstream = header.serializeForIpPacket();
		if (_DEBUG > 2)
			cout
					<< "Session::_sendMessageThread: IP header serialization time: "
					<< (cv::getCPUTickCount() - time) / cv::getTickFrequency()
					<< endl;

		vector<unsigned char> outputBitstream;
		outputBitstream.reserve(headerBitstream->size() + msgBitstream->size());
		outputBitstream.insert(outputBitstream.end(), headerBitstream->begin(),
				headerBitstream->end());
		outputBitstream.insert(outputBitstream.end(), msgBitstream->begin(),
				msgBitstream->end());

		boost::system::error_code error;

		write(*_socket,
				boost::asio::buffer(outputBitstream, outputBitstream.size()),
				error);

		delete msgBitstream;

#ifndef GUI
		if (notifySendEnd)
			NodeNetworkSystem::messageSent();
#endif
		if (error) {
			_interface->errorHandler(this, error);
			return;
		}
	}

	if (_DEBUG)
		cout << "Session::_sendMessageThread: End" << endl;

}