#include <chrono>
#include <cmath>

#include "security_services.hh"

using namespace std; // Required for isnan()
#include "EtsiTs103097Codec_ToBeSignedData.hh"
#include "EtsiTs103097Codec_Data.hh"
#include "EtsiTs103097Codec_Certificate.hh"

#include "sha256.hh"
#include "sha384.hh"
#include "security_ecc.hh"

#include "Params.hh"

#include "loggers.hh"

#include "converter.hh"

security_services * security_services::instance = nullptr;

security_services::security_services() : _setup_done{false}, _ec_keys_enc(nullptr), _security_cache(new security_cache), _security_db(nullptr), _last_generation_time(0), _unknown_certificate(), _latitude(0), _longitude(0), _elevation(0) {
  loggers::get_instance().log(">>> security_services::security_services");
} // End of ctor

int security_services::setup(Params& p_params) { // FIXME Rename this method
  loggers::get_instance().log(">>> security_services::setup");
  _params = p_params;
  _params.log();

  if (_setup_done) {
    loggers::get_instance().warning("security_services::setup: Already done");
    return 0;
  }
  _setup_done = true;
  
  // Build the certificate caching
  _security_db.reset(new security_db(_params[Params::sec_db_path]));
  if (_security_db.get() == nullptr) { // Memory allocation issue
    loggers::get_instance().warning("security_services::setup: _security_db pointer is NULL");
    return -1;
  }
  
  // Initialise encryption mechanism
  if (_params[Params::encrypted_mode].compare("1") == 0) {
    Params::const_iterator it = _params.find(Params::cypher);
    if (it == _params.cend()) {
      _ec_keys_enc.reset(new security_ecc(ec_elliptic_curves::nist_p_256));
      _params.insert(std::pair<std::string, std::string>(Params::cypher, std::string("NISTP-256")));
      p_params.insert(std::pair<std::string, std::string>(Params::cypher, std::string("NISTP-256")));
    } else if (it->second.compare("NISTP-256")) {
      _ec_keys_enc.reset(new security_ecc(ec_elliptic_curves::nist_p_256));
    } else if (it->second.compare("BP-256")) {
      _ec_keys_enc.reset(new security_ecc(ec_elliptic_curves::brainpool_p_256_r1));
    } else {
      loggers::get_instance().warning("security_services::setup: Failed to encode ToBeSignedData");
      return -1;
    }
  }
  return 0;
}

int security_services::store_certificate(const CHARSTRING& p_cert_id, const OCTETSTRING& p_cert, const OCTETSTRING& p_private_key, const OCTETSTRING& p_public_key_x, const OCTETSTRING& p_public_key_y, const OCTETSTRING& p_hashid8, const OCTETSTRING& p_issuer, const OCTETSTRING& p_private_enc_key, const OCTETSTRING& p_public_enc_key_x, const OCTETSTRING& p_public_enc_key_y) {
  loggers::get_instance().log_msg(">>> security_services::store_certificate: ", p_cert_id);

  // Sanity checks
  if (_security_db.get() == nullptr) { // Setup not called
    loggers::get_instance().warning("security_services::store_certificate: Not initialised");
    return -1;
  }
  return _security_db.get()->store_certificate(p_cert_id, p_cert, p_private_key, p_public_key_x, p_public_key_y, p_hashid8, p_issuer, p_private_enc_key, p_public_enc_key_x, p_public_enc_key_y);
}

int security_services::verify_and_extract_gn_payload(const OCTETSTRING& p_secured_gn_payload, const bool p_verify, IEEE1609dot2::Ieee1609Dot2Data& p_ieee_1609dot2_data, OCTETSTRING& p_unsecured_gn_payload, Params& p_params) {
  loggers::get_instance().log_msg(">>> security_services::verify_and_extract_gn_payload: ", p_secured_gn_payload);

  // Sanity checks
  if (p_secured_gn_payload.lengthof() == 0) {
    return -1;
  }
  
  // Decode the secured message (OER encoding)
  EtsiTs103097Codec_Data codec;
  codec.decode(p_secured_gn_payload, p_ieee_1609dot2_data, &p_params);
  // Sanity checks
  if (!p_ieee_1609dot2_data.is_bound()) {
    loggers::get_instance().warning("security_services::verify_and_extract_gn_payload: Unbound value, discard it");
    return -1;
  }
  if (p_verify && ((unsigned int)(int)p_ieee_1609dot2_data.protocolVersion() != security_services::ProtocolVersion)) {
    loggers::get_instance().warning("security_services::verify_and_extract_gn_payload: Wrong version protocol, discard it");
    return -1;
  }

  return process_ieee_1609_dot2_content(p_ieee_1609dot2_data.content(), p_verify, p_unsecured_gn_payload, p_params);
} // End of method verify_and_extract_gn_payload

int security_services::process_ieee_1609_dot2_content(const IEEE1609dot2::Ieee1609Dot2Content& p_ieee_1609_dot2_content, const bool p_verify, OCTETSTRING& p_unsecured_payload, Params& p_params) {
  loggers::get_instance().log_msg(">>> security_services::process_ieee_1609_dot2_content: ", p_ieee_1609_dot2_content);
  
  if (p_ieee_1609_dot2_content.ischosen(IEEE1609dot2::Ieee1609Dot2Content::ALT_unsecuredData)) { // Unsecured packet, End of recursivity
    p_unsecured_payload = p_ieee_1609_dot2_content.unsecuredData();
  } else if (p_ieee_1609_dot2_content.ischosen(IEEE1609dot2::Ieee1609Dot2Content::ALT_signedData)) {
    const IEEE1609dot2::SignedData& signedData = p_ieee_1609_dot2_content.signedData();
    if (process_ieee_1609_dot2_signed_data(signedData, p_verify, p_unsecured_payload, p_params) != 0) {
      if (p_verify) {
        return -1;
      }
    }
  } else if (p_ieee_1609_dot2_content.ischosen(IEEE1609dot2::Ieee1609Dot2Content::ALT_encryptedData)) {
    const IEEE1609dot2::EncryptedData& encrypted_data = p_ieee_1609_dot2_content.encryptedData();
    OCTETSTRING signed_payload;
    if (security_services::process_ieee_1609_dot2_encrypted_data(encrypted_data, p_verify, signed_payload, p_params) != 0) {
      if (p_verify) {
        return -1;
      }
    }
    loggers::get_instance().log_msg("security_services::process_ieee_1609_dot2_content: Decrypted payload: ", signed_payload);
    IEEE1609dot2::Ieee1609Dot2Data ieee_1609dot2_data; // TODO Check if it could be reused
    if (verify_and_extract_gn_payload(signed_payload, p_verify, ieee_1609dot2_data, p_unsecured_payload, p_params) != 0) {
      if (p_verify) {
        return -1;
      }
    }
  } else if (p_ieee_1609_dot2_content.ischosen(IEEE1609dot2::Ieee1609Dot2Content::ALT_signedCertificateRequest)) {
    // Reset certificate timer
    loggers::get_instance().log("security_services::process_ieee_1609_dot2_content: Set Certificate re-transmission flag and reset timer");
    _last_generation_time = 0;
    return 0;
  } else { // Shall never be reached
    loggers::get_instance().warning("security_services::process_ieee_1609_dot2_content: Undefined IEEE 1609.2 Content, discard it");
    if (p_verify) {
      return -1;
    }
  }
  
  loggers::get_instance().log_msg("<<< security_services::process_ieee_1609_dot2_content: ", p_unsecured_payload);
  return 0;
} // End of method process_ieee_1609_dot2_content

