#ifdef DEBUG_LOW

#include "skpm.h"
#include "debug.h"
#include "platform.h"
#include "crypto_module.h"
#include "skpm_util.h"
#include "skpm_crypto_util.h"

extern uint8_t sDrkCert[MAX_CERT_SIZE];
extern uint16_t sDrkCertLen;

extern uint8_t sServiceCert[MAX_CERT_SIZE];
extern uint16_t sServiceCertLen;

extern uint8_t sServiceKey[MAX_CERT_SIZE];
extern uint16_t sServiceKeyLen;

extern rsa_key_t sServiceRsaKey;

void ecc_test() {
    ecc_key_t ecc_key_info;
    EC_KEY *eccKey = NULL;

   int i;

    for (i = 0; i < 3; i++) {
        crypto_gen_ecc_key(&eccKey, &ecc_key_info);

        if ((ecc_key_info.pubkey_binary[1] == 0x00 && ecc_key_info.pubkey_binary[2] < 128)
            || (ecc_key_info.pubkey_binary[33] == 0x00 && ecc_key_info.pubkey_binary[34] < 128 )) {
            hex_print_tag_debug("pub_key", ecc_key_info.pubkey_binary, ecc_key_info.pubkey_size);            
        }
        
        crypto_clear_ecc_key(&eccKey);
    }
}

void rsa_test() {
    CRYPTO_STATUS ret;

    uint8_t testIn[AP_ID_SIZE] = {0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F, 0x10};
    uint8_t sig[MAX_CERT_SIZE];
    uint32_t sig_size = MAX_CERT_SIZE;

    RSA *rsaKey;
    rsa_key_t rsa_key_info;
    
    //RSA *regenRsaKey;

    ret = crypto_gen_rsa_key(&rsaKey, &rsa_key_info);

    LOGD("crypto_rsa_sign_pkcs - crypto_gen_rsa_key");
    ret = crypto_rsa_sign_pkcs(MD_TYPE_SHA256, 0, rsaKey, testIn, sizeof(testIn), sig, &sig_size);
    hex_print_tag_debug("Signature", sig, sig_size);
    ret = crypto_rsa_verify_pkcs(MD_TYPE_SHA256, 0, rsaKey, testIn, sizeof(testIn), sig, sig_size);
    LOGD("crypto_rsa_verify_pkcs ret = %d", ret);

#ifndef USE_SCRYPTO
    LOGD("crypto_rsa_sign_pss - crypto_gen_rsa_key");
    ret = crypto_rsa_sign_pss(MD_TYPE_SHA256, 0, rsaKey, testIn, sizeof(testIn), sig, &sig_size);
    hex_print_tag_debug("Signature", sig, sig_size);
    ret = crypto_rsa_verify_pss(MD_TYPE_SHA256, 0, rsaKey, testIn, sizeof(testIn), sig, sig_size);
    LOGD("crypto_rsa_verify_pss ret = %d", ret);
#endif

    crypto_clear_rsa_key(&rsaKey);

#if 0
#ifdef DEBUG_LOW
    hex_print_tag_debug("sServiceKey", sServiceKey, sServiceKeyLen);
    hex_print_tag_debug("rsa_key->modulus", sServiceRsaKey.modulus, sServiceRsaKey.modulus_size);
    hex_print_tag_debug("rsa_key->publicExponent", sServiceRsaKey.publicExponent, sServiceRsaKey.publicExponent_size);
    hex_print_tag_debug("rsa_key->privateExponent", sServiceRsaKey.privateExponent, sServiceRsaKey.privateExponent_size);
    hex_print_tag_debug("rsa_key->prime1", sServiceRsaKey.prime1, sServiceRsaKey.prime1_size);
    hex_print_tag_debug("rsa_key->prime2", sServiceRsaKey.prime2, sServiceRsaKey.prime2_size);
    hex_print_tag_debug("rsa_key->exponent1", sServiceRsaKey.exponent1, sServiceRsaKey.exponent1_size);
    hex_print_tag_debug("rsa_key->exponent2", sServiceRsaKey.exponent2, sServiceRsaKey.exponent2_size);
    hex_print_tag_debug("rsa_key->coefficient", sServiceRsaKey.coefficient, sServiceRsaKey.coefficient_size);
#endif
#endif

#if 0
    ret = crypto_regen_rsa_privkey(&regenRsaKey, &sServiceRsaKey);
    LOGD("crypto_rsa_sign_pkcs - crypto_regen_rsa_privkey");
    ret = crypto_rsa_sign_pkcs(regenRsaKey, testIn, sizeof(testIn), sig, &sig_size);
    hex_print_tag_debug("Signature", sig, sig_size);
    ret = crypto_rsa_verify_pkcs(regenRsaKey, testIn, sizeof(testIn), sig, sig_size);
    LOGD("crypto_rsa_verify_pkcs ret = %d", ret);
    LOGD("crypto_rsa_sign_pss - crypto_regen_rsa_privkey");
    ret = crypto_rsa_sign_pss(regenRsaKey, testIn, sizeof(testIn), sig, &sig_size);
    hex_print_tag_debug("Signature", sig, sig_size);
    ret = crypto_rsa_verify_pss(MD_TYPE_SHA256, regenRsaKey, testIn, sizeof(testIn), sig, sig_size);
    LOGD("crypto_rsa_verify_pss ret = %d", ret);
#endif
}

