#include <fstream>
#include <vector>
#include <iterator>
#include <iostream>
#include <exception>
#include <boost/program_options.hpp>
#include <memory>

#include <openssl/ec.h>
#include <openssl/err.h>
#include "openssl/conf.h"
#include "openssl/err.h"
#include "openssl/engine.h"
#include "openssl/ssl.h"

#include "TTCN3.hh"
#include "EtsiTs103097Module.hh"
#include "EtsiTs102941MessagesCa.hh"

#include "its/utils.hh"
#include "its/itspki-debug.hh"
#include "its/itspki-session.hh"
#include "its/itspki-etsi.hh"

bool ItsPkiException::initialized = false;

ItsPkiSession::ItsPkiSession(ItsPkiInternalData &_idata)
{
	TTCN_EncDec::set_error_behavior(TTCN_EncDec::ET_ALL, TTCN_EncDec::EB_DEFAULT);
	TTCN_EncDec::clear_error();

	idata = &_idata;
}

ItsPkiSession::~ItsPkiSession()
{
        ECKey_Free(sessionTechnicalKey);
        ECKey_Free(sessionItsEcVerificationKey);
        ECKey_Free(sessionItsEcEncryptionKey);
        ECKey_Free(sessionItsAtVerificationKey);
        ECKey_Free(sessionItsAtEncryptionKey);
	OpenSSL_cleanup();
}

OCTETSTRING &
ItsPkiSession::sessionGetItsCanonicalId()
{
        DEBUGC_STREAM_CALLED;
	if (!its_cid.is_bound() || its_cid.lengthof() == 0)
		its_cid = idata->GetItsCanonicalId();

        DEBUGC_STREAM_RETURNS_OK;
	return its_cid;
}

bool
ItsPkiSession::setSKeyContext(OCTETSTRING &skey_id, OCTETSTRING &aes_key, OCTETSTRING &tag)
{
	return etsiServices.setDecryptContextWithSKey(skey_id, aes_key, tag);
};

bool
ItsPkiSession::GetPublicVerificationKey(void *ec_key, IEEE1609dot2BaseTypes::PublicVerificationKey &pubkey)
{
        int nid = -1;
        OCTETSTRING x, y;

        DEBUGC_STREAM_CALLED;
        if (!ECKey_GetPublicKeyComponents(ec_key, nid, x, y))   {
                ERROR_STREAMC << "something wrong with EC PublicKey components" << std::endl;
                return false;
        }

        if (nid == NID_X9_62_prime256v1)   {
                IEEE1609dot2BaseTypes::EccP256CurvePoint ec_point;
                ec_point.uncompressedP256().x() = x;
                ec_point.uncompressedP256().y() = y;
                pubkey.ecdsaNistP256() = ec_point;
        }
        else if (nid == NID_brainpoolP256r1)   {
                IEEE1609dot2BaseTypes::EccP256CurvePoint ec_point;
                ec_point.uncompressedP256().x() = x;
                ec_point.uncompressedP256().y() = y;
                pubkey.ecdsaBrainpoolP256r1() = ec_point;
        }
        else if (nid == NID_brainpoolP384r1)   {
                IEEE1609dot2BaseTypes::EccP384CurvePoint ec_point;
                ec_point.uncompressedP384().x() = x;
                ec_point.uncompressedP384().y() = y;
                pubkey.ecdsaBrainpoolP384r1() = ec_point;
        }
        else    {
                ERROR_STREAMC << "no support for EC curve '" << OBJ_nid2sn(nid) << "'" << std::endl;
                return false;
        }

        DEBUGC_STREAM_RETURNS_OK;
        return true;
}

bool
ItsPkiSession::GetItsEcPublicVerificationKey(IEEE1609dot2BaseTypes::PublicVerificationKey &pubkey)
{
	if (idata == NULL)
		return false;
	void *key = idata->GetItsEcVerificationKey();
	if (key == NULL)
		key = sessionItsEcVerificationKey;
	if (key == NULL)
		return false;

        return GetPublicVerificationKey(key, pubkey);
}

bool
ItsPkiSession::GetItsAtPublicVerificationKey(IEEE1609dot2BaseTypes::PublicVerificationKey &pubkey)
{
        DEBUGC_STREAM_CALLED;

	if (idata == NULL)
		return false;
	void *key = idata->GetItsAtVerificationKey();
	if (key == NULL)
		key = sessionItsAtVerificationKey;
	if (key == NULL)
		return false;

        return GetPublicVerificationKey(key, pubkey);
}

bool
ItsPkiSession::GetPublicEncryptionKey(void *ec_key, IEEE1609dot2BaseTypes::PublicEncryptionKey &pubkey)
{
        int nid = -1;
        OCTETSTRING x, y;

        DEBUGC_STREAM_CALLED;

        if (!ECKey_GetPublicKeyComponents(ec_key, nid, x, y))   {
                ERROR_STREAMC << "something wrong with EC PublicKey components" << std::endl;
                return false;
        }

        if (nid == NID_X9_62_prime256v1)   {
                IEEE1609dot2BaseTypes::EccP256CurvePoint ec_point;
                ec_point.uncompressedP256().x() = x;
                ec_point.uncompressedP256().y() = y;
                pubkey.publicKey().eciesNistP256() = ec_point;
        }
        else if (nid == NID_brainpoolP256r1)   {
                IEEE1609dot2BaseTypes::EccP256CurvePoint ec_point;
                ec_point.uncompressedP256().x() = x;
                ec_point.uncompressedP256().y() = y;
                pubkey.publicKey().eciesBrainpoolP256r1() = ec_point;
        }
        else    {
                ERROR_STREAMC << "unexpected EC curve: " << OBJ_nid2sn(nid) << std::endl;
                return false;
        }

        pubkey.supportedSymmAlg() = IEEE1609dot2BaseTypes::SymmAlgorithm::aes128Ccm;

        DEBUGC_STREAM_RETURNS_OK;
        return true;
}

bool
ItsPkiSession::GetItsEcPublicEncryptionKey(IEEE1609dot2BaseTypes::PublicEncryptionKey &pubkey)
{
	if (idata == NULL)
		return false;
	void *key = idata->GetItsEcEncryptionKey();
	if (key == NULL)
		key = sessionItsEcEncryptionKey;
	if (key == NULL)
		return false;

        return GetPublicEncryptionKey(key, pubkey);
}

bool
ItsPkiSession::GetItsAtPublicEncryptionKey(IEEE1609dot2BaseTypes::PublicEncryptionKey &pubkey)
{
        DEBUGC_STREAM_CALLED;

	if (idata == NULL)
		return false;
	void *key = idata->GetItsAtEncryptionKey();
	if (key == NULL)
		key = sessionItsAtEncryptionKey;
	if (key == NULL)
		return false;

        return GetPublicEncryptionKey(key, pubkey);
}

OCTETSTRING &
ItsPkiSession::sessionGetItsEcCert()
{
	if (idata != NULL && idata->GetItsEcCertBlob().is_bound())
		return idata->GetItsEcCertBlob();
	return sessionItsEcCert;
};

OCTETSTRING &
ItsPkiSession::sessionGetItsEcId()
{
	if (idata != NULL && idata->GetItsEcId().is_bound())
		return idata->GetItsEcId();
	return sessionItsEcId;
};

bool
ItsPkiSession::GetIEEE1609dot2Signature(ItsPkiInternalData &idata, OCTETSTRING &data, OCTETSTRING &signer, void *key,
		IEEE1609dot2BaseTypes::Signature &out_signature)
{
	DEBUGC_STREAM_CALLED;

	if (key == NULL)   {
		ERROR_STREAMC << "invalid argument: no key" << std::endl;
		return false;
	}

	OCTETSTRING key_id;
	if (!ECKey_PublicKeyHashedID(key, key_id))   {
		ERROR_STREAMC << "Cannot get Key ID" << std::endl;
		return false;
	}
	DEBUGC_STREAM << "Signer Key: '" << oct2str(key_id) << "'" << std::endl;

	int nid = ECKey_GetNid(key);

	OCTETSTRING rSig, sSig;
	if (!IEEE1609dot2_Sign(data, signer, key, rSig, sSig))   {
		ERROR_STREAMC << "Signature failed" << std::endl;
		return false;
	}

	dump_ttcn_object(rSig, "Signature::rSig: ");
	dump_ttcn_object(sSig, "Signature::sSig: ");

	DEBUGC_STREAM << "nid '" << OBJ_nid2sn(nid) << "'" << std::endl;
	if (nid == NID_X9_62_prime256v1)   {
                IEEE1609dot2BaseTypes::EccP256CurvePoint curve_point;
                curve_point.x__only() = rSig;

                IEEE1609dot2BaseTypes::EcdsaP256Signature signature;
                signature.rSig() = curve_point;
                signature.sSig() = sSig;

		out_signature.ecdsaNistP256Signature() = signature;
	}
	else if (nid == NID_brainpoolP256r1)  {
                IEEE1609dot2BaseTypes::EccP256CurvePoint curve_point;
                curve_point.x__only() = rSig;

                IEEE1609dot2BaseTypes::EcdsaP256Signature signature;
                signature.rSig() = curve_point;
                signature.sSig() = sSig;

		out_signature.ecdsaBrainpoolP256r1Signature() = signature;
	}
	else if (nid == NID_brainpoolP384r1)   {
                IEEE1609dot2BaseTypes::EccP384CurvePoint curve_point;
                curve_point.x__only() = rSig;

                IEEE1609dot2BaseTypes::EcdsaP384Signature signature;
                signature.rSig() = curve_point;
                signature.sSig() = sSig;

		out_signature.ecdsaBrainpoolP384r1Signature() = signature;
	}
	else   {
		ERROR_STREAMC << "non supported EC curve '" << OBJ_nid2sn(nid) << "'"  << std::endl;
		return false;
	}

	DEBUGC_STREAM_RETURNS_OK;
	return true;
}

bool
ItsPkiSession::IEEE1609dot2_VerifyToBeEncoded(OCTETSTRING &data, OCTETSTRING &signer, void *key,
                OCTETSTRING &rSig, OCTETSTRING &sSig)
{
        DEBUGC_STREAM_CALLED;

	OCTETSTRING key_id;
	if (!ECKey_PublicKeyHashedID(key, key_id))   {
		ERROR_STREAMC << "Cannot get Key ID" << std::endl;
		return false;
	}
	DEBUGC_STREAM << "Verify Key: '" << oct2str(key_id) << "'" << std::endl;

        switch (idata->GetHashAlgorithm())   {
        case IEEE1609dot2BaseTypes::HashAlgorithm::sha256:
		if (!IEEE1609dot2_VerifyWithSha256(key, rSig, sSig, data, signer))  {
                        ERROR_STREAMC << "IEEE1609dot2 SHA256 verify failed" << std::endl;
                        return false;
                }
                break;
        case IEEE1609dot2BaseTypes::HashAlgorithm::sha384:
		if (!IEEE1609dot2_VerifyWithSha384(key, rSig, sSig, data, signer))  {
                        ERROR_STREAMC << "IEEE1609dot2 SHA384 verify failed" << std::endl;
                        return false;
                }
                break;
        default:
                ERROR_STREAMC << "Unsupporteds hash algorithm" << idata->GetHashAlgorithm() << std::endl;
                return false;
        }

        DEBUGC_STREAM_RETURNS_OK;
        return true;
}

bool
ItsPkiSession::IEEE1609dot2_VerifySignedData(IEEE1609dot2::SignedData &signed_data, void *key,
		IEEE1609dot2::SignedDataPayload &ret_payload)
{
        DEBUGC_STREAM_CALLED;

	OCTETSTRING sSig, rSig;
	IEEE1609dot2BaseTypes::Signature signature = signed_data.signature__();
	if (signature.ischosen(IEEE1609dot2BaseTypes::Signature::ALT_ecdsaNistP256Signature))   {
		sSig = signature.ecdsaNistP256Signature().sSig();
		if (signature.ecdsaNistP256Signature().rSig().ischosen(IEEE1609dot2BaseTypes::EccP256CurvePoint::ALT_x__only))   {
			rSig = signature.ecdsaNistP256Signature().rSig().x__only();
		}
		else {
        		ERROR_STREAMC << "not supported EccP256CurvePoint type other then ALT_x__only" << std::endl;
			return false;
		}
	}
	else if (signature.ischosen(IEEE1609dot2BaseTypes::Signature::ALT_ecdsaBrainpoolP256r1Signature))   {
		sSig = signature.ecdsaBrainpoolP256r1Signature().sSig();
		if (signature.ecdsaBrainpoolP256r1Signature().rSig().ischosen(IEEE1609dot2BaseTypes::EccP256CurvePoint::ALT_x__only))   {
			rSig = signature.ecdsaBrainpoolP256r1Signature().rSig().x__only();
		}
		else {
        		ERROR_STREAMC << "not supported EccP256CurvePoint type other then ALT_x__only" << std::endl;
			return false;
		}
	}
	else   {
        	ERROR_STREAMC << "not supported Signature type : " << signature.get_selection() << std::endl;
		return false;
	}

	IEEE1609dot2::ToBeSignedData tbs_data = signed_data.tbsData();
        OCTETSTRING tbs_encoded;
	if (!encToBeSignedData(tbs_data, tbs_encoded))   {
	       	ERROR_STREAMC << "cannot encode ToBeSignedData" << std::endl;
		return false;
	}

	OCTETSTRING signer_encoded;
	if (signed_data.signer().ischosen(IEEE1609dot2::SignerIdentifier::ALT_self__))   {
		signer_encoded = OCTETSTRING(0, NULL);
	}
	else if (signed_data.signer().ischosen(IEEE1609dot2::SignerIdentifier::ALT_digest))  {
		if (idata->GetEAId() == signed_data.signer().digest())   {
			signer_encoded = idata->GetEACertBlob();
		}
		else if (idata->GetAAId() == signed_data.signer().digest())   {
			signer_encoded = idata->GetAACertBlob();
		}
		else if (idata->GetItsEcId() == signed_data.signer().digest())   {
			signer_encoded = idata->GetItsEcCertBlob();
		}
		else   {
	       		ERROR_STREAMC << "Expected to be signed by EA or by AA" << std::endl;
			return false;
		}

		if (!idata->getVerificationKeyFromCertificate(signer_encoded, &key))   {
	       		ERROR_STREAMC << "Cannot get verification key from certificate" << std::endl;
			return false;
		}
	}
	else   {
	       	ERROR_STREAMC << "verify with signer other the 'self' do not implemented" << std::endl;
		return false;
	}

	if (key == NULL)   {
		ERROR_STREAMC << "No key to verify signature" << std::endl;
		return false;
	}

	if (!IEEE1609dot2_VerifyToBeEncoded(tbs_encoded, signer_encoded, key, rSig, sSig))  {
                ERROR_STREAMC << "Cannot verify signature" << std::endl;
                return false;
	}

	ret_payload = tbs_data.payload();

	DEBUGC_STREAM_RETURNS_OK;
        return true;
}