int security_services::process_ieee_1609_dot2_signed_data(const IEEE1609dot2::SignedData& p_signed_data, const bool p_verify, OCTETSTRING& p_unsecured_payload, Params& p_params) {
  loggers::get_instance().log_msg(">>> security_services::process_ieee_1609_dot2_signed_data: ", p_signed_data);

  // Check the headerInfo content
  const IEEE1609dot2::HeaderInfo& header_info = p_signed_data.tbsData().headerInfo();
  if (!header_info.generationTime().is_present()) {
    loggers::get_instance().warning("security_services::process_ieee_1609_dot2_signed_data: HeaderInfo::GenerationTime field is missing");
    if (p_verify) {
      return -1;
    }
  } else {
    const OPTIONAL<INTEGER>& v = dynamic_cast<const OPTIONAL<INTEGER>& >(header_info.generationTime());
    unsigned long long gt = ((INTEGER&)(*v.get_opt_value())).get_long_long_val() * 1000 - 1072911600000L;
    // Get current time timestamp
    unsigned long long ms = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count() - 1072911600000L;  // TODO Add method such as its_tme() & its_time_mod() beacuse it is used also in LibItsCommon_externals
    loggers::get_instance().log("security_services::process_ieee_1609_dot2_signed_data: generation time check %ld / %ld", header_info.generationTime(), ms);
    if (abs((double)gt - (double)ms) >= 5.0) { // TODO Use a params for generation_time_epsilon
      loggers::get_instance().warning("security_services::process_ieee_1609_dot2_signed_data: Invalid generation time, discard it");
      if (p_verify) {
        return -1;
      }
    }
  }

  // Check encryption keys if present
  if (header_info.encryptionKey().is_present()) {
    // TODO
  }

  // Check request certificate
  if (header_info.inlineP2pcdRequest().is_present()) {
      loggers::get_instance().error("security_services::process_ieee_1609_dot2_signed_data: inlineP2pcdRequest not supported yet");
      // TODO
  }

  // Check requested certificate
  if (header_info.requestedCertificate().is_present()) {
      loggers::get_instance().error("security_services::process_ieee_1609_dot2_signed_data: requestedCertificate not supported yet");
      // TODO
  }

  // Check and extract unsecured payload
  if (p_signed_data.tbsData().payload().data().is_present()) {
    // Check protocol version
    const OPTIONAL<IEEE1609dot2::Ieee1609Dot2Data>& v = dynamic_cast<const OPTIONAL<IEEE1609dot2::Ieee1609Dot2Data>& >(p_signed_data.tbsData().payload().data());
    loggers::get_instance().log_msg("security_services::process_ieee_1609_dot2_signed_data: SignedDataPayload.data = ", v);
    const IEEE1609dot2::Ieee1609Dot2Data& ieee_1609dot2_data = static_cast<const IEEE1609dot2::Ieee1609Dot2Data&>(*v.get_opt_value());
    if (p_verify && ((unsigned int)(int)ieee_1609dot2_data.protocolVersion() != security_services::ProtocolVersion)) {
      loggers::get_instance().warning("security_services::process_ieee_1609_dot2_signed_data: Wrong version protocol, discard it");
      if (p_verify) {
        return -1;
      }
    }
    if (process_ieee_1609_dot2_content(ieee_1609dot2_data.content(), p_verify, p_unsecured_payload, p_params) != 0) {
      loggers::get_instance().warning("security_services::process_ieee_1609_dot2_signed_data: Failed to process SignedData, discard it");
      if (p_verify) {
        return -1;
      }
    }
  } else if (p_signed_data.tbsData().payload().extDataHash().is_present()) {
    loggers::get_instance().warning("security_services::process_ieee_1609_dot2_signed_data: Unsupported extDataHash, discard it");
    if (p_verify) {
      return -1;
    }
  } else { // Shall not be reached
    loggers::get_instance().warning("security_services::process_ieee_1609_dot2_signed_data: Unsupported SignedDataPayload, discard it");
    return -1;
  }

  // Encode the ToBeSignedData
  EtsiTs103097Codec_ToBeSignedData tbs_data_codec;
  OCTETSTRING os;
  tbs_data_codec.encode(p_signed_data.tbsData(), os);
  if (os.lengthof() == 0) {
    loggers::get_instance().warning("security_services::process_ieee_1609_dot2_signed_data: Failed to encode ToBeSignedData");
    return -1;
  }
  loggers::get_instance().log_msg("security_services::process_ieee_1609_dot2_signed_data: encoded tbs_data = ", os);
  // Calculate the hash according to the hashId
  OCTETSTRING hashed_data;
  int result = -1;
  if (p_signed_data.hashId() == IEEE1609dot2BaseTypes::HashAlgorithm::sha256) {
    result = hash_sha256(os, hashed_data);
  } else {
    result = hash_sha384(os, hashed_data);
  }
  loggers::get_instance().log_msg("security_services::process_ieee_1609_dot2_signed_data: hashed_data = ", hashed_data);
  if (result != 0) {
    loggers::get_instance().warning("security_services::process_ieee_1609_dot2_signed_data: Failed to create hash");
    return -1;
  }
  // Retrieve certificate identifier
  loggers::get_instance().log_msg("security_services::process_ieee_1609_dot2_signed_data: signer = ", p_signed_data.signer());
  std::string certificate_id;
  result = -1;
  if (p_signed_data.signer().ischosen(IEEE1609dot2::SignerIdentifier::ALT_digest)) {
    // Retrieve the certificate identifier from digest
    loggers::get_instance().log("security_services::process_ieee_1609_dot2_signed_data: Retrieve the certificate identifier from digest");
    result = _security_db.get()->get_certificate_id(p_signed_data.signer().digest(), certificate_id);
    if (result == -1) {
      // Check in the cache
      if (_security_cache.get()->get_certificate_id(p_signed_data.signer().digest(), certificate_id) == -1) {
        // Unknown certificate, request it
        loggers::get_instance().log("security_services::process_ieee_1609_dot2_signed_data: Unknown certificate, request it");
        const OCTETSTRING& os = p_signed_data.signer().digest();
        _unknown_certificate.resize(3);
        const unsigned char* p = static_cast<const unsigned char*>(os) + os.lengthof() - 3;
        for (int i = 0; i < 3; i++) {
          _unknown_certificate[i] = *(p + i);
        } // End of 'for' statement
        loggers::get_instance().log_to_hexa("security_services::process_ieee_1609_dot2_signed_data: HashedId3: ", _unknown_certificate.data(), _unknown_certificate.size());
      }
      // Reset certificate timer
      loggers::get_instance().log("security_services::process_ieee_1609_dot2_signed_data: Set Certificate re-transmission flag and reset timer");
      _last_generation_time = 0;
    }
  } else if (p_signed_data.signer().ischosen(IEEE1609dot2::SignerIdentifier::ALT_certificate) && (p_signed_data.signer().certificate().size_of() != 0)) {
    // Extract the certificates
    for (int i = 0; i < p_signed_data.signer().certificate().size_of(); i++) {
      IEEE1609dot2::CertificateBase cert = p_signed_data.signer().certificate()[i];
      if (cert.issuer().ischosen(IEEE1609dot2::IssuerIdentifier::ALT_sha256AndDigest)) {
        result = _security_db.get()->get_certificate_id(cert.issuer().sha256AndDigest(), certificate_id);
        if (result == -1) { // Not found in current DB
          if (_security_cache.get()->get_certificate_id(cert.issuer().sha256AndDigest(), certificate_id) == -1) { // Not found in TS cache
            loggers::get_instance().log_msg("security_services::process_ieee_1609_dot2_signed_data: Store new certificate in cache: ", cert);
            const std::vector<unsigned char> v(static_cast<const unsigned char*>(cert.issuer().sha256AndDigest()), static_cast<const unsigned char*>(cert.issuer().sha256AndDigest()) + cert.issuer().sha256AndDigest().lengthof());
            certificate_id = converter::get_instance().bytes_to_hexa(v);
            // Add it into the cache
            OCTETSTRING public_key_x, public_key_y;
            if (cert.toBeSigned().verifyKeyIndicator().verificationKey().ischosen(IEEE1609dot2BaseTypes::PublicVerificationKey::ALT_ecdsaNistP256)) {
              public_key_x = cert.toBeSigned().verifyKeyIndicator().verificationKey().ecdsaNistP256().uncompressedP256().x();
              public_key_y = cert.toBeSigned().verifyKeyIndicator().verificationKey().ecdsaNistP256().uncompressedP256().y();
            } else if (cert.toBeSigned().verifyKeyIndicator().verificationKey().ischosen(IEEE1609dot2BaseTypes::PublicVerificationKey::ALT_ecdsaBrainpoolP256r1)) {
              public_key_x = cert.toBeSigned().verifyKeyIndicator().verificationKey().ecdsaBrainpoolP256r1().uncompressedP256().x();
              public_key_y = cert.toBeSigned().verifyKeyIndicator().verificationKey().ecdsaBrainpoolP256r1().uncompressedP256().y();
            } else {
              loggers::get_instance().error("security_services::process_ieee_1609_dot2_signed_data: Unsupported VerificationKey");
              return -1;
            }
            // Add encryption keys
            OCTETSTRING public_enc_key_x, public_enc_key_y;
            if (extract_encryption_keys(cert, public_enc_key_x, public_enc_key_y) == -1) {
              loggers::get_instance().error("security_services::process_ieee_1609_dot2_signed_data: Unsupported EncryptionKey");
              return -1;
            }
            // Encode certificate
            EtsiTs103097Codec_Certificate codec;
            OCTETSTRING enc_cert;
            codec.encode(cert, enc_cert);
            OCTETSTRING hash_cert;
            hash_sha256(enc_cert, hash_cert);
            // And store it into the cache
            _security_cache.get()->store_certificate(
                                                     CHARSTRING(certificate_id.c_str()),
                                                     enc_cert,
                                                     int2oct(0, 32), // No way to get the private key here
                                                     public_key_x,
                                                     public_key_y,
                                                     hash_cert,
                                                     cert.issuer().sha256AndDigest(),
                                                     int2oct(0, 32), // Encryption private not used
                                                     public_enc_key_x,
                                                     public_enc_key_y
                                                     );
          }
        }
      } else if (cert.issuer().ischosen(IEEE1609dot2::IssuerIdentifier::ALT_sha384AndDigest)) {
        result = _security_db.get()->get_certificate_id(cert.issuer().sha384AndDigest(), certificate_id);
        if (result == -1) {
          if (_security_cache.get()->get_certificate_id(cert.issuer().sha384AndDigest(), certificate_id) == -1) {
            loggers::get_instance().log_msg("security_services::process_ieee_1609_dot2_signed_data: Store new certificate in cache: ", cert);
            const std::vector<unsigned char> v(static_cast<const unsigned char*>(cert.issuer().sha384AndDigest()), static_cast<const unsigned char*>(cert.issuer().sha384AndDigest()) + cert.issuer().sha384AndDigest().lengthof());
            certificate_id = converter::get_instance().bytes_to_hexa(v);
            // Add it into the cache
            OCTETSTRING public_key_x, public_key_y;
            if (cert.toBeSigned().verifyKeyIndicator().verificationKey().ischosen(IEEE1609dot2BaseTypes::PublicVerificationKey::ALT_ecdsaBrainpoolP384r1)) {
              public_key_x = cert.toBeSigned().verifyKeyIndicator().verificationKey().ecdsaBrainpoolP384r1().uncompressedP384().x();
              public_key_y = cert.toBeSigned().verifyKeyIndicator().verificationKey().ecdsaBrainpoolP384r1().uncompressedP384().y();
            } else {
              loggers::get_instance().error("security_services::process_ieee_1609_dot2_signed_data: Unsupported VerificationKey");
              return -1;
            }
            // Add encryption keys
            OCTETSTRING public_enc_key_x, public_enc_key_y;
            if (extract_encryption_keys(cert, public_enc_key_x, public_enc_key_y) == -1) {
              loggers::get_instance().error("security_services::process_ieee_1609_dot2_signed_data: Unsupported EncryptionKey");
              return -1;
            }
            // Encode certificate
            EtsiTs103097Codec_Certificate codec;
            OCTETSTRING enc_cert;
            codec.encode(cert, enc_cert);
            OCTETSTRING hash_cert;
            hash_sha384(enc_cert, hash_cert);
            // And store it into the cache
            _security_cache.get()->store_certificate(
                                                     CHARSTRING(certificate_id.c_str()),
                                                     enc_cert,
                                                     int2oct(0, 48), // No way to get the private key here
                                                     public_key_x,
                                                     public_key_y,
                                                     hash_cert,
                                                     cert.issuer().sha384AndDigest(),
                                                     int2oct(0, 32), // Encryption private not used
                                                     public_enc_key_x,
                                                     public_enc_key_y
                                                     );
          }
        }
      } else {
        loggers::get_instance().error("security_services::process_ieee_1609_dot2_signed_data: Unsupported certificate issuer");
        return -1;
      }
    } // End of 'for' statement
    IEEE1609dot2::CertificateBase cert = p_signed_data.signer().certificate()[0];
  } else {
    loggers::get_instance().warning("security_services::process_ieee_1609_dot2_signed_data: Unsupported SignerIdentifier");
    return -1;
  }
  if (result == -1) {
    loggers::get_instance().warning("security_services::process_ieee_1609_dot2_signed_data: Certificate not found for the specified signer, it will be requested");
    return -1;
  }
  loggers::get_instance().log("security_services::process_ieee_1609_dot2_signed_data: certificate id = '%s'", certificate_id.c_str());
  // Verify the signature of the ToBeSignedData
  loggers::get_instance().log_msg("security_services::process_ieee_1609_dot2_signed_data: signature = ", p_signed_data.signature__());
  result = -1;
  if (p_signed_data.signature__().ischosen(IEEE1609dot2BaseTypes::Signature::ALT_ecdsaNistP256Signature)) {
    result = verify_sign_ecdsa_nistp256(hashed_data, p_signed_data.signature__(), certificate_id, p_params);
  } else {
    // TODO
    loggers::get_instance().error("security_services::process_ieee_1609_dot2_content: TODO");
  }
  if (result != 0) {
    loggers::get_instance().warning("security_services::process_ieee_1609_dot2_signed_data: Failed to verify signature");
    return -1;
  }
  
  loggers::get_instance().log_msg("<<< security_services::process_ieee_1609_dot2_signed_data: ", p_unsecured_payload);
  return 0;
} // End of method process_ieee_1609_dot2_signed_data

