#include <stdarg.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include "libs_tlc/include/QSEEComAPI/QSEEComAPI.h"
#include "pebble.h"
#include "gen_cert/gen_cert.h"
#include "libs_tlc/include/dk_native_client/libdk_native_client.h"

uint8_t cert[MAX_CERT_SIZE];
uint32_t cert_len = MAX_CERT_SIZE;

struct QSEECom_handle *l_QSEEComHandle = NULL;
tciMessage_t *send_msg;
tciMessage_t *resp_msg;
uint32_t len = sizeof(tciMessage_t);

void usage() {
	printf("Usage is ./tee_test <command id> <key id>\n");
	printf("Command ID:\n");
	printf("           1 --> CMD_LOAD_CERT\n");
	printf("           2 --> CMD_GEN_ATN_NONCE\n");
	printf("           3 --> CMD_GEN_MASTER_KEY\n");
	printf("           4 --> CMD_DERIVE_SUBKEY\n");
	printf("           5 --> backup & recovery commands\n\n");
//	printf("Key ID (for CMD_SSS_SPLIT & CMD_SSS_JOIN)\n");
//	printf("           0 --> 10 bytes\n");
//	printf("           1 --> 16 bytes\n");
//	printf("           2 --> 24 bytes\n");
//	printf("           3 --> 32 bytes\n");
}

void dump_hex(char *buf, size_t len) {
	size_t i = 0;
	char *cp = buf;
	printf("[dump_hex: %lu]", len);
	for (i = 0; i < len; i++, cp++) {
		printf("%02X", *cp);
	}
	printf("\n");
}

void test1(cert_chain_t *cert_chain) {
	uint32_t ret = 0;
	memset(send_msg, 0, sizeof(tciMessage_t));
	memset(resp_msg, 0, sizeof(tciMessage_t));

	send_msg->header.id = CMD_LOAD_CERT;
	generate_pebble_cert(cert, &cert_len);
	//getDrkCertificate(KEY_TYPE_RSA, (char *)cert, cert_len);
	send_msg->payload.load_cert_cmd.wrapped_key.len = cert_len;
	memcpy(send_msg->payload.load_cert_cmd.wrapped_key.buf, cert,
			send_msg->payload.load_cert_cmd.wrapped_key.len);
	printf("%s: load_cert_cmd.wrapped_key.len = %d\n", __func__,
			send_msg->payload.load_cert_cmd.wrapped_key.len);
	dump_hex((char*) send_msg->payload.load_cert_cmd.wrapped_key.buf,
			send_msg->payload.load_cert_cmd.wrapped_key.len);

	ret = QSEECom_send_cmd(l_QSEEComHandle, send_msg, len, resp_msg, len);
	if (ret) {
		printf("%s: Send command failed with ret = %d\n", __func__, ret);
	} else {
		printf("\nrespose for load_cert:\n");
		if (cert_chain != NULL)
			cert_chain->num_certs =
					resp_msg->payload.load_cert_resp.cert_chain.num_certs;
		for (uint32_t i = 0;
				i < resp_msg->payload.load_cert_resp.cert_chain.num_certs;
				i++) {
			if (cert_chain != NULL) {
				memcpy(cert_chain->cert[i].blob,
						resp_msg->payload.load_cert_resp.cert_chain.cert[i].blob,
						resp_msg->payload.load_cert_resp.cert_chain.cert[i].len);
				cert_chain->cert[i].len =
						resp_msg->payload.load_cert_resp.cert_chain.cert[i].len;
			}
//                    printf("cert %d:\n", i);
//                    dump_hex((char *)(resp_msg->payload.load_cert_resp.cert_chain.cert[i].blob),
//                                    resp_msg->payload.load_cert_resp.cert_chain.cert[i].len);
		}
	}
}

void test2() {
	uint32_t ret = 0;
	memset(send_msg, 0, sizeof(tciMessage_t));
	memset(resp_msg, 0, sizeof(tciMessage_t));

	send_msg->header.id = CMD_GEN_ATN_NONCE;
//	send_msg->payload.gen_atn_nonce_cmd.len = 24; // 8 <= nonce len <= 32

	ret = QSEECom_send_cmd(l_QSEEComHandle, send_msg, len, resp_msg, len);
	if (ret) {
		printf("%s: Send command failed with ret = %d\n", __func__, ret);
	} else {
		printf("\nrespose for CMD_GEN_ATN_NONCE:\n");
		dump_hex((char*) (resp_msg->payload.gen_atn_nonce_resp.nonce.buf),
				resp_msg->payload.gen_atn_nonce_resp.nonce.len);
		printf("\nPRINT: %s", resp_msg->payload.gen_atn_nonce_resp.nonce.buf);
	}
}

