/*
* Imagimob Studio 5.1.1818.65534+396956ed1117c887044d045bd0b5090c686f8a93
* Copyright © 2023- Imagimob AB, All Rights Reserved.
* 
* Generated at 08/01/2024 14:13:33 UTC. Any changes will be lost.
* 
* Model ID  2759e08d-2185-4bfe-88d0-c253d934ca3a
* 
* Memory    Size                      Efficiency
* Buffers   120 bytes (RAM)           80 %
* State     232 bytes (RAM)           100 %
* Readonly  836 bytes (Flash)         100 %
* 
* Backend              tensorflow
* Keras Version        2.15.0
* Backend Model Type   Sequential
* Backend Model Name   conv_fast2
* 
* Class Index | Symbol Label
* 0           | unlabelled
* 1           | right
* 2           | left
* 
* Layer                          Shape           Type       Function
* Sliding Window (data points)   [6,1]           float      dequeue
*    window_shape = [6,1]
*    stride = 1
*    buffer_multiplier = 1
* Input Layer                    [6,1]           float      dequeue
*    shape = [6,1]
* Convolution 1D                 [3,4]           float      dequeue
*    filters = 4
*    kernel_size = 4
*    dilation_rate = 1
*    strides = 1
*    padding = valid
*    activation = relu
*    use_bias = True
*    trainable = True
*    weight = float[4,1,4]
*    bias = float[4]
* Batch Normalization            [3,4]           float      dequeue
*    epsilon = 0.001
*    trainable = True
*    scale = True
*    center = True
*    axis = 2
*    gamma = float[4]
*    beta = float[4]
*    mean = float[4]
*    variance = float[4]
* Convolution 1D                 [1,6]           float      dequeue
*    filters = 6
*    kernel_size = 3
*    dilation_rate = 1
*    strides = 1
*    padding = valid
*    activation = relu
*    use_bias = True
*    trainable = True
*    weight = float[3,4,6]
*    bias = float[6]
* Dense                          [10]            float      dequeue
*    units = 10
*    use_bias = True
*    activation = relu
*    trainable = True
*    weight = float[6,10]
*    bias = float[10]
* Flatten                        [10]            float      dequeue
* Dense                          [3]             float      dequeue
*    units = 3
*    use_bias = True
*    activation = softmax
*    trainable = True
*    weight = float[10,3]
*    bias = float[3]
* 
* Exported functions:
* 
* int GES_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[3].
* 
* int GES_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 GES_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 97.030 %
* (F1S) F1 Score 97.086 %
* 
* Name of class                                            (unlabelled)             left            right
* (TP) True Positive or Correct Positive Prediction                6568              639              698
* (FN) False Negative or Incorrect Negative Prediction              176               27               39
* (FP) False Positive or Incorrect Positive Prediction               66              126               50
* (TN) True Negative or Correct Negative Prediction                1337             7355             7360
* (TPR) True Positive Rate or Sensitivity, Recall               97.39 %          95.95 %          94.71 %
* (TNR) True Negative Rate or Specificity, Selectivity          95.30 %          98.32 %          99.33 %
* (PPV) Positive Predictive Value or Precision                  99.01 %          83.53 %          93.32 %
* (NPV) Negative Predictive Value                               88.37 %          99.63 %          99.47 %
* (FNR) False Negative Rate or Miss Rate                         2.61 %           4.05 %           5.29 %
* (FPR) False Positive Rate or Fall-Out                          4.70 %           1.68 %           0.67 %
* (FDR) False Discovery Rate                                     0.99 %          16.47 %           6.68 %
* (FOR) False Omission Rate                                     11.63 %           0.37 %           0.53 %
* (F1S) F1 Score                                                98.19 %          89.31 %          94.01 %
*/

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

#include "gestures_nn.h"

// Working memory
static int8_t _buffer[120];
static int8_t _state[232];

// Parameters
static const uint32_t _K5[] = {
    0x3dfee50b, 0x3b387b8a, 0xbf105202, 0x3e6bf526, 0xbf1fd7f0, 0xbdf8c81b, 0x3ec23f02, 0xbd6086cb, 
    0x3ef5b511, 0x3e248db0, 0xbe66e6c6, 0xbec28729, 0xbf386c67, 0x3e03fe2c, 0x3f04df07, 0x3de99f8e
};

static const uint32_t _K6[] = {
    0xbb07bcdd, 0xbbc29680, 0x3ed08634, 0x3f7fd4ac
};

static const uint32_t _K9[] = {
    0x40140182, 0x4002557d, 0x3ec5e819, 0x3eb1f443
};

static const uint32_t _K10[] = {
    0xbf025b4f, 0xbe8500b4, 0xbf807fee, 0xbf92f3a9
};