int security_services::process_ieee_1609_dot2_encrypted_data(const IEEE1609dot2::EncryptedData& p_encrypted_data, const bool p_verify, OCTETSTRING& p_unsecured_payload, Params& p_params) {
  loggers::get_instance().log_msg(">>> security_services::process_ieee_1609_dot2_encrypted_data: ", p_encrypted_data);


  if (_params[Params::cypher].compare("NISTP-256") == 0) {
	  _ec_keys_enc.reset(new security_ecc(ec_elliptic_curves::nist_p_256));
  } else if (_params[Params::cypher].compare("BP-256") == 0) {
	  _ec_keys_enc.reset(new security_ecc(ec_elliptic_curves::brainpool_p_256_r1));
  } else {
    loggers::get_instance().warning("security_services::setup: Failed to encode ToBeSignedData");
    return -1;
  }



  // 1. Retrieve recipient's public keys
  // OCTETSTRING recipients_key_x;
  // OCTETSTRING recipients_key_y;
  // if (_security_db.get()->get_public_enc_keys(certificate_id, recipients_key_x, recipients_key_y) == -1) {
  //   loggers::get_instance().warning("security_services::encrypt_gn_payload: Failed to retrieve recipient's public keys");
  //   return -1;
  // }




  loggers::get_instance().log_msg("<<< security_services::process_ieee_1609_dot2_encrypted_data: ", p_unsecured_payload);
  return -1;
} // End of method process_ieee_1609_dot2_encrypted_data