void test3(gen_master_key_resp_t *gen_master_key_resp) {
	uint32_t ret = 0;
	memset(send_msg, 0, sizeof(tciMessage_t));
	memset(resp_msg, 0, sizeof(tciMessage_t));

	send_msg->header.id = CMD_GEN_MASTER_KEY;
	send_msg->payload.gen_master_key_cmd.cert_chain.num_certs = 3;
	memcpy(send_msg->payload.gen_master_key_cmd.cert_chain.cert[0].blob,
			aks_cert0, strlen((char* )aks_cert0));
	send_msg->payload.gen_master_key_cmd.cert_chain.cert[0].len = strlen(
			(char*) aks_cert0);
	memcpy(send_msg->payload.gen_master_key_cmd.cert_chain.cert[1].blob,
			aks_cert1, strlen((char* )aks_cert1));
	send_msg->payload.gen_master_key_cmd.cert_chain.cert[1].len = strlen(
			(char*) aks_cert1);
	memcpy(send_msg->payload.gen_master_key_cmd.cert_chain.cert[2].blob,
			aks_cert2, strlen((char* )aks_cert2));
	send_msg->payload.gen_master_key_cmd.cert_chain.cert[2].len = strlen(
			(char*) aks_cert2);
	//aks root has been hard coded in TA. no need to send to TA
	//memcpy(send_msg->payload.gen_master_key_cmd.cert_chain.cert[3].blob, aks_cert3, strlen((char *)aks_cert3));
	//send_msg->payload.gen_master_key_cmd.cert_chain.cert[3].len = strlen((char *)aks_cert3);
//    printf("cert chain:");
//    for (uint32_t i = 0; i < send_msg->payload.gen_master_key_cmd.cert_chain.num_certs; i++) {
//            printf("cert %d:\n", i);
//            printf("%s\n", send_msg->payload.gen_master_key_cmd.cert_chain.cert[i].blob);
//            dump_hex((char *)send_msg->payload.gen_master_key_cmd.cert_chain.cert[i].blob,
//                            send_msg->payload.gen_master_key_cmd.cert_chain.cert[i].len);
//    }
	memcpy(send_msg->payload.gen_master_key_cmd.gcm_aad.buf, gcm_aad,
			sizeof(gcm_aad));
	send_msg->payload.gen_master_key_cmd.gcm_aad.len = sizeof(gcm_aad);

	ret = QSEECom_send_cmd(l_QSEEComHandle, send_msg, len, resp_msg, len);
	if (ret) {
		printf("%s: Send command failed with ret = %d\n", __func__, ret);
	} else {
		if (gen_master_key_resp != NULL)
			memcpy(gen_master_key_resp,
					&(resp_msg->payload.gen_master_key_resp),
					sizeof(gen_master_key_resp_t));
		printf("\nresponse for CMD_GEN_MASTER_KEY:\n");
		printf("master_key_wrapped_ta:\n");
		dump_hex(
				(char*) (resp_msg->payload.gen_master_key_resp.master_key_wrapped_ta.buf),
				resp_msg->payload.gen_master_key_resp.master_key_wrapped_ta.len);
		printf("secret_ct:\n");
		dump_hex((char*) (resp_msg->payload.gen_master_key_resp.secret_ct.buf),
				resp_msg->payload.gen_master_key_resp.secret_ct.len);
		printf("gcm_iv:\n");
		dump_hex((char*) (resp_msg->payload.gen_master_key_resp.gcm_iv),
				WRAPPING_AES_IV_LEN);
		printf("gcm_tag:\n");
		dump_hex((char*) (resp_msg->payload.gen_master_key_resp.gcm_tag),
				WRAPPING_AES_TAG_LEN);
		printf("wrapped_gcm_key:\n");
		dump_hex(
				(char*) (resp_msg->payload.gen_master_key_resp.wrapped_gcm_key.buf),
				resp_msg->payload.gen_master_key_resp.wrapped_gcm_key.len);
	}
}

