/*
* Imagimob Studio 5.1.1876.65534+49e6155340215ca3d06c12b18af3707d859c2860
* Copyright © 2023- Imagimob AB, All Rights Reserved.
* 
* Generated at 08/22/2024 14:28:19 UTC. Any changes will be lost.
* 
* Model ID  5ffb4d7f-db2a-4ff2-a357-fea63b96bc4d
* 
* Memory    Size                      Efficiency
* Buffers   92 bytes (RAM)            87 %
* State     220 bytes (RAM)           100 %
* Readonly  428 bytes (Flash)         100 %
* 
* Backend              tensorflow
* Keras Version        2.15.0
* Backend Model Type   Sequential
* 
* Class Index | Symbol Label
* 0           | unlabelled
* 1           | touch
* 
* Layer                          Shape           Type       Function
* Sliding Window (data points)   [3]             float      dequeue
*    window_shape = [3]
*    stride = 1
*    buffer_multiplier = 1
* Input Layer                    [3]             float      dequeue
*    shape = [3]
* Dense                          [10]            float      dequeue
*    units = 10
*    use_bias = True
*    activation = relu
*    trainable = True
*    weight = float[3,10]
*    bias = float[10]
* Dense                          [5]             float      dequeue
*    units = 5
*    use_bias = True
*    activation = relu
*    trainable = True
*    weight = float[10,5]
*    bias = float[5]
* Dense                          [2]             float      dequeue
*    units = 2
*    use_bias = True
*    activation = softmax
*    trainable = True
*    weight = float[5,2]
*    bias = float[2]
* 
* Exported functions:
* 
* int BTN_NN_dequeue(float *restrict data_out)
*    Description: Dequeue features. RET_SUCCESS (0) on success, RET_NODATA (-1) if no data is available, RET_NOMEM (-2) on internal memory error
*    Parameter data_out is Output of size float[2].
* 
* int BTN_NN_enqueue(const float *restrict data_in)
*    Description: Enqueue features. Returns SUCCESS (0) on success, else RET_NOMEM (-2) when low on memory.
*    Parameter data_in is Input of size float[1].
* 
* void BTN_NN_init(void)
*    Description: Initializes buffers to initial state. This function also works as a reset function.
* 
* 
* Disclaimer:
*   The generated code relies on the optimizations done by the C compiler.
*   For example many for-loops of length 1 must be removed by the optimizer.
*   This can only be done if the functions are inlined and simplified.
*   Check disassembly if unsure.
*   tl;dr Compile using gcc with -O3 or -Ofast
*/

/*
* Tensorflow Test Set
* 
* (ACC) Accuracy 99.335 %
* (F1S) F1 Score 99.337 %
* 
* Name of class                                            (unlabelled)            touch
* (TP) True Positive or Correct Positive Prediction               77066            31536
* (FN) False Negative or Incorrect Negative Prediction              685               42
* (FP) False Positive or Incorrect Positive Prediction               42              685
* (TN) True Negative or Correct Negative Prediction               31536            77066
* (TPR) True Positive Rate or Sensitivity, Recall               99.12 %          99.87 %
* (TNR) True Negative Rate or Specificity, Selectivity          99.87 %          99.12 %
* (PPV) Positive Predictive Value or Precision                  99.95 %          97.87 %
* (NPV) Negative Predictive Value                               97.87 %          99.95 %
* (FNR) False Negative Rate or Miss Rate                         0.88 %           0.13 %
* (FPR) False Positive Rate or Fall-Out                          0.13 %           0.88 %
* (FDR) False Discovery Rate                                     0.05 %           2.13 %
* (FOR) False Omission Rate                                      2.13 %           0.05 %
* (F1S) F1 Score                                                99.53 %          98.86 %
*/

#include <math.h>
#include <stdint.h>
#include <string.h>

#include "button_nn.h"

// Working memory
static int8_t _buffer[92];
static int8_t _state[220];

// Parameters
static const uint32_t _K4[] = {
    0xbed4369f, 0xbbd96676, 0xbd0daba3, 0x3f1eb0f9, 0x3e9d38f3, 0xbe489ff1, 0x3d6ab5a6, 0xbf68e908, 
    0xbe0f0890, 0xbd01a4ff, 0xbe1cd899, 0xbd6b0d56, 0xbcba1a90, 0xbe991572, 0xbebe14b1, 0xbece5f00, 
    0xbdd81233, 0x3e443303, 0xbe9b61b5, 0x3e5a0f03, 0xbe8b9f30, 0x3e5230b5, 0xbedfd462, 0xbeca8c3c, 
    0xbe4eb9eb, 0xbcf92ac6, 0x3d274d4a, 0xbedd2dc3, 0xbdedac39, 0xbeb64a24
};