bool
ItsPkiSession::IEEE1609dot2_VerifySignedData_C(IEEE1609dot2::SignedData &signed_data, void *key,
		IEEE1609dot2::Ieee1609Dot2Content &ret_content)
{
        DEBUGC_STREAM_CALLED;

	IEEE1609dot2::SignedDataPayload payload;
	if (!IEEE1609dot2_VerifySignedData(signed_data, key, payload))   {
                ERROR_STREAMC << "cannot verify signed data" << std::endl;
                return false;
	}

	if (payload.data().is_present())   {
		IEEE1609dot2::Ieee1609Dot2Data data = payload.data();
		if (data.protocolVersion() != Ieee1609Dot2Data_ProtocolVersion)   {
                	ERROR_STREAMC << "Unsupported Ieee1609Dot2Data ProtocolVersion : " << data.protocolVersion() << std::endl;
                	return false;
		}
		ret_content = data.content();
	}
	else   {
                ERROR_STREAMC << "expected SignedData payload" << std::endl;
                return false;
	}

	DEBUGC_STREAM_RETURNS_OK;
        return true;
}

bool
ItsPkiSession::IEEE1609dot2_VerifySignedData_H(IEEE1609dot2::SignedData &signed_data, void *key,
		IEEE1609dot2::HashedData &ret_extDataHash)
{
        DEBUGC_STREAM_CALLED;

	IEEE1609dot2::SignedDataPayload payload;
	if (!IEEE1609dot2_VerifySignedData(signed_data, key, payload))   {
                ERROR_STREAMC << "cannot verify signed data" << std::endl;
                return false;
	}

	if (payload.extDataHash().is_present()) {
		ret_extDataHash = payload.extDataHash();
	}
	else   {
                ERROR_STREAMC << "expected SignedData ExtDataHash" << std::endl;
                return false;
	}

        DEBUGC_STREAM_RETURNS_OK;
        return true;
}

bool
ItsPkiSession::EcEnrollmentRequest_InnerEcRequest(ItsPkiInternalData &idata, EtsiTs102941TypesEnrolment::InnerEcRequest &inner_ec_request)
{
	DEBUGC_STREAM_CALLED;

	OCTETSTRING cid = sessionGetItsCanonicalId();
	if (!cid.is_bound() || cid.is_bound() == 0)   {
		ERROR_STREAMC << "failed: ITS Canonical ID is not set" << std::endl;
		return false;
	}

	if (!idata.EcCheckAidSsp())   {
		ERROR_STREAMC << "invalid EC permissions" << std::endl;
		return false;
	}

	// IEEE1609dot2BaseTypes::SequenceOfPsidSsp ssp_seq = idata.EcGetAppPermsSsp();
	// requestedSubjectAttributes := { id := omit, validityPeriod := omit, region := omit, assuranceLevel := omit,
	// 	appPermissions := { { psid := 0, ssp := { bitmapSsp := ''O } } },
	// 	certIssuePermissions := omit }

	IEEE1609dot2BaseTypes::Duration duration = idata.GetItsEcCertDuration();
	INTEGER valid_from = idata.GetItsEcCertValidFrom();
	if (valid_from == 0)
		valid_from = timeNow_InSec();
	IEEE1609dot2BaseTypes::ValidityPeriod validity_period = IEEE1609dot2BaseTypes::ValidityPeriod(valid_from, duration);

	EtsiTs102941BaseTypes::CertificateSubjectAttributes cert_attrs;
	cert_attrs.id() = OMIT_VALUE;
	cert_attrs.validityPeriod() = validity_period;
	cert_attrs.region() = OMIT_VALUE;
	cert_attrs.assuranceLevel() = OMIT_VALUE;
	cert_attrs.appPermissions() = idata.EcGetAppPermsSsp();
	cert_attrs.certIssuePermissions() = OMIT_VALUE;

	// verificationKey := { ecdsaNistP256 := { uncompressedP256 := { x := ''O, y := ''O } } },
	IEEE1609dot2BaseTypes::PublicVerificationKey verif_pubkey;
	if (!GetPublicVerificationKey(sessionGetItsEcVerificationKey(), verif_pubkey))   {
		ERROR_STREAMC << "cannot get Ec PublicVerificationKey: " << std::endl;
		return false;
	}

	EtsiTs102941BaseTypes::PublicKeys pubkeys = EtsiTs102941BaseTypes::PublicKeys(verif_pubkey, OMIT_VALUE);

	// encryptionKey := { supportedSymmAlg := aes128Ccm (0), publicKey := { eciesNistP256 := { uncompressedP256 := { x := ''O, y := ''O } } } }
	if (sessionGetItsEcEncryptionKey() != NULL)   {
		IEEE1609dot2BaseTypes::PublicEncryptionKey encryption_pubkey;
		if (!GetPublicEncryptionKey(sessionGetItsEcEncryptionKey(), encryption_pubkey))   {
			ERROR_STREAMC << "cannot get Ec PublicEncryptionKey: " << std::endl;
			return false;
		}

		pubkeys.encryptionKey() = encryption_pubkey;
	}

	//  InnerEcRequest := { itsId := "", certificateFormat := 1, publicKeys := { verificationKey := { ... }, encryptionKey := { ... } }, requestedSubjectAttributes := { ... } }
	inner_ec_request = EtsiTs102941TypesEnrolment::InnerEcRequest( cid, ItsPkiInternalData::CertificateFormat::ts103097v131, pubkeys, cert_attrs);

	DEBUGC_STREAM_RETURNS_OK;
	return true;
}

INTEGER
ItsPkiSession::timeNow_InSec()
{
	struct tm tm_tm = {0, 0, 0, 1, 0, 2004 - 1900, 0, 0, 0}, lt = {0};
	time_t t = time(NULL);
	localtime_r(&t, &lt);

	int now = (t - lt.tm_gmtoff - mktime(&tm_tm));
	return INTEGER(now);
}

INTEGER
ItsPkiSession::timeNow_InMkSec()
{
	INTEGER now = timeNow_InSec();
	now.set_long_long_val((long long int)(now) * 1000000l);
	return now;
}

bool
ItsPkiSession::EcEnrollmentRequest_HeaderInfo(ItsPkiInternalData &idata, IEEE1609dot2::HeaderInfo &header_info)
{
	DEBUGC_STREAM_CALLED;

	// printf("%s +%i: ########## TODO TAI clock ##########\n", __FILE__, __LINE__);
	INTEGER time_now = timeNow_InMkSec();
	header_info = IEEE1609dot2::HeaderInfo(
			INTEGER(ITS_APP_NAME_SECURED_CERT_REQUEST),
			time_now,
			OMIT_VALUE, OMIT_VALUE, OMIT_VALUE, OMIT_VALUE, OMIT_VALUE, OMIT_VALUE, OMIT_VALUE );

	DEBUGC_STREAM_RETURNS_OK;
	return true;
}

bool
ItsPkiSession::EcEnrollmentRequest_InnerData(ItsPkiInternalData &idata,
		EtsiTs102941TypesEnrolment::InnerEcRequest &inner_request,
		IEEE1609dot2::Ieee1609Dot2Data &inner_data)
{
	DEBUGC_STREAM_CALLED;

	OCTETSTRING inner_ec_request_encoded;
	encInnerEcRequest(inner_request, inner_ec_request_encoded);

	IEEE1609dot2::Ieee1609Dot2Content inner_dot_content;
	inner_dot_content.unsecuredData() = inner_ec_request_encoded;
	inner_dot_content.unsecuredData().set_opaque(&inner_request);

	// data := { protocolVersion := 3, content := { unsecuredData := { ... } } }
	IEEE1609dot2::Ieee1609Dot2Data inner_dot_data = IEEE1609dot2::Ieee1609Dot2Data(Ieee1609Dot2Data_ProtocolVersion, inner_dot_content);
	// payload := { data := { ... }, extDataHash := omit }
	IEEE1609dot2::SignedDataPayload	signed_payload = IEEE1609dot2::SignedDataPayload(inner_dot_data, OMIT_VALUE);

	// headerInfo := { psid := 0, generationTime := 0l,
	// 	expiryTime := omit, generationLocation := omit, p2pcdLearningRequest := omit, missingCrlIdentifier := omit, encryptionKey := omit,
	// 	inlineP2pcdRequest := omit, requestedCertificate := omit }
	IEEE1609dot2::HeaderInfo header_info;
	if (!EcEnrollmentRequest_HeaderInfo(idata, header_info))   {
		ERROR_STREAMC << "cannot build HeaderInfo" << std::endl;
		return false;
	}

	// tbsData := { payload := { ... }, headerInfo  := { ... } }
	IEEE1609dot2::ToBeSignedData tbs = IEEE1609dot2::ToBeSignedData(signed_payload, header_info);
	OCTETSTRING tbs_encoded;
	if (!encToBeSignedData(tbs, tbs_encoded))  {
		ERROR_STREAMC << "cannot encode ToBeSigned" << std::endl;
		return false;
	}

	dump_ttcn_object(tbs_encoded, "To be signed by ITS Ec Verification Key (encoded): ");
	IEEE1609dot2BaseTypes::HashAlgorithm hash_algo = idata.GetHashAlgorithm();
	IEEE1609dot2::SignerIdentifier signer_id;
	signer_id.self__() = ASN_NULL(ASN_NULL_VALUE);

	// signature_ := { ecdsaNistP256Signature := { rSig := { x_only := ''O }, sSig := ''O } }
	IEEE1609dot2BaseTypes::Signature signature;
	if (!GetIEEE1609dot2Signature(idata, tbs_encoded, sessionGetItsEcCert(), sessionGetItsEcVerificationKey(), signature))   {
		ERROR_STREAMC << "signing failed" << std::endl;
		return false;
	}

	// signedData := { hashId := sha256 (0), tbsData := { ... }, signer := { self := NULL }, signature_ := { ... } }
	IEEE1609dot2::SignedData inner_sdata;
	inner_sdata.hashId() = hash_algo;
	inner_sdata.tbsData() = tbs;
	inner_sdata.signer() = signer_id;
	inner_sdata.signature__() = signature;

	// content := { signedData := { ... } }
	IEEE1609dot2::Ieee1609Dot2Content inner_content;
	inner_content.signedData() = inner_sdata;

	// enrolmentRequest := { protocolVersion := 3, content := { signedData := { ...  } } }
	inner_data = IEEE1609dot2::Ieee1609Dot2Data(Ieee1609Dot2Data_ProtocolVersion, inner_content);

	DEBUGC_STREAM_RETURNS_OK;
	return true;
}

bool
ItsPkiSession::EncryptSignedData_ForEa(ItsPkiInternalData &idata, OCTETSTRING &tbe,
		IEEE1609dot2::Ieee1609Dot2Data &ret_encrypted)
{
	DEBUGC_STREAM_CALLED;

	OCTETSTRING cert_blob = idata.GetEACertBlob();
	if (cert_blob.lengthof() == 0)   {
		ERROR_STREAMC << "invalid EA recipient's certificate blob" << std::endl;
		return false;
	}

	OCTETSTRING rr;
	if (!etsiServices.setRecipient(cert_blob, NULL))   {
                ERROR_STREAMC << "cannot setup EncryptFor context" << std::endl;
		return false;
	}
	if (!etsiServices.GetRecipientHashedID8(rr))   {
                ERROR_STREAMC << "cannot setup EncryptFor context" << std::endl;
		return false;
	}

	// data-encrypted := { protocolVersion := 3, content := { encryptedData := {
	// 	recipients := { { certRecipInfo := { recipientId := ''O, encKey := { eciesNistP256 := { v := { compressed_y_0 := ''O }, c := ''O, t := ''O } } } } },
	// 	ciphertext := { aes128ccm := { nonce := ''O, ccmCiphertext := ''O } }
	// } } }
	EtsiTs103097Module::EtsiTs103097Data__Encrypted__My data_encrypted;
	if (!etsiServices.EncryptPayload(tbe, data_encrypted))   {
                ERROR_STREAMC << "failed to encrypt payload" << std::endl;
		return false;
	}
	else if (!data_encrypted.is_bound())   {
                ERROR_STREAMC << "cannot encode encrypted payload" << std::endl;
		return false;
	}
	dump_ttcn_object(data_encrypted, "Data Encrypted': ");

	ret_encrypted = data_encrypted;

	DEBUGC_STREAM_RETURNS_OK;
	return true;
}

bool
ItsPkiSession::EncryptSignedData_ForEa(ItsPkiInternalData &idata, OCTETSTRING &tbe, OCTETSTRING &ret_encrypted)
{
	DEBUGC_STREAM_CALLED;

	IEEE1609dot2::Ieee1609Dot2Data data_encrypted;
	if (!EncryptSignedData_ForEa(idata, tbe, data_encrypted))   {
		ERROR_STREAMC << "failed to encrypt signed data for EA" << std::endl;
		return false;
	}

	if (!encIeee1609Dot2Data(data_encrypted, ret_encrypted))   {
		ERROR_STREAMC << "failed to encode IEEE1609dot2::Ieee1609Dot2Data for EA" << std::endl;
		return false;
	}

	DEBUGC_STREAM_RETURNS_OK;
	return true;
}