void test4() {
	uint32_t ret = 0;
	gen_master_key_resp_t gen_master_key_resp;
	test3(&gen_master_key_resp);

	memset(send_msg, 0, sizeof(tciMessage_t));
	memset(resp_msg, 0, sizeof(tciMessage_t));
	send_msg->header.id = CMD_DERIVE_SUBKEY;
	send_msg->payload.derive_subkey_cmd.cert_chain.num_certs = 3;
	memcpy(send_msg->payload.derive_subkey_cmd.cert_chain.cert[0].blob,
			aks_cert0, strlen((char* )aks_cert0));
	send_msg->payload.derive_subkey_cmd.cert_chain.cert[0].len = strlen(
			(char*) aks_cert0);
	memcpy(send_msg->payload.derive_subkey_cmd.cert_chain.cert[1].blob,
			aks_cert1, strlen((char* )aks_cert1));
	send_msg->payload.derive_subkey_cmd.cert_chain.cert[1].len = strlen(
			(char*) aks_cert1);
	memcpy(send_msg->payload.derive_subkey_cmd.cert_chain.cert[2].blob,
			aks_cert2, strlen((char* )aks_cert2));
	send_msg->payload.derive_subkey_cmd.cert_chain.cert[2].len = strlen(
			(char*) aks_cert2);
	//memcpy(send_msg->payload.derive_subkey_cmd.cert_chain.cert[3].blob, aks_cert3, strlen((char *)aks_cert3));
	//send_msg->payload.derive_subkey_cmd.cert_chain.cert[3].len = strlen((char *)aks_cert3);
//    printf("cert chain:");
//    for (uint32_t i = 0; i < send_msg->payload.derive_subkey_cmd.cert_chain.num_certs; i++) {
//            printf("cert %d:\n", i);
//            printf("%s\n", send_msg->payload.derive_subkey_cmd.cert_chain.cert[i].blob);
//    }
	memcpy(send_msg->payload.derive_subkey_cmd.master_key_wrapped_ta.buf,
			gen_master_key_resp.master_key_wrapped_ta.buf,
			gen_master_key_resp.master_key_wrapped_ta.len);
	send_msg->payload.derive_subkey_cmd.master_key_wrapped_ta.len =
			gen_master_key_resp.master_key_wrapped_ta.len;
	memcpy(send_msg->payload.derive_subkey_cmd.alias.buf, subkey_alias,
			strlen(subkey_alias));
	send_msg->payload.derive_subkey_cmd.alias.len = strlen(subkey_alias);
	memcpy(send_msg->payload.derive_subkey_cmd.gcm_aad.buf, gcm_aad,
			sizeof(gcm_aad));
	send_msg->payload.derive_subkey_cmd.gcm_aad.len = sizeof(gcm_aad);

	ret = QSEECom_send_cmd(l_QSEEComHandle, send_msg, len, resp_msg, len);
	if (ret) {
		printf("%s: Send command failed with ret = %d\n", __func__, ret);
	} else {
		printf("\nrespose for CMD_DERIVE_SUBKEY:\n");
		printf("secret_ct:\n");
		dump_hex((char*) (resp_msg->payload.derive_subkey_resp.secret_ct.buf),
				resp_msg->payload.derive_subkey_resp.secret_ct.len);
		printf("gcm_iv:\n");
		dump_hex((char*) (resp_msg->payload.derive_subkey_resp.gcm_iv),
				WRAPPING_AES_IV_LEN);
		printf("gcm_tag:\n");
		dump_hex((char*) (resp_msg->payload.derive_subkey_resp.gcm_tag),
				WRAPPING_AES_TAG_LEN);
		printf("wrapped_gcm_key:\n");
		dump_hex(
				(char*) (resp_msg->payload.derive_subkey_resp.wrapped_gcm_key.buf),
				resp_msg->payload.derive_subkey_resp.wrapped_gcm_key.len);
	}
}

#if 0
void testSplit(uint32_t keyId) {
	uint32_t ret = 0;
	memset(send_msg, 0, sizeof(tciMessage_t));
	memset(resp_msg, 0, sizeof(tciMessage_t));

	send_msg->header.id = CMD_SSS_SPLIT;
	send_msg->payload.split_cmd.len = strlen(sss_tests[keyId]);
	memcpy(send_msg->payload.split_cmd.data, sss_tests[keyId],
			send_msg->payload.split_cmd.len);
	send_msg->payload.split_cmd.k = 2;
	send_msg->payload.split_cmd.n = 3;
	printf("%s: split_cmd.len = %d\n", __func__,
			send_msg->payload.split_cmd.len);
	printf("%s: split_cmd.data = \n", __func__);
	dump_hex((char*) send_msg->payload.split_cmd.data,
			send_msg->payload.split_cmd.len);
	printf("%s: split_cmd.k = %d\n", __func__, send_msg->payload.split_cmd.k);
	printf("%s: split_cmd.n = %d\n", __func__, send_msg->payload.split_cmd.n);

	ret = QSEECom_send_cmd(l_QSEEComHandle, send_msg, len, resp_msg, len);
	if (ret) {
		printf("%s: Send command failed with ret = %d\n", __func__, ret);
	} else {
		printf("CMD_SSS_SPLIT result: count: %d\n",
				resp_msg->payload.split_resp.count);
		printf("CMD_SSS_SPLIT result: len: %d\n",
				resp_msg->payload.split_resp.len);
		dump_hex((char*) (resp_msg->payload.split_resp.data0),
				resp_msg->payload.split_resp.len);
		dump_hex((char*) (resp_msg->payload.split_resp.data1),
				resp_msg->payload.split_resp.len);
		dump_hex((char*) (resp_msg->payload.split_resp.data2),
				resp_msg->payload.split_resp.len);
	}
}