int security_services::secure_gn_payload(const OCTETSTRING& p_unsecured_gn_payload, OCTETSTRING& p_secured_gn_payload, Params& p_params) {
  loggers::get_instance().log_msg(">>> security_services::secure_gn_payload: ", p_unsecured_gn_payload);
  p_params.log();

  OCTETSTRING signed_payload;
  if (sign_gn_payload(p_unsecured_gn_payload, signed_payload, p_params) != 0) {
    p_secured_gn_payload = p_unsecured_gn_payload;
    loggers::get_instance().warning("security_services::secure_gn_payload: Failed to signed payload");
    return -1;
  }

  if (_params[Params::encrypted_mode].compare("1") == 0) {
    if (encrypt_gn_payload(signed_payload, p_secured_gn_payload, p_params) != 0) {
      p_secured_gn_payload = signed_payload;
      loggers::get_instance().warning("security_services::secure_gn_payload: Failed to encrypt payload");
      return -1;
    }
  } else { // No encryption required
    loggers::get_instance().log("security_services::secure_gn_payload: Encryption mode not set");
    p_secured_gn_payload = signed_payload;
  }

  return 0;
}

int security_services::sign_gn_payload(const OCTETSTRING& p_unsecured_gn_payload, OCTETSTRING& p_signed_gn_payload, Params& p_params) {
  loggers::get_instance().log_msg(">>> security_services::sign_gn_payload: ", p_unsecured_gn_payload);

  // Set unsecured data
  IEEE1609dot2::Ieee1609Dot2Content unsecured_data_content;
  unsecured_data_content.unsecuredData() = p_unsecured_gn_payload;
  IEEE1609dot2::Ieee1609Dot2Data unsecured_data(ProtocolVersion, unsecured_data_content);
  // Set hash algorithm
  IEEE1609dot2BaseTypes::HashAlgorithm hashId(IEEE1609dot2BaseTypes::HashAlgorithm::sha256);
  if (p_params[Params::hash].compare("SHA-384") == 0) {
    hashId = IEEE1609dot2BaseTypes::HashAlgorithm::sha384;
  }
  // Set SignedDataPayload
  IEEE1609dot2::SignedDataPayload payload;
  payload.data() = unsecured_data;
  payload.extDataHash().set_to_omit();
  IEEE1609dot2::HeaderInfo header_info;
  // Set secured field according to the payload!
  header_info.psid() = converter::get_instance().string_to_int(p_params[Params::its_aid]);
  header_info.expiryTime().set_to_omit();
  header_info.generationLocation().set_to_omit();
  header_info.p2pcdLearningRequest().set_to_omit();
  header_info.missingCrlIdentifier().set_to_omit();
  if (_params[Params::encrypted_mode].compare("1") == 0) {
    // TODO Set the encrytion key
    header_info.encryptionKey().set_to_omit();
  } else {
    header_info.encryptionKey().set_to_omit();
  }
  Params::const_iterator it = p_params.find(Params::payload_type);
  if (it != p_params.cend()) {
    loggers::get_instance().log("security_services::sign_gn_payload: Payload type: %s", it->second.c_str());
    if (it->second.compare("1") == 0) { // DENM
      OPTIONAL<IEEE1609dot2BaseTypes::ThreeDLocation> location(IEEE1609dot2BaseTypes::ThreeDLocation(_latitude, _longitude, _elevation));
      loggers::get_instance().log_msg("security_services::sign_gn_payload: generationLocation: ", location);
      header_info.generationLocation() = location;
      loggers::get_instance().log_msg("security_services::sign_gn_payload: generationLocation: ", header_info.generationLocation());
    } else if (it->second.compare("2") == 0) { // CAM
      // Noting to do
    } else {
      // Noting to do
    }
  } else { // Process it as a GeoNetworking payload
    loggers::get_instance().log("security_services::sign_gn_payload: Payload type not set");
    // Noting to do
  }
  unsigned long long ms = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count() - 1072911600000L;  // TODO Add method such as its_tme() & its_time_mod() beacuse it is used also in LibItsCommon_externals
  INTEGER i;
  i.set_long_long_val((unsigned int)ms);
  header_info.generationTime() = OPTIONAL<INTEGER>(i);
  // Check if a certificate shall be requested
  if (_unknown_certificate.size() == 3) { // HashedId3
    IEEE1609dot2BaseTypes::SequenceOfHashedId3 s;
    s[0] = OCTETSTRING(_unknown_certificate.size(), _unknown_certificate.data());
    header_info.inlineP2pcdRequest() = OPTIONAL<IEEE1609dot2BaseTypes::SequenceOfHashedId3>(s);
    _unknown_certificate.clear();
  } else {
    header_info.inlineP2pcdRequest().set_to_omit();
  }
  header_info.requestedCertificate().set_to_omit();

  IEEE1609dot2::ToBeSignedData tbs_data;
  tbs_data.payload() = payload;
  tbs_data.headerInfo() = header_info;
  loggers::get_instance().log_msg("security_services::sign_gn_payload: tbs_data = ", tbs_data);
  // Sign the ToBeSignedData data structure
  IEEE1609dot2BaseTypes::Signature signature;
  if (sign_tbs_data(tbs_data, hashId, signature, p_params) != 0) {
    loggers::get_instance().warning("security_services::sign_gn_payload: Failed to secure payload");
    return -1;
  }
  IEEE1609dot2::SignerIdentifier signer;
  loggers::get_instance().log("security_services::sign_gn_payload: ms = %d - _last_generation_time = %d - ms - _last_generation_time = %d", (unsigned int)ms, _last_generation_time, (unsigned int)(ms - _last_generation_time));
  std::string certificate_id = p_params[Params::certificate];
  loggers::get_instance().log("security_services::sign_gn_payload: certificate_id = %s", certificate_id.c_str());
  if ((unsigned int)(ms - _last_generation_time) >= 1000 * 0.95) { // Need to add certificate
    IEEE1609dot2::CertificateBase cert;
    if (_security_db->get_certificate(certificate_id, cert) != 0) {
      loggers::get_instance().warning("security_services:sign_gn_payload: Failed to secure payload");
      return -1;
    }
    IEEE1609dot2::SequenceOfCertificate sequenceOfCertificate;
    sequenceOfCertificate[0] = cert;
    signer.certificate() = sequenceOfCertificate;
    // Reset send certificate timer
    _last_generation_time = ms;
  } else {
    OCTETSTRING digest;
    if (_security_db->get_hashed_id(certificate_id, digest) != 0) {
      loggers::get_instance().warning("security_services::sign_gn_payload: Failed to secure payload");
    return -1;
    }
    signer.digest() = digest;
  }
  IEEE1609dot2::SignedData signed_data(
                                       hashId,
                                       tbs_data,
                                       signer,
                                       signature
                                       );
  loggers::get_instance().log_msg("security_services::sign_gn_payload: signed_data = ", signed_data);
  IEEE1609dot2::Ieee1609Dot2Content ieee_dot2_content;
  ieee_dot2_content.signedData() = signed_data;
  IEEE1609dot2::Ieee1609Dot2Data ieee_1609dot2_data(
                                                    security_services::ProtocolVersion,
                                                    ieee_dot2_content
                                                    );
  loggers::get_instance().log_msg("security_services::sign_gn_payload: ieee_1609dot2_data = ", ieee_1609dot2_data);
  EtsiTs103097Codec_Data codec;
  codec.encode(ieee_1609dot2_data, p_signed_gn_payload);
  if (!p_signed_gn_payload.is_bound()) {
    loggers::get_instance().warning("security_services::sign_gn_payload: Failed to encode Ieee1609Dot2Data");
    return -1;
  }
  
  return 0;
}