void sha_test() {
    uint8_t apId[AP_ID_SIZE] = {0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F, 0x10};
    uint8_t digest[SHA1_DIGEST_SIZE];
    uint8_t digest2[SHA256_DIGEST_SIZE];

    LOGD("Sha Test");
    crypto_sha(MD_TYPE_SHA1, digest, apId, sizeof(apId), NULL);
    hex_print_tag_debug("apID sha1", digest, SHA1_DIGEST_SIZE);

    crypto_sha(MD_TYPE_SHA256, digest2, apId, sizeof(apId), NULL);
    hex_print_tag_debug("apID sha256", digest2, SHA256_DIGEST_SIZE);
}

void ecdsa_test() {
    ecc_key_t ecc_key_info;
    EC_KEY *eccKey = NULL, *regen_eccKey = NULL;
    uint8_t testIn[AP_ID_SIZE] = {0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F, 0x10};
    uint8_t sig_r[33], sig_s[33];
    uint32_t sig_r_len, sig_s_len;
#if 0
    uint8_t signature[128];
    uint32_t signatureLen;
    int32_t i, cnt = 3;
    int32_t sig_r_over_32 = 0, sig_s_over_32 = 0;
    int32_t sig_r_under_32 = 0, sig_s_under_32 = 0;
    uint8_t re_sig_r[33], re_sig_s[33];
    uint32_t re_sig_r_len, re_sig_s_len;
#endif

    LOGD("ecdsa_test");

    crypto_gen_ecc_key(&eccKey, &ecc_key_info);
    crypto_regen_ecc_pubkey(&regen_eccKey, ecc_key_info.pubkey_binary, ecc_key_info.pubkey_size);

#if 0
    for (i = 0; i < cnt; i++) {
        crypto_ecdsa_sign(MD_TYPE_SHA256, eccKey, testIn, sizeof(testIn), sig_r, &sig_r_len, sig_s, &sig_s_len);
        hex_print_tag_debug("public key", ecc_key_info.pubkey_binary, ecc_key_info.pubkey_size);
        LOGD("~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~");
        hex_print_tag_debug("signature.r", sig_r, sig_r_len);
        hex_print_tag_debug("signature.s", sig_s, sig_s_len);
    
        if (sig_r_len > 32) {
            sig_r_over_32++;
            hex_print_tag_debug("abnormal signature.r", sig_r, sig_r_len);
            hex_print_tag_debug("abnormal signature.s", sig_s, sig_s_len);
        } else if (sig_r_len < 32) {
            sig_r_under_32++;
            hex_print_tag_debug("abnormal signature.r", sig_r, sig_r_len);
            hex_print_tag_debug("abnormal signature.s", sig_s, sig_s_len);

            
            LOGD("~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~");
            rebuildAsn1FromRawSigWithEcc(sig_r, sig_r_len, sig_s, sig_s_len, signature, &signatureLen);
            hex_print_tag_debug("ASN.1 sig.", signature, signatureLen);
            extractRawSigFromAsn1WithEcc(signature, signatureLen, re_sig_r, &re_sig_r_len, re_sig_s, &re_sig_s_len);
            LOGD("~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~");
            hex_print_tag_debug("re_sig_r", re_sig_r, re_sig_r_len);
            hex_print_tag_debug("re_sig_s", re_sig_s, re_sig_s_len);
            LOGD("");
        }

        if (sig_s_len > 32) {
            sig_s_over_32++;
            hex_print_tag_debug("abnormal signature.r", sig_r, sig_r_len);
            hex_print_tag_debug("abnormal signature.s", sig_s, sig_s_len);
        } else if (sig_s_len < 32) {
            sig_s_under_32++;
            hex_print_tag_debug("abnormal signature.r", sig_r, sig_r_len);
            hex_print_tag_debug("abnormal signature.s", sig_s, sig_s_len);

            
            LOGD("~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~");
            rebuildAsn1FromRawSigWithEcc(sig_r, sig_r_len, sig_s, sig_s_len, signature, &signatureLen);
            hex_print_tag_debug("ASN.1 sig.", signature, signatureLen);
            extractRawSigFromAsn1WithEcc(signature, signatureLen, re_sig_r, &re_sig_r_len, re_sig_s, &re_sig_s_len);
            LOGD("~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~");
            hex_print_tag_debug("re_sig_r", re_sig_r, re_sig_r_len);
            hex_print_tag_debug("re_sig_s", re_sig_s, re_sig_s_len);
            LOGD("");
        }
    }
    LOGD("~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~");
    LOGD("sig_r_over_32 : %d, sig_s_over_32 :%d", sig_r_over_32, sig_s_over_32);
    LOGD("sig_r_under_32 : %d, sig_s_under_32 :%d", sig_r_under_32, sig_s_under_32);    
#else
    crypto_ecdsa_sign(MD_TYPE_SHA256, eccKey, testIn, sizeof(testIn), sig_r, &sig_r_len, sig_s, &sig_s_len);
    hex_print_tag_debug("public key", ecc_key_info.pubkey_binary, ecc_key_info.pubkey_size);
    LOGD("~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~");
    hex_print_tag_debug("signature.r", sig_r, sig_r_len);
    hex_print_tag_debug("signature.s", sig_s, sig_s_len);

    LOGD("crypto_ecdsa_verify test 1");
    crypto_ecdsa_verify(MD_TYPE_SHA256, eccKey, testIn, sizeof(testIn), sig_r, sig_r_len, sig_s, sig_s_len);
    LOGD("crypto_ecdsa_verify test 2");
    crypto_ecdsa_verify(MD_TYPE_SHA256, regen_eccKey, testIn, sizeof(testIn), sig_r, sig_r_len, sig_s, sig_s_len);
#endif

    crypto_clear_ecc_key(&eccKey);
    crypto_clear_ecc_key(&regen_eccKey);
}

