#define LOG_MODULE PacketLogModuleSomeIpSdLayer

#include "SomeIpSdLayer.h"
#include <algorithm>
#include <cstring>
#include <sstream>
#include <stdexcept>

namespace pcpp
{

/*
 * SomeIpSdOption
 */
SomeIpSdOption::~SomeIpSdOption()
{
	if (m_ShadowData != nullptr)
		delete[] m_ShadowData;
}

SomeIpSdOption::OptionType SomeIpSdOption::getType() const {
	return static_cast<OptionType>(getSomeIpSdOptionHeader()->type);
}

uint8_t* SomeIpSdOption::getDataPtr() const
{
	if (m_DataContainer != nullptr)
		return m_DataContainer->getDataPtr(m_Offset);

	return m_ShadowData;
}

SomeIpSdOption::someipsdhdroptionsbase *SomeIpSdOption::getSomeIpSdOptionHeader() const
{
	return (someipsdhdroptionsbase *)getDataPtr();
}

void SomeIpSdOption::initStdFields(OptionType type)
{
	someipsdhdroptionsbase *optionHdr = getSomeIpSdOptionHeader();

	optionHdr->type = static_cast<uint8_t>(type);
	/* Length field is excluding length field itself and uint8_t type field */
	optionHdr->length = htobe16((uint16_t)(m_DataLen - sizeof(optionHdr->length) - sizeof(optionHdr->type)));
}

/*
 * SomeIpSdIPv4Option
 */
SomeIpSdIPv4Option::SomeIpSdIPv4Option(IPv4OptionType type, IPv4Address ipAddress, uint16_t port,
									   SomeIpSdProtocolType l4Protocol)
{
	m_DataLen = sizeof(someipsdhdroptionsipv4);
	m_ShadowData = new uint8_t[m_DataLen];
	memset(m_ShadowData, 0, m_DataLen);

	switch(type)
	{
	case IPv4OptionType::IPv4Endpoint:
		initStdFields(OptionType::IPv4Endpoint);
		break;
	case IPv4OptionType::IPv4Multicast:
		initStdFields(OptionType::IPv4Multicast);
		break;
	case IPv4OptionType::IPv4SdEndpoint:
		initStdFields(OptionType::IPv4SdEndpoint);
		break;
	}

	someipsdhdroptionsipv4 *hdr = (someipsdhdroptionsipv4 *)getDataPtr();
	hdr->ipv4Address = ipAddress.toInt();
	hdr->portNumber = htobe16(port);
	hdr->l4Protocol = l4Protocol;
}

SomeIpSdIPv4Option::SomeIpSdIPv4Option(const IDataContainer *dataContainer, size_t offset)
	: SomeIpSdOption(dataContainer, offset)
{
	m_DataLen = sizeof(someipsdhdroptionsipv4);
}

IPv4Address SomeIpSdIPv4Option::getIpAddress() const
{
	someipsdhdroptionsipv4 *hdr = (someipsdhdroptionsipv4 *)getDataPtr();
	IPv4Address ipAddr(hdr->ipv4Address);

	return ipAddr;
}

uint16_t SomeIpSdIPv4Option::getPort() const
{
	someipsdhdroptionsipv4 *hdr = (someipsdhdroptionsipv4 *)getDataPtr();
	return be16toh(hdr->portNumber);
}

SomeIpSdProtocolType SomeIpSdIPv4Option::getProtocol() const
{
	someipsdhdroptionsipv4 *hdr = (someipsdhdroptionsipv4 *)getDataPtr();
	return hdr->l4Protocol;
}

/*
 * SomeIpSdIPv6Option
 */
SomeIpSdIPv6Option::SomeIpSdIPv6Option(IPv6OptionType type, IPv6Address ipAddress, uint16_t port,
									   SomeIpSdProtocolType l4Protocol)
{
	m_DataLen = sizeof(someipsdhdroptionsipv6);
	m_ShadowData = new uint8_t[m_DataLen];
	memset(m_ShadowData, 0, m_DataLen);

	switch(type)
	{
	case IPv6OptionType::IPv6Endpoint:
		initStdFields(OptionType::IPv6Endpoint);
		break;
	case IPv6OptionType::IPv6Multicast:
		initStdFields(OptionType::IPv6Multicast);
		break;
	case IPv6OptionType::IPv6SdEndpoint:
		initStdFields(OptionType::IPv6SdEndpoint);
		break;
	}

	someipsdhdroptionsipv6 *hdr = (someipsdhdroptionsipv6 *)getDataPtr();
	std::memcpy(hdr->ipv6Address, ipAddress.toBytes(), 16);
	hdr->portNumber = htobe16(port);
	hdr->l4Protocol = l4Protocol;
}

SomeIpSdIPv6Option::SomeIpSdIPv6Option(const IDataContainer *dataContainer, size_t offset)
	: SomeIpSdOption(dataContainer, offset)
{
	m_DataLen = sizeof(someipsdhdroptionsipv6);
}

IPv6Address SomeIpSdIPv6Option::getIpAddress() const
{
	someipsdhdroptionsipv6 *hdr = (someipsdhdroptionsipv6 *)getDataPtr();
	IPv6Address ipAddr(hdr->ipv6Address);

	return ipAddr;
}

uint16_t SomeIpSdIPv6Option::getPort() const
{
	someipsdhdroptionsipv6 *hdr = (someipsdhdroptionsipv6 *)getDataPtr();
	return be16toh(hdr->portNumber);
}

SomeIpSdProtocolType SomeIpSdIPv6Option::getProtocol() const
{
	someipsdhdroptionsipv6 *hdr = (someipsdhdroptionsipv6 *)getDataPtr();
	return hdr->l4Protocol;
}

/*
 * SomeIpSdConfigurationOption
 */
SomeIpSdConfigurationOption::SomeIpSdConfigurationOption(const std::string &configurationString)
{
	m_DataLen = configurationString.length() + sizeof(someipsdhdroptionsbase);
	m_ShadowData = new uint8_t[m_DataLen];
	memset(m_ShadowData, 0, m_DataLen);

	initStdFields(OptionType::ConfigurationString);
	std::memcpy(getDataPtr() + sizeof(someipsdhdroptionsbase), configurationString.c_str(),
				configurationString.length());
}

SomeIpSdConfigurationOption::SomeIpSdConfigurationOption(const IDataContainer *dataContainer, size_t offset)
	: SomeIpSdOption(dataContainer, offset)
{
	m_DataLen = sizeof(someipsdhdroptionsbase) - 1 + be16toh(getSomeIpSdOptionHeader()->length);
}

std::string SomeIpSdConfigurationOption::getConfigurationString() const
{
	return std::string((char *)getDataPtr() + sizeof(someipsdhdroptionsbase),
					   be16toh(getSomeIpSdOptionHeader()->length) - 1);
}

/*
 * SomeIpSdLoadBalancingOption
 */
SomeIpSdLoadBalancingOption::SomeIpSdLoadBalancingOption(uint16_t priority, uint16_t weight)
{
	m_DataLen = sizeof(someipsdhdroptionsload);
	m_ShadowData = new uint8_t[m_DataLen];
	memset(m_ShadowData, 0, m_DataLen);

	initStdFields(OptionType::LoadBalancing);

	someipsdhdroptionsload *hdr = (someipsdhdroptionsload *)getDataPtr();
	hdr->priority = htobe16(priority);
	hdr->weight = htobe16(weight);
}

SomeIpSdLoadBalancingOption::SomeIpSdLoadBalancingOption(const IDataContainer *dataContainer, size_t offset)
	: SomeIpSdOption(dataContainer, offset)
{
	m_DataLen = sizeof(someipsdhdroptionsload);
}

uint16_t SomeIpSdLoadBalancingOption::getPriority() const
{
	someipsdhdroptionsload *hdr = (someipsdhdroptionsload *)getDataPtr();
	return be16toh(hdr->priority);
}

uint16_t SomeIpSdLoadBalancingOption::getWeight() const
{
	someipsdhdroptionsload *hdr = (someipsdhdroptionsload *)getDataPtr();
	return be16toh(hdr->weight);
}

/*
 *  SomeIpSdEntry
 */

SomeIpSdEntry::SomeIpSdEntry(EntryType type, uint16_t serviceID, uint16_t instanceID, uint8_t majorVersion,
							 uint32_t TTL, uint32_t minorVersion)
{
	initStdFields(type, serviceID, instanceID, majorVersion, TTL);
	setMinorVersion(minorVersion);
}

SomeIpSdEntry::SomeIpSdEntry(EntryType type, uint16_t serviceID, uint16_t instanceID, uint8_t majorVersion,
							 uint32_t TTL, uint8_t counter, uint16_t eventGroupID)
{
	initStdFields(type, serviceID, instanceID, majorVersion, TTL);
	setCounter(counter);
	setEventgroupId(eventGroupID);
}

SomeIpSdEntry::SomeIpSdEntry(const SomeIpSdLayer *pSomeIpSdLayer, size_t offset)
	: m_Layer(pSomeIpSdLayer), m_Offset(offset), m_ShadowData(nullptr)
{
	EntryType entryType;

	someipsdhdrentry *hdr = getSomeIpSdEntryHeader();
	TypeInternal internalType = static_cast<TypeInternal>(hdr->type);
	auto ttl = getTtl();

	switch(internalType)
	{
	case SomeIpSdEntry::TypeInternal::FindService_Internal:
		entryType = SomeIpSdEntry::EntryType::FindService;
		break;
	case SomeIpSdEntry::TypeInternal::OfferService_Internal:
		if (ttl == 0)
		{
			entryType = EntryType::StopOfferService;
		}
		else
		{
			entryType = EntryType::OfferService;
		}
		break;
	case SomeIpSdEntry::TypeInternal::SubscribeEventgroup_Internal:
		if (ttl == 0)
		{
			entryType = EntryType::StopSubscribeEventgroup;
		}
		else
		{
			entryType = EntryType::SubscribeEventgroup;
		}
		break;
	case SomeIpSdEntry::TypeInternal::SubscribeEventgroupAck_Internal:
		if (ttl == 0)
		{
			entryType = EntryType::SubscribeEventgroupNack;
		}
		else
		{
			entryType = EntryType::SubscribeEventgroupAck;
		}
		break;
	default:
		entryType = EntryType::UnknownEntryType;
		break;
	}

	m_EntryType = entryType;
}

SomeIpSdEntry::~SomeIpSdEntry()
{
	if (m_ShadowData != nullptr)
		delete[] m_ShadowData;
}

uint8_t *SomeIpSdEntry::getDataPtr() const
{
	if (m_Layer != nullptr)
		return m_Layer->getDataPtr(m_Offset);

	return m_ShadowData;
}

SomeIpSdEntry::someipsdhdrentry *SomeIpSdEntry::getSomeIpSdEntryHeader() const
{
	return (someipsdhdrentry *)getDataPtr();
}

uint32_t SomeIpSdEntry::getNumOptions() const
{
	auto *hdr = getSomeIpSdEntryHeader();
	return hdr->nrOpt1 + hdr->nrOpt2;
}

uint16_t SomeIpSdEntry::getServiceId() const
{
	return be16toh(getSomeIpSdEntryHeader()->serviceID);
}

void SomeIpSdEntry::setServiceId(uint16_t serviceId)
{
	getSomeIpSdEntryHeader()->serviceID = htobe16(serviceId);
}

uint16_t SomeIpSdEntry::getInstanceId() const
{
	return be16toh(getSomeIpSdEntryHeader()->instanceID);
}

void SomeIpSdEntry::setInstanceId(uint16_t instanceId)
{
	getSomeIpSdEntryHeader()->instanceID = htobe16(instanceId);
}

uint8_t SomeIpSdEntry::getMajorVersion() const
{
	return (be32toh(getSomeIpSdEntryHeader()->majorVersion_ttl) & ~SOMEIPSD_HDR_ENTRY_MASK_TTL) >> 24;
}

void SomeIpSdEntry::setMajorVersion(uint8_t majorVersion)
{
	someipsdhdrentry *hdr = getSomeIpSdEntryHeader();
	uint32_t val = (majorVersion << 24) | (be32toh(hdr->majorVersion_ttl) & SOMEIPSD_HDR_ENTRY_MASK_TTL);
	hdr->majorVersion_ttl = htobe32(val);
}

uint32_t SomeIpSdEntry::getTtl() const
{
	return be32toh(getSomeIpSdEntryHeader()->majorVersion_ttl) & SOMEIPSD_HDR_ENTRY_MASK_TTL;
}

void SomeIpSdEntry::setTtl(uint32_t ttl)
{
	someipsdhdrentry *hdr = getSomeIpSdEntryHeader();
	uint32_t val = (ttl & SOMEIPSD_HDR_ENTRY_MASK_TTL) | (be32toh(hdr->majorVersion_ttl) & ~SOMEIPSD_HDR_ENTRY_MASK_TTL);
	hdr->majorVersion_ttl = htobe32(val);
}

uint32_t SomeIpSdEntry::getMinorVersion() const
{
	return be32toh(getSomeIpSdEntryHeader()->data);
}

void SomeIpSdEntry::setMinorVersion(uint32_t minorVersion)
{
	getSomeIpSdEntryHeader()->data = htobe32(minorVersion);
}

uint8_t SomeIpSdEntry::getCounter() const
{
	return (uint8_t)((be32toh(getSomeIpSdEntryHeader()->data) >> 16) & 0x0F);
}

void SomeIpSdEntry::setCounter(uint8_t counter)
{
	someipsdhdrentry *hdr = getSomeIpSdEntryHeader();
	hdr->data = htobe32((be32toh(hdr->data) & 0xFFF0FFFF) | ((counter & 0x0F) << 16));
}

uint16_t SomeIpSdEntry::getEventgroupId() const
{
	return (uint16_t)(be32toh(getSomeIpSdEntryHeader()->data) & 0x0000FFFF);
}

void SomeIpSdEntry::setEventgroupId(uint16_t eventgroupID)
{
	someipsdhdrentry *hdr = getSomeIpSdEntryHeader();
	hdr->data = htobe32((be32toh(hdr->data) & 0xFFFF0000) | eventgroupID);
}

void SomeIpSdEntry::initStdFields(EntryType type, uint16_t serviceID, uint16_t instanceID, uint8_t majorVersion,
								  uint32_t TTL)
{
	m_EntryType = type;
	m_Layer = nullptr;
	m_Offset = 0;

	size_t dataLen = sizeof(someipsdhdrentry);
	m_ShadowData = new uint8_t[dataLen];
	memset(m_ShadowData, 0, dataLen);

	someipsdhdrentry *hdr = getSomeIpSdEntryHeader();
	setServiceId(serviceID);
	setInstanceId(instanceID);
	setMajorVersion(majorVersion);
	setTtl(TTL);

	switch (type)
	{
	case EntryType::FindService:
	{
		hdr->type = static_cast<uint8_t>(TypeInternal::FindService_Internal);
		break;
	}
	case EntryType::OfferService:
	case EntryType::StopOfferService:
	{
		hdr->type = static_cast<uint8_t>(TypeInternal::OfferService_Internal);
		break;
	}
	case EntryType::SubscribeEventgroup:
	case EntryType::StopSubscribeEventgroup:
	{
		hdr->type = static_cast<uint8_t>(TypeInternal::SubscribeEventgroup_Internal);
		break;
	}
	case EntryType::SubscribeEventgroupAck:
	case EntryType::SubscribeEventgroupNack:
	{
		hdr->type = static_cast<uint8_t>(TypeInternal::SubscribeEventgroupAck_Internal);
		break;
	}
	default:
		break;
	}
}

/*
 *  SomeIpSdLayer
 */
SomeIpSdLayer::SomeIpSdLayer(uint8_t *data, size_t dataLen, Layer *prevLayer, Packet *packet)
	: SomeIpLayer(data, dataLen, prevLayer, packet)
{
	countOptions(m_NumOptions, data);
}

SomeIpSdLayer::SomeIpSdLayer(uint16_t serviceID, uint16_t methodID, uint16_t clientID, uint16_t sessionID,
							 uint8_t interfaceVersion, MsgType type, uint8_t returnCode, uint8_t flags)
{
	m_Protocol = SomeIP;
	m_DataLen = sizeof(someipsdhdr) + 2 * sizeof(uint32_t);
	m_Data = new uint8_t[m_DataLen];
	memset(m_Data, 0, m_DataLen);

	m_NumOptions = 0;

	setServiceID(serviceID);
	setMethodID(methodID);
	setPayloadLength(sizeof(uint32_t) * 3); // Flags+Reserved, Length Entries, Length Options
	setClientID(clientID);
	setSessionID(sessionID);
	setProtocolVersion(0x01);
	setInterfaceVersion(interfaceVersion);
	setMessageType(type);
	setReturnCode(returnCode);
	setFlags(flags);
}

uint8_t SomeIpSdLayer::getFlags() const
{
	someipsdhdr *hdr = (someipsdhdr *)m_Data;
	return hdr->flags;
}

void SomeIpSdLayer::setFlags(uint8_t flags)
{
	someipsdhdr *hdr = (someipsdhdr *)m_Data;
	hdr->flags = flags;
}

uint32_t SomeIpSdLayer::getNumEntries() const
{
	return (uint32_t)(getLenEntries() / sizeof(SomeIpSdEntry::someipsdhdrentry));
}

uint32_t SomeIpSdLayer::getNumOptions() const
{
	return m_NumOptions;
}

const SomeIpSdLayer::EntriesVec SomeIpSdLayer::getEntries() const
{
	size_t remainingLen = getLenEntries();
	size_t offset = sizeof(someipsdhdr) + sizeof(uint32_t);

	EntriesVec vecEntries;
	EntryPtr entry;

	while (remainingLen > 0)
	{
		entry =	new SomeIpSdEntry(this, offset);

		size_t entryLen = entry->getLength();
		remainingLen -= entryLen;
		offset += entryLen;

		vecEntries.push_back(entry);
	}

	return vecEntries;
};

const SomeIpSdLayer::OptionsVec SomeIpSdLayer::getOptions() const
{
	OptionsVec vecOptions;
	OptionPtr option;

	size_t remainingLen = getLenOptions();
	size_t offset = sizeof(someipsdhdr) + sizeof(uint32_t) + getLenEntries() + sizeof(uint32_t);

	while (remainingLen > 0)
	{
		SomeIpSdOption::someipsdhdroptionsbase *hdr = (SomeIpSdOption::someipsdhdroptionsbase *)(m_Data + offset);
		SomeIpSdOption::OptionType optionType = static_cast<SomeIpSdOption::OptionType>(hdr->type);

		option = parseOption(optionType, offset);

		if (option != nullptr)
		{
			vecOptions.push_back(std::move(option));
		}

		size_t optionLen = be16toh(hdr->length) + 3;
		remainingLen -= optionLen;
		offset += optionLen;
	}

	return vecOptions;
}

const SomeIpSdLayer::OptionsVec SomeIpSdLayer::getOptionsFromEntry(uint32_t index) const
{
	OptionsVec vecOptions;
	OptionPtr option;

	if (index >= getNumEntries())
		return vecOptions;

	size_t remainingLen = getLenOptions();
	size_t offset = sizeof(someipsdhdr) + sizeof(uint32_t) + getLenEntries() + sizeof(uint32_t);

	size_t offsetToEntry = sizeof(someipsdhdr) + sizeof(uint32_t) + index * sizeof(SomeIpSdEntry::someipsdhdrentry);
	SomeIpSdEntry::someipsdhdrentry *hdrEntry = (SomeIpSdEntry::someipsdhdrentry *)(m_Data + offsetToEntry);
	uint8_t startIdxRun1 = hdrEntry->indexFirstOption;
	uint8_t lenRun1 = hdrEntry->nrOpt1;
	uint8_t startIdxRun2 = hdrEntry->indexSecondOption;
	uint8_t lenRun2 = hdrEntry->nrOpt2;

	int idx = 0;

	while (remainingLen > 0)
	{
		SomeIpSdOption::someipsdhdroptionsbase *hdrOption = (SomeIpSdOption::someipsdhdroptionsbase *)(m_Data + offset);

		if (((idx >= startIdxRun1) && (idx < (startIdxRun1 + lenRun1))) ||
			((idx >= startIdxRun2) && (idx < (startIdxRun2 + lenRun2))))
		{
			SomeIpSdOption::OptionType optionType = static_cast<SomeIpSdOption::OptionType>(hdrOption->type);

			option = parseOption(optionType, offset);

			if (option != nullptr)
			{
				vecOptions.push_back(std::move(option));
			}
		}

		size_t optionLen = be16toh(hdrOption->length) + 3;
		remainingLen -= optionLen;
		offset += optionLen;
		++idx;
	}

	return vecOptions;
}

bool SomeIpSdLayer::addOptionTo(uint32_t indexEntry, const SomeIpSdOption &option)
{
	if (indexEntry >= getNumEntries())
	{
		return false;
	}

	uint32_t indexOption = findOption(option);
	bool success = addOptionIndex(indexEntry, indexOption);

	if (!success)
	{
		return false;
	}

	if (indexOption == m_NumOptions)
	{
		addOption(option);
	}

	return true;
}

std::string SomeIpSdLayer::toString() const
{
	std::stringstream dataStream;

	dataStream << "SOME/IP-SD Layer, " << getNumEntries() << " entries, " << getNumOptions() << " options";

	return dataStream.str();
}

uint32_t SomeIpSdLayer::addEntry(const SomeIpSdEntry &entry)
{
	size_t lenEntries = getLenEntries();
	int offsetToAddAt = sizeof(someipsdhdr) + sizeof(uint32_t) + lenEntries;

	extendLayer(offsetToAddAt, entry.getLength());

	setLenEntries(lenEntries + entry.getLength());

	memcpy(m_Data + offsetToAddAt, entry.getDataPtr(), entry.getLength());

	auto hdr = getSomeIpHeader();
	hdr->length = htobe32(be32toh(hdr->length) + (uint32_t)entry.getLength());

	return getNumEntries() - 1;
}

bool SomeIpSdLayer::isDataValid(const uint8_t* data, size_t dataLen)
{
	uint32_t count;
	if (!data ||
		dataLen < sizeof(someipsdhdr) + sizeof(uint32_t) ||
		dataLen < sizeof(someipsdhdr) + sizeof(uint32_t) + getLenEntries(data) + sizeof(uint32_t) ||
		dataLen < sizeof(someipsdhdr) + sizeof(uint32_t) + getLenEntries(data) + sizeof(uint32_t) + getLenOptions(data) ||
		!countOptions(count, data))
	{
		return false;
	}

	return true;
}

bool SomeIpSdLayer::countOptions(uint32_t& count, const uint8_t* data)
{
	size_t offsetOption = sizeof(someipsdhdr) + sizeof(uint32_t) + getLenEntries(data) + sizeof(uint32_t);
	size_t lenOptions = getLenOptions(data);
	uint32_t len = 0;

	count = 0;
	while (len < lenOptions)
	{
		if (len + sizeof(uint16_t) + 3 * sizeof(uint8_t) > lenOptions)
			return false;

		uint32_t lenOption = be16toh(*((uint16_t *)(data + offsetOption + len))) + 3 * sizeof(uint8_t);
		len += lenOption;
		if (len > lenOptions) // the last one must be equal to lenOptions
			return false;

		++(count);
	}
	return true;
}

uint32_t SomeIpSdLayer::findOption(const SomeIpSdOption &option)
{
	size_t offsetOption = sizeof(someipsdhdr) + sizeof(uint32_t) + getLenEntries() + sizeof(uint32_t);

	uint32_t i = 0;
	while (i < m_NumOptions)
	{
		uint32_t lenOption = be16toh(*((uint16_t *)(m_Data + offsetOption))) + 3 * sizeof(uint8_t);

		if (option.getLength() == lenOption)
		{
			if (memcmp(m_Data + offsetOption, option.getDataPtr(), option.getLength()) == 0)
			{
				return i;
			}
		}

		offsetOption += lenOption;
		++i;
	}
	return i;
}

void SomeIpSdLayer::addOption(const SomeIpSdOption &option)
{
	int offsetToAddAt = (int)getHeaderLen();

	extendLayer(offsetToAddAt, option.getLength());
	memcpy(m_Data + offsetToAddAt, option.getDataPtr(), option.getLength());

	setLenOptions(uint32_t(getLenOptions() + option.getLength()));

	auto hdr = getSomeIpHeader();
	hdr->length = htobe32(be32toh(hdr->length) + (uint32_t)option.getLength());

	++m_NumOptions;
}

bool SomeIpSdLayer::addOptionIndex(uint32_t indexEntry, uint32_t indexOffset)
{
	/* The SOME/IP-SD protocol supports two option runs. Runs meaning that two different starting indices with differing
	length can be provided. Of course, this only works if the indices in both runs are consecutive.

		So, indices like this would work:
			1 2 3 ; 7 8

		What wouldn't work is this:
			1 2 3 ; 7 9
			1 3 ; 7 8
	*/

	size_t offsetToAddAt = sizeof(someipsdhdr) + sizeof(uint32_t) + indexEntry*sizeof(SomeIpSdEntry::someipsdhdrentry);
	auto hdrEntry = (SomeIpSdEntry::someipsdhdrentry *)(m_Data + offsetToAddAt);

	uint8_t indexFirstOption = hdrEntry->indexFirstOption;
	uint8_t lenFirstOption = hdrEntry->nrOpt1;

	if (lenFirstOption == 0)
	{
		hdrEntry->indexFirstOption = indexOffset;
		++hdrEntry->nrOpt1;
		return true;
	}

	if (static_cast<uint32_t>(indexFirstOption + lenFirstOption + 1) == indexOffset)
	{
		++hdrEntry->nrOpt1;
		return true;
	}

	uint8_t indexSecondOption = hdrEntry->indexSecondOption;
	uint8_t lenSecondOption = hdrEntry->nrOpt2;

	if (lenSecondOption == 0)
	{
		hdrEntry->indexFirstOption = indexOffset;
		++hdrEntry->nrOpt1;
		return true;
	}

	if (static_cast<uint32_t>(indexSecondOption + lenSecondOption + 1) == indexOffset)
	{
		++hdrEntry->nrOpt2;
		return true;
	}

	return false;
}

SomeIpSdLayer::OptionPtr SomeIpSdLayer::parseOption(SomeIpSdOption::OptionType type, size_t offset) const
{
	switch (type)
	{
	case SomeIpSdOption::OptionType::IPv4Endpoint:
	case SomeIpSdOption::OptionType::IPv4Multicast:
	case SomeIpSdOption::OptionType::IPv4SdEndpoint:
	{
		return new SomeIpSdIPv4Option(this, offset);
	}
	case SomeIpSdOption::OptionType::IPv6Endpoint:
	case SomeIpSdOption::OptionType::IPv6Multicast:
	case SomeIpSdOption::OptionType::IPv6SdEndpoint:
	{
		return new SomeIpSdIPv6Option(this, offset);
	}
	case SomeIpSdOption::OptionType::ConfigurationString:
	{
		return new SomeIpSdConfigurationOption(this, offset);
	}
	case SomeIpSdOption::OptionType::LoadBalancing:
	{
		return new SomeIpSdLoadBalancingOption(this, offset);
	}
	default:
		break;
	}
	return nullptr;
}

size_t SomeIpSdLayer::getLenEntries() const
{
	return getLenEntries(m_Data);
}

size_t SomeIpSdLayer::getLenEntries(const uint8_t* data)
{
	return be32toh(*((uint32_t *)(data + sizeof(someipsdhdr))));
}

size_t SomeIpSdLayer::getLenOptions() const
{
	return getLenOptions(m_Data);
}

size_t SomeIpSdLayer::getLenOptions(const uint8_t* data)
{
	return be32toh(*((uint32_t *)(data + sizeof(someipsdhdr) + sizeof(uint32_t) + getLenEntries(data))));
}

void SomeIpSdLayer::setLenEntries(uint32_t length)
{
	*((uint32_t *)(m_Data + sizeof(someipsdhdr))) = htobe32(length);
}

void SomeIpSdLayer::setLenOptions(uint32_t length)
{
	*((uint32_t *)(m_Data + sizeof(someipsdhdr) + sizeof(uint32_t) + getLenEntries())) = htobe32(length);
}

} // namespace pcpp