bool
ItsPkiSession::EncryptSignedData_ForAa(ItsPkiInternalData &idata, OCTETSTRING &tbe, OCTETSTRING &ret_encrypted)
{
	return false;
}

bool
ItsPkiSession::EncryptSignedData_ForAa(ItsPkiInternalData &idata, OCTETSTRING &tbe, IEEE1609dot2::Ieee1609Dot2Data &ret_encrypted)
{
	DEBUGC_STREAM_CALLED;

	OCTETSTRING cert_blob = idata.GetAACertBlob();

	if (!etsiServices.setRecipient(cert_blob, NULL))   {
                ERROR_STREAMC << "cannot setup EncryptFor AA context" << std::endl;
		return false;
	}

	EtsiTs103097Module::EtsiTs103097Data__Encrypted__My data_encrypted;
	if (!etsiServices.EncryptPayload(tbe, data_encrypted))   {
                ERROR_STREAMC << "failed to encrypt payload for AA" << std::endl;
		return false;
	}
	else if (!data_encrypted.is_bound())   {
                ERROR_STREAMC << "failed to encrypt payload for AA" << std::endl;
		return false;
	}

	ret_encrypted = data_encrypted;
	dump_ttcn_object(ret_encrypted, "returns data 'encrypted for': ");

	DEBUGC_STREAM_RETURNS_OK;
	return true;
}

bool
ItsPkiSession::EcEnrollmentResponse_Parse(OCTETSTRING &recipient_cert, OCTETSTRING &response_raw, OCTETSTRING &ret_cert)
{
	if (!etsiServices.setRecipient(recipient_cert, NULL))   {
		ERROR_STREAMC << "cannot setup Ec Enrollment Parse Response context" << std::endl;
		return false;
	}

	return EcEnrollmentResponse_Parse(response_raw, ret_cert);
}

bool
ItsPkiSession::EcEnrollmentResponse_Parse(OCTETSTRING &response_raw, OCTETSTRING &ret_cert)
{
	if (!EcEnrollmentResponse_Parse(response_raw))
		return false;

	ret_cert = sessionItsEcCert;
	return true;
}

/*
 * TODO: initialized sessionItsEcCert and sessionItsEcId have an impacto onto the not first EC enrollment
 */
bool
ItsPkiSession::EcEnrollmentResponse_Parse(OCTETSTRING &response_raw)
{
	DEBUGC_STREAM_CALLED;

	dump_ttcn_object(response_raw, "EcEnrollment Response: ");
	OCTETSTRING payload;
	if (!etsiServices.DecryptPayload(response_raw, payload))   {
                ERROR_STREAMC << "decrypt payload failed" << std::endl;
                return false;
	}

	EtsiTs103097Module::EtsiTs103097Data__Signed__My response_data_signed = decEtsiTs103097DataSigned(payload);
	dump_ttcn_object(response_data_signed, "EtsiTs103097Module::EtsiTs103097DataSigned response: ");

	IEEE1609dot2::Ieee1609Dot2Content content;
	if (!IEEE1609dot2_VerifySignedData_C(response_data_signed.content().signedData(), NULL, content))   {
                ERROR_STREAMC << "Cannot verify Signed Data signature" << std::endl;
                return false;
	}
	else if (!content.ischosen(IEEE1609dot2::Ieee1609Dot2Content::ALT_unsecuredData))  {
        	ERROR_STREAMC << "expected data type 'Ieee1609Dot2Content::ALT_unsecuredData'" << std::endl;
		return false;
	}
	dump_ttcn_object(content, "Signed data content: ");

	OCTETSTRING inner_unsecured_data = content.unsecuredData();
	EtsiTs102941MessagesCa::EtsiTs102941Data response_inner_data = decEtsiTs102941Data(inner_unsecured_data);
	dump_ttcn_object(response_inner_data, "Response data: ");

	if (!response_inner_data.content().ischosen(EtsiTs102941MessagesCa::EtsiTs102941DataContent::ALT_enrolmentResponse))  {
        	ERROR_STREAMC << "invalid choice" << std::endl;
		return false;
	}
	else if (response_inner_data.content().enrolmentResponse().responseCode() != EtsiTs102941TypesEnrolment::EnrolmentResponseCode::ok)   {
        	ERROR_STREAMC << "enrollment failed with status code '" << response_inner_data.content().enrolmentResponse().responseCode() << "'" << std::endl;
		return false;
	}

	EtsiTs103097Module::EtsiTs103097Certificate cert = response_inner_data.content().enrolmentResponse().certificate();
	dump_ttcn_object(cert, "ITS EC Certificate: ");

	if (!encEtsiTs103097Certificate(cert, sessionItsEcCert))    {
        	ERROR_STREAMC << "cannot encode EtsiTs103097Certificate" << std::endl;
		return false;
	}
	else if (!getEtsiTs103097CertId(sessionItsEcCert, sessionItsEcId))   {
        	ERROR_STREAMC << "cannot set EtsiTs103097Certificate ID" << std::endl;
		return false;
	}

	dump_ttcn_object(sessionItsEcCert, "ITS EC Certificate (encoded): ");

	DEBUGC_STREAM_RETURNS_OK;
	return true;
}

bool
ItsPkiSession::EcEnrollmentResponse_Status(OCTETSTRING &response_raw)
{
	DEBUGC_STREAM_CALLED;

	OCTETSTRING payload;
	if (!etsiServices.DecryptPayload(response_raw, payload))   {
        	ERROR_STREAMC << "cannot decrypt payload" << std::endl;
		return false;
	}

	EtsiTs103097Module::EtsiTs103097Data__Signed__My data_signed = decEtsiTs103097DataSigned(payload);
	IEEE1609dot2::Ieee1609Dot2Content content;
	if (!IEEE1609dot2_VerifySignedData_C(data_signed.content().signedData(), NULL, content))   {
                ERROR_STREAMC << "Cannot verify Signed Data signature" << std::endl;
                return false;
	}
	else if (!content.ischosen(IEEE1609dot2::Ieee1609Dot2Content::ALT_unsecuredData))  {
        	ERROR_STREAMC << "expected data type 'Ieee1609Dot2Content::ALT_unsecuredData'" << std::endl;
		return false;
	}
	dump_ttcn_object(content, "Signed data content: ");

	EtsiTs102941MessagesCa::EtsiTs102941Data inner_data = decEtsiTs102941Data(content.unsecuredData());
	if (!inner_data.content().ischosen(EtsiTs102941MessagesCa::EtsiTs102941DataContent::ALT_enrolmentResponse))   {
        	ERROR_STREAMC << "expected 'EnrollmentResponse' inner data type " << std::endl;
		return false;
	}

	EtsiTs102941TypesEnrolment::EnrolmentResponseCode respCode = inner_data.content().enrolmentResponse().responseCode();
	if (respCode != EtsiTs102941TypesEnrolment::EnrolmentResponseCode::ok)   {
        	ERROR_STREAMC << "expected response code 'OK', received '" << respCode.enum_to_str(respCode) << "'" << std::endl;
		return false;
	}

	DEBUGC_STREAM_RETURNS_OK;
	return true;
}

bool
ItsPkiSession::EcEnrollmentRequest_Parse(OCTETSTRING &request_raw, void *recipient_prvkey, EtsiTs102941TypesEnrolment::InnerEcRequest &inner_ec_request)
{
	DEBUGC_STREAM_CALLED;

	if (!etsiServices.setDecryptContext(recipient_prvkey, NULL))   {
                ERROR_STREAMC << "Cannot set EC enrollment 'Request Parse' context" << std::endl;
                return false;
	}

	OCTETSTRING payload;
	if (!etsiServices.DecryptPayload(request_raw, payload))   {
                ERROR_STREAMC << "decrypt payload failed" << std::endl;
                return false;
	}

	EtsiTs103097Module::EtsiTs103097Data__Signed__My request_data_signed = decEtsiTs103097DataSigned(payload);
	dump_ttcn_object(request_data_signed, "Signed request: ");

	IEEE1609dot2::Ieee1609Dot2Content content;
	if (!IEEE1609dot2_VerifySignedData_C(request_data_signed.content().signedData(), sessionGetTechnicalKey(), content))   {
                ERROR_STREAMC << "Cannot verify Signed Data signature" << std::endl;
                return false;
	}
	else if (!content.ischosen(IEEE1609dot2::Ieee1609Dot2Content::ALT_unsecuredData))  {
        	ERROR_STREAMC << "expected data type 'Ieee1609Dot2Content::ALT_unsecuredData'" << std::endl;
		return false;
	}

	EtsiTs102941MessagesCa::EtsiTs102941Data inner_data;
	try   {
		EtsiTs102941MessagesCa::EtsiTs102941Data_decoder(content.unsecuredData(), inner_data, "OER");
	}
	catch (const TC_Error& tc_error) {
        	ERROR_STREAMC << "cannot decode Ec Enrollment inner-data" << std::endl;
		return false;
	}

	dump_ttcn_object(inner_data, "Inner request data: ");
	if (!inner_data.content().ischosen(EtsiTs102941MessagesCa::EtsiTs102941DataContent::ALT_enrolmentRequest))  {
        	ERROR_STREAMC << "expected data type 'EtsiTs102941DataContent::ALT_enrolmentRequest'" << std::endl;
		return false;
	}

	IEEE1609dot2::Ieee1609Dot2Data enrolment_request = inner_data.content().enrolmentRequest();
	dump_ttcn_object(enrolment_request, "EC enrolment request: ");
	if (enrolment_request.protocolVersion() != Ieee1609Dot2Data_ProtocolVersion)   {
        	ERROR_STREAMC << "Invalid inner data protocol version: " << enrolment_request.protocolVersion() << std::endl;
		return false;
	}
	if (!enrolment_request.content().ischosen(IEEE1609dot2::Ieee1609Dot2Content::ALT_signedData))  {
        	ERROR_STREAMC << "Expected type 'Ieee1609Dot2Content::ALT_signedData'" << std::endl;
		return false;
	}
	IEEE1609dot2::SignedData signed_data = enrolment_request.content().signedData();
	dump_ttcn_object(signed_data, "Signed Data: ");

	if (!IEEE1609dot2_VerifySignedData_C(signed_data, sessionGetItsEcVerificationKey(), content))   {
                ERROR_STREAMC << "Cannot verify Signed Data signature" << std::endl;
                return false;
	}
	else if (!content.ischosen(IEEE1609dot2::Ieee1609Dot2Content::ALT_unsecuredData))  {
        	ERROR_STREAMC << "expected data type 'Ieee1609Dot2Content::ALT_unsecuredData'" << std::endl;
		return false;
	}

	try   {
		EtsiTs102941TypesEnrolment::InnerEcRequest_decoder(content.unsecuredData(), inner_ec_request, "OER");
	}
	catch (const TC_Error& tc_error) {
        	ERROR_STREAMC << "cannot decode InnerEcRequest data" << std::endl;
		return false;
	}
	dump_ttcn_object(inner_ec_request, "Inner EC request: ");

	DEBUGC_STREAM_RETURNS_OK;
	return true;
}

bool
ItsPkiSession::IEEE1609dot2_Sign(OCTETSTRING &data, OCTETSTRING &signer,
                void *key,
                OCTETSTRING &rSig, OCTETSTRING &sSig)
{
        DEBUGC_STREAM_CALLED;

        switch (idata->GetHashAlgorithm())   {
        case IEEE1609dot2BaseTypes::HashAlgorithm::sha256:
                if (!IEEE1609dot2_SignWithSha256(data, signer, key, rSig, sSig))   {
                        ERROR_STREAMC << "IEEE1609dot2 SHA256 signature failed" << std::endl;
                        return false;
                }
                break;
        case IEEE1609dot2BaseTypes::HashAlgorithm::sha384:
                if (!IEEE1609dot2_SignWithSha384(data, signer, key, rSig, sSig))   {
                        ERROR_STREAMC << "IEEE1609dot2 SHA384 signature failed" << std::endl;
                        return false;
                }
                break;
        default:
                ERROR_STREAMC << "Unsupporteds hash algorithm" << idata->GetHashAlgorithm() << std::endl;
                return false;
        }

        DEBUGC_STREAM_RETURNS_OK;
        return true;
}

void *
ItsPkiSession::sessionGetTechnicalKey()
{
	if (idata == NULL)
		return NULL;

	if (idata->GetItsTechnicalKey() != NULL)
		return idata->GetItsTechnicalKey();

	return sessionTechnicalKey;
}

void *
ItsPkiSession::sessionGetItsEcVerificationKey()
{
	if (idata == NULL)
		return NULL;

	if (idata->GetItsEcVerificationKey() != NULL)
		return idata->GetItsEcVerificationKey();

	return sessionItsEcVerificationKey;
}

void *
ItsPkiSession::sessionGetItsEcEncryptionKey()
{
	if (idata == NULL)
		return NULL;

	if (idata->GetItsEcEncryptionKey() != NULL)
		return idata->GetItsEcEncryptionKey();

	return sessionItsEcEncryptionKey;
}

void *
ItsPkiSession::sessionGetItsAtVerificationKey()
{
	if (idata == NULL)
		return NULL;

	if (idata->GetItsAtVerificationKey() != NULL)
		return idata->GetItsAtVerificationKey();

	return sessionItsAtVerificationKey;
}

void *
ItsPkiSession::sessionGetItsAtEncryptionKey()
{
	if (idata == NULL)
		return NULL;

	if (idata->GetItsAtEncryptionKey() != NULL)
		return idata->GetItsAtEncryptionKey();
	return sessionItsAtEncryptionKey;
}