#define NB_TESTS 3
void ccm_test() {
    unsigned char key[] = {
        0x40, 0x41, 0x42, 0x43, 0x44, 0x45, 0x46, 0x47,
        0x48, 0x49, 0x4a, 0x4b, 0x4c, 0x4d, 0x4e, 0x4f
    };

    unsigned char iv[] = {
        0x10, 0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17,
        0x18, 0x19, 0x1a, 0x1b
    };

    unsigned char ad[] = {
        0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
        0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f,
        0x10, 0x11, 0x12, 0x13
    };

    unsigned char msg[] = {
        0x20, 0x21, 0x22, 0x23, 0x24, 0x25, 0x26, 0x27,
        0x28, 0x29, 0x2a, 0x2b, 0x2c, 0x2d, 0x2e, 0x2f,
        0x30, 0x31, 0x32, 0x33, 0x34, 0x35, 0x36, 0x37,
    };

    size_t iv_len [NB_TESTS] = { 7, 8,  12 };
    size_t add_len[NB_TESTS] = { 8, 16, 20 };
    size_t msg_len[NB_TESTS] = { 4, 16, 24 };
    size_t tag_len[NB_TESTS] = { 4, 6,  8  };

    unsigned char res[NB_TESTS][32] = {
        {   0x71, 0x62, 0x01, 0x5b, 0x4d, 0xac, 0x25, 0x5d },
        {   0xd2, 0xa1, 0xf0, 0xe0, 0x51, 0xea, 0x5f, 0x62,
            0x08, 0x1a, 0x77, 0x92, 0x07, 0x3d, 0x59, 0x3d,
            0x1f, 0xc6, 0x4f, 0xbf, 0xac, 0xcd },
        {   0xe3, 0xb2, 0x01, 0xa9, 0xf5, 0xb7, 0x1a, 0x7a,
            0x9b, 0x1c, 0xea, 0xec, 0xcd, 0x97, 0xe7, 0x0b,
            0x61, 0x76, 0xaa, 0xd9, 0xa4, 0x42, 0x8a, 0xa5,
            0x48, 0x43, 0x92, 0xfb, 0xc1, 0xb0, 0x99, 0x51 }
    };

    CRYPTO_STATUS ret;
    int i;
//    ((datalen+15)/16)*16)+taglen
    unsigned char out[40], out2[40];
    unsigned long out_len, out2_len;
    
    for (i = 0; i < NB_TESTS; i++) {

        LOGD("CCM-AES #%u:", (unsigned int)i + 1);
        out_len = sizeof(out);
        ret = crypto_aes_ccm_encrypt( iv, iv_len[i], 
                                      key, sizeof(key),
                                      ad, add_len[i],
                                      msg, msg_len[i],
                                      out, &out_len,
                                      tag_len[i] );

        LOGD("crypto_aes_ccm_encrypt ret : %d", ret);
        hex_print_tag_debug("crypto_aes_ccm_encrypt", out, out_len);
        if ( ret != CRYPTO_STATUS_SUCCESS || memcmp( out, res[i], msg_len[i] + tag_len[i] ) != 0 ) {
            LOGE("failed");
            return;
        }

        out2_len = sizeof(out2);
        ret = crypto_aes_ccm_decrypt(
                                    iv, iv_len[i],
                                    key, sizeof(key),
                                    ad, add_len[i],
                                    res[i], out_len,
                                    out2, &out2_len,
                                    tag_len[i] );

        LOGD("crypto_aes_ccm_decrypt ret : %d", ret);
        hex_print_tag_debug("crypto_aes_ccm_decrypt", out2, out2_len);        
        if ( ret != CRYPTO_STATUS_SUCCESS || memcmp( out2, msg, msg_len[i] ) != 0 ) {
            LOGE("failed" );
            return;
        }
    
        LOGD("passed");
    }


}