static const uint32_t _K13[] = {
    0xbf08c9af, 0xbeeb6636, 0xbf22ab33, 0xbd9e7b86, 0xbf03558b, 0xbe1c695d, 0x3cdd03f5, 0x3e76184a, 
    0xbe5cbb68, 0x3e00bf37, 0xbce11666, 0x3f1e70bf, 0xbefe03b9, 0x3cb22106, 0x3f4c9545, 0xbf143edd, 
    0xbea06e19, 0x3d5f6be3, 0xbf03b5c1, 0x3dcfb4b6, 0x3e248626, 0x3e920959, 0xbbb67084, 0x3e674ed2, 
    0x3da4dc01, 0xbe994337, 0x3e1f76c3, 0x3ec76457, 0xbf0299b2, 0x3ea0dbb1, 0x3c7d9a44, 0x3e2f4e40, 
    0xbec00f7d, 0xbe6eb2bc, 0xbec271a2, 0xbfba87c2, 0xbe860d76, 0xbf21e5dd, 0x3e6b8d1f, 0xbe8c8b8e, 
    0x3f586205, 0xbf6eb6ad, 0xbe00b44b, 0xbdf135a8, 0xbf0692ba, 0x3dac6f25, 0xbf83769c, 0x3df2b0b1, 
    0x3f192a1d, 0x3dcb0296, 0xbfa4402d, 0xbe6c7b6d, 0xbdf9f88b, 0x3d121fe8, 0xbeb8c182, 0xbe2fba6d, 
    0x3e6b2b6f, 0x3d9d1531, 0xbebeeac9, 0x3ed9e69d, 0x3f258f38, 0xbdf3109b, 0x3e051aa9, 0x3f535211, 
    0x3e4aac52, 0xbe93a0b4, 0xbddcc297, 0x3d11fd13, 0x3cd5d677, 0xbf0b7ac6, 0xbd636933, 0xbe7232df
};

static const uint32_t _K14[] = {
    0x3ed08b64, 0x3f545d13, 0x3e76517c, 0x3e0a2616, 0x3daebdf6, 0x3ed02e14
};

static const uint32_t _K16[] = {
    0x3e88c37d, 0x3ed13888, 0x3f5670c6, 0xbb72a39d, 0xbf439b1b, 0xbee812dd, 0xbf067c2c, 0x3f00ecde, 
    0x3f01fd1f, 0xbe705930, 0xbf69a50a, 0xbf43db6a, 0x3ea0b1b0, 0x3f10176d, 0xbf6d4042, 0xbf940cc9, 
    0x3f13d0ce, 0x3f0f1575, 0xbf23043a, 0xbc0b033a, 0x3ec209ac, 0x3f667215, 0x3da7de11, 0xbdbc27cf, 
    0xbf07f0ba, 0xbde9a654, 0x3f26d64e, 0x3f30df0d, 0x3f7d199a, 0xbf2c97e0, 0xbe59dfdf, 0x3f88e5f2, 
    0x3d9b4326, 0x3d23f974, 0xbf64ce2d, 0x3f524578, 0x3f9fdaa9, 0xbe754b48, 0xbe1e7f38, 0xbeab966c, 
    0x3cdf8795, 0x3ef9b207, 0x3f5398c0, 0xbf3583d8, 0xbf36ff6c, 0x3e882637, 0x3e167531, 0xbd0dfa87, 
    0xbe26f8ee, 0xbc0044e8, 0xbed44365, 0x3f549b5c, 0x3f15c501, 0xbdf8c449, 0x3e8b2cc3, 0xbf9de414, 
    0xbf23f860, 0x3f4d53dd, 0x3e2491ac, 0xbc182b18
};

static const uint32_t _K18[] = {
    0x3ec3f4f2, 0x3ea83d82, 0xbda1b185, 0x3ea404aa, 0xbdfbdea9, 0x3e8a6083, 0x3f1cdfb7, 0x3e91ef1f, 
    0x3f1a4829, 0x3e6a0dcf
};

static const uint32_t _K21[] = {
    0xbe275455, 0xbf8c2461, 0x3eddeeb5, 0x3f059162, 0x3f65db7c, 0x3f34da22, 0x3e8824ad, 0xbdcc5390, 
    0x3f08f42e, 0xbf4c2f64, 0xbffe9dab, 0xbff2be93, 0xbf39d717, 0xbf830cb7, 0xbe399164, 0xbf5b69ba, 
    0x3ecf99d5, 0x3f4d7a93, 0x3ef649ca, 0x3f13d149, 0x3edd9012, 0x3faf3291, 0xbf5234d4, 0xbe781643, 
    0x3ee6bc23, 0xbcb54d44, 0xc008559b, 0xbe87cc1f, 0xbe4ab4d3, 0xbe191beb
};