bool
ItsPkiSession::sessionCheckEcEnrollmentArguments(ItsPkiInternalData &idata)
{
	DEBUGC_STREAM_CALLED;

	if (sessionGetTechnicalKey() == NULL)   {
		ERROR_STREAMC << "ItsPkiSession::sessionCheckEcEnrollmentArguments() no ITS Technical Key" << std::endl;
		return false;
	}

	if (!idata.CheckEcEnrollmentArguments())   {
		ERROR_STREAMC << "ItsPkiSession::sessionCheckEcEnrollmentArguments() invalid internal EC enrollment request data" << std::endl;
		return false;
	}

	if ((sessionItsEcVerificationKey == NULL) && (idata.GetItsEcVerificationKey() == NULL))   {
		sessionItsEcVerificationKey = ECKey_GeneratePrivateKey();
		if (sessionItsEcVerificationKey == NULL)   {
			ERROR_STREAMC << "ItsPkiSession::sessionCheckEcEnrollmentArguments() cannot generaete EC Verification key" << std::endl;
			return false;
		}
	}

	if ((sessionItsEcEncryptionKey == NULL) && (idata.GetItsEcEncryptionKey() == NULL) && idata.IsItsEcEncryptionKeyEnabled())   {
		sessionItsEcEncryptionKey = ECKey_GeneratePrivateKey();
       		if (sessionItsEcEncryptionKey == NULL)   {
			ERROR_STREAMC << "ItsPkiSession::sessionCheckEcEnrollmentArguments() cannot generaete EC Encryption key" << std::endl;
			return false;
		}
	}

	// if (!sessionGetCanonicalId(idata).is_bound() || sessionGetCanonicalId(idata).lengthof() == 0)   {
	if (!sessionGetItsCanonicalId().is_bound() || sessionGetItsCanonicalId().lengthof() == 0)   {
		ERROR_STREAMC << "ItsPkiSession::sessionCheckEcEnrollmentArguments() cannot get session ITS Canonical ID" << std::endl;
		return false;
	}

	DEBUGC_STREAM_RETURNS_OK;
	return true;
}

bool
ItsPkiSession::EcEnrollmentRequest_Create(ItsPkiInternalData &idata, OCTETSTRING &request_encoded)
{
	DEBUGC_STREAM_CALLED;

	if (!sessionCheckEcEnrollmentArguments(idata))   {
		ERROR_STREAMC << "ItsPkiSession::EcEnrollmentRequest_Create() invalid Ec session data" << std::endl;
		return false;
	}

	// InnerEcRequest
	EtsiTs102941TypesEnrolment::InnerEcRequest inner_ec_request;
	EcEnrollmentRequest_InnerEcRequest(idata, inner_ec_request);
	dump_ttcn_object(inner_ec_request, "InnerEcRequest : ");

	// EtsiTS102941Data
	IEEE1609dot2::Ieee1609Dot2Data inner_data;
	if (!EcEnrollmentRequest_InnerData(idata, inner_ec_request, inner_data))   {
		ERROR_STREAMC << "cannot compose InnerData " << std::endl;
		return false;
	}

	EtsiTs102941MessagesCa::EtsiTs102941DataContent etsits_102941_data_content;
	etsits_102941_data_content.enrolmentRequest() = inner_data;

	EtsiTs102941MessagesCa::EtsiTs102941Data etsits_102941_data = EtsiTs102941MessagesCa::EtsiTs102941Data(EtsiTs102941Data_Version, etsits_102941_data_content);

	OCTETSTRING encoded_etsits_102941_data;
	if (!encEtsiTs102941Data(etsits_102941_data, encoded_etsits_102941_data))   {
		ERROR_STREAMC << "cannot encode EtsiTs102941MessagesCa::EtsiTs102941Data" << std::endl;
		return false;
	}

	dump_ttcn_object(etsits_102941_data, "EtsiTs102941Data : ");
	dump_ttcn_object(encoded_etsits_102941_data, "EtsiTs102941Data (encoded): ");

	IEEE1609dot2::Ieee1609Dot2Content content_for_payload;
	content_for_payload.unsecuredData() = encoded_etsits_102941_data;
	content_for_payload.unsecuredData().set_opaque(&etsits_102941_data);

	IEEE1609dot2::Ieee1609Dot2Data env_for_payload = IEEE1609dot2::Ieee1609Dot2Data(Ieee1609Dot2Data_ProtocolVersion, content_for_payload);

	IEEE1609dot2::SignedDataPayload	signed_payload = IEEE1609dot2::SignedDataPayload(env_for_payload,  OMIT_VALUE);

	IEEE1609dot2::HeaderInfo header_info;
	if (!EcEnrollmentRequest_HeaderInfo(idata, header_info))   {
		ERROR_STREAMC << "cannot compose HeaderInfo" << std::endl;
		return false;
	}

	IEEE1609dot2::ToBeSignedData tbs = IEEE1609dot2::ToBeSignedData(signed_payload, header_info);
	OCTETSTRING tbs_encoded;
	if (!encToBeSignedData(tbs, tbs_encoded))   {
		ERROR_STREAMC << "cannot encode ToBeSignedData" << std::endl;
		return false;
	}

	dump_ttcn_object(tbs, "ToBeSignedData by Technical Key: ");
	dump_ttcn_object(tbs_encoded, "ToBeSignedData Technical Key (encoded): ");

	IEEE1609dot2BaseTypes::HashAlgorithm hash_algo = idata.GetHashAlgorithm();
	IEEE1609dot2::SignerIdentifier signer_id;
	signer_id.self__() = ASN_NULL(ASN_NULL_VALUE);

	IEEE1609dot2BaseTypes::Signature signature;
	OCTETSTRING signer = OCTETSTRING(0, NULL);
	if (!GetIEEE1609dot2Signature(idata, tbs_encoded, signer, sessionGetTechnicalKey(), signature))   {
		ERROR_STREAMC << "failed to sign" << std::endl;
		return false;
	}

	// EtsiTs103097Data-Signed  (outer)
	IEEE1609dot2::SignedData outer_sdata;
	outer_sdata.hashId() = hash_algo;
	outer_sdata.tbsData() = tbs;
	outer_sdata.signer() = signer_id;
	outer_sdata.signature__() = signature;

	IEEE1609dot2::Ieee1609Dot2Content outer_content;
	outer_content.signedData() = outer_sdata;

	// EtsiTs103097Data-data  (outer)
	IEEE1609dot2::Ieee1609Dot2Data outer_data = IEEE1609dot2::Ieee1609Dot2Data(Ieee1609Dot2Data_ProtocolVersion, outer_content);
        OCTETSTRING outer_data_encoded;
	encIeee1609Dot2Data(outer_data, outer_data_encoded);

	dump_ttcn_object(outer_data, "IEEE1609dot2::Ieee1609Dot2Data : ");
	dump_ttcn_object(outer_data_encoded, "IEEE1609dot2::Ieee1609Dot2Data (encoded): ");

	if (!EncryptSignedData_ForEa(idata, outer_data_encoded, request_encoded))   {
		ERROR_STREAMC << "cannot encrypt signed data for EA" << std::endl;
		return false;
	}
	dump_ttcn_object(request_encoded, "Encrypted and encoded EC request: ");

	DEBUGC_STREAM_RETURNS_OK;
	return true;
}

// At Request
bool
ItsPkiSession::sessionCheckAtEnrollmentArguments(ItsPkiInternalData &idata)
{
	DEBUGC_STREAM_CALLED;

	if (!idata.CheckAtEnrollmentArguments())   {
		ERROR_STREAMC << "ItsPkiSession::sessionCheckAtEnrollmentArguments() invalid internal AT enrollment request data" << std::endl;
		return false;
	}

	if ((sessionItsAtVerificationKey == NULL) && (idata.GetItsAtVerificationKey() == NULL))   {
		sessionItsAtVerificationKey = ECKey_GeneratePrivateKey();
		if (sessionItsAtVerificationKey == NULL)   {
			ERROR_STREAMC << "ItsPkiSession::sessionCheckAtEnrollmentArguments() cannot generaete At Verification key" << std::endl;
			return false;
		}
	}

	if ((sessionItsAtEncryptionKey == NULL) && (idata.GetItsAtEncryptionKey() == NULL) && idata.IsItsAtEncryptionKeyEnabled())   {
		sessionItsAtEncryptionKey = ECKey_GeneratePrivateKey();
       		if (sessionItsAtEncryptionKey == NULL)   {
			ERROR_STREAMC << "ItsPkiSession::sessionCheckAtEnrollmentArguments() cannot generaete At Encryption key" << std::endl;
			return false;
		}
	}

	if (!sessionGetItsCanonicalId().is_bound() || sessionGetItsCanonicalId().lengthof() == 0)   {
		ERROR_STREAMC << "ItsPkiSession::sessionCheckEcEnrollmentArguments() cannot get session ITS Canonical ID" << std::endl;
		return false;
	}

	DEBUGC_STREAM_RETURNS_OK;
	return true;
}

bool
ItsPkiSession::AtEnrollmentRequest_HeaderInfo(ItsPkiInternalData &idata, IEEE1609dot2::HeaderInfo &header_info)
{
	DEBUGC_STREAM_CALLED;
	if (!EcEnrollmentRequest_HeaderInfo(idata, header_info))   {
        	ERROR_STREAMC << "cannot compose AT HeaderInfo " << std::endl;
		return false;
	}

	DEBUGC_STREAM_RETURNS_OK;
	return true;
}

bool
ItsPkiSession::AtEnrollmentRequest_SignedExternalPayload(ItsPkiInternalData &idata,
		EtsiTs102941TypesAuthorization::SharedAtRequest &sharedAtRequest,
		EtsiTs103097Module::EtsiTs103097Data__SignedExternalPayload &ret)
{
	DEBUGC_STREAM_CALLED;

	IEEE1609dot2::HeaderInfo header_info;
	if (!AtEnrollmentRequest_HeaderInfo(idata, header_info))   {
        	ERROR_STREAMC << "failed to compose AT HeaderInfo " << std::endl;
		return false;
	}

	OCTETSTRING sharedAtRequest_encoded;
	if (!encSharedAtRequest(sharedAtRequest, sharedAtRequest_encoded))   {
		ERROR_STREAMC << "cannot encode EtsiTs102941TypesAuthorization::SharedAtRequest" << std::endl;
		return false;
	}
	dump_ttcn_object(sharedAtRequest_encoded, "SharedAtRequest encoded: ");

	OCTETSTRING sharedAtRequest_hash;
	if (!hash_256(sharedAtRequest_encoded, sharedAtRequest_hash))   {
		ERROR_STREAMC << "cannot get hash of sharedAtRequest" << std::endl;
		return false;
	}
	dump_ttcn_object(sharedAtRequest_hash, "SharedAtRequest encoded hash: ");

	IEEE1609dot2::HashedData sharedAtRequest_hashedData;
	sharedAtRequest_hashedData.sha256HashedData() = sharedAtRequest_hash;
	dump_ttcn_object(sharedAtRequest_hashedData, "SharedAtRequest hashedData: ");

	IEEE1609dot2::SignedDataPayload	signed_payload = IEEE1609dot2::SignedDataPayload(OMIT_VALUE, sharedAtRequest_hashedData);

	IEEE1609dot2::ToBeSignedData tbs = IEEE1609dot2::ToBeSignedData(signed_payload, header_info);
	OCTETSTRING tbs_encoded;
	if (!encToBeSignedData(tbs, tbs_encoded))   {
		ERROR_STREAMC << "failed to encode ToBeSignedData" << std::endl;
		return false;
	}
	dump_ttcn_object(tbs_encoded, "ToBeSigned by ITS Ec Verification Key (encoded): ");

	IEEE1609dot2::SignerIdentifier signer_id;
	signer_id.digest() = sessionGetItsEcId();

	IEEE1609dot2BaseTypes::Signature signature;
	if (!GetIEEE1609dot2Signature(idata, tbs_encoded, sessionGetItsEcCert(), sessionGetItsEcVerificationKey(), signature))   {
		ERROR_STREAMC << "failed to sign" << std::endl;
		return false;
	}

	IEEE1609dot2::SignedData sep_sdata;
	sep_sdata.hashId() = idata.GetHashAlgorithm();
	sep_sdata.tbsData() = tbs;
	sep_sdata.signer() = signer_id;
	sep_sdata.signature__() = signature;

	IEEE1609dot2::Ieee1609Dot2Content sep_content;
	sep_content.signedData() = sep_sdata;

	ret = EtsiTs103097Module::EtsiTs103097Data__SignedExternalPayload(Ieee1609Dot2Data_ProtocolVersion, sep_content);
	dump_ttcn_object(ret, "EtsiTs103097Data__SignedExternalPayload: ");

	DEBUGC_STREAM_RETURNS_OK;
	return true;
}

bool
ItsPkiSession::AtEnrollmentRequest_POP(ItsPkiInternalData &idata,
		EtsiTs102941MessagesCa::EtsiTs102941Data &etsiTsdata,
		EtsiTs103097Module::EtsiTs103097Data__Signed__My  &ret)
{
	DEBUGC_STREAM_CALLED;

	OCTETSTRING etsiTsdata_encoded;
	if (!encEtsiTs102941Data(etsiTsdata, etsiTsdata_encoded))   {
		ERROR_STREAMC << "cannot encode EtsiTs102941MessagesCa::EtsiTs102941Data" << std::endl;
		return false;
	}

	IEEE1609dot2::Ieee1609Dot2Content dot2_content;
	dot2_content.unsecuredData() = etsiTsdata_encoded;
	dot2_content.unsecuredData().set_opaque(&etsiTsdata);

	IEEE1609dot2::Ieee1609Dot2Data dot2_data = IEEE1609dot2::Ieee1609Dot2Data(Ieee1609Dot2Data_ProtocolVersion, dot2_content);
	IEEE1609dot2::SignedDataPayload	signed_payload = IEEE1609dot2::SignedDataPayload(dot2_data, OMIT_VALUE);

	IEEE1609dot2::HeaderInfo header_info;
	if (!AtEnrollmentRequest_HeaderInfo(idata, header_info))   {
        	ERROR_STREAMC << "cannot compose AT HeaderInfo " << std::endl;
		return false;
	}

	IEEE1609dot2::ToBeSignedData tbs = IEEE1609dot2::ToBeSignedData(signed_payload, header_info);
	OCTETSTRING tbs_encoded;
	if (!encToBeSignedData(tbs, tbs_encoded))   {
		ERROR_STREAMC << "failed to encode ToBeSignedData" << std::endl;
		return false;
	}
	dump_ttcn_object(tbs_encoded, "To be signed by ITS At Verification Key (encoded): ");

	IEEE1609dot2BaseTypes::HashAlgorithm hash_algo = idata.GetHashAlgorithm();

	IEEE1609dot2::SignerIdentifier signer_id;
	signer_id.self__() = ASN_NULL(ASN_NULL_VALUE);

	IEEE1609dot2BaseTypes::Signature signature;
	OCTETSTRING signer = OCTETSTRING(0, NULL);
	if (!GetIEEE1609dot2Signature(idata, tbs_encoded, signer, sessionGetItsAtVerificationKey(), signature))   {
		ERROR_STREAMC << "failed to sign" << std::endl;
		return false;
	}

	IEEE1609dot2::SignedData signedDataContentSignedData;
	signedDataContentSignedData.hashId() = hash_algo;
	signedDataContentSignedData.tbsData() = tbs;
	signedDataContentSignedData.signer() = signer_id;
	signedDataContentSignedData.signature__() = signature;

	IEEE1609dot2::Ieee1609Dot2Content signedDataContent;
	signedDataContent.signedData() = signedDataContentSignedData;
	ret = EtsiTs103097Module::EtsiTs103097Data__Signed__My(Ieee1609Dot2Data_ProtocolVersion, signedDataContent);

	DEBUGC_STREAM_RETURNS_OK;
	return true;
}