void hmac_test() {

    const uint8_t key[] = "012345678";
    const char data[] = "hello world";
    uint8_t result[SHA1_DIGEST_SIZE], result2[SHA256_DIGEST_SIZE];

    crypto_hmac(MD_TYPE_SHA1, result, key, sizeof(key) - 1, data, strlen(data), NULL);
    hex_print_tag_debug("hamc_sha1", result, sizeof(result));

    crypto_hmac(MD_TYPE_SHA256, result2, key, sizeof(key) - 1, data, strlen(data), NULL);
    hex_print_tag_debug("hamc_sha256", result2, sizeof(result2));
}

void ecc_gen_secret_test() {
    uint8_t priv[] = {0x87, 0x2f, 0x99, 0xfc, 0x46, 0x1a, 0x0f, 0x6c, 0x74, 0x93, 0xac, 0x5d, 0x5f, 0x78, 0x1e, 0xfb, 0xf6, 0xab, 0x15, 0xe9, 0x12, 0x58, 0xff, 0x35, 0xa6, 0x06, 0x5d, 0x56, 0x62, 0x6f, 0xdc, 0x38};
    uint8_t peer[] = {0x04, 0x80, 0x72, 0xe2, 0x24, 0xd7, 0x51, 0x8d, 0x28, 0x26, 0x08, 0xdf, 0xc5, 0x40, 0x46, 0x78, 0x9f, 0xcb, 0xc7, 0xe6, 0x02, 0x11, 0xa5, 0xc2, 0x0e, 0x54, 0xcd, 0xf3, 0xb4, 0x13, 0xe1, 0x65, 0xa9, 0xa9, 0x6f, 0x01, 0xbb, 0x7c, 0xae, 0x40, 0xe0, 0x7b, 0x83, 0xcb, 0xfa, 0x4f, 0x29, 0x2e, 0xe9, 0x56, 0x6c, 0x86, 0x6e, 0xf4, 0xc1, 0x3c, 0xd1, 0x7b, 0x00, 0x8e, 0x37, 0x6b, 0xe5, 0x61, 0x82};
    uint8_t secrekey[ECC_SECRET_SIZE];

    hex_print_tag_debug("priv", priv, sizeof(priv));
    hex_print_tag_debug("peer", peer, sizeof(peer));

    crypto_gen_ecc_secret(priv, sizeof(priv), peer, sizeof(peer), secrekey);

    hex_print_tag_debug("secretkey", secrekey, ECC_SECRET_SIZE);
}

