#include <stdint.h>
#include <stdbool.h>
#include <string.h>
#include <stdarg.h>
#include "crypto_aead.h"
#include "api.h"
#include "uartp.h"
#include "main.h"

#define MAX_BYTES 100

//#define DEBUG
#define CRYPTO_BUSY_GPIO_Port LD1_GPIO_Port
#define CRYPTO_BUSY_Pin LD1_Pin

uint8_t npub[CRYPTO_NPUBBYTES];
uint8_t nsec[CRYPTO_NSECBYTES];
uint8_t k[CRYPTO_KEYBYTES];
uint8_t ad[MAX_BYTES];
unsigned long long int adlen;
uint8_t m[MAX_BYTES];
unsigned long long int mlen;
uint8_t c[MAX_BYTES];
unsigned long long int clen;
int res;

void setup();
void loop();

void uart_wbyte(uint8_t x) {
    while (!LL_USART_IsActiveFlag_TXE(USART2));
    LL_USART_TransmitData8(USART2, x);
}

uint8_t uart_rbyte() {
    while (!LL_USART_IsActiveFlag_RXNE(USART2));
    uint8_t r = (uint8_t) LL_USART_ReceiveData8(USART2);
    LL_USART_TransmitData8(USART2, r);
    return r;
}

static inline void noInterrupts() {
    // TODO: if necessary, provide a way to disable interrupts
}

static inline void interrupts() {
    // TODO: if necessary, provide a way to enable interrupts
}

int dbg_printf(const char *format, ...) {
    int r = 0;
#ifdef DEBUG
#define DEBUG_BUF_LEN 80
    char printbuf[DEBUG_BUF_LEN+2];

    va_list vargs;
    va_start(vargs, format);
    r = vsnprintf(printbuf+1, DEBUG_BUF_LEN, format, vargs);
    va_end(vargs);

    if (r < 0) {
        memcpy(printbuf+1, "DEBUG ERROR\r\n", 13);
        r = 13;
    }

    r = r > DEBUG_BUF_LEN ? DEBUG_BUF_LEN : r;
    printbuf[0] = 0xde;  // Debug messages should start with "\xde"
    uartp_send(printbuf, r+1);
#endif
    return r;
}

void my_assert(bool b) {
    if (b)
        return;
    dbg_printf("Assertion failed\r\n");
    Error_Handler();  
    for(;;);
}

void test_setup() {
    LL_GPIO_SetOutputPin(CRYPTO_BUSY_GPIO_Port, CRYPTO_BUSY_Pin);
    LL_mDelay(100);
    for (int i = 0; i < 13; i++) {
        uart_wbyte("Hello, World!"[i]);
    }
    LL_USART_ReceiveData8(USART2);
}


void test_loop() {
    uart_rbyte();
    return;
    static uint8_t buf[256];
    uint16_t len = uartp_recv(buf, 255);
    uint8_t action = buf[0];
    if (len == 0 || len > 255)
        return;

    uint16_t l = len - 1;
    uint16_t rl = 0;
    uint8_t *var = buf+1;
    switch (action) {
        case 'm': my_assert(l <= MAX_BYTES);        memcpy(m,    var, l); mlen = l; break;
        case 'c': my_assert(l <= MAX_BYTES);        memcpy(c,    var, l); clen = l; break;
        case 'a': my_assert(l <= MAX_BYTES);        memcpy(ad,   var, l); adlen = l; break;
        case 'k': my_assert(l == CRYPTO_KEYBYTES);  memcpy(k,    var, l); break;
        case 'p': my_assert(l == CRYPTO_NPUBBYTES); memcpy(npub, var, l); break;
        case 's': my_assert(l == CRYPTO_NSECBYTES); memcpy(nsec, var, l); break;
        case 'e':
			noInterrupts();
			asm("nop");
            LL_GPIO_ResetOutputPin(CRYPTO_BUSY_GPIO_Port, CRYPTO_BUSY_Pin);
            res = crypto_aead_encrypt(c, &clen, m, mlen, ad, adlen, nsec, npub, k);
            LL_GPIO_SetOutputPin(CRYPTO_BUSY_GPIO_Port, CRYPTO_BUSY_Pin);
			asm("nop");
			interrupts();
            break;
        case 'd':
			noInterrupts();
			asm("nop");
            LL_GPIO_ResetOutputPin(CRYPTO_BUSY_GPIO_Port, CRYPTO_BUSY_Pin);
            res = crypto_aead_decrypt(m, &mlen, nsec, c, clen, ad, adlen, npub, k);
            LL_GPIO_SetOutputPin(CRYPTO_BUSY_GPIO_Port, CRYPTO_BUSY_Pin);
			asm("nop");
			interrupts();
            break;
        case'M': var = m;    rl = mlen; break;
        case'C': var = c;    rl = clen; break;
        case'A': var = ad;   rl = adlen; break;
        case'K': var = k;    rl = CRYPTO_KEYBYTES; break;
        case'P': var = npub; rl = CRYPTO_NPUBBYTES; break;
        case'S': var = nsec; rl = CRYPTO_NSECBYTES; break;
        case'R': var = (uint8_t *) &res; rl = sizeof(res); break;
        default:
            dbg_printf("Unknown command\r\n");
            my_assert(false);
    }
    buf[0] = action;
    memcpy(buf+1, var, rl);
    uartp_send(buf, rl+1);
}