int security_services::encrypt_gn_payload(const OCTETSTRING& p_unsecured_gn_payload, OCTETSTRING& p_enc_gn_payload, Params& p_params) {
  loggers::get_instance().log_msg(">>> security_services::encrypt_gn_payload: ", p_unsecured_gn_payload);
  
  // Sanity checks
//  if (_ec_keys_enc.get() == nullptr) {
//    loggers::get_instance().warning("security_services::encrypt_gn_payload: Encryption not initialised");
//    return -1;
//  }

  // 1. Retrieve recipient's public keys
  // OCTETSTRING recipients_key_x;
  // OCTETSTRING recipients_key_y;
  // if (_security_db.get()->get_public_enc_keys(certificate_id, recipients_key_x, recipients_key_y) == -1) {
  //   loggers::get_instance().warning("security_services::encrypt_gn_payload: Failed to retrieve recipient's public keys");
  //   return -1;
  // }

  // 2. Generate new ephemeral Private/Public keys
  if (_ec_keys_enc.get()->generate() == -1) {
    loggers::get_instance().warning("security_services::encrypt_gn_payload: Failed to generate ephemeral keys");
    return -1;
  }

  // 3. Generate and derive shared secret
  // std::vector<unsigned char> peer_public_key_x(static_cast<const unsigned char *>(recipients_key_x), recipients_key_x.lengthof() + static_cast<const unsigned char *>(recipients_key_x));
  // std::vector<unsigned char> peer_public_key_y(static_cast<const unsigned char *>(recipients_key_y), recipients_key_y.lengthof() + static_cast<const unsigned char *>(recipients_key_y));
  // if (ec.generate_and_derive_ephemeral_key(encryption_algotithm::aes_128_ccm, peer_public_key_x, peer_public_key_y) == -1) {
  //   loggers::get_instance().warning("security_services::encrypt_gn_payload: Failed to generate and derive secret key");
  //   return -1;
  // }
  


















  // AES-128 encryption of the data
  std::vector<unsigned char> message(static_cast<const unsigned char*>(p_unsecured_gn_payload), p_unsecured_gn_payload.lengthof() + static_cast<const unsigned char*>(p_unsecured_gn_payload));
  std::vector<unsigned char> enc_message;
  if (_ec_keys_enc.get()->encrypt(encryption_algotithm::aes_128_ccm, message, enc_message) == -1) {
    loggers::get_instance().warning("security_services::encrypt_gn_payload: Failed to encrypt message");
    return -1;
  }
  // _ec_keys_enc object contains the key, the none and the tag
  OCTETSTRING nonce = OCTETSTRING(_ec_keys_enc.get()->nonce().size(), _ec_keys_enc.get()->nonce().data());
  OCTETSTRING tag = OCTETSTRING(_ec_keys_enc.get()->tag().size(), _ec_keys_enc.get()->tag().data());
  OCTETSTRING enc_payload = OCTETSTRING(enc_message.size(), enc_message.data());
  IEEE1609dot2::AesCcmCiphertext aes_128_ccm(nonce, enc_payload + tag); // Add tag at the end of the ciphered text
  // Build SymmetricCiphertext
  IEEE1609dot2::SymmetricCiphertext cipher_text;
  cipher_text.aes128ccm() = aes_128_ccm;
  loggers::get_instance().log_msg("security_services::encrypt_gn_payload: aes_128_ccm = ", cipher_text);
  
  // Build the recipient_id
  std::string certificate_id = p_params[Params::certificate];
  OCTETSTRING recipient_id;
  _security_db.get()->get_hashed_id(certificate_id, recipient_id); // SHA-256 of the certificate which contain the encryption private/public keys
  // TODO Encrypt the AES-128 key

  OCTETSTRING public_enc_key_x;
  OCTETSTRING public_enc_key_y;
  _security_db.get()->get_public_enc_keys(certificate_id, public_enc_key_x, public_enc_key_y);

  // TODO
  //int unused = 0;
  //std::vector<unsigned char> x, y;
  //_ec_keys_enc.get()->generate_ephemeral_key(encryption_algotithm::aes_128_ccm, public_enc_key_x, public_enc_key_y, unused);
  OCTETSTRING encrypt_aes_128_key; // TODO
  OCTETSTRING encrypt_aes_128_tag; // TODO
  // Build the encryption keys
  IEEE1609dot2BaseTypes::EccP256CurvePoint eccP256CurvePoint;
  eccP256CurvePoint.uncompressedP256().x() = public_enc_key_x;
  eccP256CurvePoint.uncompressedP256().y() = public_enc_key_y;
  IEEE1609dot2BaseTypes::EciesP256EncryptedKey ecies_key(
                                                         eccP256CurvePoint,
                                                         encrypt_aes_128_key,
                                                         encrypt_aes_128_tag
                                                         );
  // Build the encryption data
  IEEE1609dot2::EncryptedDataEncryptionKey enc_data_key;
  Params::const_iterator it = p_params.find(Params::cypher);
  if (it == p_params.cend()) {
    enc_data_key.eciesNistP256() = ecies_key;
  } else if (it->second.compare("NISTP-256")) {
    enc_data_key.eciesNistP256() = ecies_key;
  } else if (it->second.compare("BP-256")) {
    enc_data_key.eciesBrainpoolP256r1() = ecies_key;
  }
  loggers::get_instance().log_msg("security_services::encrypt_gn_payload: enc_data_key = ", enc_data_key);
  // Finalise the encryption
  IEEE1609dot2::PKRecipientInfo cert_recipient_info(recipient_id, enc_data_key);
  IEEE1609dot2::RecipientInfo recipient_info;
  recipient_info.certRecipInfo() = cert_recipient_info;
  IEEE1609dot2::SequenceOfRecipientInfo recipients;
  recipients[0] = recipient_info;
  IEEE1609dot2::EncryptedData encrypted_data(recipients, cipher_text);
  // Encode it
  loggers::get_instance().log_msg("security_services::encrypt_gn_payload: encrypted_data = ", encrypted_data);
  IEEE1609dot2::Ieee1609Dot2Content ieee_dot2_content;
  ieee_dot2_content.encryptedData() = encrypted_data;
  IEEE1609dot2::Ieee1609Dot2Data ieee_1609dot2_data(
                                                    security_services::ProtocolVersion,
                                                    ieee_dot2_content
                                                    );
  loggers::get_instance().log_msg("security_services::sign_gn_payload: ieee_1609dot2_data = ", ieee_1609dot2_data);
  EtsiTs103097Codec_Data codec;
  codec.encode(ieee_1609dot2_data, p_enc_gn_payload);
  if (!p_enc_gn_payload.is_bound()) {
    loggers::get_instance().warning("security_services::sign_gn_payload: Failed to encode Ieee1609Dot2Data");
    return -1;
  }
  return 0;
}