void gcm_test() {
    uint8_t key_1[16] = {0x67, 0xf2, 0xf2, 0xb8, 0x82, 0xce, 0xe3, 0x37, 0xdc, 0x73, 0x05, 0x85, 0x0f, 0x09, 0xea, 0xfe};
    uint8_t key_2[16] = {0xc5, 0xfc, 0x09, 0x48, 0xa7, 0x1f, 0xf7, 0x4c, 0xef, 0x73, 0xfd, 0xb2, 0x6f, 0x10, 0x72, 0x91};

    uint8_t plain[16] = {0x14, 0x00, 0x00, 0x0c, 0xe2, 0xf4, 0x60, 0x05, 0x9b, 0x1b, 0x0f, 0x50, 0x10, 0x75, 0x9f, 0xa3};
    uint8_t cipher[100];
    uint32_t cipherlen = sizeof(cipher);

    uint8_t in_cipher[16] = {0x0e, 0x50, 0xa1, 0x61, 0xf5, 0x87, 0x4f, 0x92, 0x7e, 0x8f, 0x4e, 0xb0, 0xa6, 0x8a, 0xe4, 0xfe};
    uint8_t rePlain[100];
    uint32_t rePlainlen = sizeof(in_cipher);

    uint8_t aad[13] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x16, 0x03, 0x03, 0x00, 0x10}; 

    uint8_t enc_iv[12] = {0xca, 0x67, 0xa0, 0xa1, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
    uint8_t dec_iv[12] = {0xcd, 0x72, 0xf5, 0x52, 0xea, 0x9c, 0xef, 0x59, 0x13, 0x0e, 0xd8, 0x99};

    uint8_t enc_tag[16] = {0xa6, 0x46, 0x02, 0xf3, 0xf9, 0x9f, 0xd0, 0x94, 0x22, 0x67, 0x6c, 0x67, 0xfa, 0x1d, 0x15, 0xc3};
    uint8_t dec_tag[16] = {0x17, 0x81, 0x69, 0x2f, 0xd9, 0x6d, 0xfe, 0xdc, 0xb9, 0x3a, 0x04, 0x46, 0x4e, 0x8c, 0xce, 0xf5};

    unsigned long tempCipherlen = cipherlen;
    unsigned long tempRePlainlen;

    LOGI("crypto_aes_gcm_encrypt");
    hex_print_tag_debug("plain", plain, sizeof(plain));

    crypto_aes_gcm_encrypt(enc_iv, sizeof(enc_iv), key_1, sizeof(key_1), aad, sizeof(aad), 
                                plain, sizeof(plain), cipher, &tempCipherlen, enc_tag, sizeof(enc_tag));
    cipherlen = tempCipherlen;
    hex_print_tag_debug("cipher", cipher, cipherlen);
    hex_print_tag_debug("After encryption : tag", enc_tag, sizeof(enc_tag));

    tempRePlainlen = rePlainlen;
    crypto_aes_gcm_decrypt(dec_iv, sizeof(dec_iv), key_2, sizeof(key_2), aad, sizeof(aad), 
                                in_cipher, sizeof(in_cipher), rePlain, &tempRePlainlen, dec_tag, sizeof(dec_tag));
    rePlainlen = tempRePlainlen;
    hex_print_tag_debug("rePlain", rePlain, rePlainlen);
    hex_print_tag_debug("After decryption : tag", dec_tag, sizeof(dec_tag));
}