static const uint32_t _K6[] = {
    0xbf27609a, 0xc0e4cecb, 0xbe2cabc8, 0x00000000, 0xbe33b45a, 0xbf241d47, 0xbeccc7ed, 0xbe1619ab, 
    0xbe8df7f7, 0x00000000
};

static const uint32_t _K9[] = {
    0xbe38db27, 0xbe004b74, 0xbecf11cc, 0xbebd2b33, 0xbe688911, 0xbf53f194, 0x3d25a1bb, 0x3d952177, 
    0xbf864225, 0x3e73deb3, 0x3de82b77, 0xbd3fecc6, 0xbfe80502, 0xbe2f3d1d, 0x3e4718d4, 0x3d5f9049, 
    0xbd4b8316, 0x3ea1bf5d, 0xbf00d0dd, 0x3e6a1043, 0x3e0810d9, 0xbf4a70bc, 0x3f1740ef, 0x3e1ca25a, 
    0x3eec2035, 0xbe2c16e5, 0xbf1dac54, 0x3ed3978e, 0xbeb14ae3, 0x3e5217b7, 0x3e8117d1, 0x3ea3d044, 
    0xbf0c5342, 0x3e43e02f, 0x3dbfa33d, 0x3e353bb7, 0x3dbd5fd3, 0xbdba017a, 0x3b0d9a7d, 0x3ec46617, 
    0xbea2cc6b, 0xbd1fbd10, 0x3e113b7c, 0xbe4852a6, 0x3d60c928, 0x3d0351d6, 0xbe85c5f2, 0xbf0e36a6, 
    0xbe7c7732, 0xbe9e073e
};

static const uint32_t _K11[] = {
    0xbe3752ae, 0x3f721e4b, 0x4041699b, 0xc098a826, 0xbe33b8c7
};

static const uint32_t _K14[] = {
    0xbe812691, 0xbec16727, 0x3fc4f779, 0xbe33c715, 0xbb8d72a3, 0x3c7f7707, 0xbec93371, 0xbedba472, 
    0x3e92db5c, 0x3ebae851
};

static const uint32_t _K16[] = {
    0x3fd9a1dd, 0xbfd9a1a5
};

// Memory mapped buffers
#define _K11             ((float *)_K11)                     // f32[5] (20 bytes) 
#define _K14             ((float *)_K14)                     // f32[2,5] (40 bytes) 
#define _K16             ((float *)_K16)                     // f32[2] (8 bytes) 
#define _K4              ((float *)_K4)                      // f32[10,3] (120 bytes) 
#define _K6              ((float *)_K6)                      // f32[10] (40 bytes) 
#define _K9              ((float *)_K9)                      // f32[5,10] (200 bytes) 
#define _K3              ((int8_t *)(_state + 0x00000000))   // s8[220] (220 bytes) 
#define _K10             ((float *)(_buffer + 0x00000028))   // f32[5] (20 bytes) 
#define _K12             ((float *)(_buffer + 0x00000000))   // f32[5] (20 bytes) 
#define _K13             ((float *)(_buffer + 0x00000014))   // f32[5] (20 bytes) 
#define _K15             ((float *)(_buffer + 0x00000000))   // f32[2] (8 bytes) 
#define _K17             ((float *)(_buffer + 0x00000008))   // f32[2] (8 bytes) 
#define _K2              ((float *)(_buffer + 0x00000000))   // f32[3] (12 bytes) 
#define _K5              ((float *)(_buffer + 0x0000000c))   // f32[10] (40 bytes) 
#define _K7              ((float *)(_buffer + 0x00000034))   // f32[10] (40 bytes) 
#define _K8              ((float *)(_buffer + 0x00000000))   // f32[10] (40 bytes) 

// Represents a Circular Buffer
// https://en.wikipedia.org/wiki/Circular_buffer
typedef struct
{
	char *buf;
	int size;		// total bytes allocated in *buf
	int used;		// current bytes used in buffer.
	int read;
	int write;
} cbuffer_t;

#define CBUFFER_SUCCESS 0
#define CBUFFER_NOMEM -1

// Initializes a cbuffer handle with given memory and size.
static inline void cbuffer_init(cbuffer_t *dest, void *mem, int size) {
	dest->buf = mem;
	dest->size = size;
	dest->used = 0;
	dest->read = 0;
	dest->write = 0;
}

// Returns the number of free bytes in buffer.
static inline int cbuffer_get_free(cbuffer_t *buf) {
	return buf->size - buf->used;
}

