#include "dk_apdu.h"
#include "dk_log.h"

uint16_t get_sw(secEse_7816_rpdu_t *rpdu) {
    if (!rpdu) {
        return 0x0000;
    }

    return (uint16_t) (rpdu->sw1 << 8 | rpdu->sw2);
}

void rpdu_clear(secEse_7816_rpdu_t* rpdu) {
    if (!rpdu) {
        return;
    }
    if (rpdu->pdata) {
        dk_memset(rpdu->pdata, 0, MAX_RAPDU_DATA_SIZE);
    }
    dk_memset(rpdu, 0, sizeof(rpdu));
}

// TODO: do buffer input validation
DK_Result barray_to_cpdu(byte* buf, size_t buflen, secEse_7816_cpdu_t* cpdu) {
    if (buf == NULL || buflen == 0 || cpdu == NULL) { 
        return DK_ERROR_GENERIC; 
    }

    cpdu->cla = buf[APDU_OFFSET_CLA];
    cpdu->ins = buf[APDU_OFFSET_INS];
    cpdu->p1 = buf[APDU_OFFSET_P1];
    cpdu->p2 = buf[APDU_OFFSET_P2];
    cpdu->lc = buf[APDU_OFFSET_LC];
    dk_memcpy(cpdu->pdata, buf + APDU_OFFSET_CDATA, buf[APDU_OFFSET_LC]);
    if (cpdu->lc + APDU_OFFSET_CDATA < buflen) {
        cpdu->le = buf[cpdu->lc + APDU_OFFSET_CDATA];
        cpdu->le_type = 1;
    }
    else {
        cpdu->le = 0;
        cpdu->le_type = 0;
    }

    return DK_SUCCESS;
}

void barray_clear(byte* buf, size_t buflen) {
    if (!buf) {
        return;
    }
    dk_memset(buf, 0, buflen);
}

// TODO: do buffer input validation
DK_Result rpdu_to_barray(byte* dst, size_t* dstlen, secEse_7816_rpdu_t* rpdu) {
    if (dst == NULL || *dstlen == 0 || rpdu == NULL) {
        return DK_ERROR_GENERIC;
    }
    if ((rpdu->len + APDU_SW_SIZE) > *dstlen) {
        return DK_ERROR_GENERIC;
    }

    if (rpdu->len > 0 && rpdu->pdata != NULL)
        dk_memcpy(dst, rpdu->pdata, rpdu->len);
    dst[rpdu->len] = rpdu->sw1;
    dst[rpdu->len + 1] = rpdu->sw2;
    *dstlen = rpdu->len + APDU_SW_SIZE;

    return DK_SUCCESS;
}

DK_Result barray_to_rpdu(byte* src, size_t size, secEse_7816_rpdu_t* dest)
{
    DK_Result rc;
    if (dest == NULL || dest->pdata == NULL || src == NULL || size < 2) {
        rc = DK_ERROR_BAD_PARAM;
        goto catch_error;
    }

    dest->sw1 = src[size-2];
    dest->sw2 = src[size-1];

    dk_memcpy(dest->pdata, src, size);

    dest->len = size-2;
    
    rc = DK_SUCCESS;

catch_error:
    return rc;
}

void print_line(char *buffer, const int length, const int max_line_length) {
    char *line_buffer = (char *)dk_malloc(sizeof(char)*max_line_length+1);

    for (int i=0; i<length; i+= max_line_length) {
        int line_length = length-i < max_line_length ? length-i : max_line_length;
        dk_memcpy(line_buffer, buffer+i, line_length);
        line_buffer[line_length] = '\0';
        TTY_LOG("0x%s", line_buffer);
    }
    dk_free(line_buffer);
}

void rpdu_dump(secEse_7816_rpdu_t* rpdu) {
    if (NULL == rpdu) {
        return;
    }

    TTY_LOG("SW 0x%02x 0x%02x", rpdu->sw1, rpdu->sw2);
    TTY_LOG("rpdu len: 0x%02x", rpdu->len);
    TTY_LOG("rpdu->pdata");
    if (rpdu->len > 0) {
        char buffer[500];

        for (int i=0; i<rpdu->len; i++) {
            sprintf(buffer+(i*2), "%02x", *(rpdu->pdata+i));
        }

        print_line(buffer, rpdu->len, 80);
    }
}

void cpdu_dump(secEse_7816_cpdu_t* cpdu) {
    if (NULL == cpdu) {
        return;
    }

    char buffer[500];
    const int length = 10 + (cpdu->lc*2);

    sprintf(buffer, "%02x", cpdu->cla);
    sprintf(buffer+2, "%02x", cpdu->ins);
    sprintf(buffer+4, "%02x", cpdu->p1);
    sprintf(buffer+6, "%02x", cpdu->p2);
    sprintf(buffer+8, "%02x", cpdu->lc);

    for (int i=0; i<cpdu->lc; i++) {
        sprintf(buffer+10+(i*2), "%02x", *(cpdu->pdata+i));
    }

    TTY_LOG("cpdu length %d", length);

    print_line(buffer, length, 80);
}