static const uint32_t _K23[] = {
    0x3da98fa9, 0xbe10b4db, 0xbc80bec4
};

// Memory mapped buffers
#define _K10             ((float *)_K10)                     // f32[4] (16 bytes) 
#define _K13             ((float *)_K13)                     // f32[6,3,4] (288 bytes) 
#define _K14             ((float *)_K14)                     // f32[6] (24 bytes) 
#define _K16             ((float *)_K16)                     // f32[10,6] (240 bytes) 
#define _K18             ((float *)_K18)                     // f32[10] (40 bytes) 
#define _K21             ((float *)_K21)                     // f32[3,10] (120 bytes) 
#define _K23             ((float *)_K23)                     // f32[3] (12 bytes) 
#define _K5              ((float *)_K5)                      // f32[4,4,1] (64 bytes) 
#define _K6              ((float *)_K6)                      // f32[4] (16 bytes) 
#define _K9              ((float *)_K9)                      // f32[4] (16 bytes) 
#define _K3              ((int8_t *)(_state + 0x00000000))   // s8[232] (232 bytes) 
#define _K11             ((float *)(_buffer + 0x00000048))   // f32[3,4] (48 bytes) 
#define _K12             ((float *)(_buffer + 0x00000030))   // f32[1,6] (24 bytes) 
#define _K15             ((float *)(_buffer + 0x00000000))   // f32[1,6] (24 bytes) 
#define _K17             ((float *)(_buffer + 0x00000000))   // f32[10] (40 bytes) 
#define _K19             ((float *)(_buffer + 0x00000028))   // f32[10] (40 bytes) 
#define _K2              ((float *)(_buffer + 0x00000000))   // f32[6,1] (24 bytes) 
#define _K20             ((float *)(_buffer + 0x00000000))   // f32[10] (40 bytes) 
#define _K22             ((float *)(_buffer + 0x00000028))   // f32[3] (12 bytes) 
#define _K24             ((float *)(_buffer + 0x00000000))   // f32[3] (12 bytes) 
#define _K4              ((float *)(_buffer + 0x00000018))   // f32[3,4] (48 bytes) 
#define _K7              ((float *)(_buffer + 0x00000048))   // f32[3,4] (48 bytes) 
#define _K8              ((float *)(_buffer + 0x00000000))   // f32[3,4] (48 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 conv1d_flat_f32(
	const float* restrict input,
	const float* restrict weight,
	float* restrict output,
	int top,
	int bottom,
	int n_output_rows,
	int filters,
	int strides,
	int kernel_size)
{
	for (int i = 0; i < n_output_rows; i++) {
		const float* wp = weight;		// Weight matrix
		const float* bp = input;		// Input matrix
		const int step = i * strides;	// Row size
		int len = kernel_size;			// Normally do one kernel

		int skip = top - step;			// Pad top?
		if (skip > 0) {
			len -= skip;				// Trim kernel length
			wp += skip;					// Advance kernel
		}
		else {						// No top padding,
			bp -= skip;					// Rollback input
		}

		skip = step + len - bottom;		// Pad bottom?
		if (skip > 0)
			len -= skip;				// Just cut the kernel at end

		float* op = output + i * filters;
		for (int j = 0; j < filters; j++) {
			*op++ = mac_f32(wp + j * kernel_size, bp, len);
		}
	}
}

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 mul_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 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 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 GES_NN_dequeue(float *restrict data_out) {
    __RETURN_ERROR(fixwin_dequeuef32(_K3, _K2, 1));
    conv1d_flat_f32(_K2, _K5, _K4, 0, 6, 3, 4, 1, 4);
    add_f32(_K4, _K6, 1, 1, 1, 3, 4, _K7);
    relu_f32(_K7, 12, _K4);
    mul_f32(_K4, _K9, 1, 1, 1, 3, 4, _K11);
    add_f32(_K11, _K10, 1, 1, 1, 3, 4, _K8);
    conv1d_flat_f32(_K8, _K13, _K12, 0, 12, 1, 6, 4, 12);
    add_f32(_K12, _K14, 1, 1, 1, 1, 6, _K15);
    relu_f32(_K15, 6, _K12);
    dott_f32(_K16, _K12, _K17, 6, 10, 1);
    add_f32(_K17, _K18, 1, 1, 1, 1, 10, _K19);
    relu_f32(_K19, 10, _K20);
    dott_f32(_K21, _K20, _K22, 10, 3, 1);
    add_f32(_K22, _K23, 1, 1, 1, 1, 3, _K24);
    softmax_f32(_K24, 3, data_out);
    return 0;
}

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

void GES_NN_init(void) {
    fixwin_initf32(_K3, 4, 6);
}