int security_services::sign_tbs_data(const IEEE1609dot2::ToBeSignedData& p_tbs_data, const IEEE1609dot2BaseTypes::HashAlgorithm& p_hashAlgorithm, IEEE1609dot2BaseTypes::Signature& p_signature, Params& p_params) {
  loggers::get_instance().log_msg(">>> security_services::sign_tbs_data: ", p_tbs_data);
  
  // Encode the ToBeSignedData
  EtsiTs103097Codec_ToBeSignedData tbs_data_codec;
  OCTETSTRING os;
  tbs_data_codec.encode(p_tbs_data, os);
  if (os.lengthof() == 0) {
    loggers::get_instance().warning("security_services::sign_tbs_data: Failed to encode ToBeSignedData");
    return -1;
  }
  loggers::get_instance().log_msg("security_services::sign_tbs_data: encoded tbs_data = ", os);
  // Hash ToBeSignedData
  OCTETSTRING hashed_data;
  int result = -1;
  if (p_hashAlgorithm == IEEE1609dot2BaseTypes::HashAlgorithm::sha256) {
    result = hash_sha256(os, hashed_data);
  } else {
    result = hash_sha384(os, hashed_data);
  }
  loggers::get_instance().log_msg("security_services::sign_tbs_data: encoded hashed_data = ", hashed_data);
  if (result != 0) {
    loggers::get_instance().warning("security_services::sign_tbs_data: Failed to create hash");
    return -1;
  }
  // Sign ToBeSignedData
  result = -1;
  loggers::get_instance().log("security_services::sign_tbs_data: encoded Params::signature = '%s'", p_params[Params::signature].c_str());
  loggers::get_instance().log("security_services::sign_tbs_data: encoded Params::certificate = '%s'", p_params[Params::certificate].c_str());
  if (p_params[Params::signature].compare("NISTP-256") == 0) {
    result = sign_ecdsa_nistp256(hashed_data, p_signature, p_params);
  } else if (p_params[Params::signature].compare("BP-256") == 0) {
    //result = sign_ecdsa_brainpoolp256(hashed_data, p_signature, p_params);
    loggers::get_instance().error("security_services::sign_tbs_data: TODO");
    result = -1;
  } else if (p_params[Params::signature].compare("BP-384") == 0) {
    //result = sign_ecdsa_brainpoolp256(hashed_data, p_signature, p_params);
    loggers::get_instance().error("security_services::sign_tbs_data: TODO");
    result = -1;
  } else  {
    loggers::get_instance().error("security_services::sign_tbs_data: Unsupported signature algorithm");
    result = -1;
  }
  if (result != 0) {
    loggers::get_instance().warning("security_services::sign_tbs_data: Failed to sign payload");
    return -1;
  }
  
  return 0;
}

