1 files changed, 99 insertions, 0 deletions
diff --git a/lib/zigzag.c b/lib/zigzag.c
new file mode 100644
index 0000000..50549e2
--- /dev/null
+++ b/lib/zigzag.c
@@ -0,0 +1,99 @@
+// Zigzag
+//
+// zigzag
+//
+// ZigZag Filter
+//
+// 
+//
+// Inputs:    1
+// Outputs:   2
+// Constants: 1
+
+
+#include "ts.h"
+
+
+typedef struct {
+    fp_t threshold;
+} ts_zigzag_params_t;
+
+ts_parameter_t const ts_zigzag_p = { .n_inputs = 1, .n_outputs = 2, .n_constants = 1, .params_size = sizeof(ts_zigzag_params_t) };
+
+
+uint_t ts_zigzag_offset( ts_constant_t const *c )
+{
+	( void )c;
+	return ( 0 );
+}
+
+int ts_zigzag(
+	fp_t * const *y, uint_t y_n,
+	fp_t const * const *x, uint_t x_n,
+	uint_t n, ts_constant_t const *c
+)
+{
+	TS_PROLOGUE( ts_zigzag_p );
+	if ( !c ) return -1;
+	if (n == 0) return -1;
+
+	fp_t threshold = ((ts_zigzag_params_t *)c)->threshold;
+	if (threshold <= 0 || isnan(threshold) || isinf(threshold)) return -1;
+
+	y[0][0] = x[0][0];
+	y[1][0] = 0;
+
+	for( uint_t i = 1; i < n; ++i )
+	{
+		fp_t last_zz = y[0][i - 1];
+		fp_t dir = y[1][i - 1];
+		fp_t change = last_zz != 0 ? ( x[0][i] - last_zz ) / last_zz * 100.0 : 0;
+
+		if( dir == 0 )
+		{
+			if( change >= threshold )
+			{
+				y[0][i] = x[0][i];
+				y[1][i] = 1;
+			}
+			else if( change <= -threshold )
+			{
+				y[0][i] = x[0][i];
+				y[1][i] = -1;
+			}
+			else
+			{
+				y[0][i] = last_zz;
+				y[1][i] = 0;
+			}
+		}
+		else if( dir == 1 )
+		{
+			if( change <= -threshold )
+			{
+				y[0][i] = x[0][i];
+				y[1][i] = -1;
+			}
+			else
+			{
+				y[0][i] = last_zz;
+				y[1][i] = 1;
+			}
+		}
+		else
+		{
+			if( change >= threshold )
+			{
+				y[0][i] = x[0][i];
+				y[1][i] = 1;
+			}
+			else
+			{
+				y[0][i] = last_zz;
+				y[1][i] = -1;
+			}
+		}
+	}
+	return 0;
+}
+