#include <stdlib.h>
#include <stdio.h>
#include <stddef.h>
#include <string.h>

#include <ykdef.h>
#include <ykcore.h>
#include <yubikey.h>
#include "ykpers.h"

#define REQ_RESPONSE_LENGTH 64

static int
configure_cfg(YKP_CONFIG *cfg, YK_STATUS *st, char *server_key, int slot)
{
	// Generate Config
	ykp_configure_version(cfg, st);
    
	if(!ykp_configure_command(cfg, ((slot == 1) ? SLOT_CONFIG : SLOT_CONFIG2)))
	{
        fprintf(stderr,"Couldn't configure command");
		return false;
	}
	
	if(!ykp_set_tktflag_CHAL_RESP(cfg, true))
	{
        fprintf(stderr,"Couldn't set CHAL_RESP flag");
		return false;
	}
    if(!ykp_set_cfgflag_CHAL_HMAC(cfg, true))
	{
        fprintf(stderr,"Couldn't set CHAL_HMAC flag");
		return false;
	}

    int res = ykp_HMAC_key_from_raw(cfg, server_key);
    if (res) {
        fprintf(stderr,"Bad HMAC key");
        return false;
    }

    size_t key_bytes = (size_t)ykp_get_supported_key_length(cfg);
    if(key_bytes != 20)
    {
        fprintf(stderr,"Yubikey doesn't support HMAC Challenge");
        return false;
    }


	return true;
}

int
challenge_response(YK_KEY *yk, int slot, bool may_block, unsigned int challenge_len, const unsigned char *challenge,
                   unsigned char *response, int verbosity)
{
    if (verbosity > 6)
    {
        fprintf(stdout, "Sending %i bytes HMAC challenge to slot %i\n", challenge_len, slot);
    }

    int yk_cmd = (slot == 1) ? SLOT_CHAL_HMAC1 : SLOT_CHAL_HMAC2;

    if(! yk_challenge_response(yk, yk_cmd, may_block, challenge_len,
                               challenge, REQ_RESPONSE_LENGTH, response))
    {
        return false;
    }

    if(verbosity > 6)
    {
        unsigned char output_buf[(SHA1_MAX_BLOCK_SIZE * 2) + 1] = { 0 };
        yubikey_hex_encode((char *)output_buf, (char *)response, 20);
        printf("%s\n", output_buf);
    }

    return true;
}

int
key_init(YK_KEY **yk, YK_STATUS *st)
{
    if (!yk_init()) {
        return false;
    }
    if (!(*yk = yk_open_first_key())) {
        printf("Could not open YubiKey. Is the YubiKey connected to the computer?");
        return false;
    }

    if (!yk_get_status(*yk, st))
    {
        return false;
    }

    if (!(yk_check_firmware_version2(st)))
    {
        return false;
    }

    return true;
}

int
import_server_key(YK_KEY *yk, YK_STATUS *st, char *server_key, char *acc_code_hex, int verbosity, int slot)
{
    YKP_CONFIG *cfg = ykp_alloc();
    char data[1024] = {0};
    unsigned char *access_code = NULL;
    int exit_code = OPENVPN_PLUGIN_FUNC_ERROR;

    configure_cfg(cfg, st, server_key, slot);

    if(verbosity > 6)
    {
        ykp_export_config(cfg, data, 1024, YKP_FORMAT_LEGACY);
        fwrite(data, 1, strlen(data), stdout);
    }

    if (acc_code_hex)
    {
        if(!yubikey_hex_p(acc_code_hex))
        {
            fprintf(stderr,
                    "Invalid access code string: %s\n",
                    acc_code_hex);
            goto import_err;
        }
        unsigned int access_code_len = strlen(acc_code_hex) / 2;
        access_code = calloc(access_code_len, sizeof(unsigned char));
		yubikey_hex_decode(access_code, acc_code_hex, access_code_len);
        ykp_set_access_code(cfg, access_code, access_code_len);
	}

    // Apply Config
    YK_CONFIG *ycfg = NULL;
    ycfg = ykp_core_config(cfg);

    if (!yk_write_command(yk,
                          ycfg,
                          ykp_command(cfg),
                          acc_code_hex ? access_code : NULL))
    {
        fprintf(stderr, "Failed writing server key to YubiKey\n");
        goto import_err;
    }
    if(verbosity > 3)
    {
        puts("Successfully wrote server key to YubiKey");
    }
    exit_code = OPENVPN_PLUGIN_FUNC_SUCCESS;
import_err:
    if (cfg)
        ykp_free_config(cfg);
    free(access_code);
    return exit_code;
}