void testJoin(uint32_t keyId) {
	uint32_t ret = 0;
	memset(send_msg, 0, sizeof(tciMessage_t));
	memset(resp_msg, 0, sizeof(tciMessage_t));

	send_msg->header.id = CMD_SSS_JOIN;
	send_msg->payload.join_cmd.count = 2;
	if (keyId == 0) {
		send_msg->payload.join_cmd.len = 11;
		memcpy(send_msg->payload.join_cmd.data0, sss_shares_0[0],
				send_msg->payload.join_cmd.len);
		memcpy(send_msg->payload.join_cmd.data1, sss_shares_0[1],
				send_msg->payload.join_cmd.len);
		memcpy(send_msg->payload.join_cmd.data2, sss_shares_0[2],
				send_msg->payload.join_cmd.len);
	} else if (keyId == 1) {
		send_msg->payload.join_cmd.len = 17;
		memcpy(send_msg->payload.join_cmd.data0, sss_shares_1[0],
				send_msg->payload.join_cmd.len);
		memcpy(send_msg->payload.join_cmd.data1, sss_shares_1[1],
				send_msg->payload.join_cmd.len);
		memcpy(send_msg->payload.join_cmd.data2, sss_shares_1[2],
				send_msg->payload.join_cmd.len);
	} else if (keyId == 2) {
		send_msg->payload.join_cmd.len = 25;
		memcpy(send_msg->payload.join_cmd.data0, sss_shares_2[0],
				send_msg->payload.join_cmd.len);
		memcpy(send_msg->payload.join_cmd.data1, sss_shares_2[1],
				send_msg->payload.join_cmd.len);
		memcpy(send_msg->payload.join_cmd.data2, sss_shares_2[2],
				send_msg->payload.join_cmd.len);
	} else if (keyId == 3) {
		send_msg->payload.join_cmd.len = 33;
		memcpy(send_msg->payload.join_cmd.data0, sss_shares_3[0],
				send_msg->payload.join_cmd.len);
		memcpy(send_msg->payload.join_cmd.data1, sss_shares_3[1],
				send_msg->payload.join_cmd.len);
		memcpy(send_msg->payload.join_cmd.data2, sss_shares_3[2],
				send_msg->payload.join_cmd.len);
	}
	printf("%s: join_cmd.count = %d\n", __func__,
			send_msg->payload.join_cmd.count);
	printf("%s: join_cmd.len = %d\n", __func__, send_msg->payload.join_cmd.len);
	dump_hex((char*) (send_msg->payload.join_cmd.data0),
			send_msg->payload.join_cmd.len);
	dump_hex((char*) (send_msg->payload.join_cmd.data1),
			send_msg->payload.join_cmd.len);
	dump_hex((char*) (send_msg->payload.join_cmd.data2),
			send_msg->payload.join_cmd.len);

	ret = QSEECom_send_cmd(l_QSEEComHandle, send_msg, len, resp_msg, len);
	if (ret) {
		printf("%s: Send command failed with ret = %d\n", __func__, ret);
	} else {
		printf("CMD_SSS_JOIN result: \n");
		dump_hex((char*) (resp_msg->payload.join_resp.data),
				resp_msg->payload.join_resp.len);
	}
}
#endif