// Returns the number of used bytes in buffer.
static inline int cbuffer_get_used(cbuffer_t *buf) {
	return buf->used;
}

// Writes given data to buffer.
// Returns CBUFFER_SUCCESS or CBUFFER_NOMEM if out of memory.
static inline int cbuffer_enqueue(cbuffer_t *buf, const void *data, int data_size) {
	int free = cbuffer_get_free(buf);

	// Out of memory?
	if (free < data_size)
		return CBUFFER_NOMEM;

	// Is the data split in the end?
	if (buf->write + data_size > buf->size) {
		int first_size = buf->size - buf->write;
		memcpy(buf->buf + buf->write, data, first_size);
		memcpy(buf->buf, ((char *)data) + first_size, data_size - first_size);
	}
	else {
		memcpy(buf->buf + buf->write, data, data_size);
	}
	buf->write += data_size;
	if (buf->write >= buf->size)
		buf->write -= buf->size;

	buf->used += data_size;
	return CBUFFER_SUCCESS;
}

// Advances the read pointer by given count.
// Returns CBUFFER_SUCCESS on success or CBUFFER_NOMEM if count is more than available data
static inline int cbuffer_advance(cbuffer_t *buf, int count) {
	int used = cbuffer_get_used(buf);

	if (count > used)
		return CBUFFER_NOMEM;

	buf->read += count;
	if (buf->read >= buf->size)
		buf->read -= buf->size;

	// Reset pointers to 0 if buffer is empty in order to avoid unwanted wrapps.
	if (buf->read == buf->write) {
		buf->read = 0;
		buf->write = 0;
	}

	buf->used -= count;
	return CBUFFER_SUCCESS;
}

// Reset instance (clear buffer)
static inline void cbuffer_reset(cbuffer_t *buf) {
	buf->read = 0;
	buf->write = 0;
	buf->used = 0;
}

// Copies given "count" bytes to the "dst" buffer without advancing the buffer read offset.
// Returns CBUFFER_SUCCESS on success or CBUFFER_NOMEM if count is more than available data.
static inline int cbuffer_copyto(cbuffer_t *buf, void *dst, int count, int offset) {
	
	if (count > cbuffer_get_used(buf))
		return CBUFFER_NOMEM;

	int a0 = buf->read + offset;
	if (a0 >= buf->size)
		a0 -= buf->size;

	int c0 = count;
	if (a0 + c0 > buf->size)
		c0 = buf->size - a0;
	
	memcpy(dst, buf->buf + a0, c0);
	
	int c1 = count - c0;

	if (c1 > 0)
		memcpy(((char *)dst) + c0, buf->buf, c1);

	return CBUFFER_SUCCESS;
}

// Returns a read pointer at given offset and  
// updates *can_read_bytes (if not NULL) with the number of bytes that can be read.
// 
// Note! Byte count written to can_read_bytes can be less than what cbuffer_get_used() returns.
// This happens when the read has to be split in two since it's a circular buffer.
static inline void *cbuffer_readptr(cbuffer_t* buf, int offset, int *can_read_bytes)
{
	int a0 = buf->read + offset;
	if (a0 >= buf->size)
		a0 -= buf->size;
	if (can_read_bytes != NULL)
	{
		int c0 = buf->used;
		if (a0 + c0 > buf->size)
			c0 = buf->size - a0;

		*can_read_bytes = c0;
	}
	return buf->buf + a0;
}

typedef struct {
	cbuffer_t data_buffer;			// Circular Buffer for features
	int input_size;					// Number of bytes in each input chunk
	int window_count;				// Number of input chunks in output window.
} fixwin_t;

#ifdef _MSC_VER
static_assert(sizeof(fixwin_t) <= 64, "Data structure 'fixwin_t' is too big");
#endif

#define IPWIN_RET_SUCCESS 0
#define IPWIN_RET_NODATA -1
#define IPWIN_RET_NOMEM -2

/*
* Try to dequeue a window.
*
* @param handle Pointer to an initialized handle.
* @param dst Pointer where to write window.
* @param stride_count Number of items (of size handle->input_size) to stride window.
* @return IPWIN_RET_SUCCESS (0) or IPWIN_RET_NODATA (-1) is no data is available.
*/
static inline int fixwin_dequeuef32(void* restrict handle, void* restrict dst, int stride_count)
{
	fixwin_t* fep = (fixwin_t*)handle;

	const int stride_bytes = stride_count * fep->input_size;
	const int size = fep->window_count * fep->input_size;
	if (cbuffer_get_used(&fep->data_buffer) >= size) {
		if (cbuffer_copyto(&fep->data_buffer, dst, size, 0) != 0)
			return IPWIN_RET_NOMEM;

		if (cbuffer_advance(&fep->data_buffer, stride_bytes) != 0)
			return IPWIN_RET_NOMEM;

		return IPWIN_RET_SUCCESS;
	}
	return IPWIN_RET_NODATA;
}