bool
ItsPkiSession::AtEnrollmentRequest_Create(ItsPkiInternalData &idata, OCTETSTRING &ret)
{
	DEBUGC_STREAM_CALLED;

	if (!sessionCheckAtEnrollmentArguments(idata))   {
		ERROR_STREAMC << "ItsPkiSession::AtEnrollmentRequest_Create() invalid At session data" << std::endl;
		return false;
	}

	OCTETSTRING v_pubkey_encoded;
	IEEE1609dot2BaseTypes::PublicVerificationKey v_pubkey;
        if (!GetItsAtPublicVerificationKey(v_pubkey))   {
		ERROR_STREAMC << "cannot get ITS AT PublicVerificationKey" << std::endl;
		return false;
	}
	else if (!encPublicVerificationKey(v_pubkey, v_pubkey_encoded))   {
		ERROR_STREAMC << "cannot encode ITS AT PublicVerificationKey" << std::endl;
		return false;
	}

	IEEE1609dot2BaseTypes::PublicEncryptionKey e_pubkey;
	OCTETSTRING e_pubkey_encoded = OCTETSTRING(0, NULL);
	if(sessionGetItsAtEncryptionKey() != NULL)   {
		if (!GetItsAtPublicEncryptionKey(e_pubkey))   {
			ERROR_STREAMC << "cannot get ITS AT PublicEncryptionKey" << std::endl;
			return false;
		}

		if (!encPublicEncryptionKey(e_pubkey, e_pubkey_encoded))   {
			ERROR_STREAMC << "cannot encode ITS AT PublicEncryptionKey" << std::endl;
			return false;
		}
	}

	OCTETSTRING hmac = random_OCTETSTRING(SHA256_DIGEST_LENGTH);
	dump_ttcn_object(hmac, "HMAC: ");

	OCTETSTRING keyTag;
	OCTETSTRING pub_keys = v_pubkey_encoded + e_pubkey_encoded;
	dump_ttcn_object(pub_keys, "PubKeys encoded: ");

	if (!hmac_sha256(pub_keys, hmac, keyTag))   {
		ERROR_STREAMC << "failed to generate HMAC: " << std::endl;
		return false;
	}
	dump_ttcn_object(keyTag, "KeyTag: ");

	IEEE1609dot2BaseTypes::Duration duration = idata.GetItsAtCertDuration();
	INTEGER valid_from = idata.GetItsAtCertValidFrom();
	if (valid_from == 0)
		valid_from = timeNow_InSec();
	IEEE1609dot2BaseTypes::ValidityPeriod validity_period = IEEE1609dot2BaseTypes::ValidityPeriod(valid_from, duration);

        IEEE1609dot2BaseTypes::SequenceOfPsidSsp ssp_seq = idata.AtGetAppPermsSsp();

        EtsiTs102941BaseTypes::CertificateSubjectAttributes cert_attrs = EtsiTs102941BaseTypes::CertificateSubjectAttributes(
			OMIT_VALUE, validity_period, OMIT_VALUE, OMIT_VALUE, ssp_seq, OMIT_VALUE);

#if 0
	IEEE1609dot2BaseTypes::Duration duration = idata.GetItsEcCertDuration();
	INTEGER valid_from = idata.GetItsEcCertValidFrom();
	if (valid_from == 0)
		valid_from = timeNow_InSec();
	IEEE1609dot2BaseTypes::ValidityPeriod validity_period = IEEE1609dot2BaseTypes::ValidityPeriod(valid_from, duration);

	EtsiTs102941BaseTypes::CertificateSubjectAttributes cert_attrs;
	cert_attrs.id() = OMIT_VALUE;
	cert_attrs.validityPeriod() = validity_period;
	cert_attrs.region() = OMIT_VALUE;
	cert_attrs.assuranceLevel() = OMIT_VALUE;
	cert_attrs.appPermissions() = idata.EcGetAppPermsSsp();
	cert_attrs.certIssuePermissions() = OMIT_VALUE;

#endif

	EtsiTs102941TypesAuthorization::SharedAtRequest sharedAtRequest;
	sharedAtRequest.eaId() = idata.GetEAId();
	sharedAtRequest.keyTag() = keyTag;
	sharedAtRequest.certificateFormat() = ItsPkiInternalData::CertificateFormat::ts103097v131;
	sharedAtRequest.requestedSubjectAttributes() = cert_attrs;
	dump_ttcn_object(sharedAtRequest, "EtsiTs102941TypesAuthorization::SharedAtRequest: ");

	EtsiTs103097Module::EtsiTs103097Data__SignedExternalPayload signedExternalPayload;
	if (!AtEnrollmentRequest_SignedExternalPayload(idata, sharedAtRequest, signedExternalPayload))   {
                ERROR_STREAMC << "cannot create SignedExternalPayload " << std::endl;
                return false;
	}
	dump_ttcn_object(signedExternalPayload, "SignedExternalPayload : ");

	OCTETSTRING signedExternalPayload_encoded;
	if (!encSignedExternalPayload(signedExternalPayload, signedExternalPayload_encoded))   {
                ERROR_STREAMC << "cannot encode SignedExternalPayload " << std::endl;
                return false;
	}

	EtsiTs103097Module::EtsiTs103097Data__Encrypted__My signedExternalPayload_encrypted;
	if (!EncryptSignedData_ForEa(idata, signedExternalPayload_encoded, signedExternalPayload_encrypted))   {
                ERROR_STREAMC << "failed to encrypt signed data for EA" << std::endl;
		return false;
	}
	dump_ttcn_object(signedExternalPayload_encrypted, "signedExternalPayload encrypted: ");

	EtsiTs102941BaseTypes::EcSignature ec_signature;
	ec_signature.encryptedEcSignature() = signedExternalPayload_encrypted;
	dump_ttcn_object(ec_signature, "EcSignature: ");

	// InnerAtRequest { publicKeys, hmacKey, sharedAtRequest, ecSignature, ...
	EtsiTs102941TypesAuthorization::InnerAtRequest innerAtRequest;
	innerAtRequest.hmacKey() = hmac;
	innerAtRequest.sharedAtRequest() = sharedAtRequest;
	innerAtRequest.ecSignature() = ec_signature;

	innerAtRequest.publicKeys() = EtsiTs102941BaseTypes::PublicKeys(v_pubkey, OMIT_VALUE);
	if(sessionGetItsAtEncryptionKey() != NULL)
		innerAtRequest.publicKeys().encryptionKey() = e_pubkey;
	dump_ttcn_object(innerAtRequest.publicKeys(), "innerAtRequest.publicKeys: ");

	EtsiTs102941MessagesCa::EtsiTs102941DataContent atDataContent;
	atDataContent.authorizationRequest() = innerAtRequest;

	// EtsiTs102941Data { InnerAtRequest ...
	EtsiTs102941MessagesCa::EtsiTs102941Data atData = EtsiTs102941MessagesCa::EtsiTs102941Data(EtsiTs102941Data_Version, atDataContent);
	dump_ttcn_object(atData, "EtsiTs102941Data: ");

	EtsiTs103097Module::EtsiTs103097Data__Signed__My signedData;
	if (!AtEnrollmentRequest_POP(idata, atData, signedData))   {
                ERROR_STREAMC << "POP failed" << std::endl;
		return false;
	}
	dump_ttcn_object(signedData, "EtsiTs103097Data__Signed__My: ");

	OCTETSTRING signedData_encoded;
	encEtsiTs103097Data__Signed__My(signedData, signedData_encoded);

	EtsiTs103097Module::EtsiTs103097Data__Encrypted__My dataEncrypted;
	if (!EncryptSignedData_ForAa(idata, signedData_encoded, dataEncrypted))   {
                ERROR_STREAMC << "cannot encrypt Data-Signed for AA " << std::endl;
                return false;
	}
	dump_ttcn_object(dataEncrypted, "EtsiTs102941Data-Encrypted: ");
	if (!encIeee1609Dot2Data(dataEncrypted, ret))   {
                ERROR_STREAMC << "cannot encode encrypted Data-Signed for AA " << std::endl;
                return false;
	}
	dump_ttcn_object(ret, "EtsiTs102941Data-Encrypted (encoded): ");

	DEBUGC_STREAM_RETURNS_OK;
	return true;
}

bool
ItsPkiSession::AtEnrollmentRequest_Parse(OCTETSTRING &request_raw, void *ea_prvkey, void *aa_prvkey)
{
	DEBUGC_STREAM_CALLED;

	if (!etsiServices.setDecryptContext(aa_prvkey, NULL))   {
                ERROR_STREAMC << "Cannot set EC enrollment 'Request Parse' context" << std::endl;
                return false;
	}

	OCTETSTRING payload;
	if (!etsiServices.DecryptPayload(request_raw, payload))   {
                ERROR_STREAMC << "decrypt payload failed" << std::endl;
                return false;
	}

	EtsiTs103097Module::EtsiTs103097Data__Signed__My request_data_signed = decEtsiTs103097DataSigned(payload);
	dump_ttcn_object(request_data_signed, "Signed request: ");

	IEEE1609dot2::Ieee1609Dot2Content content;
	if (!IEEE1609dot2_VerifySignedData_C(request_data_signed.content().signedData(), sessionGetItsAtVerificationKey(), content))   {
                ERROR_STREAMC << "Cannot verify Signed Data signature" << std::endl;
                return false;
	}
	else if (!content.ischosen(IEEE1609dot2::Ieee1609Dot2Content::ALT_unsecuredData))  {
        	ERROR_STREAMC << "expected data type 'Ieee1609Dot2Content::ALT_unsecuredData'" << std::endl;
		return false;
	}

	EtsiTs102941MessagesCa::EtsiTs102941Data etsi_ts_data = decEtsiTs102941Data(content.unsecuredData());
	dump_ttcn_object(etsi_ts_data, "Etsi TS Data: ");

	if (etsi_ts_data.version() != EtsiTs102941Data_Version)  {
        	ERROR_STREAMC << "unsupported EtsiTs102941Data version: " << etsi_ts_data.version() << std::endl;
		return false;
	}
	else if (!etsi_ts_data.content().ischosen(EtsiTs102941MessagesCa::EtsiTs102941DataContent::ALT_authorizationRequest))   {
        	ERROR_STREAMC << "expected 'authorizationRequest' data type" << std::endl;
		return false;
	}

	EtsiTs102941TypesAuthorization::InnerAtRequest inner_at_request = etsi_ts_data.content().authorizationRequest();
	EtsiTs102941BaseTypes::PublicKeys pubkeys = inner_at_request.publicKeys();
	OCTETSTRING hmac_key = inner_at_request.hmacKey();
	EtsiTs102941TypesAuthorization::SharedAtRequest shared_at_request = inner_at_request.sharedAtRequest();
	EtsiTs102941BaseTypes::EcSignature ec_signature = inner_at_request.ecSignature();
	dump_ttcn_object(ec_signature, "EcSignature: ");

        OCTETSTRING pubkeys_encoded;
        if (!encPublicVerificationKey(pubkeys.verificationKey(), pubkeys_encoded))   {
        	ERROR_STREAMC << "cannot encode public verification key" << std::endl;
		return false;
	}
	if (pubkeys.encryptionKey().is_present())    {
        	OCTETSTRING e_pubkey_encoded;
		if (!encPublicEncryptionKey(pubkeys.encryptionKey(), e_pubkey_encoded))   {
        		ERROR_STREAMC << "cannot encode public encryption key" << std::endl;
			return false;
		}
		pubkeys_encoded += e_pubkey_encoded;
	}

        OCTETSTRING key_tag;
        if (!hmac_sha256(pubkeys_encoded, hmac_key, key_tag))   {
                ERROR_STREAMC << "failed to generate HMAC of PublicKeys data" << std::endl;
                return false;
        }
	dump_ttcn_object(key_tag, "PublicKeys tag: ");
	if (shared_at_request.keyTag() != key_tag)   {
                ERROR_STREAMC << "unexpected value of PublicKey HMAC" << std::endl;
                return false;
	}

	OCTETSTRING ec_signature_payload;
	if (ec_signature.ischosen(EtsiTs102941BaseTypes::EcSignature::ALT_encryptedEcSignature))   {
        	EtsiTs103097Module::EtsiTs103097Data__Encrypted__My ec_signature_encrypted = ec_signature.encryptedEcSignature();

		if (!etsiServices.setRecipient(idata->GetEACertBlob(), ea_prvkey))   {
                	ERROR_STREAMC << "cannot setup EncryptFor context" << std::endl;
			return false;
		}
		else if (!etsiServices.DecryptPayload(ec_signature_encrypted, ec_signature_payload))   {
                	ERROR_STREAMC << "decrypt payload failed" << std::endl;
                	return false;
		}
	}
	else if (ec_signature.ischosen(EtsiTs102941BaseTypes::EcSignature::ALT_ecSignature))   {
        	ERROR_STREAMC << "'ecSignature' type of EcSignature do not supported" << std::endl;
		return false;
	}
	else   {
        	ERROR_STREAMC << "unsupported EcSignature type" << std::endl;
		return false;
	}
	dump_ttcn_object(ec_signature_payload, "Decrypted payload: ");

        EtsiTs103097Module::EtsiTs103097Data__SignedExternalPayload external_payload;
	try  {
        	EtsiTs103097Module::EtsiTs103097Data__SignedExternalPayload_decoder(ec_signature_payload, external_payload, "OER");
	}
	catch (const TC_Error& tc_error) {
        	ERROR_STREAMC << "cannot decode EtsiTs103097Data SignedExternalPayload data" << std::endl;
		return false;
	}
	dump_ttcn_object(external_payload, "Decoded Exrternal Payload: ");

	if (external_payload.protocolVersion() != Ieee1609Dot2Data_ProtocolVersion)   {
        	ERROR_STREAMC << "unexpected ExternalPayload protocol version: " << external_payload.protocolVersion() << std::endl;
		return false;
	}
	else if (!external_payload.content().ischosen(IEEE1609dot2::Ieee1609Dot2Content::ALT_signedData))   {
        	ERROR_STREAMC << "unexpected Ieee1609Dot2Content data type of external payload: " << external_payload.content().get_selection() << std::endl;
		return false;
	}

	IEEE1609dot2::HashedData ext_data_hash;
	if (!IEEE1609dot2_VerifySignedData_H(external_payload.content().signedData(), sessionGetItsEcVerificationKey(), ext_data_hash))   {
		ERROR_STREAMC << "Cannot verify external payload signed data " << std::endl;
		return false;
	}

	OCTETSTRING sar_encoded, sar_hash;
	if (!encSharedAtRequest(shared_at_request, sar_encoded))   {
		ERROR_STREAMC << "cannot encode EtsiTs102941TypesAuthorization::SharedAtRequest" << std::endl;
		return false;
	}
	else if (!hash_256(sar_encoded, sar_hash))   {
		ERROR_STREAMC << "cannot get hash of sharedAtRequest" << std::endl;
		return false;
	}
	else if (sar_hash != ext_data_hash.sha256HashedData())   {
		ERROR_STREAMC << "unexpected ExternalDataHashvalue" << std::endl;
		return false;
	}

	DEBUGC_STREAM_RETURNS_OK;
	return true;
}