int security_services::hash_sha256(const OCTETSTRING& p_data, OCTETSTRING& p_hash_data) {
  loggers::get_instance().log_msg(">>> security_services::hash_sha256: ", p_data);
  
  sha256 hash;
  std::vector<unsigned char> tbh(static_cast<const unsigned char *>(p_data), p_data.lengthof() + static_cast<const unsigned char *>(p_data));
  std::vector<unsigned char> hashData;
  hash.generate(tbh, hashData);
  p_hash_data = OCTETSTRING(hashData.size(), hashData.data());

  return 0;
}

int security_services::hash_sha384(const OCTETSTRING& p_data, OCTETSTRING& p_hash_data) {
  loggers::get_instance().log_msg(">>> security_services::hash_sha384: ", p_data);
  
  sha384 hash;
  std::vector<unsigned char> tbh(static_cast<const unsigned char *>(p_data), p_data.lengthof() + static_cast<const unsigned char *>(p_data));
  std::vector<unsigned char> hashData;
  hash.generate(tbh, hashData);
  p_hash_data = OCTETSTRING(hashData.size(), hashData.data());
  
  return 0;
}

int security_services::sign_ecdsa_nistp256(const OCTETSTRING& p_hash, IEEE1609dot2BaseTypes::Signature& p_signature, Params& p_params) {
  loggers::get_instance().log_msg(">>> security_services::sign_ecdsa_nistp256: ", p_hash);
  
  std::string certificate_id = p_params[Params::certificate];
  loggers::get_instance().log("security_services::sign_ecdsa_nistp256: encoded certificate_id = '%s'", certificate_id.c_str());
  OCTETSTRING pkey;
  if (_security_db->get_private_key(certificate_id, pkey) != 0) {
    loggers::get_instance().warning("security_services::sign_ecdsa_nistp256: Failed to get private key");
    return -1;
  }
  std::vector<unsigned char> private_key(static_cast<const unsigned char *>(pkey), static_cast<const unsigned char *>(pkey) + pkey.lengthof());
  OCTETSTRING public_key_x;
  OCTETSTRING public_key_y;
  if (_security_db->get_public_keys(certificate_id, public_key_x, public_key_y) != 0) {
    loggers::get_instance().warning("security_services::sign_ecdsa_nistp256: Failed to get public keys");
    return -1;
  }
  
  std::vector<unsigned char> hashed_data(static_cast<const unsigned char *>(p_hash), static_cast<const unsigned char *>(p_hash) + p_hash.lengthof());
  security_ecc k(ec_elliptic_curves::nist_p_256, private_key);
  std::vector<unsigned char> r_sig;
  std::vector<unsigned char> s_sig;
  if (k.sign(hashed_data, r_sig, s_sig) != 0) {
    loggers::get_instance().warning("security_services::sign_ecdsa_nistp256: Failed to sign payload");
    return -1;
  }
  IEEE1609dot2BaseTypes::EccP256CurvePoint ep;
  ep.x__only() = OCTETSTRING(r_sig.size(), r_sig.data());
  p_signature.ecdsaNistP256Signature() = IEEE1609dot2BaseTypes::EcdsaP256Signature(
                                                                                   ep,
                                                                                   OCTETSTRING(s_sig.size(), s_sig.data())
                                                                                   );
  loggers::get_instance().log_msg("security_services::sign_ecdsa_nistp256: signature = ", p_signature);
  
  return 0;
}