void test5() {
	cert_chain_t cert_chain;
	gen_master_key_resp_t gen_master_key_resp;  //master_key_wrapped_taa
	uint8_t backup_reqs_jws[MAX_BUDDIES][JWE_JWS_MAX_SIZE],
			jws[JWE_JWS_MAX_SIZE];
	uint8_t wrapped_shares[MAX_BUDDIES][SK_WRAP_KEY_LEN], checksums[MAX_BUDDIES
			* CHECKSUM_LEN];
	uint32_t ret = 0, n = 3, k = 2, wshare_len, checksums_len = 0;

	test1(&cert_chain);
	test3(&gen_master_key_resp);

	//CMD_GEN_BACKUP_REQ  cmd 0x00000008
	memset(send_msg, 0, sizeof(tciMessage_t));
	memset(resp_msg, 0, sizeof(tciMessage_t));
	send_msg->header.id = CMD_GEN_BACKUP_REQ;
	memcpy(send_msg->payload.gen_backup_req_cmd.wrapped_master_key.buf,
			gen_master_key_resp.master_key_wrapped_ta.buf,
			gen_master_key_resp.master_key_wrapped_ta.len);
	send_msg->payload.gen_backup_req_cmd.wrapped_master_key.len =
			gen_master_key_resp.master_key_wrapped_ta.len;
	for (int i = 0; i < 3; i++) {
		//memcpy((void *)&send_msg->payload.gen_backup_req_cmd.buddies_cert_chains[i], (void *) &cert_chain, sizeof(cert_chain_t));

		for (uint32_t j = 0; j < cert_chain.num_certs; j++) {
			memcpy(
					send_msg->payload.gen_backup_req_cmd.buddies_cert_chains[i].cert[j].blob,
					cert_chain.cert[j].blob, cert_chain.cert[j].len);
			send_msg->payload.gen_backup_req_cmd.buddies_cert_chains[i].cert[j].len =
					cert_chain.cert[j].len;
		}
		send_msg->payload.gen_backup_req_cmd.buddies_cert_chains[i].num_certs =
				cert_chain.num_certs;

	}
	send_msg->payload.gen_backup_req_cmd.n = n;
	send_msg->payload.gen_backup_req_cmd.k = k;
	char metadata[] =
			"{\"owner\": \"s000\", \"n\": 3, \"k\": 2, \"buddies\": [\"s001\", \"s002\", \"s003\"]}";
	memcpy(send_msg->payload.gen_backup_req_cmd.metadata, metadata,
			sizeof(metadata));

	ret = QSEECom_send_cmd(l_QSEEComHandle, send_msg, len, resp_msg, len);
	if (ret) {
		printf("%s: Send command failed with ret = %d\n", __func__, ret);
	} else {
		printf("\nresponse from gen_backup_req\n");
		memcpy(backup_reqs_jws,
				resp_msg->payload.gen_backup_req_resp.backup_reqs_jws,
				sizeof(backup_reqs_jws));
		for (uint32_t i = 0; i < n; i++) {
			printf("Backup req %d jws:\n%s\n", i, backup_reqs_jws[i]);
		}
	}

	//CMD_GEN_BACKUP_ACK  cmd 0x00000009
	// for share 0
	memset(send_msg, 0, sizeof(tciMessage_t));
	memset(resp_msg, 0, sizeof(tciMessage_t));
	send_msg->header.id = CMD_GEN_BACKUP_ACK;
	memcpy(&(send_msg->payload.gen_backup_ack_cmd.backup_req_jws),
			backup_reqs_jws[0], JWE_JWS_MAX_SIZE);
	memcpy(&(send_msg->payload.gen_backup_ack_cmd.buddy_cert_chain),
			&cert_chain, sizeof(cert_chain_t));
	ret = QSEECom_send_cmd(l_QSEEComHandle, send_msg, len, resp_msg, len);
	if (ret) {
		printf("%s: Send command failed with ret = %d\n", __func__, ret);
	} else {
		printf("\nresponse from gen_backup_ack\n");
		memcpy(jws, resp_msg->payload.gen_backup_ack_resp.backup_ack_jws,
				JWE_JWS_MAX_SIZE);
		printf("Backup ack jws:\n%s\nwrapped share:\n", jws);
		dump_hex(resp_msg->payload.gen_backup_ack_resp.wrapped_share.buf,
				resp_msg->payload.gen_backup_ack_resp.wrapped_share.len);
		printf("n:%d  i:%d ", resp_msg->payload.gen_backup_ack_resp.n,
				resp_msg->payload.gen_backup_ack_resp.i);
		checksums_len = resp_msg->payload.gen_backup_ack_resp.checksums.len;
		memcpy(checksums, resp_msg->payload.gen_backup_ack_resp.checksums.buf,
				checksums_len);
		printf("checksums_len:%d\nchecksums:\n", checksums_len);
		dump_hex(checksums, checksums_len);
		printf("metadata:\n%s\n",
				resp_msg->payload.gen_backup_ack_resp.metadata);
		wshare_len = resp_msg->payload.gen_backup_ack_resp.wrapped_share.len;
		memcpy(wrapped_shares[0],
				resp_msg->payload.gen_backup_ack_resp.wrapped_share.buf,
				wshare_len);
	}
	// for share 2, i.e., share n-1
	memset(send_msg, 0, sizeof(tciMessage_t));
	memset(resp_msg, 0, sizeof(tciMessage_t));
	send_msg->header.id = CMD_GEN_BACKUP_ACK;
	memcpy(&(send_msg->payload.gen_backup_ack_cmd.backup_req_jws),
			backup_reqs_jws[n - 1], JWE_JWS_MAX_SIZE);
	memcpy(&(send_msg->payload.gen_backup_ack_cmd.buddy_cert_chain),
			&cert_chain, sizeof(cert_chain_t));
	ret = QSEECom_send_cmd(l_QSEEComHandle, send_msg, len, resp_msg, len);
	if (ret) {
		printf("%s: Send command failed with ret = %d\n", __func__, ret);
	} else {
		printf("\nresponse from gen_backup_ack\n");
		memcpy(jws, resp_msg->payload.gen_backup_ack_resp.backup_ack_jws,
				JWE_JWS_MAX_SIZE);
		printf("Backup ack jws:\n%s\nwrapped share:\n", jws);
		dump_hex(resp_msg->payload.gen_backup_ack_resp.wrapped_share.buf,
				resp_msg->payload.gen_backup_ack_resp.wrapped_share.len);
		printf("n:%d  i:%d \nchecksums:\n",
				resp_msg->payload.gen_backup_ack_resp.n,
				resp_msg->payload.gen_backup_ack_resp.i);
		dump_hex(resp_msg->payload.gen_backup_ack_resp.checksums.buf,
				resp_msg->payload.gen_backup_ack_resp.checksums.len);
		printf("metadata:\n%s\n",
				resp_msg->payload.gen_backup_ack_resp.metadata);
		wshare_len = resp_msg->payload.gen_backup_ack_resp.wrapped_share.len;
		memcpy(wrapped_shares[1],
				resp_msg->payload.gen_backup_ack_resp.wrapped_share.buf,
				wshare_len);
	}

	//CMD_GEN_RECOVERY_REQ  cmd 0x0000000A
	memset(send_msg, 0, sizeof(tciMessage_t));
	memset(resp_msg, 0, sizeof(tciMessage_t));
	char alias[] = "pebble master key";
	send_msg->header.id = CMD_GEN_RECOVERY_REQ;
	memcpy(send_msg->payload.gen_recovery_req_cmd.key_alias, alias,
			sizeof(alias));
	ret = QSEECom_send_cmd(l_QSEEComHandle, send_msg, len, resp_msg, len);
	if (ret) {
		printf("%s: Send command failed with ret = %d\n", __func__, ret);
	} else {
		printf("\nresponse from gen_recovery_req\n");
		memcpy(jws, resp_msg->payload.gen_recovery_req_resp.recovery_req_jws,
				JWE_JWS_MAX_SIZE);
		printf("Recovery req jws:\n%s\n", jws);
	}

	//CMD_GEN_RECOVERY_ACK  cmd 0x0000000B
	memset(send_msg, 0, sizeof(tciMessage_t));
	memset(resp_msg, 0, sizeof(tciMessage_t));
	send_msg->header.id = CMD_GEN_RECOVERY_ACK;
	memcpy(&(send_msg->payload.gen_recovery_ack_cmd.recovery_req_jws), jws,
			JWE_JWS_MAX_SIZE);
	memcpy(send_msg->payload.gen_recovery_ack_cmd.wrapped_share.buf,
			wrapped_shares[0], wshare_len);
	send_msg->payload.gen_recovery_ack_cmd.wrapped_share.len = wshare_len;
	send_msg->payload.gen_recovery_ack_cmd.n = n;
	send_msg->payload.gen_recovery_ack_cmd.i = 0;
	memcpy(send_msg->payload.gen_recovery_ack_cmd.checksums.buf, checksums,
			checksums_len);
	send_msg->payload.gen_recovery_ack_cmd.checksums.len = checksums_len;
	memcpy(send_msg->payload.gen_recovery_ack_cmd.metadata, metadata,
			sizeof(metadata));
	memcpy(&(send_msg->payload.gen_recovery_ack_cmd.buddy_cert_chain),
			&cert_chain, sizeof(cert_chain_t));
	ret = QSEECom_send_cmd(l_QSEEComHandle, send_msg, len, resp_msg, len);
	if (ret) {
		printf("%s: Send command failed with ret = %d\n", __func__, ret);
	} else {
		printf("\nresponse from gen_recovery_ack\n");
		memcpy(jws, resp_msg->payload.gen_recovery_ack_resp.recovery_ack_jws,
				JWE_JWS_MAX_SIZE);
		printf("Recovery ack jws:\n%s\n", jws);
	}

	//CMD_HANDLE_RECOVERY_ACK  cmd 0x0000000C
	memset(send_msg, 0, sizeof(tciMessage_t));
	memset(resp_msg, 0, sizeof(tciMessage_t));
	send_msg->header.id = CMD_HANDLE_RECOVERY_ACK;
	memcpy(&(send_msg->payload.handle_recovery_ack_cmd.recovery_ack_jws), jws,
			JWE_JWS_MAX_SIZE); // jws is from CMD_GEN_RECOVERY_ACK
	memcpy(&(send_msg->payload.handle_recovery_ack_cmd.buddy_cert_chain),
			&cert_chain, sizeof(cert_chain_t));
	ret = QSEECom_send_cmd(l_QSEEComHandle, send_msg, len, resp_msg, len);
	if (ret) {
		printf("%s: Send command failed with ret = %d\n", __func__, ret);
	} else {
		printf("\nresponse from handle_recovery_ack\n");
		dump_hex(resp_msg->payload.handle_recovery_ack_resp.wrapped_share.buf,
				resp_msg->payload.handle_recovery_ack_resp.wrapped_share.len);
		printf("n:%d  i:%d \nchecksums:\n",
				resp_msg->payload.handle_recovery_ack_resp.n,
				resp_msg->payload.handle_recovery_ack_resp.i);
		dump_hex(resp_msg->payload.handle_recovery_ack_resp.checksums.buf,
				resp_msg->payload.handle_recovery_ack_resp.checksums.len);
		printf("metadata:\n%s\n",
				resp_msg->payload.handle_recovery_ack_resp.metadata);
	}

	//CMD_RECONSTRUCT_KEY  cmd 0x0000000D
	memset(send_msg, 0, sizeof(tciMessage_t));
	memset(resp_msg, 0, sizeof(tciMessage_t));
	send_msg->header.id = CMD_RECONSTRUCT_KEY;
	memcpy(send_msg->payload.reconstruct_key_cmd.wrapped_shares, wrapped_shares,
			sizeof(wrapped_shares));
	send_msg->payload.reconstruct_key_cmd.wshare_len = wshare_len;
	send_msg->payload.reconstruct_key_cmd.k = 2;
	send_msg->payload.reconstruct_key_cmd.master_key_req.cert_chain.num_certs =
			3;
	memcpy(
			send_msg->payload.reconstruct_key_cmd.master_key_req.cert_chain.cert[0].blob,
			aks_cert0, strlen((char* )aks_cert0));
	send_msg->payload.reconstruct_key_cmd.master_key_req.cert_chain.cert[0].len =
			strlen((char*) aks_cert0);
	memcpy(
			send_msg->payload.reconstruct_key_cmd.master_key_req.cert_chain.cert[1].blob,
			aks_cert1, strlen((char* )aks_cert1));
	send_msg->payload.reconstruct_key_cmd.master_key_req.cert_chain.cert[1].len =
			strlen((char*) aks_cert1);
	memcpy(
			send_msg->payload.reconstruct_key_cmd.master_key_req.cert_chain.cert[2].blob,
			aks_cert2, strlen((char* )aks_cert2));
	send_msg->payload.reconstruct_key_cmd.master_key_req.cert_chain.cert[2].len =
			strlen((char*) aks_cert2);
	memcpy(send_msg->payload.reconstruct_key_cmd.master_key_req.gcm_aad.buf,
			gcm_aad, sizeof(gcm_aad));
	send_msg->payload.reconstruct_key_cmd.master_key_req.gcm_aad.len =
			sizeof(gcm_aad);
	ret = QSEECom_send_cmd(l_QSEEComHandle, send_msg, len, resp_msg, len);
	if (ret) {
		printf("%s: Send command failed with ret = %d\n", __func__, ret);
	} else {
		printf("\nresponse from reconstruct_key\n");
		printf("master_key_wrapped_ta:\n");
		dump_hex(
				(char*) (resp_msg->payload.reconstruct_key_resp.master_key_resp.master_key_wrapped_ta.buf),
				resp_msg->payload.reconstruct_key_resp.master_key_resp.master_key_wrapped_ta.len);
		printf("secret_ct:\n");
		dump_hex(
				(char*) (resp_msg->payload.reconstruct_key_resp.master_key_resp.secret_ct.buf),
				resp_msg->payload.reconstruct_key_resp.master_key_resp.secret_ct.len);
		printf("gcm_iv:\n");
		dump_hex(
				(char*) (resp_msg->payload.reconstruct_key_resp.master_key_resp.gcm_iv),
				WRAPPING_AES_IV_LEN);
		printf("gcm_tag:\n");
		dump_hex(
				(char*) (resp_msg->payload.reconstruct_key_resp.master_key_resp.gcm_tag),
				WRAPPING_AES_TAG_LEN);
		printf("wrapped_gcm_key:\n");
		dump_hex(
				(char*) (resp_msg->payload.reconstruct_key_resp.master_key_resp.wrapped_gcm_key.buf),
				resp_msg->payload.gen_master_key_resp.wrapped_gcm_key.len);
	}

}