bool
ItsPkiSession::AtEnrollmentResponse_Status(OCTETSTRING &response_raw)
{
	DEBUGC_STREAM_CALLED;

	OCTETSTRING payload;
	if (!etsiServices.DecryptPayload(response_raw, payload))   {
        	ERROR_STREAMC << "cannot decrypt payload" << std::endl;
		return false;
	}

	EtsiTs103097Module::EtsiTs103097Data__Signed__My data_signed = decEtsiTs103097DataSigned(payload);

	IEEE1609dot2::Ieee1609Dot2Content content;
	if (!IEEE1609dot2_VerifySignedData_C(data_signed.content().signedData(), NULL, content))   {
                ERROR_STREAMC << "Cannot verify Signed Data signature" << std::endl;
                return false;
	}
	else if (!content.ischosen(IEEE1609dot2::Ieee1609Dot2Content::ALT_unsecuredData))  {
        	ERROR_STREAMC << "expected data type 'Ieee1609Dot2Content::ALT_unsecuredData'" << std::endl;
		return false;
	}
	dump_ttcn_object(content, "Signed data content: ");

	EtsiTs102941MessagesCa::EtsiTs102941Data inner_data = decEtsiTs102941Data(content.unsecuredData());
	dump_ttcn_object(inner_data, "Response Inner Data: ");

	if (!inner_data.content().ischosen(EtsiTs102941MessagesCa::EtsiTs102941DataContent::ALT_authorizationResponse))   {
        	ERROR_STREAMC << "expected 'AuthorizationResponse' inner data type " << std::endl;
		return false;
	}

	EtsiTs102941TypesAuthorization::AuthorizationResponseCode respCode = inner_data.content().authorizationResponse().responseCode();
	if (respCode != EtsiTs102941TypesAuthorization::AuthorizationResponseCode::ok)   {
        	ERROR_STREAMC << "expected response code 'OK', received '" << respCode.enum_to_str(respCode) << "'" << std::endl;
		return false;
	}

	DEBUGC_STREAM_RETURNS_OK;
	return true;
}

bool
ItsPkiSession::AtEnrollmentResponse_Parse(OCTETSTRING &response_raw, OCTETSTRING &ret_cert)
{
	DEBUGC_STREAM_CALLED;

	dump_ttcn_object(response_raw, "AtEnrollment Response: ");
	OCTETSTRING payload;
	if (!etsiServices.DecryptPayload(response_raw, payload))   {
                ERROR_STREAMC << "decrypt payload failed" << std::endl;
                return false;
	}

	EtsiTs103097Module::EtsiTs103097Data__Signed__My data_signed = decEtsiTs103097DataSigned(payload);
	dump_ttcn_object(data_signed, "Response EtsiTs103097DataSigned__My: ");

	IEEE1609dot2::Ieee1609Dot2Content content;
	if (!IEEE1609dot2_VerifySignedData_C(data_signed.content().signedData(), NULL, content))   {
                ERROR_STREAMC << "Cannot verify Signed Data signature" << std::endl;
                return false;
	}
	else if (!content.ischosen(IEEE1609dot2::Ieee1609Dot2Content::ALT_unsecuredData))  {
        	ERROR_STREAMC << "expected data type 'Ieee1609Dot2Content::ALT_unsecuredData'" << std::endl;
		return false;
	}
	dump_ttcn_object(content, "Signed data content: ");

	EtsiTs102941MessagesCa::EtsiTs102941Data inner_data = decEtsiTs102941Data(content.unsecuredData());
	dump_ttcn_object(inner_data, "Response Inner Data: ");

	if (!inner_data.content().ischosen(EtsiTs102941MessagesCa::EtsiTs102941DataContent::ALT_authorizationResponse))  {
        	ERROR_STREAMC << "invalid response inner data type: '" << inner_data.content().get_selection() << "'" << std::endl;
		return false;
	}

	EtsiTs102941TypesAuthorization::InnerAtResponse inner_at_response = inner_data.content().authorizationResponse();
	dump_ttcn_object(inner_at_response, "EtsiTs102941TypesAuthorization::InnerAtResponse : ");
	if (inner_at_response.responseCode() != EtsiTs102941TypesAuthorization::AuthorizationResponseCode::ok)   {
        	ERROR_STREAMC << "enrollment failed with response status '" << inner_at_response.responseCode() << "'" << std::endl;
		return false;
	}

	EtsiTs103097Module::EtsiTs103097Certificate cert = inner_at_response.certificate();
	dump_ttcn_object(cert, "ITS AT Certificate: ");

	if (!encEtsiTs103097Certificate(cert, sessionItsAtCert))    {
        	ERROR_STREAMC << "cannot encode AT EtsiTs103097Certificate" << std::endl;
		return false;
	}
	dump_ttcn_object(sessionItsAtCert, "ITS AT Certificate (encoded): ");

	ret_cert = sessionItsAtCert;
	DEBUGC_STREAM_RETURNS_OK;
	return true;
}


#include <EigenFaceTemplate.h>
#include <EigenFaceVector.h>

#include <QBuffer>
#include <QDomDocument>
#include <QIODevice>
#include <QFile>
#include <QStringList>

#include <EigenFace.h>
#include <Return.h>

const QString EigenFaceTemplate::eigenFaceTemplateTagName("INDI-EigenFace-Template");
const QString EigenFaceTemplate::eigenFaceVectorTagName("INDI-EigenFace-EigenFaceVector");

EigenFaceTemplate::EigenFaceTemplate(void)
{
    DEFINE_PROPERTIES_CTORS(EFTEMPLATE_PROPERTIES);
    Return::add(EigenFace::ReturnStatusNull, "INDI EigenFace Null Status", Warning);
}

EigenFaceTemplate::EigenFaceTemplate(const QByteArray & ba)
{
    DEFINE_PROPERTIES_CTORS(EFTEMPLATE_PROPERTIES);
    Return::add(EigenFace::ReturnStatusNull, "INDI EigenFace Null Status", Warning);
    clear();
    EigenFaceTemplate eft;
    eft.read(ba);
    if (eft.isValid())
        *this = eft;
} // QByteArray c'tor

EigenFaceTemplate::~EigenFaceTemplate()
{
}

bool EigenFaceTemplate::isValid(void) const
{
    if (distance_efVector_mmap.isEmpty())
        return false;
    return true;
}

int EigenFaceTemplate::consistency(void) const
{
    int rtn = 0;
    foreach (EigenFaceVector vec, distance_efVector_mmap)
        if (vec.consistency() > rtn)
            rtn = vec.consistency();
    return rtn;
}

void EigenFaceTemplate::add(const EigenFaceTemplate & other)
{
    distance_efVector_mmap += other.distance_efVector_mmap;
}

Return EigenFaceTemplate::read(const QDomElement & de)
{
    Return rtn;

    int k = 0, n = de.attribute("Vectors").toInt();

    QDomElement deVector = de.firstChildElement(EigenFaceTemplate::eigenFaceVectorTagName);
    while ( ! deVector.isNull())
    {
        EigenFaceVector vec;
        rtn = vec.read(deVector);
        if (rtn.isError())
            return rtn;
        if (vec.isValid())
            distance_efVector_mmap.insert(vec.consistency(), vec), ++k;
        deVector = deVector.nextSiblingElement();
    }

    if (k != n)
        return Return(EigenFace::ReturnConflict, "Vector Count", k, n);

    return rtn;
} // read(de)

Return EigenFaceTemplate::read(const QByteArray & ba)
{
    QBuffer buf;
    buf.setData(ba);
    return read(&buf);
} // read(ba)

Return EigenFaceTemplate::read(const QImage & image)
{
    QString xml = image.text("INDIface");
    if (xml.isEmpty())
        return Return(EigenFace::ReturnNoTemplate);

    QDomDocument doc("INDIface");
    QString errorMsg;
    int errorLine, errorColumn;
    if ( ! doc.setContent(xml, &errorMsg, &errorLine, &errorColumn))
        return Return(Return::ReturnXmlParse, "INDIface image",
                      errorMsg, errorLine, errorColumn);

    QDomElement de = doc.firstChildElement(eigenFaceTemplateTagName);
    if (de.isNull())
        return Return(Return::ReturnXmlParse, "INDIface image",
                      "Missing " + eigenFaceTemplateTagName, 0,0);

    return read(de);
} // read(image)

Return EigenFaceTemplate::read(const QString & imageFilename)
{
    QImage image(imageFilename);
    if (image.isNull())
        return Return(EigenFace::ReturnNullImage, imageFilename);
    else
        return read(image);
} // read(imageFilename)

Return EigenFaceTemplate::read(QIODevice * io)
{
    Return rtn;
    int i;

    clear();

    // Check Stream Id
    io->read((char *)&i, sizeof(i));
    if (StreamId != i)
        return Return(EigenFace::ReturnInvalidStructure, "EFT:StreamId");

    // Read data vector
    io->read((char *)&i, sizeof(i));
    while (i--)
    {
        EigenFaceVector vec;
        rtn = vec.read(io);
        if (rtn.isError())
            return rtn;
        if (vec.isValid())
            distance_efVector_mmap.insert(vec.consistency(), vec);
    }

    if (isValid())
        return Return();
    else
        return Return(EigenFace::ReturnInvalidStructure, "Template");
} // read(io)

Return EigenFaceTemplate::write(QDomDocument * doc) const
{
    QDomElement de = doc->firstChildElement(EigenFaceTemplate::eigenFaceTemplateTagName);
    while ( ! de.isNull())
    {
        doc->removeChild(de);
        de = de.nextSiblingElement(EigenFaceTemplate::eigenFaceTemplateTagName);
    }

    QDomElement tag = doc->createElement(EigenFaceTemplate::eigenFaceTemplateTagName);
    Return rtn = write(&tag);
    if ( ! rtn.isError())
        doc->appendChild(tag);

    return rtn;
}

Return EigenFaceTemplate::write(QDomElement * de) const
{
    de->setTagName(EigenFaceTemplate::eigenFaceTemplateTagName);
    de->setAttribute("Vectors", distance_efVector_mmap.size());

    QDomNodeList nodes = de->childNodes();
    for (int i = 0; i < nodes.size(); ++i)
        de->removeChild(nodes.item(i));

    foreach (EigenFaceVector vector, distance_efVector_mmap)
    {
        QDomElement deVector = de->ownerDocument().createElement(EigenFaceTemplate::eigenFaceVectorTagName);
        Return rtn = vector.write(&deVector);
        if (rtn.isError())
            return rtn;
        else
            de->appendChild(deVector);
    }

    return Return();
}

Return EigenFaceTemplate::write(QByteArray * ba) const
{
    Return rtn;

    if ( ! ba)
        return Return(EigenFace::ReturnStatusNull);
    if ( ! isValid())
        return Return(EigenFace::ReturnInvalidStructure, "Template");

    QBuffer buf;
    rtn = write(&buf);
    if (rtn.isError())
        return rtn;

    *ba = buf.data();
    return Return();
}

Return EigenFaceTemplate::write(QIODevice * io) const
{
    if ( ! isValid())
        return Return(EigenFace::ReturnInvalidStructure, "Template");

    // write stream id
    int i = StreamId;
    io->write((char *)&i, sizeof(i));

    i = distance_efVector_mmap.size();
    io->write((char *)&i, sizeof(i));
    foreach(EigenFaceVector vec, distance_efVector_mmap)
    {
        Return rtn = vec.write(io);
        if (rtn.isError())
            return rtn;
    }

    return Return();
} // write(io)

