#include "layer2.h"

//RIOT core

void startCDIProtocol()
{

	//step 1 : Derive Device ID
	uint8_t* UDS_ID = calloc(1,sizeof(uint8_t)*SHA256_DGST_SIZE);
	readUDS(UDS_ID);

	// for(int i = 0; i < SHA256_DGST_SIZE; i++)
	//  	printf("%x",UDS_ID[i]);
	//  printf(" : UDS_ID, main\n");

	//step 2 : Derive Firmware ID
	uint8_t* FW_ID = calloc(1,sizeof(uint8_t)*SHA256_DGST_SIZE);
	readFWID(FW_ID);

	// for(int i = 0; i < SHA256_DGST_SIZE; i++)
	//  	printf("%x",FW_ID[i]);
	//  printf(" : FW_ID, main\n");


	//setp 3 : Derive Composite Device ID
	uint8_t* CD_ID = calloc(1,sizeof(uint8_t)*SHA256_DGST_SIZE);
	calcCDID(UDS_ID,FW_ID,CD_ID);
	free(UDS_ID);
	free(FW_ID);


	// //wrapper funciton to avoid exposing UDS and FW measurements

	// uint8_t* _CD_ID = calloc(1,sizeof(uint8_t)*SHA256_DGST_SIZE);
	// _calcCDID(_CD_ID);
	// printf( "CDID wrap : ");
	// for(int i = 0; i < SHA256_DGST_SIZE; i++)
	//  	printf("%x",_CD_ID[i]);
	// printf( "\n\n\n");

	uint8_t* KEY_OUT = calloc(1,sizeof(uint8_t)*KDF_KEY_SIZE);
	size_t KEY_LEN = KDF_KEY_SIZE;			//need to pass pointer to the out key size, not the value


	if(deriveKDF(KEY_OUT, &KEY_LEN, CD_ID, SHA256_DGST_SIZE, PASSPHRASE, lenofstr(PASSPHRASE)))
		printf("\n\nKDF call success\n");;


	for(int i = 0; i <= KDF_KEY_SIZE; i++)
	 	printf("%x,",KEY_OUT[i]);
	printf( "  :  KEY_OUT\n");


	//End block
	
	free(CD_ID);
	//free(_CD_ID);			//for wrapper function
}


int deriveKDF(uint8_t * out, size_t * out_len, uint8_t * secret, int secret_len, unsigned char * passphrase, int pass_len)
{
	//create comtext
	//ctx set params
		//passphrase
		//secret 		//CDI
		//alg			//not taken as input. fixed to sha256
		//salt 			//meh, hardcode salt too
		//out

	//hkdf derive key
	
	// for(int i = 0; i < SHA256_DGST_SIZE; i++)
	//  	printf("%x",secret[i]);
	// printf( " : secret\n");


	//sample kdf prog
	EVP_PKEY_CTX * pctx;
	//uint8_t * OUT = calloc(1,sizeof(uint8_t)*KDF_KEY_SIZE);
	//size_t keylen = KDF_KEY_SIZE;

	uint8_t salt[32] = {0x31,0xe2,0x3e,0xcc,0x28,0xc5,0x7b,0xbb,0x38,0x7d,0xe6,0x66,0xbb,
		0xbe,0x67,0x0a,0xf8,0xf3,0x92,0x0e,0xba,0x68,0xd1,0x56,0xea,0x34,0x3f,0xbc,0x4f,
		0xf1,0xd9,0x1e};

	pctx = EVP_PKEY_CTX_new_id(EVP_PKEY_HKDF, NULL);		//..new_id() allocates pub key alg to ctx 

	if(EVP_PKEY_derive_init(pctx) <= 0)
		perror("pkey init failed:");
	// if (EVP_PKEY_CTX_hkdf_mode(pctx,EVP_PKEY_HKDEF_MODE_EXPAND_ONLY ) <= 0)
 		//perror("set message mode failed:");
	if (EVP_PKEY_CTX_set_hkdf_md(pctx, EVP_sha256()) <= 0)
    	perror("set message digest failed:");
	if (EVP_PKEY_CTX_set1_hkdf_salt(pctx, salt, sizeof(salt)) <= 0)
 		perror("set salt failed:");
	if (EVP_PKEY_CTX_set1_hkdf_key(pctx, secret, secret_len) <= 0)
    	perror("set secret failed:");
	if (EVP_PKEY_CTX_add1_hkdf_info(pctx, passphrase, pass_len) <= 0)
    	perror("set label failed:");
	if (EVP_PKEY_derive(pctx, out, out_len) <= 0)
    	perror("pkey derivation failed:");

 //    for(int i = 0; i <= KDF_KEY_SIZE; i++)
	//  	printf("%x,",out[i]);
	// printf( ": OUT\n");

	EVP_PKEY_CTX_free(pctx);
	
	return 1; 
}