#include <chrono>

#include <openssl/ec.h>
#include <openssl/ecdsa.h>

#include "security_services.hh"

#include "EtsiTs103097Codec_ToBeSignedData.hh"
#include "EtsiTs103097Codec_Data.hh"

#include "sha256.hh"
#include "sha384.hh"
#include "ec_keys.hh"

#include "Params.hh"

#include "loggers.hh"

#include "converter.hh"

security_services * security_services::instance = NULL;

security_services::security_services() : _ec_keys(nullptr), _security_db(nullptr) {
  loggers::get_instance().log(">>> security_services::security_services");
} // End of ctor

int security_services::setup(Params& p_params) {
  loggers::get_instance().log("security_services::setup");
  p_params.log();

  _security_db.reset(new security_db(p_params[Params::sec_db_path]));
  if (_security_db.get() == nullptr) { // Memory allocation issue
    return -1;
  }
  
  return 0;
}

int security_services::verify_and_extract_gn_payload(const OCTETSTRING& p_secured_gn_payload, const bool p_verify, 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)
  IEEE1609dot2::Ieee1609Dot2Data ieee_1609dot2_data;
  EtsiTs103097Codec_Data codec;
  codec.decode(p_secured_gn_payload, ieee_1609dot2_data, &p_params);
  // Sanity checks
  if (!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)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(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) {
      return -1;
    }
  } else if (p_ieee_1609_dot2_content.ischosen(IEEE1609dot2::Ieee1609Dot2Content::ALT_encryptedData)) {
    loggers::get_instance().warning("security_services::process_ieee_1609_dot2_content: Unsupported IEEE 1609.2 Content, discard it");
    return -1;
  } else if (p_ieee_1609_dot2_content.ischosen(IEEE1609dot2::Ieee1609Dot2Content::ALT_signedCertificateRequest)) {
    // TODO Set Certificate re-transmission flag and reset timer
    loggers::get_instance().error("security_services::process_ieee_1609_dot2_content: TODO Set Certificate re-transmission flag and reset timer");
    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");
    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 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");
      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");
      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");
    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)) {
    // TODO Retrieve the certificate identifier from digest
    loggers::get_instance().log("security_services::process_ieee_1609_dot2_signed_data: TODO Retrieve the certificate identifier from digest");
    result = _security_db.get()->get_certificate_id(p_signed_data.signer().digest(), certificate_id);
  } else if (p_signed_data.signer().ischosen(IEEE1609dot2::SignerIdentifier::ALT_certificate)) {
    loggers::get_instance().error("security_services::process_ieee_1609_dot2_signed_data: Unsupported SignerIdentifier");
    return -1;
  } else {
    loggers::get_instance().warning("security_services::process_ieee_1609_dot2_signed_data: Unsupported SignerIdentifier");
    return -1;
  }
  loggers::get_instance().log("security_services::process_ieee_1609_dot2_signed_data: certificate id = '%s'", certificate_id.c_str());
  if (result == -1) {
    loggers::get_instance().warning("security_services::process_ieee_1609_dot2_signed_data: Certificate not found for the specified signer");
    return -1;
  }
  // 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(os, 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);

  //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, const bool p_add_certificate, 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();

  // 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;
  header_info.psid() = converter::get_instance().string_to_int(p_params[Params::its_aid]);
  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
  header_info.generationTime() = ms;
  header_info.expiryTime().set_to_omit();
  header_info.generationLocation().set_to_omit();
  header_info.p2pcdLearningRequest().set_to_omit();
  header_info.missingCrlIdentifier().set_to_omit();
  header_info.encryptionKey().set_to_omit();
  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::secure_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::secure_gn_payload: Failed to secure payload");
    return -1;
  }
  IEEE1609dot2::SignerIdentifier signer;
  std::string certificate_id = p_params[Params::certificate] + "_at";
  loggers::get_instance().log("security_services::secure_gn_payload: certificate_id = %s", certificate_id.c_str());
  if (!p_add_certificate) {
    OCTETSTRING digest;
    if (_security_db->get_hashed_id(certificate_id, digest) != 0) {
      loggers::get_instance().warning("security_services::secure_gn_payload: Failed to secure payload");
    return -1;
    }
    signer.digest() = digest;
  } else {
    OCTETSTRING cert;
    if (_security_db->get_certificate(certificate_id, cert) != 0) {
      loggers::get_instance().warning("security_services:secure_gn_payload: Failed to secure payload");
      return -1;
    }
    loggers::get_instance().error("security_services::secured_gn_payload: TODO Add certifcate case");
    // FIXME Need to decode certifcate, shall be done once is security_db
  }
  IEEE1609dot2::SignedData signed_data(
                                       hashId,
                                       tbs_data,
                                       signer,
                                       signature
                                       );
  loggers::get_instance().log_msg("security_services::secure_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::secure_gn_payload: ieee_1609dot2_data = ", ieee_1609dot2_data);
  EtsiTs103097Codec_Data codec;
  codec.encode(ieee_1609dot2_data, p_secured_gn_payload);
  if (!p_secured_gn_payload.is_bound()) {
    loggers::get_instance().warning("security_services::secure_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 {
    // TODO Add other signature algorithm
    loggers::get_instance().error("security_services::sign_tbs_data: TODO Add other 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] + "_at";
  loggers::get_instance().log("security_services::sign_tbs_data: 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());
  ec_keys 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.uncompressedP256() = IEEE1609dot2BaseTypes::EccP256CurvePoint_uncompressedP256(
                                                                                    public_key_x,
                                                                                    public_key_y
                                                                                    );
  p_signature.ecdsaNistP256Signature() = IEEE1609dot2BaseTypes::EcdsaP256Signature(
                                                                                   ep,
                                                                                   OCTETSTRING(r_sig.size(), r_sig.data()) + 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 = p_signature.ecdsaNistP256Signature().rSig().x__only() + p_signature.ecdsaNistP256Signature().sSig();
  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());
  ec_keys k(ec_elliptic_curves::nist_p_256, key_x, key_y);
  if (k.sign_verif(hashData, signature) == 0) {
    return 0;
  }
  
  return -1;
}