int main(int argc, char *argv[]) {
	uint32_t ret = 0;

	static char appname[10] = "pebble"; //Need to micmic visa_pay to test drk service before adding service name "pebble" into drk lib/TA
	struct qseecom_app_info app_info;
	char *ptr = NULL;
	int commandId = 0;
	int keyId = 0;

	send_msg = malloc(sizeof(tciMessage_t));
	resp_msg = malloc(sizeof(tciMessage_t));

	if (len & QSEECOM_ALIGN_MASK)
		len = QSEECOM_ALIGN(len);

	if (argc < 2) {
		usage();
		return 0;
	}

	ret = QSEECom_start_app(&l_QSEEComHandle, "/vendor/firmware_mnt/image",
			appname, 3 * sizeof(tciMessage_t));
	if (ret) {
		printf("%s: Loading app -%s failed\n", __func__, appname);
		return 0;
	} else {
		printf("Loading app -%s succeded\n", appname);
	}

	ret = QSEECom_set_bandwidth(l_QSEEComHandle, true);
	if (ret) {
		printf("%s: Set Bandwith True Failed with ret = %d\n", __func__, ret);
	} else {
		printf("%s: Set Bandwith True succeded\n", __func__);
	}

	ret = QSEECom_get_app_info(l_QSEEComHandle, &app_info);
	if (ret) {
		printf("%s: Fail to get app_info\n", __func__);
		//return -1;
	}

	commandId = strtol(argv[1], &ptr, 10);
	printf("Command ID is %d, keyId is %d\n", commandId, keyId);

	if (commandId == 0) {
		char drk_cert_buf[8192];
		memset(drk_cert_buf, 0, sizeof(drk_cert_buf));
		ret = getDrkCertificate(KEY_TYPE_RSA, drk_cert_buf,
				sizeof(drk_cert_buf));
		printf("ret: %x\n", ret);
		dump_hex(drk_cert_buf, sizeof(drk_cert_buf));

	} else if (commandId == CMD_LOAD_CERT) {
		printf("test CMD_LOAD_CERT\n");
		test1(NULL);
	} else if (commandId == CMD_GEN_ATN_NONCE) {
		printf("test CMD_GEN_ATN_NONCE\n");
		test2();
	} else if (commandId == CMD_GEN_MASTER_KEY) {
		printf("test CMD_GEN_MASTER_KEY\n");
		test3(NULL);
	} else if (commandId == CMD_DERIVE_SUBKEY) {
		printf("test CMD_DERIVE_SUBKEY\n");
		test4();
//	} else if (commandId == CMD_SSS_SPLIT) {
//		printf("test CMD_SSS_SPLIT\n");
//		if (argc > 2) {
//			keyId = strtol(argv[2], &ptr, 10);
//			test4(keyId);
//		} else {
//			printf("please select test case 0 ~ 3");
//		}
//	} else if (commandId == CMD_SSS_JOIN) {
//		printf("test CMD_SSS_JOIN\n");
//		if (argc > 2) {
//			keyId = strtol(argv[2], &ptr, 10);
//			test5(keyId);
//		} else {
//			printf("please select test case 0 ~ 3");
//		}
	} else if (commandId == CMD_GEN_BACKUP_REQ
			|| commandId == CMD_GEN_BACKUP_ACK) {
		printf("test CMD_GEN_BACKUP_REQ and CMD_GEN_BACKUP_ACK\n");
		test5();
	} else {
		usage();
	}

	/*
	 if (app_info.is_secure_app_64bit) {
	 printf("%s: is_secure_app_64bit\n",__func__);
	 } else {
	 printf("%s: NOT is_secure_app_64bit\n",__func__);
	 }
	 */

	ret = QSEECom_set_bandwidth(l_QSEEComHandle, false);
	if (ret) {
		printf("%s: Set Bandwith False Failed with ret = %d\n", __func__, ret);
	} else {
		printf("%s: Set Bandwith False succeded\n", __func__);
	}

	ret = QSEECom_shutdown_app(&l_QSEEComHandle);
	if (ret) {
		printf("%s: Shutdown app failed with ret = %d\n", __func__, ret);
	} else {
		free(send_msg);
		free(resp_msg);
		printf("shutdown app: pass\n");
	}

	return 0;
}