QStringList EigenFaceTemplate::idList(void) const
{
    QStringList result;
    foreach (EigenFaceVector vec, distance_efVector_mmap.values())
        result << vec.id();
    return result;
}

void EigenFaceTemplate::setVectorId(const QString & id)
{
    QMutableMapIterator<qreal, EigenFaceVector> it(distance_efVector_mmap);
    while (it.hasNext())
    {
        it.next();
        EigenFaceVector vec = it.value();
        vec.setId(id);
        it.setValue(vec);
    }
}

void EigenFaceTemplate::removeById(const QString & id)
{
    QMutableMapIterator<qreal, EigenFaceVector> it(distance_efVector_mmap);
    while (it.hasNext())
    {
        it.next();
        if (id == it.value().id())
            it.remove();
    }
}

qreal EigenFaceTemplate::distance(const EigenFaceTemplate & other)
{
    if ( ! isValid() || ! other.isValid())  return -1.0;

    EigenFaceVector vec1(distance_efVector_mmap
                         .value(distance_efVector_mmap.keys().first()));
    EigenFaceVector vec2(other.distance_efVector_mmap
                         .value(other.distance_efVector_mmap.keys().first()));
    return vec1.distance(vec2);
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_RROR_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_RROR_HPP_INCLUDED

#include <boost/simd/function/rol.hpp>
#include <boost/simd/function/ror.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( rror_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bd::scalar_< bd::arithmetic_<A0> >
                          , bd::scalar_< bd::int_<A1> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0, A1 a1) const BOOST_NOEXCEPT
    {
      return (a1 > 0) ? ror(a0, a1) :rol(a0, -a1);
    }
  };
  BOOST_DISPATCH_OVERLOAD ( rror_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bd::scalar_< bd::arithmetic_<A0> >
                          , bd::scalar_< bd::uint_<A1> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0, A1 a1) const BOOST_NOEXCEPT
    {
      return ror(a0, a1);
    }
  };
} } }

#endif


#include <boost/detail/algorithm.hpp>
#include <boost/graph/adjacency_iterator.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/adjacency_list_io.hpp>
#include <boost/graph/adjacency_matrix.hpp>
#include <boost/graph/adj_list_serialize.hpp>
#include <boost/graph/astar_search.hpp>
#include <boost/graph/bandwidth.hpp>
#include <boost/graph/bc_clustering.hpp>
#include <boost/graph/bellman_ford_shortest_paths.hpp>
#include <boost/graph/betweenness_centrality.hpp>
#include <boost/graph/biconnected_components.hpp>
#include <boost/graph/bipartite.hpp>
#include <boost/graph/boyer_myrvold_planar_test.hpp>
#include <boost/graph/boykov_kolmogorov_max_flow.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/bron_kerbosch_all_cliques.hpp>
#include <boost/graph/buffer_concepts.hpp>
#include <boost/graph/chrobak_payne_drawing.hpp>
#include <boost/graph/circle_layout.hpp>
#include <boost/graph/closeness_centrality.hpp>
#include <boost/graph/clustering_coefficient.hpp>
#include <boost/graph/compressed_sparse_row_graph.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/graph/copy.hpp>
#include <boost/graph/core_numbers.hpp>
#include <boost/graph/create_condensation_graph.hpp>
#include <boost/graph/cuthill_mckee_ordering.hpp>
#include <boost/graph/cycle_canceling.hpp>
#include <boost/graph/dag_shortest_paths.hpp>
#include <boost/graph/degree_centrality.hpp>
#include <boost/graph/depth_first_search.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/dijkstra_shortest_paths_no_color_map.hpp>
#include <boost/graph/dimacs.hpp>
#include <boost/graph/directed_graph.hpp>
#include <boost/graph/dll_import_export.hpp>
#include <boost/graph/dominator_tree.hpp>
#include <boost/graph/eccentricity.hpp>
#include <boost/graph/edge_coloring.hpp>
#include <boost/graph/edge_connectivity.hpp>
#include <boost/graph/edge_list.hpp>
#include <boost/graph/edmonds_karp_max_flow.hpp>
#include <boost/graph/erdos_renyi_generator.hpp>
#include <boost/graph/exception.hpp>
#include <boost/graph/exterior_property.hpp>
#include <boost/graph/filtered_graph.hpp>
#include <boost/graph/find_flow_cost.hpp>
#include <boost/graph/floyd_warshall_shortest.hpp>
#include <boost/graph/fruchterman_reingold.hpp>
#include <boost/graph/geodesic_distance.hpp>
#include <boost/graph/graph_archetypes.hpp>
#include <boost/graph/graph_as_tree.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/graph/graphml.hpp>
#include <boost/graph/graph_mutability_traits.hpp>
#include <boost/graph/graph_selectors.hpp>
#include <boost/graph/graph_stats.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/graph/grid_graph.hpp>
#include <boost/graph/gursoy_atun_layout.hpp>
#include <boost/graph/hawick_circuits.hpp>
#include <boost/graph/howard_cycle_ratio.hpp>
#include <boost/graph/incremental_components.hpp>
#include <boost/graph/is_kuratowski_subgraph.hpp>
#include <boost/graph/isomorphism.hpp>
#include <boost/graph/is_straight_line_drawing.hpp>
#include <boost/graph/iteration_macros.hpp>
#include <boost/graph/iteration_macros_undef.hpp>
#include <boost/graph/johnson_all_pairs_shortest.hpp>
#include <boost/graph/kamada_kawai_spring_layout.hpp>
#include <boost/graph/king_ordering.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>
#include <boost/graph/labeled_graph.hpp>
#include <boost/graph/lookup_edge.hpp>
#include <boost/graph/loop_erased_random_walk.hpp>
#include <boost/graph/make_biconnected_planar.hpp>
#include <boost/graph/make_connected.hpp>
#include <boost/graph/make_maximal_planar.hpp>
#include <boost/graph/matrix_as_graph.hpp>
#include <boost/graph/max_cardinality_matching.hpp>
#include <boost/graph/maximum_adjacency_search.hpp>
#include <boost/graph/maximum_weighted_matching.hpp>
#include <boost/graph/mcgregor_common_subgraphs.hpp>
#include <boost/graph/mesh_graph_generator.hpp>
#include <boost/graph/metis.hpp>
#include <boost/graph/metric_tsp_approx.hpp>
#include <boost/graph/minimum_degree_ordering.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/graph/named_graph.hpp>
#include <boost/graph/neighbor_bfs.hpp>
#include <boost/graph/numeric_values.hpp>
#include <boost/graph/one_bit_color_map.hpp>
#include <boost/graph/overloading.hpp>
#include <boost/graph/page_rank.hpp>
#include <boost/graph/planar_canonical_ordering.hpp>
#include <boost/graph/planar_face_traversal.hpp>
#include <boost/graph/plod_generator.hpp>
#include <boost/graph/point_traits.hpp>
#include <boost/graph/prim_minimum_spanning_tree.hpp>
#include <boost/graph/profile.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/property_iter_range.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>
#include <boost/graph/random.hpp>
#include <boost/graph/random_layout.hpp>
#include <boost/graph/random_spanning_tree.hpp>
#include <boost/graph/r_c_shortest_paths.hpp>
#include <boost/graph/read_dimacs.hpp>
#include <boost/graph/relax.hpp>
#include <boost/graph/reverse_graph.hpp>
#include <boost/graph/rmat_graph_generator.hpp>
#include <boost/graph/sequential_vertex_coloring.hpp>
#include <boost/graph/simple_point.hpp>
#include <boost/graph/sloan_ordering.hpp>
#include <boost/graph/smallest_last_ordering.hpp>
#include <boost/graph/small_world_generator.hpp>
#include <boost/graph/ssca_graph_generator.hpp>
#include <boost/graph/st_connected.hpp>
#include <boost/graph/stoer_wagner_min_cut.hpp>
#include <boost/graph/strong_components.hpp>
#include <boost/graph/subgraph.hpp>
#include <boost/graph/successive_shortest_path_nonnegative_weights.hpp>
#include <boost/graph/tiernan_all_cycles.hpp>
#include <boost/graph/topological_sort.hpp>
#include <boost/graph/topology.hpp>
#include <boost/graph/transitive_closure.hpp>
#include <boost/graph/transitive_reduction.hpp>
#include <boost/graph/transpose_graph.hpp>
#include <boost/graph/tree_traits.hpp>
#include <boost/graph/two_bit_color_map.hpp>
#include <boost/graph/two_graphs_common_spanning_trees.hpp>
#include <boost/graph/undirected_dfs.hpp>
#include <boost/graph/undirected_graph.hpp>
#include <boost/graph/use_mpi.hpp>
#include <boost/graph/vector_as_graph.hpp>
#include <boost/graph/vertex_and_edge_range.hpp>
#include <boost/graph/vf2_sub_graph_iso.hpp>
#include <boost/graph/visitors.hpp>
#include <boost/graph/wavefront.hpp>
#include <boost/graph/write_dimacs.hpp>
#include <boost/pending/bucket_sorter.hpp>
#include <boost/pending/container_traits.hpp>
#include <boost/pending/disjoint_sets.hpp>
#include <boost/pending/fenced_priority_queue.hpp>
#include <boost/pending/fibonacci_heap.hpp>
#include <boost/pending/indirect_cmp.hpp>
#include <boost/pending/is_heap.hpp>
#include <boost/pending/mutable_heap.hpp>
#include <boost/pending/mutable_queue.hpp>
#include <boost/pending/property.hpp>
#include <boost/pending/property_serialize.hpp>
#include <boost/pending/queue.hpp>
#include <boost/pending/stringtok.hpp>

#include <sstream>

using namespace std;
using namespace boost;

int
main ()
{
  // This code uses the graphviz parser which is one of the few compiled parts
  // of Boost.Graph.
  //

  // Vertex properties.
  //
  typedef property<vertex_name_t, string, property<vertex_color_t, float>>
      vertex_p;

  // Edge properties.
  //
  typedef property<edge_weight_t, double> edge_p;

  // Graph properties.
  //
  typedef property<graph_name_t, string> graph_p;

  // adjacency_list-based type.
  //
  typedef adjacency_list<vecS, vecS, directedS, vertex_p, edge_p, graph_p>
      graph_t;

  // Construct an empty graph and prepare the dynamic_property_maps.
  //
  graph_t graph (0);
  dynamic_properties dp;

  property_map<graph_t, vertex_name_t>::type name (get (vertex_name, graph));
  dp.property ("node_id", name);

  property_map<graph_t, vertex_color_t>::type mass (get (vertex_color, graph));
  dp.property ("mass", mass);

  property_map<graph_t, edge_weight_t>::type weight (get (edge_weight, graph));
  dp.property ("weight", weight);

  // Use ref_property_map to turn a graph property into a property map.
  //
  boost::ref_property_map<graph_t*, string> gname (
      get_property (graph, graph_name));
  dp.property ("name", gname);

  // Sample graph as an istream.
  //
  istringstream gvgraph (
      "digraph { graph [name=\"graphname\"]  a  c e [mass = 6.66] }");

  return read_graphviz (gvgraph, graph, dp, "node_id") ? 0 : 1;
}


// Copyright 2020 Autoware Foundation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

/**
 * @file memory_monitor.cpp
 * @brief Memory monitor class
 */

#include "system_monitor/mem_monitor/mem_monitor.hpp"

#include "system_monitor/system_monitor_utility.hpp"

#include <boost/process.hpp>

#include <fmt/format.h>

#include <string>
#include <vector>

namespace bp = boost::process;

MemMonitor::MemMonitor(const rclcpp::NodeOptions & options)
: Node("mem_monitor", options),
  updater_(this),
  usage_warn_(declare_parameter<float>("usage_warn", 0.95)),
  usage_error_(declare_parameter<float>("usage_error", 0.99))
{
  gethostname(hostname_, sizeof(hostname_));
  updater_.setHardwareID(hostname_);
  updater_.add("Memory Usage", this, &MemMonitor::checkUsage);
}

void MemMonitor::update() { updater_.force_update(); }

void MemMonitor::checkUsage(diagnostic_updater::DiagnosticStatusWrapper & stat)
{
  // Remember start time to measure elapsed time
  const auto t_start = SystemMonitorUtility::startMeasurement();

  // Get total amount of free and used memory
  bp::ipstream is_out;
  bp::ipstream is_err;
  bp::child c("free -tb", bp::std_out > is_out, bp::std_err > is_err);
  c.wait();
  if (c.exit_code() != 0) {
    std::ostringstream os;
    is_err >> os.rdbuf();
    stat.summary(DiagStatus::ERROR, "free error");
    stat.add("free", os.str().c_str());
    return;
  }

  int level = DiagStatus::OK;
  std::string line;
  int index = 0;
  std::vector<std::string> list;
  float usage;

  /*
   Output example of `free -tb`

             list[0]     list[1]     list[2]     list[3]     list[4]     list[5]     list[6]
   index 0 |               total        used        free      shared  buff/cache   available
   index 1 | Mem:       32809744    12554780    13090376      292840     7164588    19622092
   index 2 | Swap:      33554428     1767680    31786748
   index 3 | Total:     66364172    14322460    44877124
  */
  while (std::getline(is_out, line) && !line.empty()) {
    // Skip header
    if (index <= 0) {
      ++index;
      continue;
    }

    boost::split(list, line, boost::is_space(), boost::token_compress_on);

    // Physical memory
    if (index == 1) {
      // available divided by total is available memory including calculation for buff/cache,
      // so the subtraction of this from 1 gives real usage.
      usage = 1.0f - std::atof(list[6].c_str()) / std::atof(list[1].c_str());

      if (usage >= usage_error_) {
        level = DiagStatus::ERROR;
      } else if (usage >= usage_warn_) {
        level = DiagStatus::WARN;
      }

      stat.addf(fmt::format("{} usage", list[0]), "%.2f%%", usage * 1e+2);
    }

    stat.add(fmt::format("{} total", list[0]), toHumanReadable(list[1]));
    stat.add(fmt::format("{} used", list[0]), toHumanReadable(list[2]));
    stat.add(fmt::format("{} free", list[0]), toHumanReadable(list[3]));

    // Add an additional information for physical memory
    if (index == 1) {
      stat.add(fmt::format("{} shared", list[0]), toHumanReadable(list[4]));
      stat.add(fmt::format("{} buff/cache", list[0]), toHumanReadable(list[5]));
      stat.add(fmt::format("{} available", list[0]), toHumanReadable(list[6]));
    }
    ++index;
  }

  stat.summary(level, usage_dict_.at(level));

  // Measure elapsed time since start time and report
  SystemMonitorUtility::stopMeasurement(t_start, stat);
}