int security_services::verify_sign_ecdsa_nistp256(const OCTETSTRING& p_hash, const IEEE1609dot2BaseTypes::Signature& p_signature, const std::string& p_certificate_id, Params& p_params) {
  loggers::get_instance().log_msg(">>> security_services::verify_sign_ecdsa_nistp256: ", p_hash);
  
  OCTETSTRING public_key_x;
  OCTETSTRING public_key_y;
  if (_security_db->get_public_keys(p_certificate_id, public_key_x, public_key_y) != 0) {
    loggers::get_instance().warning("security_services::verify_sign_ecdsa_nistp256: Failed to get public keys");
    return -1;
  }
  
  std::vector<unsigned char> hashData(static_cast<const unsigned char *>(p_hash), static_cast<const unsigned char *>(p_hash) + p_hash.lengthof());
  OCTETSTRING os;
  if (p_signature.ecdsaNistP256Signature().rSig().ischosen(IEEE1609dot2BaseTypes::EccP256CurvePoint::ALT_x__only)) {
    os = p_signature.ecdsaNistP256Signature().rSig().x__only() + p_signature.ecdsaNistP256Signature().sSig();
  } else if (p_signature.ecdsaNistP256Signature().rSig().ischosen(IEEE1609dot2BaseTypes::EccP256CurvePoint::ALT_compressed__y__0)) {
    os = p_signature.ecdsaNistP256Signature().rSig().compressed__y__0() + p_signature.ecdsaNistP256Signature().sSig();
  } else if (p_signature.ecdsaNistP256Signature().rSig().ischosen(IEEE1609dot2BaseTypes::EccP256CurvePoint::ALT_compressed__y__1)) {
    os = p_signature.ecdsaNistP256Signature().rSig().compressed__y__1() + p_signature.ecdsaNistP256Signature().sSig();
  } else if (p_signature.ecdsaNistP256Signature().rSig().ischosen(IEEE1609dot2BaseTypes::EccP256CurvePoint::ALT_uncompressedP256)) {
    os = p_signature.ecdsaNistP256Signature().rSig().uncompressedP256().x() + p_signature.ecdsaNistP256Signature().rSig().uncompressedP256().y() + p_signature.ecdsaNistP256Signature().sSig();
  } else {
    loggers::get_instance().warning("security_services::verify_sign_ecdsa_nistp256: Invalid curve point");
    return -1;
  }
  std::vector<unsigned char> signature(static_cast<const unsigned char *>(os), static_cast<const unsigned char *>(os) + os.lengthof());
  std::vector<unsigned char> key_x(static_cast<const unsigned char *>(public_key_x), static_cast<const unsigned char *>(public_key_x) + public_key_x.lengthof());
  std::vector<unsigned char> key_y(static_cast<const unsigned char *>(public_key_y), static_cast<const unsigned char *>(public_key_y) + public_key_y.lengthof());
  security_ecc k(ec_elliptic_curves::nist_p_256, key_x, key_y);
  if (k.sign_verif(hashData, signature) == 0) {
    return 0;
  }
  
  return -1;
}

int security_services::extract_encryption_keys(const IEEE1609dot2::CertificateBase& p_cert, OCTETSTRING& p_public_enc_key_x, OCTETSTRING& p_public_enc_key_y) {
  loggers::get_instance().log("security_services::extract_encryption_keys");
  
  if (p_cert.toBeSigned().encryptionKey().ispresent()) {
    const IEEE1609dot2BaseTypes::PublicEncryptionKey& p = static_cast<const IEEE1609dot2BaseTypes::PublicEncryptionKey&>(p_cert.toBeSigned().encryptionKey());
    if (p.publicKey().ischosen(IEEE1609dot2BaseTypes::BasePublicEncryptionKey::ALT_eciesNistP256)) {
      p_public_enc_key_x = p.publicKey().eciesNistP256().uncompressedP256().x();
      p_public_enc_key_y = p.publicKey().eciesNistP256().uncompressedP256().y();
    } else if (p.publicKey().ischosen(IEEE1609dot2BaseTypes::BasePublicEncryptionKey::ALT_eciesBrainpoolP256r1)) {
      p_public_enc_key_x = p.publicKey().eciesBrainpoolP256r1().uncompressedP256().x();
      p_public_enc_key_y = p.publicKey().eciesBrainpoolP256r1().uncompressedP256().y();
    } else {
      loggers::get_instance().error("security_services::process_ieee_1609_dot2_signed_data: Unsupported EncryptionKey");
      return -1;
    }
  } else {
    p_public_enc_key_x = int2oct(0, 32);
    p_public_enc_key_y = int2oct(0, 32);
  }

  return 0;
} // End of method extract_encryption_keys

int security_services::read_certificate(const CHARSTRING& p_certificate_id, OCTETSTRING& p_certificate) const {
  return _security_db.get()->get_certificate(std::string(static_cast<const char*>(p_certificate_id)), p_certificate);
}
  
int security_services::read_certificate_digest(const CHARSTRING& p_certificate_id, OCTETSTRING& p_digest) const {
  return _security_db.get()->get_hashed_id(std::string(static_cast<const char*>(p_certificate_id)), p_digest);
}

int security_services::read_certificate_from_digest(const OCTETSTRING& p_digest, CHARSTRING& p_certificate_id) const {
  std::string certificate_id;
  if (_security_db.get()->get_certificate_id(p_digest, certificate_id) != -1) {
    p_certificate_id = CHARSTRING(certificate_id.c_str());
    return 0;
  }
  return -1;
}

int security_services::read_private_key(const CHARSTRING& p_certificate_id, OCTETSTRING& p_private_key) const {
  return _security_db.get()->get_private_key(std::string(static_cast<const char*>(p_certificate_id)), p_private_key);
}

int security_services::read_private_enc_key(const CHARSTRING& p_certificate_id, OCTETSTRING& p_private_enc_key) const {
  return _security_db.get()->get_private_enc_key(std::string(static_cast<const char*>(p_certificate_id)), p_private_enc_key);
}