void encoding_test() {
    ecc_key_t ecc_key_info;
    EC_KEY *eccKey = NULL;
    uint8_t key_block[WRAPPED_KEY_INFO_SIZE];
    uint16_t key_blockSize;

    crypto_gen_ecc_key(&eccKey, &ecc_key_info);    
    encodeAsn1EccPrivateKey(ecc_key_info, key_block, &key_blockSize);
    hex_print_tag_debug("encodeAsn1EccPrivateKey", key_block, key_blockSize);
    crypto_clear_ecc_key(&eccKey);
}
void crypto_test() {
    LOGI("ecc_test");
    ecc_test();
    LOGI("ecdsa_test");
    ecdsa_test();
//    LOGI("hmac_test");
//    hmac_test();
//    LOGI("ecc_gen_secret_test");
//    ecc_gen_secret_test();
//    LOGI("encoding_test");
//    encoding_test();
//    LOGI("sha_test");
//    sha_test();
//    LOGI("ccm_test");
//    ccm_test();

//    LOGI("rsa_test");
//    rsa_test();

//    LOGI("gcm_test");
//    gcm_test();

    LOGI("fisnish_test");
}

void unwrap_test(p_cmd_t cmd) {
    int32_t ret = RET_SUCCESS;

    uint8_t input[MAX_CERT_SIZE] = {0, };
    uint32_t inputSize;
    uint8_t output[MAX_CERT_SIZE] = {0, };
    uint32_t outputSize = MAX_CERT_SIZE;

    #ifdef USE_BLOWFISH
    const uint8_t SEM_TID[TID_SIZE] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x53, 0x4b, 0x4d};
#endif
#ifdef USE_QSEE
    const uint8_t SEM_TID[128] = "sem";
#endif

    inputSize = cmd->dataLen;
    memcpy(input, cmd->data, inputSize);

    hex_print_tag_debug("input", input, inputSize);
#if defined(USE_BLOWFISH) || defined(USE_QSEE)
    ret = unwrap_secure_object(SEM_TID, input, inputSize, output, &outputSize);
#else
    ret = unwrap_secure_object(input, inputSize, output, &outputSize);
#endif
    if (ret == RET_SUCCESS) {
        hex_print_tag_debug("output", output, outputSize);
    }
}

#endif