std::string MemMonitor::toHumanReadable(const std::string & str)
{
  const char * units[] = {"B", "K", "M", "G", "T"};
  int count = 0;
  double size = std::atol(str.c_str());

  while (size > 1024) {
    size /= 1024;
    ++count;
  }
  const char * format = (size > 0 && size < 10) ? "{:.1f}{}" : "{:.0f}{}";
  return fmt::format(format, size, units[count]);
}

#include <rclcpp_components/register_node_macro.hpp>
RCLCPP_COMPONENTS_REGISTER_NODE(MemMonitor)


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2008 Roland Lichters
 Copyright (C) 2009, 2014 Jose Aparicio

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#include <boost/make_shared.hpp>

#include <ql/experimental/credit/basket.hpp>
#include <ql/experimental/credit/loss.hpp>
#include <ql/time/daycounters/actualactual.hpp>
#include <ql/experimental/credit/defaultlossmodel.hpp>

using namespace std;

namespace QuantLib {

    Basket::Basket(const Date& refDate,
        const vector<string>& names,
        const vector<Real>& notionals,
        const boost::shared_ptr<Pool> pool,
        Real attachment,
        Real detachment,
        const boost::shared_ptr<Claim>& claim
        )
    : notionals_(notionals),
      pool_(pool),
      claim_(claim),
      attachmentRatio_(attachment),
      detachmentRatio_(detachment),
      basketNotional_(0.0),
      attachmentAmount_(0.0),
      detachmentAmount_(0.0),
      trancheNotional_(0.0),
      refDate_(refDate)
    {
        QL_REQUIRE(!notionals_.empty(), "notionals empty");
        QL_REQUIRE (attachmentRatio_ >= 0 &&
                    attachmentRatio_ <= detachmentRatio_ &&
                    detachmentRatio_ <= 1,
                    "invalid attachment/detachment ratio");
        QL_REQUIRE(pool_, "Empty pool pointer.");
        QL_REQUIRE(notionals_.size() == pool_->size(),
                   "unmatched data entry sizes in basket");

        // registrations relevant to the loss status, not to the expected
        // loss values; those are through models.
        registerWith(Settings::instance().evaluationDate());
        registerWith(claim_);

        computeBasket();

        // At this point Issuers in the pool might or might not have
        //   probability term structures for the defultKeys(eventType+
        //   currency+seniority) entering in this basket. This is not
        //   neccessarily a problem.
        for (Size i = 0; i < notionals_.size(); i++) {
            basketNotional_ += notionals_[i];
            attachmentAmount_ += notionals_[i] * attachmentRatio_;
            detachmentAmount_ += notionals_[i] * detachmentRatio_;
        }
        trancheNotional_ = detachmentAmount_ - attachmentAmount_;
    }

    /*\todo Alternatively send a relinkable handle so it can be changed from
    the outside. In that case reconsider the observability chain.
    */
    void Basket::setLossModel(
        const boost::shared_ptr<DefaultLossModel>& lossModel) {

        if (lossModel_)
            unregisterWith(lossModel_);
        lossModel_ = lossModel;
        if (lossModel_) {
            //recovery quotes, defaults(once Issuer is observable)etc might
            //  trigger us:
            registerWith(lossModel_);
        }
        LazyObject::update(); //<- just set calc=false
    }

    void Basket::performCalculations() const {
        // Calculations for status
        computeBasket();// or we might be called from an statistic member
                        // without being intialized yet (first called)
        QL_REQUIRE(lossModel_, "Basket has no default loss model assigned.");

        /* The model must notify us if the another basket calls it for
        reasignment. The basket works as an argument to the deafult loss models
        so, even if the models dont cache anything, they will be using the wrong
        defautl TS. \todo: This has a possible optimization: the basket
        incorporates trancheability and many models do their compuations
        independently of that (some do but do it inefficiently when asked for
        two tranches on the same basket; e,g, recursive model) so it might be
        more efficient sending the pool only; however the modtionals and other
        basket info are still used.*/
        lossModel_->setBasket(const_cast<Basket*>(this));
    }

    Disposable<vector<Real> > Basket::probabilities(const Date& d) const {
        vector<Real> prob(size());
        vector<DefaultProbKey> defKeys = defaultKeys();
        for (Size j = 0; j < size(); j++)
            prob[j] = pool_->get(pool_->names()[j]).defaultProbability(
                defKeys[j])->defaultProbability(d);
        return prob;
    }

    Real Basket::cumulatedLoss(const Date& endDate) const {
        QL_REQUIRE(endDate >= refDate_,
            "Target date lies before basket inception");
        Real loss = 0.0;
        for (Size i = 0; i < size(); i++) {
            boost::shared_ptr<DefaultEvent> credEvent =
                pool_->get(pool_->names()[i]).defaultedBetween(refDate_,
                    endDate, pool_->defaultKeys()[i]);
            if (credEvent) {
                /* \todo If the event has not settled one would need to
                introduce some model recovery rate (independently of a loss
                model) This remains to be done.
                */
                if(credEvent->hasSettled())
                    loss += claim_->amount(credEvent->date(),
                            // notionals_[i],
                            exposure(pool_->names()[i], credEvent->date()),
                            credEvent->settlement().recoveryRate(
                                pool_->defaultKeys()[i].seniority()));
            }
        }
        return loss;
    }

    Real Basket::settledLoss(const Date& endDate) const {
        QL_REQUIRE(endDate >= refDate_,
            "Target date lies before basket inception");

        Real loss = 0.0;
        for (Size i = 0; i < size(); i++) {
            boost::shared_ptr<DefaultEvent> credEvent =
                pool_->get(pool_->names()[i]).defaultedBetween(refDate_,
                    endDate, pool_->defaultKeys()[i]);
            if (credEvent) {
                if(credEvent->hasSettled()) {
                    loss += claim_->amount(credEvent->date(),
                            //notionals_[i],
                            exposure(pool_->names()[i], credEvent->date()),
                            //NOtice I am requesting an exposure in the past...
                            /* also the seniority does not belong to the
                            counterparty anymore but to the position.....*/
                            credEvent->settlement().recoveryRate(
                                pool_->defaultKeys()[i].seniority()));
                }
            }
        }
        return loss;
    }

    Real Basket::remainingNotional() const {
        return evalDateRemainingNot_;
    }

    Disposable<std::vector<Size> >
        Basket::liveList(const Date& endDate) const {
        std::vector<Size> calcBufferLiveList;
        for (Size i = 0; i < size(); i++)
            if (!pool_->get(pool_->names()[i]).defaultedBetween(
                    refDate_,
                    endDate,
                    pool_->defaultKeys()[i]))
                calcBufferLiveList.push_back(i);

        return calcBufferLiveList;
    }

    Real Basket::remainingNotional(const Date& endDate) const {
        Real notional = 0;
        vector<DefaultProbKey> defKeys = defaultKeys();
        for (Size i = 0; i < size(); i++) {
            if (!pool_->get(pool_->names()[i]).defaultedBetween(refDate_,
                                                        endDate,
                                                        defKeys[i]))
                notional += notionals_[i];
        }
        return notional;
    }

    Disposable<vector<Real> >
        Basket::remainingNotionals(const Date& endDate) const
    {
        QL_REQUIRE(endDate >= refDate_,
            "Target date lies before basket inception");

        std::vector<Real> calcBufferNotionals;
        const std::vector<Size>& alive = liveList(endDate);
        for(Size i=0; i<alive.size(); i++)
            calcBufferNotionals.push_back(
                exposure(pool_->names()[i], endDate)
                );// some better way to trim it?
        return calcBufferNotionals;
    }

    Disposable<std::vector<Probability> >
        Basket::remainingProbabilities(const Date& d) const
    {
        QL_REQUIRE(d >= refDate_, "Target date lies before basket inception");
        vector<Real> prob;
        const std::vector<Size>& alive = liveList();

        for(Size i=0; i<alive.size(); i++)
            prob.push_back(pool_->get(pool_->names()[i]).defaultProbability(
                pool_->defaultKeys()[i])->defaultProbability(d, true));
        return prob;
    }

    /* It is supossed to return the addition of ALL notionals from the
    requested ctpty......*/
    Real Basket::exposure(const std::string& name, const Date& d) const {
        //'this->names_' contains duplicates, contrary to 'pool->names'
        std::vector<std::string>::const_iterator match =
            std::find(pool_->names().begin(), pool_->names().end(), name);
        QL_REQUIRE(match != pool_->names().end(), "Name not in basket.");
        Real totalNotional = 0.;
        do{
            totalNotional +=
             // NOT IMPLEMENTED YET:
    //positions_[std::distance(names_.begin(), match)]->expectedExposure(d);
                notionals_[std::distance(pool_->names().begin(), match)];
            match++;
            match = std::find(match, pool_->names().end(), name);
        }while(match != pool_->names().end());

        return totalNotional;
        //Size position = std::distance(poolNames.begin(),
        //    std::find(poolNames.begin(), poolNames.end(), name));
        //QL_REQUIRE(position < pool_->size(), "Name not in pool list");

        //return positions_[position]->expectedExposure(d);
    }

    Disposable<std::vector<std::string> >
        Basket::remainingNames(const Date& endDate) const
    {
        // maybe return zero directly instead?:
        QL_REQUIRE(endDate >= refDate_,
            "Target date lies before basket inception");

        const std::vector<Size>& alive = liveList(endDate);
        std::vector<std::string> calcBufferNames;
        for(Size i=0; i<alive.size(); i++)
            calcBufferNames.push_back(pool_->names()[alive[i]]);
        return calcBufferNames;
    }

    Disposable<vector<DefaultProbKey> >
        Basket::remainingDefaultKeys(const Date& endDate) const
    {
        QL_REQUIRE(endDate >= refDate_,
            "Target date lies before basket inception");

        const std::vector<Size>& alive = liveList(endDate);
        vector<DefaultProbKey> defKeys;
        for(Size i=0; i<alive.size(); i++)
            defKeys.push_back(pool_->defaultKeys()[alive[i]]);
        return defKeys;
    }

    Size Basket::remainingSize() const {
        return evalDateLiveList_.size();
    }

    /* computed on the inception values, notice the positions might have
    amortized or changed in value and the total outstanding notional might
    differ from the inception one.*/
    Real Basket::remainingDetachmentAmount(const Date& endDate) const {
        return detachmentAmount_;
    }

    Real Basket::remainingAttachmentAmount(const Date& endDate) const {
        // maybe return zero directly instead?:
        QL_REQUIRE(endDate >= refDate_,
            "Target date lies before basket inception");
        Real loss = settledLoss(endDate);
        return std::min(detachmentAmount_, attachmentAmount_ +
            std::max(0.0, loss - attachmentAmount_));
    }

    Probability Basket::probOverLoss(const Date& d, Real lossFraction) const {
        // convert initial basket fraction to remaining basket fraction
        calculate();
        // if eaten up all the tranche the prob of losing any amount is 1
        //  (we have already lost it)
        if(evalDateRemainingNot_ == 0.) return 1.;

        // Turn to live (remaining) tranche units to feed into the model request
        Real xPtfl = attachmentAmount_ +
            (detachmentAmount_-attachmentAmount_)*lossFraction;
        Real xPrim = (xPtfl- evalDateAttachAmount_)/
            (detachmentAmount_-evalDateAttachAmount_);
        // in live tranche fractional units
        // if the level falls within realized losses the prob is 1.
        if(xPtfl < 0.) return 1.;

        return lossModel_->probOverLoss(d, xPrim);
    }

    Real Basket::percentile(const Date& d, Probability prob) const {
        calculate();
        return lossModel_->percentile(d, prob);

        Real percLiveFract = lossModel_->percentile(d, prob);
        return (percLiveFract*(detachmentAmount_-evalDateAttachAmount_)
            + attachmentAmount_ - evalDateAttachAmount_)
            /(detachmentAmount_-attachmentAmount_);
    }

    Real Basket::expectedTrancheLoss(const Date& d) const {
        calculate();
        return cumulatedLoss() + lossModel_->expectedTrancheLoss(d);
    }

    Disposable<std::vector<Real> >
        Basket::splitVaRLevel(const Date& date, Real loss) const {
        calculate();
        return lossModel_->splitVaRLevel(date, loss);
    }

    Real Basket::expectedShortfall(const Date& d, Probability prob) const {
        calculate();
        return lossModel_->expectedShortfall(d, prob);
    }

    Disposable<std::map<Real, Probability> >
        Basket::lossDistribution(const Date& d) const {
        calculate();
        return lossModel_->lossDistribution(d);
    }

    std::vector<Probability>
        Basket::probsBeingNthEvent(Size n, const Date& d) const {

        Size alreadyDefaulted = pool_->size() - remainingNames().size();
        if(alreadyDefaulted >=n)
            return std::vector<Probability>(remainingNames().size(), 0.);

        calculate();
        return lossModel_->probsBeingNthEvent(n-alreadyDefaulted, d);
    }

    Real Basket::defaultCorrelation(const Date& d, Size iName, Size jName) const{
        calculate();
        return lossModel_->defaultCorrelation(d, iName, jName);

    }

    /*! Returns the probaility of having a given or larger number of
    defaults in the basket portfolio at a given time.
    */
    Probability Basket::probAtLeastNEvents(Size n, const Date& d) const{
        calculate();
        return lossModel_->probAtLeastNEvents(n, d);

    }

    Real Basket::recoveryRate(const Date& d, Size iName) const {
        calculate();
        return
            lossModel_->expectedRecovery(d, iName, pool_->defaultKeys()[iName]);
    }

}