static inline float mac_f32(const float* restrict a, const float* restrict b, int count)
{
	float sum = 0;
	for (int i = 0; i < count; i++) {
		sum += *a++ * *b++;
	}
	return sum;
}

static inline void dott_f32(const float *restrict a, const float *restrict b, float *restrict out, int d0, int d1, int d2)
{
	for (int i = 0; i < d2; i++) {
		float* op = out;
		for (int j = 0; j < d1; j++) {
			*op++ = mac_f32(a + j * d0, b, d0);
		}
		out += d1;
		b += d0;
	}
}

static inline void add_f32(
	const float* restrict a,
	const float* restrict b,
	int l, int g1, int m, int g2, int r,
	float* restrict output)
{
	int index = 0;
	for (int x = 0; x < l; x++) {
		for (int i = 0; i < g1; i++) {
			for (int y = 0; y < m; y++) {
				for (int j = 0; j < g2; j++) {
					for (int z = 0; z < r; z++) {
						output[index] = a[index] + b[x * m * r + y * r + z];
						index++;
					}
				}
			}
		}
	}
}

static inline void relu_f32(const float* restrict x, int count, float* restrict result)
{
	for (int i = 0; i < count; i++) {
		const float value = *x++;
		*result++ = value > 0 ? value : 0;
	}
}

static inline void softmax_f32(const float* restrict x, int count, float* restrict result)
{
	float sum = 0;
	for (int i = 0; i < count; i++) {
		float value = expf(x[i]);
		sum += value;
		result[i] = value;
	}
	for (int i = 0; i < count; i++) {
		result[i] /= sum;
	}
}

/**
 * Enqueue handle->input_size values from given *data pointer to internal window buffer.
 *
 * @param handle Pointer to an initialized handle.
 * @param data Data to enqueue.
 * @return IPWIN_RET_SUCCESS (0) or IPWIN_RET_NOMEM (-2) if internal buffer is out of memory.
 */
static inline int fixwin_enqueuef32(void* restrict handle, const void* restrict data)
{
	fixwin_t* fep = (fixwin_t*)handle;

	if (cbuffer_enqueue(&fep->data_buffer, data, fep->input_size) != 0)
		return IPWIN_RET_NOMEM;

	return IPWIN_RET_SUCCESS;
}

/**
* Initializes a fixwin sampler handle.
*
* @param handle Pointer to a preallocated memory area of fixwin_handle_size() bytes to initialize.
*
* @param input_size Number of bytes to enqueue.
* @param window_count Number of items (of size input_size) in each window
*/
static inline void fixwin_initf32(void* restrict handle, int input_size, int window_count)
{
	fixwin_t* fep = (fixwin_t*)handle;
	fep->input_size = input_size;
	fep->window_count = window_count;

	char* mem = ((char*)handle) + sizeof(fixwin_t);

	int data_buffer = input_size * window_count;
	
	cbuffer_init(&fep->data_buffer, mem, data_buffer);
}

#define __RETURN_ERROR(_exp) do { int __ret = (_exp); if(__ret < 0) return __ret; } while(0)
#define __RETURN_ERROR_BREAK_EMPTY(_exp) {  int __ret = (_exp); if(__ret == -1) break; if(__ret < 0) return __ret;  } 

int BTN_NN_dequeue(float *restrict data_out) {
    __RETURN_ERROR(fixwin_dequeuef32(_K3, _K2, 1));
    dott_f32(_K4, _K2, _K5, 3, 10, 1);
    add_f32(_K5, _K6, 1, 1, 1, 1, 10, _K7);
    relu_f32(_K7, 10, _K8);
    dott_f32(_K9, _K8, _K10, 10, 5, 1);
    add_f32(_K10, _K11, 1, 1, 1, 1, 5, _K12);
    relu_f32(_K12, 5, _K13);
    dott_f32(_K14, _K13, _K15, 5, 2, 1);
    add_f32(_K15, _K16, 1, 1, 1, 1, 2, _K17);
    softmax_f32(_K17, 2, data_out);
    return 0;
}

int BTN_NN_enqueue(const float *restrict data_in) {
    __RETURN_ERROR(fixwin_enqueuef32(_K3, data_in));
    return 0;
}

void BTN_NN_init(void) {
    fixwin_initf32(_K3, 4, 3);
}

