Mercurial > ~darius > hgwebdir.cgi > modulator

comparison q/q.c @ 14:388074ff9474

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Add fixed point code
author Daniel O'Connor <darius@dons.net.au>
date Tue, 25 Feb 2025 13:28:29 +1030
parents
children
comparison
equal deleted inserted replaced
13:032acb7fbc04 14:388074ff9474
1 /* Project: Q-Number (Q16.16, signed) library
2 * Author: Richard James Howe
3 * License: The Unlicense
4 * Email: howe.r.j.89@gmail.com
5 * Repo: <https://github.com/q>
6 *
7 *
8 * A Q32.32 version would be useful.
9 *
10 * The following should be changed/done for this library:
11 *
12 * - Moving towards a header-only model.
13 * - Removal of dependencies such as 'isalpha', 'tolower'
14 * as they are locale dependent.
15 * - Make components optional (filters, expression parser, ...)
16 * - Make hyperbolic arc sin/cos/tan functions.
17 * - Fix bugs / inaccuracies in CORDIC code.
18 * - Improve accuracy of all the functions and quantify error and
19 * their limits.
20 *
21 * BUG: Enter: 2.71791, get 2.0625, 2.7179 works fine. (Need to
22 * limit decimal places).
23 */
24
25 #include "q.h"
26 #include <assert.h>
27 #include <ctype.h>
28 #include <inttypes.h>
29 #include <limits.h>
30 #include <stdarg.h> /* for expression evaluator error handling */
31 #include <stdio.h> /* vsnprintf, for expression evaluator */
32 #include <string.h>
33
34 #define UNUSED(X) ((void)(X))
35 #define BOOLIFY(X) (!!(X))
36 #define BUILD_BUG_ON(condition) ((void)sizeof(char[1 - 2*!!(condition)]))
37 #define MULTIPLIER (INT16_MAX)
38 #define DMIN (INT32_MIN)
39 #define DMAX (INT32_MAX)
40 #define MIN(X, Y) ((X) < (Y) ? (X) : (Y))
41 #define MAX(X, Y) ((X) < (Y) ? (Y) : (X))
42
43 #ifndef CONFIG_Q_HIDE_FUNCS /* 1 = hide hidden (testing) functions, 0 = enable them */
44 #define CONFIG_Q_HIDE_FUNCS (0)
45 #endif
46
47 typedef int16_t hd_t; /* half Q width, signed */
48 typedef uint64_t lu_t; /* double Q width, unsigned */
49
50 const qinfo_t qinfo = {
51 .whole = QBITS,
52 .fractional = QBITS,
53 .zero = (u_t)0uL << QBITS,
54 .bit = 1uL,
55 .one = (u_t)1uL << QBITS,
56 .min = (u_t)(QHIGH << QBITS),
57 .max = (u_t)((QHIGH << QBITS) - 1uL),
58
59 .pi = QPI, /* 3.243F6 A8885 A308D 31319 8A2E0... */
60 .e = QMK(0x2, 0xB7E1, 16), /* 2.B7E1 5162 8A... */
61 .sqrt2 = QMK(0x1, 0x6A09, 16), /* 1.6A09 E667 F3... */
62 .sqrt3 = QMK(0x1, 0xBB67, 16), /* 1.BB67 AE85 84... */
63 .ln2 = QMK(0x0, 0xB172, 16), /* 0.B172 17F7 D1... */
64 .ln10 = QMK(0x2, 0x4D76, 16), /* 2.4D76 3776 AA... */
65
66 .version = QVERSION,
67 };
68
69 qconf_t qconf = { /* Global Configuration Options */
70 .bound = qbound_saturate,
71 .dp = 4,
72 .base = 10,
73 };
74
75 /********* Basic Library Routines ********************************************/
76
77
78 static inline void implies(const int x, const int y) {
79 assert(!x || y);
80 }
81
82 static inline void mutual(const int x, const int y) { /* mutual implication */
83 assert(BOOLIFY(x) == BOOLIFY(y));
84 }
85
86 static inline void exclusive(const int x, const int y) {
87 assert(BOOLIFY(x) != BOOLIFY(y));
88 }
89
90 static inline void static_assertions(void) {
91 BUILD_BUG_ON(CHAR_BIT != 8);
92 // BUILD_BUG_ON((sizeof(q_t)*CHAR_BIT) != (QBITS * 2));
93 BUILD_BUG_ON( sizeof(q_t) != sizeof(u_t));
94 BUILD_BUG_ON( sizeof(u_t) != sizeof(d_t));
95 BUILD_BUG_ON(sizeof(lu_t) != sizeof(ld_t));
96 BUILD_BUG_ON(sizeof(d_t) != (sizeof(hd_t) * 2));
97 BUILD_BUG_ON(sizeof(lu_t) != (sizeof(u_t) * 2));
98 }
99
100 q_t qbound_saturate(const ld_t s) { /**< default saturation handler */
101 assert(s > DMAX || s < DMIN);
102 if (s > DMAX) return DMAX;
103 return DMIN;
104 }
105
106 q_t qbound_wrap(const ld_t s) { /**< wrap numbers on overflow */
107 assert(s > DMAX || s < DMIN);
108 if (s > DMAX) return DMIN + (s % DMAX);
109 return DMAX - ((-s) % DMAX);
110 }
111
112 static inline q_t qsat(const ld_t s) {
113 static_assertions();
114 if (s > DMAX || s < DMIN) return qconf.bound(s);
115 return s;
116 }
117
118 d_t arshift(const d_t v, const unsigned p) {
119 u_t vn = v;
120 if (v >= 0l)
121 return vn >> p;
122 const u_t leading = ((u_t)(-1l)) << ((sizeof(v) * CHAR_BIT) - p - 1);
123 return leading | (vn >> p);
124 }
125
126 static inline d_t divn(const d_t v, const unsigned p) {
127 /* return v / (1l << p); */
128 const u_t shifted = ((u_t)v) >> p;
129 if (qispositive(v))
130 return shifted;
131 const u_t leading = ((u_t)(-1l)) << ((sizeof(v)*CHAR_BIT) - p - 1);
132 return leading | shifted;
133 }
134
135 /* These really all should be moved the header for efficiency reasons */
136 static inline u_t qhigh(const q_t q) { return ((u_t)q) >> QBITS; }
137 static inline u_t qlow(const q_t q) { return ((u_t)q) & QMASK; }
138 static inline q_t qcons(const u_t hi, const u_t lo) { return (hi << QBITS) | (lo & QMASK); }
139
140 int qtoi(const q_t toi) { return ((lu_t)((ld_t)toi)) >> QBITS; }
141 q_t qint(const int toq) { return ((u_t)((d_t)toq)) << QBITS; }
142 signed char qtoc(const q_t q) { return qtoi(q); }
143 q_t qchar(signed char c) { return qint(c); }
144 short qtoh(const q_t q) { return qtoi(q); }
145 q_t qshort(short s) { return qint(s); }
146 long qtol(const q_t q) { return qtoi(q); }
147 q_t qlong(long l) { return qint(l); }
148 long long qtoll(const q_t q) { return qtoi(q); }
149 q_t qvlong(long long ll) { return qint(ll); }
150
151 q_t qisnegative(const q_t a) { return QINT(BOOLIFY(qhigh(a) & QHIGH)); }
152 q_t qispositive(const q_t a) { return QINT(!(qhigh(a) & QHIGH)); }
153 q_t qisinteger(const q_t a) { return QINT(!qlow(a)); }
154 q_t qisodd(const q_t a) { return QINT(qisinteger(a) && (qhigh(a) & 1)); }
155 q_t qiseven(const q_t a) { return QINT(qisinteger(a) && !(qhigh(a) & 1)); }
156 q_t qless(const q_t a, const q_t b) { return QINT(a < b); }
157 q_t qeqless(const q_t a, const q_t b) { return QINT(a <= b); }
158 q_t qmore(const q_t a, const q_t b) { return QINT(a > b); }
159 q_t qeqmore(const q_t a, const q_t b) { return QINT(a >= b); }
160 q_t qequal(const q_t a, const q_t b) { return QINT(a == b); }
161 q_t qunequal(const q_t a, const q_t b) { return QINT(a != b); }
162
163 q_t qnegate(const q_t a) { return (~(u_t)a) + 1ULL; }
164 q_t qmin(const q_t a, const q_t b) { return qless(a, b) ? a : b; }
165 q_t qmax(const q_t a, const q_t b) { return qmore(a, b) ? a : b; }
166 q_t qabs(const q_t a) { return qisnegative(a) ? qnegate(a) : a; }
167 q_t qadd(const q_t a, const q_t b) { return qsat((ld_t)a + (ld_t)b); }
168 q_t qsub(const q_t a, const q_t b) { return qsat((ld_t)a - (ld_t)b); }
169 q_t qcopysign(const q_t a, const q_t b) { return qisnegative(b) ? qnegate(qabs(a)) : qabs(a); }
170 q_t qand(const q_t a, const q_t b) { return a & b; }
171 q_t qxor(const q_t a, const q_t b) { return a ^ b; }
172 q_t qor(const q_t a, const q_t b) { return a | b; }
173 q_t qinvert(const q_t a) { return ~a; }
174 q_t qnot(const q_t a) { return QINT(!a); }
175 q_t qlogical(const q_t a) { return QINT(BOOLIFY(a)); }
176
177 q_t qlrs(const q_t a, const q_t b) { /* assert low bits == 0? */ return (u_t)a >> (u_t)qtoi(b); }
178 q_t qlls(const q_t a, const q_t b) { return (u_t)a << b; }
179 q_t qars(const q_t a, const q_t b) { return arshift(a, qtoi(b)); }
180 q_t qals(const q_t a, const q_t b) { return qsat((lu_t)a << b); }
181 q_t qsign(const q_t a) { return qisnegative(a) ? -QINT(1) : QINT(1); }
182 q_t qsignum(const q_t a) { return a ? qsign(a) : QINT(0); }
183
184 q_t qapproxequal(const q_t a, const q_t b, const q_t epsilon) {
185 assert(qeqmore(epsilon, qint(0)));
186 return QINT(qless(qabs(qsub(a, b)), epsilon));
187 }
188
189 q_t qapproxunequal(const q_t a, const q_t b, const q_t epsilon) {
190 return QINT(!qapproxequal(a, b, epsilon));
191 }
192
193 q_t qwithin(q_t v, q_t b1, q_t b2) {
194 const q_t hi = qmax(b1, b2);
195 const q_t lo = qmin(b1, b2);
196 if (qequal(v, b1) || qequal(v, b2))
197 return 1;
198 return qless(v, hi) && qmore(v, lo) ? QINT(1) : QINT(0);
199 }
200
201 q_t qwithin_interval(q_t v, q_t expected, q_t allowance) {
202 const q_t b1 = qadd(expected, allowance);
203 const q_t b2 = qsub(expected, allowance);
204 return qwithin(v, b1, b2);
205 }
206
207 q_t qfloor(const q_t q) {
208 return q & ~QMASK;
209 }
210
211 q_t qceil(q_t q) {
212 const q_t adj = qisinteger(q) ? QINT(0) : QINT(1);
213 q = qadd(q, adj);
214 return ((u_t)q) & (QMASK << QBITS);
215 }
216
217 q_t qtrunc(q_t q) {
218 const q_t adj = qisnegative(q) && qlow(q) ? QINT(1) : QINT(0);
219 q = qadd(q, adj);
220 return ((u_t)q) & (QMASK << QBITS);
221 }
222
223 q_t qround(q_t q) {
224 const int negative = qisnegative(q);
225 q = qabs(q);
226 const q_t adj = (qlow(q) & QHIGH) ? QINT(1) : QINT(0);
227 q = qadd(q, adj);
228 q = ((u_t)q) & (QMASK << QBITS);
229 return negative ? qnegate(q) : q;
230 }
231
232 int qpack(const q_t *q, char *buffer, const size_t length) {
233 assert(buffer);
234 if (length < sizeof(*q))
235 return -1;
236 q_t qn = *q;
237 uint8_t *b = (uint8_t*)buffer;
238 for (size_t i = 0; i < sizeof(qn); i++) {
239 b[i] = qn;
240 qn = (u_t)qn >> CHAR_BIT;
241 }
242 return sizeof(qn);
243 }
244
245 int qunpack(q_t *q, const char *buffer, const size_t length) {
246 assert(q);
247 assert(buffer);
248 if (length < sizeof(*q))
249 return -1;
250 uint8_t *b = (uint8_t*)buffer;
251 u_t nq = 0;
252 for (size_t i = 0; i < sizeof(*q); i++) {
253 nq <<= CHAR_BIT;
254 nq |= b[sizeof(*q)-i-1];
255 }
256 *q = nq;
257 return sizeof(*q);
258 }
259
260 static inline ld_t multiply(const q_t a, const q_t b) {
261 const ld_t dd = ((ld_t)a * (ld_t)b) + (lu_t)QHIGH;
262 /* N.B. portable version of "dd >> QBITS", for double width signed values */
263 return dd < 0 ? (-1ull << (2 * QBITS)) | ((lu_t)dd >> QBITS) : ((lu_t)dd) >> QBITS;
264 }
265
266 q_t qmul(const q_t a, const q_t b) {
267 return qsat(multiply(a, b));
268 }
269
270 q_t qfma(const q_t a, const q_t b, const q_t c) {
271 return qsat(multiply(a, b) + (ld_t)c);
272 }
273
274 q_t qdiv(const q_t a, const q_t b) {
275 assert(b);
276 const ld_t dd = ((ld_t)a) << QBITS;
277 ld_t bd2 = divn(b, 1);
278 if (!((dd >= 0 && b > 0) || (dd < 0 && b < 0)))
279 bd2 = -bd2;
280 /* Overflow not checked! */
281 /*return (dd/b) + (bd2/b);*/
282 return (dd + bd2) / b;
283 }
284
285 q_t qrem(const q_t a, const q_t b) {
286 return qsub(a, qmul(qtrunc(qdiv(a, b)), b));
287 }
288
289 q_t qmod(q_t a, q_t b) {
290 return qsub(a, qmul(qfloor(qdiv(a, b)), b));
291 }
292
293 static char itoch(const unsigned ch) {
294 assert(ch < 36);
295 if (ch <= 9)
296 return ch + '0';
297 return ch + 'A' - 10;
298 }
299
300 static inline void swap(char *a, char *b) {
301 assert(a);
302 assert(b);
303 const int c = *a;
304 *a = *b;
305 *b = c;
306 }
307
308 static void reverse(char *s, const size_t length) {
309 assert(s);
310 for (size_t i = 0; i < length/2; i++)
311 swap(&s[i], &s[length - i - 1]);
312 }
313
314 static int uprint(u_t p, char *s, const size_t length, const d_t base) {
315 assert(s);
316 assert(base >= 2 && base <= 36);
317 if (length < 2)
318 return -1;
319 size_t i = 0;
320 do {
321 unsigned ch = p % base;
322 p /= base;
323 s[i++] = itoch(ch);
324 } while (p && i < length);
325 if (p && i >= length)
326 return -1;
327 reverse(s, i);
328 return i;
329 }
330
331 /* <https://codereview.stackexchange.com/questions/109212> */
332 int qsprintbdp(q_t p, char *s, size_t length, const u_t base, const d_t idp) {
333 assert(s);
334 const int negative = BOOLIFY(qisnegative(p));
335 if (negative)
336 p = qnegate(p);
337 const d_t hi = qhigh(p);
338 char frac[QBITS + 2] = { '.', };
339 memset(s, 0, length);
340 assert(base >= 2 && base <= 36);
341 u_t lo = qlow(p);
342 size_t i = 1;
343 for (i = 1; lo; i++) {
344 if (idp >= 0 && (int)i > idp)
345 break;
346 lo *= base;
347 assert(i < (QBITS + 2));
348 frac[i] = itoch(lo >> QBITS);
349 lo &= QMASK;
350 }
351 if (negative)
352 s[0] = '-';
353 const int hisz = uprint(hi, s + negative, length - (1 + negative), base);
354 if (hisz < 0 || (hisz + i + negative + 1) > length)
355 return -1;
356 memcpy(s + hisz + negative, frac, i);
357 return i + hisz;
358 }
359
360 int qsprintb(q_t p, char *s, size_t length, const u_t base) {
361 return qsprintbdp(p, s, length, base, qconf.dp);
362 }
363
364 int qsprint(const q_t p, char *s, const size_t length) {
365 return qsprintb(p, s, length, qconf.base);
366 }
367
368 static inline int extract(unsigned char c, const int radix) {
369 c = tolower(c);
370 if (c >= '0' && c <= '9')
371 c -= '0';
372 else if (c >= 'a' && c <= 'z')
373 c -= ('a' - 10);
374 else
375 return -1;
376 if (c < radix)
377 return c;
378 return -1;
379 }
380
381 static inline q_t qmk(d_t integer, u_t fractional) {
382 const int negative = integer < 0;
383 integer = negative ? -integer : integer;
384 const q_t r = qcons((d_t)integer, fractional);
385 return negative ? qnegate(r) : r;
386 }
387
388 static inline u_t integer_logarithm(u_t num, const u_t base) {
389 assert(num > 0 && base >= 2 && base <= 36);
390 u_t r = -1;
391 do r++; while (num /= base);
392 return r;
393 }
394
395 int qnconvbdp(q_t *q, const char *s, size_t length, const d_t base, const u_t idp) {
396 assert(q);
397 assert(s);
398 assert(base >= 2 && base <= 36);
399 *q = QINT(0);
400 if (length < 1)
401 return -1;
402 d_t hi = 0, lo = 0, places = 1, negative = 0, overflow = 0;
403 size_t sidx = 0;
404
405 if (s[sidx] == '-') {
406 if (length < 2)
407 return -1;
408 negative = 1;
409 sidx++;
410 }
411
412 for (; sidx < length && s[sidx]; sidx++) {
413 const d_t e = extract(s[sidx], base);
414 if (e < 0)
415 break;
416 if (hi > MULTIPLIER) { /* continue on with conversion, do not accumulate */
417 overflow = 1;
418 } else {
419 hi = (hi * base) + e;
420 }
421 }
422 if (sidx >= length || !s[sidx])
423 goto done;
424 if (s[sidx] != '.')
425 return -2;
426 sidx++;
427
428 const u_t ilog = integer_logarithm(0x10000, base);
429 const u_t max = MIN(idp, ilog); /* Calculate maximum decimal places given base */
430
431 for (u_t dp = 0; sidx < length && s[sidx]; sidx++, dp++) {
432 const int ch = extract(s[sidx], base);
433 if (ch < 0)
434 return -3;
435 if (dp < max) { /* continue on with conversion , do not accumulate */
436 /* We could get more accuracy by looking at one digit
437 * passed the maximum digits allowed and rounding if
438 * that digit exists in the input. */
439 lo = (lo * base) + ch;
440 if (places >= (DMAX / base))
441 return -4;
442 places *= base;
443 }
444 assert((dp + 1) > dp);
445 }
446 if (!places)
447 return -5;
448 lo = ((d_t)((u_t)lo << QBITS) / places);
449 done:
450 if (overflow) {
451 *q = negative ? qinfo.min : qinfo.max;
452 return -6;
453 } else {
454 const q_t nq = qmk(hi, lo);
455 *q = negative ? qnegate(nq) : nq;
456
457 }
458 return 0;
459 }
460
461 int qnconvb(q_t *q, const char *s, size_t length, const d_t base) {
462 return qnconvbdp(q, s, length, base, qconf.dp);
463 }
464
465 int qnconv(q_t *q, const char *s, size_t length) {
466 return qnconvb(q, s, length, qconf.base);
467 }
468
469 int qconv(q_t *q, const char * const s) {
470 assert(s);
471 return qnconv(q, s, strlen(s));
472 }
473
474 int qconvb(q_t *q, const char * const s, const d_t base) {
475 assert(s);
476 return qnconvb(q, s, strlen(s), base);
477 }
478
479 typedef enum {
480 CORDIC_MODE_VECTOR_E/* = 'VECT'*/,
481 CORDIC_MODE_ROTATE_E/* = 'ROT'*/,
482 } cordic_mode_e;
483
484 typedef enum {
485 CORDIC_COORD_HYPERBOLIC_E = -1,
486 CORDIC_COORD_LINEAR_E = 0,
487 CORDIC_COORD_CIRCULAR_E = 1,
488 } cordic_coordinates_e;
489
490 static const d_t cordic_circular_inverse_scaling = 0x9B74; /* 1/scaling-factor */
491 static const d_t cordic_hyperbolic_inverse_scaling = 0x13520; /* 1/scaling-factor */
492
493 static inline int mulsign(d_t a, d_t b) { /* sign(a*b) */
494 const int aneg = a < 0;
495 const int bneg = b < 0;
496 return aneg ^ bneg ? -QINT(1) : QINT(1);
497 }
498
499 /* Universal CORDIC <https://en.wikibooks.org/wiki/Digital_Circuits/CORDIC>
500 *
501 * x(i+1) = x(i) - u.d(i).y(i).pow(2, -i)
502 * y(i+1) = y(i) + d(i).x(i).pow(2, -i)
503 * z(i+1) = z(i) - d(i).a(i)
504 *
505 * d(i) = sgn(z(i)) (rotation)
506 * d(i) = -sgn(x(i).y(i)) (vectoring)
507 *
508 * hyperbolic linear circular
509 * u = -1 0 1
510 * a = atanh(pow(2, -i)) pow(2, -i) atan(pow(2, -i))
511 *
512 * linear shift sequence: i = 0, 1, 2, 3, ...
513 * circular shift sequence: i = 1, 2, 3, 4, ...
514 * hyperbolic shift sequence: i = 1, 2, 3, 4, 4, 5, ... */
515 static int cordic(const cordic_coordinates_e coord, const cordic_mode_e mode, int iterations, d_t *x0, d_t *y0, d_t *z0) {
516 assert(x0);
517 assert(y0);
518 assert(z0);
519 if (mode != CORDIC_MODE_VECTOR_E && mode != CORDIC_MODE_ROTATE_E)
520 return -1;
521
522 BUILD_BUG_ON(sizeof(d_t) != sizeof(uint32_t));
523 BUILD_BUG_ON(sizeof(u_t) != sizeof(uint32_t));
524
525 static const u_t arctans[] = { /* atan(2^0), atan(2^-1), atan(2^-2), ... */
526 0xC90FuL, 0x76B1uL, 0x3EB6uL, 0x1FD5uL,
527 0x0FFAuL, 0x07FFuL, 0x03FFuL, 0x01FFuL,
528 0x00FFuL, 0x007FuL, 0x003FuL, 0x001FuL,
529 0x000FuL, 0x0007uL, 0x0003uL, 0x0001uL,
530 0x0000uL, // 0x0000uL,
531 };
532 static const size_t arctans_length = sizeof arctans / sizeof arctans[0];
533
534 static const u_t arctanhs[] = { /* atanh(2^-1), atanh(2^-2), ... */
535 0x8c9fuL, 0x4162uL, 0x202buL, 0x1005uL,
536 0x0800uL, 0x0400uL, 0x0200uL, 0x0100uL,
537 0x0080uL, 0x0040uL, 0x0020uL, 0x0010uL,
538 0x0008uL, 0x0004uL, 0x0002uL, 0x0001uL,
539 0x0000uL, // 0x0000uL,
540 };
541 static const size_t arctanhs_length = sizeof arctanhs / sizeof arctanhs[0];
542
543 static const u_t halfs[] = { /* 2^0, 2^-1, 2^-2, ..*/
544 0x10000uL,
545 0x8000uL, 0x4000uL, 0x2000uL, 0x1000uL,
546 0x0800uL, 0x0400uL, 0x0200uL, 0x0100uL,
547 0x0080uL, 0x0040uL, 0x0020uL, 0x0010uL,
548 0x0008uL, 0x0004uL, 0x0002uL, 0x0001uL,
549 //0x0000uL, // 0x0000uL,
550 };
551 static const size_t halfs_length = sizeof halfs / sizeof halfs[0];
552
553 const u_t *lookup = NULL;
554 size_t i = 0, j = 0, k = 0, length = 0;
555 const size_t *shiftx = NULL, *shifty = NULL;
556 int hyperbolic = 0;
557
558 switch (coord) {
559 case CORDIC_COORD_CIRCULAR_E:
560 lookup = arctans;
561 length = arctans_length;
562 i = 0;
563 shifty = &i;
564 shiftx = &i;
565 break;
566 case CORDIC_COORD_HYPERBOLIC_E:
567 lookup = arctanhs;
568 length = arctanhs_length;
569 hyperbolic = 1;
570 i = 1;
571 shifty = &i;
572 shiftx = &i;
573 break;
574 case CORDIC_COORD_LINEAR_E:
575 lookup = halfs;
576 length = halfs_length;
577 shifty = &j;
578 shiftx = NULL;
579 i = 1;
580 break;
581 default: /* not implemented */
582 return -2;
583 }
584
585 iterations = iterations > (int)length ? (int)length : iterations;
586 iterations = iterations < 0 ? (int)length : iterations;
587
588 d_t x = *x0, y = *y0, z = *z0;
589
590 /* rotation mode: z determines direction,
591 * vector mode: y determines direction */
592 for (; j < (unsigned)iterations; i++, j++) {
593 again:
594 {
595 const d_t m = mode == CORDIC_MODE_ROTATE_E ? z : -y /*-mulsign(x, y)*/;
596 const d_t d = -!!(m < 0);
597 const d_t xs = ((((shiftx ? divn(y, *shiftx) : 0)) ^ d) - d);
598 const d_t ys = (divn(x, *shifty) ^ d) - d;
599 const d_t xn = x - (hyperbolic ? -xs : xs);
600 const d_t yn = y + ys;
601 const d_t zn = z - ((lookup[j] ^ d) - d);
602 x = xn; /* cosine, in circular, rotation mode */
603 y = yn; /* sine, in circular, rotation mode */
604 z = zn;
605 }
606 if (hyperbolic) { /* Experimental/Needs bug fixing */
607 switch (1) { // TODO: Correct hyperbolic redo of iteration
608 case 0: break;
609 case 1: if (k++ >= 3) { k = 0; goto again; } break;
610 case 2: {
611 assert(j <= 120);
612 size_t cmp = j + 1;
613 if (cmp == 4 || cmp == 13 /*|| cmp == 40 || cmp == 121 || cmp == floor(pow(3,i-1)/2) */) {
614 if (k) {
615 k = 0;
616 } else {
617 k = 1;
618 goto again;
619 }
620 }
621 break;
622 }
623 }
624 }
625 }
626 *x0 = x;
627 *y0 = y;
628 *z0 = z;
629
630 return iterations;
631 }
632
633 /* See: - <https://dspguru.com/dsp/faqs/cordic/>
634 * - <https://en.wikipedia.org/wiki/CORDIC> */
635 static int qcordic(q_t theta, const int iterations, q_t *sine, q_t *cosine) {
636 assert(sine);
637 assert(cosine);
638
639 static const q_t pi = QPI, npi = -QPI;
640 static const q_t hpi = QPI/2, hnpi = -(QPI/2);
641 static const q_t qpi = QPI/4, qnpi = -(QPI/4);
642 static const q_t dpi = QPI*2, dnpi = -(QPI*2);
643
644 /* Convert to range -pi to pi, we could use qmod,
645 * however that uses multiplication and division, and
646 * if we can use those operators freely then there are
647 * other, better algorithms we can use instead of CORDIC
648 * for sine/cosine calculation. */
649 while (qless(theta, npi)) theta = qadd(theta, dpi);
650 while (qmore(theta, pi)) theta = qadd(theta, dnpi);
651
652 int negate = 0, shift = 0;
653
654 /* convert to range -pi/2 to pi/2 */
655 if (qless(theta, hnpi)) {
656 theta = qadd(theta, pi);
657 negate = 1;
658 } else if (qmore(theta, hpi)) {
659 theta = qadd(theta, npi);
660 negate = 1;
661 }
662
663 /* convert to range -pi/4 to pi/4 */
664 if (qless(theta, qnpi)) {
665 theta = qadd(theta, hpi);
666 shift = -1;
667 } else if (qmore(theta, qpi)) {
668 theta = qadd(theta, hnpi);
669 shift = 1;
670 }
671
672 d_t x = cordic_circular_inverse_scaling, y = 0, z = theta /* no theta scaling needed */;
673
674 /* CORDIC in Q2.16 format */
675 if (cordic(CORDIC_COORD_CIRCULAR_E, CORDIC_MODE_ROTATE_E, iterations, &x, &y, &z) < 0)
676 return -1;
677
678 /* undo shifting and quadrant changes */
679 if (shift > 0) {
680 const d_t yt = y;
681 y = x;
682 x = -yt;
683 } else if (shift < 0) {
684 const d_t yt = y;
685 y = -x;
686 x = yt;
687 }
688
689 if (negate) {
690 x = -x;
691 y = -y;
692 }
693 /* set output; no scaling needed */
694 *cosine = x;
695 *sine = y;
696 return 0;
697 }
698
699 q_t qatan(const q_t t) {
700 q_t x = qint(1), y = t, z = QINT(0);
701 cordic(CORDIC_COORD_CIRCULAR_E, CORDIC_MODE_VECTOR_E, -1, &x, &y, &z);
702 return z;
703 }
704
705 q_t qatan2(const q_t a, const q_t b) {
706 q_t x = b, y = a, z = QINT(0);
707 if (qequal(b, QINT(0))) {
708 assert(qunequal(a, QINT(0)));
709 if (qmore(a, QINT(0)))
710 return QPI/2;
711 return -(QPI/2);
712 } else if (qless(b, QINT(0))) {
713 if (qeqmore(a, QINT(0)))
714 return qadd(qatan(qdiv(a, b)), QPI);
715 return qsub(qatan(qdiv(a, b)), QPI);
716 }
717 cordic(CORDIC_COORD_CIRCULAR_E, CORDIC_MODE_VECTOR_E, -1, &x, &y, &z);
718 return z;
719 }
720
721 void qsincos(q_t theta, q_t *sine, q_t *cosine) {
722 assert(sine);
723 assert(cosine);
724 const int r = qcordic(theta, -1, sine, cosine);
725 assert(r >= 0);
726 }
727
728 q_t qsin(const q_t theta) {
729 q_t sine = QINT(0), cosine = QINT(0);
730 qsincos(theta, &sine, &cosine);
731 return sine;
732 }
733
734 q_t qcos(const q_t theta) {
735 q_t sine = QINT(0), cosine = QINT(0);
736 qsincos(theta, &sine, &cosine);
737 return cosine;
738 }
739
740 q_t qtan(const q_t theta) {
741 q_t sine = QINT(0), cosine = QINT(0);
742 qsincos(theta, &sine, &cosine);
743 return qdiv(sine, cosine); /* can use qcordic_div, with range limits it imposes */
744 }
745
746 q_t qcot(const q_t theta) {
747 q_t sine = QINT(0), cosine = QINT(0);
748 qsincos(theta, &sine, &cosine);
749 return qdiv(cosine, sine); /* can use qcordic_div, with range limits it imposes */
750 }
751
752 q_t qcordic_mul(const q_t a, const q_t b) { /* works for small values; result < 4 */
753 q_t x = a, y = QINT(0), z = b;
754 const int r = cordic(CORDIC_COORD_LINEAR_E, CORDIC_MODE_ROTATE_E, -1, &x, &y, &z);
755 assert(r >= 0);
756 return y;
757 }
758
759 q_t qcordic_div(const q_t a, const q_t b) {
760 q_t x = b, y = a, z = QINT(0);
761 const int r = cordic(CORDIC_COORD_LINEAR_E, CORDIC_MODE_VECTOR_E, -1, &x, &y, &z);
762 assert(r >= 0);
763 return z;
764 }
765
766 void qsincosh(const q_t a, q_t *sinh, q_t *cosh) {
767 assert(sinh);
768 assert(cosh);
769 q_t x = cordic_hyperbolic_inverse_scaling, y = QINT(0), z = a; /* (e^2x - 1) / (e^2x + 1) */
770 const int r = cordic(CORDIC_COORD_HYPERBOLIC_E, CORDIC_MODE_ROTATE_E, -1, &x, &y, &z);
771 assert(r >= 0);
772 *sinh = y;
773 *cosh = x;
774 }
775
776 q_t qtanh(const q_t a) {
777 q_t sinh = QINT(0), cosh = QINT(0);
778 qsincosh(a, &sinh, &cosh);
779 return qdiv(sinh, cosh);
780 }
781
782 q_t qcosh(const q_t a) {
783 q_t sinh = QINT(0), cosh = QINT(0);
784 qsincosh(a, &sinh, &cosh);
785 return cosh;
786 }
787
788 q_t qsinh(const q_t a) {
789 q_t sinh = QINT(0), cosh = QINT(0);
790 qsincosh(a, &sinh, &cosh);
791 return sinh;
792 }
793
794 q_t qcordic_exp(const q_t e) {
795 q_t s = QINT(0), h = QINT(0);
796 qsincosh(e, &s, &h);
797 return qadd(s, h);
798 }
799
800 q_t qcordic_ln(const q_t d) {
801 q_t x = qadd(d, QINT(1)), y = qsub(d, QINT(1)), z = QINT(0);
802 const int r = cordic(CORDIC_COORD_HYPERBOLIC_E, CORDIC_MODE_VECTOR_E, -1, &x, &y, &z);
803 assert(r >= 0);
804 return qadd(z, z);
805 }
806
807 q_t qcordic_sqrt(const q_t n) { /* testing only; works for 0 < x < 2 */
808 const q_t quarter = 1uLL << (QBITS - 2); /* 0.25 */
809 q_t x = qadd(n, quarter),
810 y = qsub(n, quarter),
811 z = 0;
812 const int r = cordic(CORDIC_COORD_HYPERBOLIC_E, CORDIC_MODE_VECTOR_E, -1, &x, &y, &z);
813 assert(r >= 0);
814 return qmul(x, cordic_hyperbolic_inverse_scaling);
815 }
816
817 q_t qhypot(const q_t a, const q_t b) {
818 q_t x = qabs(a), y = qabs(b), z = QINT(0); /* abs() should not be needed? */
819 const int r = cordic(CORDIC_COORD_CIRCULAR_E, CORDIC_MODE_VECTOR_E, -1, &x, &y, &z);
820 assert(r >= 0);
821 return qmul(x, cordic_circular_inverse_scaling);
822 }
823
824 q_t qatanh(q_t x) {
825 assert(qabs(qless(x, QINT(1))));
826 return qmul(qlog(qdiv(qadd(QINT(1), x), qsub(QINT(1), x))), QMK(0, 0x8000, 16));
827 }
828
829 q_t qasinh(q_t x) {
830 return qlog(qadd(x, qsqrt(qadd(qmul(x, x), QINT(1)))));
831 }
832
833 q_t qacosh(q_t x) {
834 assert(qeqmore(x, QINT(1)));
835 return qlog(qadd(x, qsqrt(qsub(qmul(x, x), QINT(1)))));
836 }
837
838 void qpol2rec(const q_t magnitude, const q_t theta, q_t *i, q_t *j) {
839 assert(i);
840 assert(j);
841 q_t sin = QINT(0), cos = QINT(0);
842 qsincos(theta, &sin, &cos);
843 *i = qmul(sin, magnitude);
844 *j = qmul(cos, magnitude);
845 }
846
847 void qrec2pol(const q_t i, const q_t j, q_t *magnitude, q_t *theta) {
848 assert(magnitude);
849 assert(theta);
850 const int is = qisnegative(i), js = qisnegative(j);
851 q_t x = qabs(i), y = qabs(j), z = QINT(0);
852 const int r = cordic(CORDIC_COORD_CIRCULAR_E, CORDIC_MODE_VECTOR_E, -1, &x, &y, &z);
853 assert(r >= 0);
854 *magnitude = qmul(x, cordic_circular_inverse_scaling);
855 if (is && js)
856 z = qadd(z, QPI);
857 else if (js)
858 z = qadd(z, QPI/2l);
859 else if (is)
860 z = qadd(z, (3l*QPI)/2l);
861 *theta = z;
862 }
863
864 q_t qcordic_hyperbolic_gain(const int n) {
865 q_t x = QINT(1), y = QINT(0), z = QINT(0);
866 const int r = cordic(CORDIC_COORD_HYPERBOLIC_E, CORDIC_MODE_ROTATE_E, n, &x, &y, &z);
867 assert(r >= 0);
868 return x;
869 }
870
871 q_t qcordic_circular_gain(const int n) {
872 q_t x = QINT(1), y = QINT(0), z = QINT(0);
873 const int r = cordic(CORDIC_COORD_CIRCULAR_E, CORDIC_MODE_ROTATE_E, n, &x, &y, &z);
874 assert(r >= 0);
875 return x;
876 }
877
878 static inline int isodd(const unsigned n) {
879 return n & 1;
880 }
881
882 d_t dpower(d_t b, unsigned e) { /* https://stackoverflow.com/questions/101439 */
883 d_t result = 1;
884 for (;;) {
885 if (isodd(e))
886 result *= b;
887 e >>= 1;
888 if (!e)
889 break;
890 b *= b;
891 }
892 return result;
893 }
894
895 d_t dlog(d_t x, const unsigned base) { /* rounds up, look at remainder to round down */
896 d_t b = 0;
897 assert(x && base > 1);
898 while ((x /= (d_t)base)) /* can use >> for base that are powers of two */
899 b++;
900 return b;
901 }
902
903 q_t qlog(q_t x) {
904 q_t logs = 0;
905 assert(qmore(x, 0));
906 static const q_t lmax = QMK(9, 0x8000, 16); /* 9.5, lower limit needs checking */
907 for (; qmore(x, lmax); x = divn(x, 1))
908 logs = qadd(logs, qinfo.ln2);
909 return qadd(logs, qcordic_ln(x));
910 }
911
912 q_t qsqr(const q_t x) {
913 return qmul(x, x);
914 }
915
916 q_t qexp(const q_t e) { /* exp(e) = exp(e/2)*exp(e/2) */
917 if (qless(e, QINT(1))) /* 1.1268 is approximately the limit for qcordic_exp */
918 return qcordic_exp(e);
919 return qsqr(qexp(divn(e, 1)));
920 }
921
922 q_t qpow(q_t n, q_t exp) {
923 implies(qisnegative(n), qisinteger(exp));
924 implies(qequal(n, QINT(0)), qunequal(exp, QINT(0)));
925 if (qequal(QINT(0), n))
926 return QINT(1);
927 if (qisnegative(n)) {
928 const q_t abspow = qpow(qabs(n), exp);
929 return qisodd(exp) ? qnegate(abspow) : abspow;
930 }
931 if (qisnegative(exp))
932 return qdiv(QINT(1), qpow(n, qabs(exp)));
933 return qexp(multiply(qlog(n), exp));
934 }
935
936 q_t qsqrt(const q_t x) { /* Newton-Rhaphson method */
937 assert(qeqmore(x, 0));
938 const q_t difference = qmore(x, QINT(100)) ? 0x0100 : 0x0010;
939 if (qequal(QINT(0), x))
940 return QINT(0);
941 q_t guess = qmore(x, qinfo.sqrt2) ? divn(x, 1) : QINT(1);
942 while (qmore(qabs(qsub(qmul(guess, guess), x)), difference))
943 guess = divn(qadd(qdiv(x, guess), guess), 1);
944 return qabs(guess); /* correct for overflow int very large numbers */
945 }
946
947 q_t qasin(const q_t t) {
948 assert(qless(qabs(t), QINT(1)));
949 /* can also use: return qatan(qdiv(t, qsqrt(qsub(QINT(1), qmul(t, t))))); */
950 return qatan2(t, qsqrt(qsub(QINT(1), qmul(t, t))));
951 }
952
953 q_t qacos(const q_t t) {
954 assert(qeqless(qabs(t), QINT(1)));
955 /* can also use: return qatan(qdiv(qsqrt(qsub(QINT(1), qmul(t, t))), t)); */
956 return qatan2(qsqrt(qsub(QINT(1), qmul(t, t))), t);
957 }
958
959 q_t qdeg2rad(const q_t deg) {
960 return qdiv(qmul(QPI, deg), QINT(180));
961 }
962
963 q_t qrad2deg(const q_t rad) {
964 return qdiv(qmul(QINT(180), rad), QPI);
965 }
966
967 void qfilter_init(qfilter_t *f, const q_t time, const q_t rc, const q_t seed) {
968 assert(f);
969 memset(f, 0, sizeof(*f));
970 f->time = time;
971 f->rc = rc;
972 f->filtered = seed; /* alpha * seed for LPF */
973 f->raw = seed;
974 }
975
976 q_t qfilter_low_pass(qfilter_t *f, const q_t time, const q_t data) {
977 assert(f);
978 /* If the calling rate is constant (for example the function is
979 * guaranteed to be always called at a rate of 5 milliseconds) we
980 * can avoid the costly alpha calculation! */
981 const q_t dt = (u_t)time - (u_t)f->time;
982 const q_t alpha = qdiv(dt, qadd(f->rc, dt));
983 f->filtered = qfma(alpha, qsub(data, f->filtered), f->filtered);
984 f->time = time;
985 f->raw = data;
986 return f->filtered;
987 }
988
989 q_t qfilter_high_pass(qfilter_t *f, const q_t time, const q_t data) {
990 assert(f);
991 const q_t dt = (u_t)time - (u_t)f->time;
992 const q_t alpha = qdiv(f->rc, qadd(f->rc, dt));
993 f->filtered = qmul(alpha, qadd(f->filtered, qsub(data, f->raw)));
994 f->time = time;
995 f->raw = data;
996 return f->filtered;
997 }
998
999 q_t qfilter_value(const qfilter_t *f) {
1000 assert(f);
1001 return f->filtered;
1002 }
1003
1004 /* Must be called at a constant rate; perhaps a PID which takes call time
1005 * into account could be made, but that would complicate things. Differentiator
1006 * term needs filtering also. It would be nice to create a version that took
1007 * into account the time delta, see
1008 * <https://www.quora.com/Do-I-need-to-sample-at-a-constant-rate-for-PID-control-or-is-it-sufficient-to-know-the-time-at-which-my-sample-was-taken-even-if-the-increment-varies>
1009 * */
1010 q_t qpid_update(qpid_t *pid, const q_t error, const q_t position) {
1011 assert(pid);
1012 const q_t p = qmul(pid->p_gain, error);
1013 pid->i_state = qadd(pid->i_state, error);
1014 pid->i_state = qmax(pid->i_state, pid->i_min);
1015 pid->i_state = qmin(pid->i_state, pid->i_max);
1016 const q_t i = qmul(pid->i_state, pid->i_gain);
1017 const q_t d = qmul(pid->d_gain, qsub(position, pid->d_state));
1018 pid->d_state = position;
1019 return qsub(qadd(p, i), d);
1020 }
1021
1022 /* Simpsons method for numerical integration, from "Math Toolkit for
1023 * Real-Time Programming" by Jack Crenshaw */
1024 q_t qsimpson(q_t (*f)(q_t), const q_t x1, const q_t x2, const unsigned n) {
1025 assert(f);
1026 assert((n & 1) == 0);
1027 const q_t h = qdiv(qsub(x2, x1), QINT(n));
1028 q_t sum = 0, x = x1;
1029 for (unsigned i = 0; i < (n / 2u); i++){
1030 sum = qadd(sum, qadd(f(x), qmul(QINT(2), f(qadd(x,h)))));
1031 x = qadd(x, qmul(QINT(2), h));
1032 }
1033 sum = qsub(qmul(QINT(2), sum), qadd(f(x1), f(x2)));
1034 return qdiv(qmul(h, sum), QINT(3));
1035 }
1036
1037 /* The matrix meta-data field is not used at the moment, but could be
1038 * used for things like versioning, determining whether the matrix is
1039 * all zeros, or is the identify matrix, whether it contains valid data,
1040 * and more. Some common matrix operations are missing, such as factorization
1041 *
1042 * A function for image kernels might be useful. */
1043
1044 enum { METADATA, LENGTH, ROW, COLUMN, DATA, };
1045
1046 int qmatrix_is_valid(const q_t *m) {
1047 const size_t size = m[LENGTH], row = m[ROW], column = m[COLUMN];
1048 const size_t elements = row * column;
1049 if (elements < row || elements < column) /* overflow */
1050 return 0;
1051 if (elements > size)
1052 return 0;
1053 return 1;
1054 }
1055
1056 int qmatrix_resize(q_t *m, const size_t row, const size_t column) {
1057 const size_t rc = row * column;
1058 const size_t sz = m[LENGTH];
1059 if ((row && column) && (rc < row || rc < column)) /* overflow */
1060 return -1;
1061 if (rc > sz)
1062 return -1;
1063 m[ROW] = row;
1064 m[COLUMN] = column;
1065 return 0;
1066 }
1067
1068 int qmatrix_apply_unary(q_t *r, const q_t *a, q_t (*func)(q_t)) {
1069 assert(r);
1070 assert(qmatrix_is_valid(r));
1071 assert(a);
1072 assert(qmatrix_is_valid(a));
1073 assert(func);
1074 const q_t *ma = &a[DATA];
1075 q_t *mr = &r[DATA];
1076 const size_t arows = a[ROW], acolumns = a[COLUMN];
1077 if (qmatrix_resize(r, arows, acolumns) < 0)
1078 return -1;
1079 for (size_t i = 0; i < arows; i++)
1080 for (size_t j = 0; j < acolumns; j++)
1081 mr[i*acolumns + j] = func(ma[i*acolumns + j]);
1082 return 0;
1083 }
1084
1085 int qmatrix_apply_scalar(q_t *r, const q_t *a, q_t (*func)(q_t, q_t), const q_t c) {
1086 assert(r);
1087 assert(qmatrix_is_valid(r));
1088 assert(a);
1089 assert(qmatrix_is_valid(a));
1090 assert(func);
1091 const q_t *ma = &a[DATA];
1092 q_t *mr = &r[DATA];
1093 const size_t arows = a[ROW], acolumns = a[COLUMN];
1094 if (qmatrix_resize(r, arows, acolumns) < 0)
1095 return -1;
1096 for (size_t i = 0; i < arows; i++)
1097 for (size_t j = 0; j < acolumns; j++)
1098 mr[i*acolumns + j] = func(ma[i*acolumns + j], c);
1099 return 0;
1100 }
1101
1102 int qmatrix_apply_binary(q_t *r, const q_t *a, const q_t *b, q_t (*func)(q_t, q_t)) {
1103 assert(a);
1104 assert(qmatrix_is_valid(a));
1105 assert(b);
1106 assert(qmatrix_is_valid(b));
1107 assert(r);
1108 assert(qmatrix_is_valid(r));
1109 assert(func);
1110 const q_t *ma = &a[DATA], *mb = &b[DATA];
1111 q_t *mr = &r[DATA];
1112 const size_t arows = a[ROW], acolumns = a[COLUMN];
1113 const size_t brows = b[ROW], bcolumns = b[COLUMN];
1114 const size_t rrows = r[ROW], rcolumns = r[COLUMN];
1115 if (arows != brows || acolumns != bcolumns)
1116 return -1;
1117 if (arows != rrows || acolumns != rcolumns)
1118 return -1;
1119 for (size_t i = 0; i < arows; i++)
1120 for (size_t j = 0; j < acolumns; j++) {
1121 const size_t idx = (i*acolumns) + j;
1122 mr[idx] = func(ma[idx], mb[idx]);
1123 }
1124 return 0;
1125 }
1126
1127 static q_t qfz(q_t a) { UNUSED(a); return QINT(0); }
1128 static q_t qf1(q_t a) { UNUSED(a); return QINT(1); }
1129
1130 int qmatrix_zero(q_t *r) { return qmatrix_apply_unary(r, r, qfz); }
1131 int qmatrix_one(q_t *r) { return qmatrix_apply_unary(r, r, qf1); }
1132 int qmatrix_logical(q_t *r, const q_t *a) { return qmatrix_apply_unary(r, a, qlogical); }
1133 int qmatrix_not(q_t *r, const q_t *a) { return qmatrix_apply_unary(r, a, qnot); }
1134 int qmatrix_signum(q_t *r, const q_t *a) { return qmatrix_apply_unary(r, a, qsignum); }
1135 int qmatrix_invert(q_t *r, const q_t *a) { return qmatrix_apply_unary(r, a, qinvert); }
1136 int qmatrix_add(q_t *r, const q_t *a, const q_t *b) { return qmatrix_apply_binary(r, a, b, qadd); }
1137 int qmatrix_sub(q_t *r, const q_t *a, const q_t *b) { return qmatrix_apply_binary(r, a, b, qsub); }
1138 int qmatrix_and(q_t *r, const q_t *a, const q_t *b) { return qmatrix_apply_binary(r, a, b, qand); }
1139 int qmatrix_or (q_t *r, const q_t *a, const q_t *b) { return qmatrix_apply_binary(r, a, b, qor); }
1140 int qmatrix_xor(q_t *r, const q_t *a, const q_t *b) { return qmatrix_apply_binary(r, a, b, qxor); }
1141
1142 int qmatrix_scalar_add(q_t *r, const q_t *a, const q_t scalar) { return qmatrix_apply_scalar(r, a, qadd, scalar); }
1143 int qmatrix_scalar_sub(q_t *r, const q_t *a, const q_t scalar) { return qmatrix_apply_scalar(r, a, qsub, scalar); }
1144 int qmatrix_scalar_mul(q_t *r, const q_t *a, const q_t scalar) { return qmatrix_apply_scalar(r, a, qmul, scalar); }
1145 int qmatrix_scalar_div(q_t *r, const q_t *a, const q_t scalar) { return qmatrix_apply_scalar(r, a, qdiv, scalar); }
1146 int qmatrix_scalar_mod(q_t *r, const q_t *a, const q_t scalar) { return qmatrix_apply_scalar(r, a, qmod, scalar); }
1147 int qmatrix_scalar_rem(q_t *r, const q_t *a, const q_t scalar) { return qmatrix_apply_scalar(r, a, qrem, scalar); }
1148 int qmatrix_scalar_and(q_t *r, const q_t *a, const q_t scalar) { return qmatrix_apply_scalar(r, a, qand, scalar); }
1149 int qmatrix_scalar_or (q_t *r, const q_t *a, const q_t scalar) { return qmatrix_apply_scalar(r, a, qor, scalar); }
1150 int qmatrix_scalar_xor(q_t *r, const q_t *a, const q_t scalar) { return qmatrix_apply_scalar(r, a, qxor, scalar); }
1151
1152 int qmatrix_is_square(const q_t *m) {
1153 assert(m);
1154 assert(qmatrix_is_valid(m));
1155 return m[COLUMN] == m[ROW];
1156 }
1157
1158 int qmatrix_identity(q_t *r) {
1159 assert(r);
1160 assert(qmatrix_is_valid(r));
1161 if (!qmatrix_is_square(r))
1162 return -1;
1163 q_t *mr = &r[DATA];
1164 const size_t length = r[ROW];
1165 for (size_t i = 0; i < length; i++)
1166 for (size_t j = 0; j < length; j++)
1167 mr[i*length + j] = i == j ? QINT(1) : QINT(0);
1168 return 0;
1169 }
1170
1171 int qmatrix_copy(q_t *r, const q_t *a) {
1172 assert(r);
1173 assert(qmatrix_is_valid(r));
1174 assert(a);
1175 assert(qmatrix_is_valid(a));
1176 const size_t arows = a[ROW], acolumns = a[COLUMN];
1177 const size_t copy = arows * acolumns * sizeof (q_t);
1178 if ((arows && acolumns) && (copy < arows || copy < acolumns))
1179 return -1;
1180 if (qmatrix_resize(r, arows, acolumns) < 0)
1181 return -1;
1182 memcpy(&r[DATA], &a[DATA], copy);
1183 return 0;
1184 }
1185
1186 q_t qmatrix_trace(const q_t *m) {
1187 assert(m);
1188 assert(qmatrix_is_square(m));
1189 const size_t length = m[ROW];
1190 const q_t *mm = &m[DATA];
1191 q_t tr = QINT(0);
1192 for (size_t i = 0; i < length; i++)
1193 for (size_t j = 0; j < length; j++)
1194 if (i == j)
1195 tr = qadd(tr, mm[i*length + j]);
1196 return tr;
1197 }
1198
1199 q_t qmatrix_equal(const q_t *a, const q_t *b) {
1200 assert(a);
1201 assert(qmatrix_is_valid(a));
1202 assert(b);
1203 assert(qmatrix_is_valid(b));
1204 const size_t arow = a[ROW], acolumn = a[COLUMN];
1205 const size_t brow = b[ROW], bcolumn = b[COLUMN];
1206 const q_t *ma = &a[DATA];
1207 const q_t *mb = &a[DATA];
1208 if (a == b)
1209 return QINT(1);
1210 if (arow != brow && acolumn != bcolumn)
1211 return QINT(0);
1212 return !memcmp(ma, mb, sizeof(q_t) * arow * brow);
1213 }
1214
1215 static q_t determine(const q_t *m, const size_t length) {
1216 assert(m);
1217 if (length == 1)
1218 return m[0];
1219 if (length == 2)
1220 return qsub(qmul(m[0], m[3]), qmul(m[1], m[2]));
1221 size_t co1 = 0, co2 = 0;
1222 q_t det = QINT(0), sgn = QINT(1);
1223 q_t co[length*length]; /* This should really be passed in */
1224 for (size_t i = 0; i < length; i++) {
1225 for (size_t j = 0; j < length; j++)
1226 for (size_t k = 0; k < length; k++)
1227 if (j && k != i) {
1228 co[co1*length + co2] = m[j*length + k];
1229 if (++co2 > (length - 2)) {
1230 co1++;
1231 co2 = 0;
1232 }
1233 }
1234 det = qadd(det, qcopysign(qmul(m[(0*length) + i], determine(co, length - 1)), sgn));
1235 sgn = qnegate(sgn);
1236 }
1237 return det;
1238 }
1239
1240 q_t qmatrix_determinant(const q_t *m) {
1241 assert(m);
1242 assert(qmatrix_is_square(m));
1243 assert(m[ROW] < 16);
1244 const size_t length = m[ROW];
1245 const q_t *mm = &m[DATA];
1246 return determine(mm, length);
1247 }
1248
1249 int qmatrix_transpose(q_t *r, const q_t *m) {
1250 assert(r);
1251 assert(qmatrix_is_valid(r));
1252 assert(m);
1253 assert(qmatrix_is_valid(m));
1254 q_t *mr = &r[DATA];
1255 const q_t *mm = &m[DATA];
1256 const size_t mrows = m[ROW], mcolumns = m[COLUMN];
1257 const size_t msize = mrows * mcolumns;
1258 const size_t rsize = r[LENGTH];
1259 if (msize > rsize)
1260 return -1;
1261 for (size_t i = 0; i < mrows; i++)
1262 for (size_t j = 0; j < mcolumns; j++)
1263 mr[i*mcolumns + j] = mm[j*mcolumns + i];
1264 r[ROW] = mcolumns;
1265 r[COLUMN] = mrows;
1266 return 0;
1267 }
1268
1269 int qmatrix_mul(q_t *r, const q_t *a, const q_t *b) {
1270 assert(a);
1271 assert(qmatrix_is_valid(a));
1272 assert(b);
1273 assert(qmatrix_is_valid(b));
1274 assert(r);
1275 assert(qmatrix_is_valid(r));
1276 q_t *mr = &r[DATA];
1277 const q_t *ma = &a[DATA], *mb = &b[DATA];
1278 const size_t arows = a[ROW], acolumns = a[COLUMN];
1279 const size_t brows = b[ROW], bcolumns = b[COLUMN];
1280 if (acolumns != brows)
1281 return -1;
1282 if (qmatrix_resize(r, arows, bcolumns) < 0)
1283 return -1;
1284 for (size_t i = 0; i < arows; i++)
1285 for (size_t j = 0; j < bcolumns; j++) {
1286 q_t s = QINT(0);
1287 for (size_t k = 0; k < brows; k++)
1288 s = qadd(s, qmul(ma[i*acolumns + k], mb[k*bcolumns + j]));
1289 mr[i*arows + j] = s;
1290 }
1291 return 0;
1292 }
1293
1294 static int addchar(char **str, size_t *length, const int ch) {
1295 assert(str && *str);
1296 assert(length);
1297 if (!length)
1298 return -1;
1299 char *s = *str;
1300 *s++ = ch;
1301 *str = s;
1302 *length -= 1;
1303 return 0;
1304 }
1305
1306 static int addstr(char **str, size_t *length, char *addme) {
1307 assert(str && *str);
1308 assert(length);
1309 assert(addme);
1310 const size_t sz = strlen(addme);
1311 for (size_t i = 0; i < sz; i++)
1312 if (addchar(str, length, addme[i]) < 0)
1313 return -1;
1314 return 0;
1315 }
1316
1317 int qmatrix_sprintb(const q_t *m, char *str, size_t length, unsigned base) {
1318 assert(str);
1319 assert(m);
1320 const q_t *mm = &m[DATA];
1321 const size_t rows = m[ROW], columns = m[COLUMN];
1322 if (base < 2 || base > 36)
1323 return -1;
1324 if (!qmatrix_is_valid(m))
1325 return addstr(&str, &length, "[ INVALID ]");
1326 if (addstr(&str, &length, "[ ") < 0)
1327 return -1;
1328 for (size_t i = 0; i < rows; i++) {
1329 for (size_t j = 0; j < columns; j++) {
1330 const int r = qsprintb(mm[i*columns + j], str, length, base);
1331 if (r < 0)
1332 return -1;
1333 if ((length - r) > length)
1334 return -1;
1335 length -= r;
1336 str += r;
1337 if (rows)
1338 if (addchar(&str, &length, columns && j < (columns - 1) ? ',' : i < rows - 1 ? ';' : ' ') < 0)
1339 return -1;
1340 if ((columns && j < (columns - 1)) || (i < (rows - 1)))
1341 if (addchar(&str, &length, ' ') < 0)
1342 return -1;
1343 }
1344 }
1345 if (addchar(&str, &length, ']') < 0)
1346 return -1;
1347 return 0;
1348 }
1349
1350 size_t qmatrix_string_length(const q_t *m) {
1351 assert(m);
1352 if (!qmatrix_is_valid(m))
1353 return 128; /* space for invalid matrix message */
1354 const size_t msize = m[LENGTH];
1355 const size_t r = (msize *
1356 (32 /*max length if base 2 used)*/
1357 + 2 /* '-' and '.' */
1358 + 2 /* space and comma/semi colon separator */
1359 )) + 16 /* space for extra formatting */;
1360 return r;
1361 }
1362
1363 /* See <https://github.com/jamesbowman/sincos>
1364 * and "Math Toolkit for Real-Time Programming" by Jack Crenshaw
1365 *
1366 * The naming of these functions ('furman_') is incorrect, they do their
1367 * computation on numbers represented in Furmans but they do not use a 'Furman
1368 * algorithm'. As I do not have a better name, the name shall stick. */
1369 static int16_t _sine(const int16_t y) {
1370 const int16_t s1 = 0x6487, s3 = -0x2953, s5 = 0x04f8;
1371 const int16_t z = arshift((int32_t)y * y, 12);
1372 int16_t prod = arshift((int32_t)z * s5, 16);
1373 int16_t sum = s3 + prod;
1374 prod = arshift((int32_t)z * sum, 16);
1375 sum = s1 + prod;
1376 return arshift((int32_t)y * sum, 13);
1377 }
1378
1379 static int16_t _cosine(int16_t y) {
1380 const int16_t c0 = 0x7fff, c2 = -0x4ee9, c4 = 0x0fbd;
1381 const int16_t z = arshift((int32_t)y * y, 12);
1382 int16_t prod = arshift((int32_t)z * c4, 16);
1383 const int16_t sum = c2 + prod;
1384 prod = arshift((int32_t)z * sum, 15);
1385 return c0 + prod;
1386 }
1387
1388 int16_t furman_sin(int16_t x) {
1389 const int16_t n = 3 & arshift(x + 0x2000, 14);
1390 x -= n << 14;
1391 const int16_t r = (n & 1) ? _cosine(x) : _sine(x);
1392 return (n & 2) ? -r : r;
1393 }
1394
1395 int16_t furman_cos(int16_t x) {
1396 return furman_sin(x + 0x4000);
1397 }
1398
1399 /* expression evaluator */
1400
1401 enum { ASSOCIATE_NONE, ASSOCIATE_LEFT, ASSOCIATE_RIGHT, };
1402 enum { LEX_NUMBER, LEX_OPERATOR, LEX_END, };
1403
1404 int qexpr_init(qexpr_t *e) {
1405 assert(e);
1406 e->lpar = qop("(");
1407 e->rpar = qop(")");
1408 e->negate = qop("negate");
1409 e->minus = qop("-");
1410 e->initialized = 1;
1411 assert(e->lpar && e->rpar && e->negate && e->minus);
1412 return 0;
1413 }
1414
1415 static int error(qexpr_t *e, const char *fmt, ...) {
1416 assert(e);
1417 assert(fmt);
1418 if (e->error)
1419 return 0;
1420 va_list ap;
1421 va_start(ap, fmt);
1422 (void)vsnprintf(e->error_string, sizeof (e->error_string), fmt, ap);
1423 va_end(ap);
1424 e->error = -1;
1425 return -QINT(1);
1426 }
1427
1428 static q_t numberify(const char *s) {
1429 assert(s);
1430 q_t q = 0;
1431 (void) qconv(&q, s);
1432 return q;
1433 }
1434
1435 static q_t qbase(q_t b) {
1436 int nb = qtoi(b);
1437 if (nb < 2 || nb > 36)
1438 return -QINT(1);
1439 qconf.base = nb;
1440 return b;
1441 }
1442
1443 static q_t qplaces(q_t places) {
1444 /* TODO: Bounds checks given base */
1445 qconf.dp = qtoi(places);
1446 return places;
1447 }
1448
1449 static q_t check_div0(qexpr_t *e, q_t a, q_t b) {
1450 assert(e);
1451 UNUSED(a);
1452 if (!b)
1453 return error(e, "division by zero");
1454 return QINT(0);
1455 }
1456
1457 static q_t check_nlz(qexpr_t *e, q_t a) { // Not Less Zero
1458 assert(e);
1459 if (qless(a, QINT(0)))
1460 return error(e, "negative argument");
1461 return QINT(0);
1462 }
1463
1464 static q_t check_nlez(qexpr_t *e, q_t a) { // Not Less Equal Zero
1465 assert(e);
1466 if (qeqless(a, QINT(0)))
1467 return error(e, "negative or zero argument");
1468 return QINT(0);
1469 }
1470
1471 static q_t check_nlo(qexpr_t *e, q_t a) { // Not less than one
1472 assert(e);
1473 if (qless(a, QINT(1)))
1474 return error(e, "out of range [1, INF]");
1475 return QINT(0);
1476 }
1477
1478 static q_t check_alo(qexpr_t *e, q_t a) {
1479 assert(e);
1480 if (qmore(qabs(a), QINT(1)))
1481 return error(e, "out of range [-1, 1]");
1482 return QINT(0);
1483 }
1484
1485 const qoperations_t *qop(const char *op) {
1486 assert(op);
1487 static const qoperations_t ops[] = {
1488 /* Binary Search Table: Use 'LC_ALL="C" sort -k 2 < table' to sort this */
1489 /* name function check function precedence arity left/right-assoc hidden */
1490 { "!", .eval.unary = qnot, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
1491 { "!=", .eval.binary = qunequal, .check.binary = NULL, 2, 2, ASSOCIATE_LEFT, 0, },
1492 { "%", .eval.binary = qrem,/*!*/ .check.binary = check_div0, 3, 2, ASSOCIATE_LEFT, 0, },
1493 { "&", .eval.binary = qand, .check.binary = NULL, 2, 2, ASSOCIATE_LEFT, 0, },
1494 { "(", .eval.unary = NULL, .check.unary = NULL, 0, 0, ASSOCIATE_NONE, 0, },
1495 { ")", .eval.unary = NULL, .check.unary = NULL, 0, 0, ASSOCIATE_NONE, 0, },
1496 { "*", .eval.binary = qmul, .check.binary = NULL, 3, 2, ASSOCIATE_LEFT, 0, },
1497 { "+", .eval.binary = qadd, .check.binary = NULL, 2, 2, ASSOCIATE_LEFT, 0, },
1498 { "-", .eval.binary = qsub, .check.binary = NULL, 2, 2, ASSOCIATE_LEFT, 0, },
1499 { "/", .eval.binary = qdiv, .check.binary = check_div0, 3, 2, ASSOCIATE_LEFT, 0, },
1500 { "<", .eval.binary = qless, .check.binary = NULL, 2, 2, ASSOCIATE_LEFT, 0, },
1501 { "<<", .eval.binary = qlls, .check.binary = NULL, 4, 2, ASSOCIATE_RIGHT, 0, },
1502 { "<=", .eval.binary = qeqless, .check.binary = NULL, 2, 2, ASSOCIATE_LEFT, 0, },
1503 { "==", .eval.binary = qequal, .check.binary = NULL, 2, 2, ASSOCIATE_LEFT, 0, },
1504 { ">", .eval.binary = qmore, .check.binary = NULL, 2, 2, ASSOCIATE_LEFT, 0, },
1505 { ">=", .eval.binary = qeqmore, .check.binary = NULL, 2, 2, ASSOCIATE_LEFT, 0, },
1506 { ">>", .eval.binary = qlrs, .check.binary = NULL, 4, 2, ASSOCIATE_RIGHT, 0, },
1507 { "^", .eval.binary = qxor, .check.binary = NULL, 2, 2, ASSOCIATE_LEFT, 0, },
1508 { "_div", .eval.binary = qcordic_div, .check.binary = NULL, 5, 2, ASSOCIATE_RIGHT, 1, },
1509 { "_exp", .eval.unary = qcordic_exp, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 1, },
1510 { "_ln", .eval.unary = qcordic_ln, .check.unary = check_nlez, 5, 1, ASSOCIATE_RIGHT, 1, },
1511 { "_mul", .eval.binary = qcordic_mul, .check.binary = NULL, 5, 2, ASSOCIATE_RIGHT, 1, },
1512 { "_sqrt", .eval.unary = qcordic_sqrt, .check.unary = check_nlz, 5, 1, ASSOCIATE_RIGHT, 1, },
1513 { "abs", .eval.unary = qabs, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
1514 { "acos", .eval.unary = qacos, .check.unary = check_alo, 5, 1, ASSOCIATE_RIGHT, 0, },
1515 { "acosh", .eval.unary = qacosh, .check.unary = check_nlo, 5, 1, ASSOCIATE_RIGHT, 0, },
1516 { "arshift", .eval.binary = qars, .check.binary = NULL, 4, 2, ASSOCIATE_RIGHT, 1, },
1517 { "asin", .eval.unary = qasin, .check.unary = check_alo, 5, 1, ASSOCIATE_RIGHT, 0, },
1518 { "asinh", .eval.unary = qasinh, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
1519 { "atan", .eval.unary = qatan, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
1520 { "atan2", .eval.binary = qatan2, .check.binary = NULL, 5, 2, ASSOCIATE_RIGHT, 1, },
1521 { "atanh", .eval.unary = qatanh, .check.unary = check_alo, 5, 1, ASSOCIATE_RIGHT, 0, },
1522 { "base", .eval.unary = qbase, .check.unary = NULL, 2, 1, ASSOCIATE_RIGHT, 0, },
1523 { "ceil", .eval.unary = qceil, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
1524 { "copysign", .eval.binary = qcopysign, .check.binary = NULL, 4, 2, ASSOCIATE_RIGHT, 1, },
1525 { "cos", .eval.unary = qcos, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
1526 { "cosh", .eval.unary = qcosh, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
1527 { "cot", .eval.unary = qcot, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
1528 { "deg2rad", .eval.unary = qdeg2rad, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
1529 { "even?", .eval.unary = qiseven, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
1530 { "exp", .eval.unary = qexp, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
1531 { "floor", .eval.unary = qfloor, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
1532 { "hypot", .eval.binary = qhypot, .check.binary = NULL, 5, 2, ASSOCIATE_RIGHT, 0, },
1533 { "int?", .eval.unary = qisinteger, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
1534 { "log", .eval.unary = qlog, .check.unary = check_nlez, 5, 1, ASSOCIATE_RIGHT, 0, },
1535 { "lshift", .eval.binary = qlls, .check.binary = NULL, 4, 2, ASSOCIATE_RIGHT, 1, },
1536 { "max", .eval.binary = qmax, .check.binary = NULL, 5, 2, ASSOCIATE_RIGHT, 1, },
1537 { "min", .eval.binary = qmin, .check.binary = NULL, 5, 2, ASSOCIATE_RIGHT, 1, },
1538 { "mod", .eval.binary = qmod, .check.binary = check_div0, 3, 2, ASSOCIATE_LEFT, 0, },
1539 { "neg?", .eval.unary = qisnegative, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
1540 { "negate", .eval.unary = qnegate, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
1541 { "odd?", .eval.unary = qisodd, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
1542 { "places", .eval.unary = qplaces, .check.unary = NULL, 2, 1, ASSOCIATE_RIGHT, 0, },
1543 { "pos?", .eval.unary = qispositive, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
1544 { "pow", .eval.binary = qpow, .check.binary = NULL, 5, 2, ASSOCIATE_RIGHT, 0, },
1545 { "rad2deg", .eval.unary = qrad2deg, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
1546 { "rem", .eval.binary = qrem, .check.binary = check_div0, 3, 2, ASSOCIATE_LEFT, 0, },
1547 { "round", .eval.unary = qround, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
1548 { "rshift", .eval.binary = qlrs, .check.binary = NULL, 4, 2, ASSOCIATE_RIGHT, 1, },
1549 { "sign", .eval.unary = qsign, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
1550 { "signum", .eval.unary = qsignum, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
1551 { "sin", .eval.unary = qsin, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
1552 { "sinh", .eval.unary = qsinh, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
1553 { "sqrt", .eval.unary = qsqrt, .check.unary = check_nlz, 5, 1, ASSOCIATE_RIGHT, 0, },
1554 { "tan", .eval.unary = qtan, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
1555 { "tanh", .eval.unary = qtanh, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
1556 { "trunc", .eval.unary = qtrunc, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
1557 { "|", .eval.binary = qor, .check.binary = NULL, 2, 2, ASSOCIATE_LEFT, 0, },
1558 { "~", .eval.unary = qinvert, .check.unary = NULL, 5, 1, ASSOCIATE_RIGHT, 0, },
1559 };
1560 const size_t length = (sizeof ops / sizeof ops[0]);
1561 size_t l = 0, r = length - 1;
1562 while (l <= r) { // Iterative Binary Search
1563 size_t m = l + ((r - l)/2u);
1564 assert (m < length);
1565 const int comp = strcmp(ops[m].name, op);
1566 if (comp == 0)
1567 return &ops[m];
1568 if (comp < 0)
1569 l = m + 1;
1570 else
1571 r = m - 1;
1572 }
1573 return NULL;
1574 }
1575
1576 static int number_push(qexpr_t *e, q_t num) {
1577 assert(e);
1578 if (e->error)
1579 return -1;
1580 if (e->numbers_count > (e->numbers_max - 1)) {
1581 error(e, "number stack overflow");
1582 return -1;
1583 }
1584 e->numbers[e->numbers_count++] = num;
1585 return 0;
1586 }
1587
1588 static q_t number_pop(qexpr_t *e) {
1589 assert(e);
1590 if (e->error)
1591 return -1;
1592 if (!(e->numbers_count)) {
1593 error(e, "number stack empty");
1594 return -1; /* error handled elsewhere */
1595 }
1596 return e->numbers[--(e->numbers_count)];
1597 }
1598
1599 static int op_push(qexpr_t *e, const qoperations_t *op) {
1600 assert(e);
1601 assert(op);
1602 if (e->error)
1603 return -1;
1604 if (e->ops_count > (e->ops_max - 1)) {
1605 error(e, "operator stack overflow");
1606 return -1;
1607 }
1608 e->ops[e->ops_count++] = op;
1609 return 0;
1610 }
1611
1612 int qexpr_error(qexpr_t *e) {
1613 assert(e);
1614 assert(e->initialized);
1615 return e->error;
1616 }
1617
1618 q_t qexpr_result(qexpr_t *e) {
1619 assert(e);
1620 assert(e->initialized);
1621 assert(e->error == 0);
1622 assert(e->numbers_count == 1);
1623 return e->numbers[0];
1624 }
1625
1626 static const qoperations_t *op_pop(qexpr_t *e) {
1627 assert(e);
1628 if (e->error)
1629 return NULL;
1630 if (!(e->ops_count)) {
1631 error(e, "operator stack empty");
1632 return NULL;
1633 }
1634 return e->ops[--(e->ops_count)];
1635 }
1636
1637 static int op_eval(qexpr_t *e) {
1638 assert(e);
1639 const qoperations_t *pop = op_pop(e);
1640 if (!pop)
1641 return -1;
1642 const q_t a = number_pop(e);
1643 const int exists = pop->arity == 1 ? BOOLIFY(pop->eval.unary) : BOOLIFY(pop->eval.binary);
1644 if (!exists) {
1645 error(e, "syntax error");
1646 return -1;
1647 }
1648 if (pop->arity == 1) {
1649 if (pop->check.unary && pop->check.unary(e, a) < 0) {
1650 error(e, "unary check failed");
1651 return -1;
1652 }
1653 return number_push(e, pop->eval.unary(a));
1654 }
1655 const q_t b = number_pop(e);
1656 if (pop->check.binary && pop->check.binary(e, b, a)) {
1657 error(e, "binary check failed");
1658 return -1;
1659 }
1660
1661 return number_push(e, pop->eval.binary(b, a));
1662 }
1663
1664 static int shunt(qexpr_t *e, const qoperations_t *op) {
1665 assert(e);
1666 assert(op);
1667 if (op == e->lpar) {
1668 return op_push(e, op);
1669 } else if (op == e->rpar) {
1670 while (e->ops_count && e->ops[e->ops_count - 1] != e->lpar)
1671 if (op_eval(e) < 0 || e->error)
1672 break;
1673 const qoperations_t *pop = op_pop(e);
1674 if (!pop || (pop != e->lpar)) {
1675 e->error = 0; /* clear error so following error is printed */
1676 error(e, "expected \"(\"");
1677 return -1;
1678 }
1679 return 0;
1680 } else if (op->assocativity == ASSOCIATE_RIGHT) {
1681 while (e->ops_count && op->precedence < e->ops[e->ops_count - 1]->precedence)
1682 if (op_eval(e) < 0 || e->error)
1683 break;
1684 } else {
1685 while (e->ops_count && op->precedence <= e->ops[e->ops_count - 1]->precedence)
1686 if (op_eval(e) < 0 || e->error)
1687 break;
1688 }
1689 return op_push(e, op);
1690 }
1691
1692 static int variable_name_is_valid(const char *n) {
1693 assert(n);
1694 if (!isalpha(*n) && !(*n == '_'))
1695 return 0;
1696 for (n++; *n; n++)
1697 if (!isalnum(*n) && !(*n == '_'))
1698 return 0;
1699 return 1;
1700 }
1701
1702 static qvariable_t *variable_lookup(qexpr_t *e, const char *name) {
1703 assert(e);
1704 assert(name);
1705 for (size_t i = 0; i < e->vars_max; i++) {
1706 qvariable_t *v = e->vars[i];
1707 assert(v->name);
1708 assert(variable_name_is_valid(v->name));
1709 if (!strcmp(v->name, name))
1710 return v;
1711 }
1712 return NULL;
1713 }
1714
1715 static int lex(qexpr_t *e, const char **expr) {
1716 assert(e);
1717 assert(expr && *expr);
1718 int r = 0;
1719 const char *s = *expr;
1720 qvariable_t *v = NULL;
1721 e->id_count = 0;
1722 e->number = 0;
1723 e->op = NULL;
1724 memset(e->id, 0, sizeof (e->id));
1725 for (; *s && isspace(*s); s++)
1726 ;
1727 if (!(*s))
1728 return LEX_END;
1729 if (isalpha(*s) || *s == '_') {
1730 for (; e->id_count < sizeof(e->id) && *s && (isalnum(*s) || *s == '_');)
1731 e->id[e->id_count++] = *s++;
1732 if ((v = variable_lookup(e, e->id))) {
1733 e->number = v->value;
1734 r = LEX_NUMBER;
1735 } else if ((e->op = qop(e->id))) {
1736 r = LEX_OPERATOR;
1737 } else {
1738 r = -1;
1739 }
1740 } else {
1741 if (ispunct(*s)) {
1742 const qoperations_t *op1 = NULL, *op2 = NULL;
1743 int set = 0;
1744 e->id[e->id_count++] = *s++;
1745 op1 = qop(e->id);
1746 if (*s && ispunct(*s)) {
1747 set = 1;
1748 e->id[e->id_count++] = *s++;
1749 op2 = qop(e->id);
1750 }
1751 r = (op1 || op2) ? LEX_OPERATOR : -1;
1752 e->op = op2 ? op2 : op1;
1753 if (e->op == op1 && set) {
1754 s--;
1755 e->id_count--;
1756 e->id[1] = 0;
1757 }
1758 } else if (isdigit(*s)) {
1759 r = LEX_NUMBER;
1760 int dot = 0;
1761 for (; e->id_count < sizeof(e->id) && *s; s++) {
1762 const int ch = *s;
1763 if (!(isdigit(ch) || (ch == '.' && !dot)))
1764 break;
1765 e->id[e->id_count++] = ch;
1766 if (ch == '.')
1767 dot = 1;
1768 }
1769 e->number = numberify(e->id);
1770 } else {
1771 r = -1;
1772 }
1773 }
1774 /*printf("id(%d) %d => %s\n", (int)(s - *expr), r, e->id);*/
1775 *expr = s;
1776 return r;
1777 }
1778
1779 int qexpr(qexpr_t *e, const char *expr) {
1780 assert(e);
1781 assert(expr);
1782 int firstop = 1;
1783 const qoperations_t *previous = NULL;
1784 if (e->initialized) {
1785 memset(e->error_string, 0, sizeof (e->error_string));
1786 e->error = 0;
1787 e->ops_count = 0;
1788 e->numbers_count = 0;
1789 e->initialized = 1;
1790 }
1791 for (int l = 0; l != LEX_END && !(e->error);) {
1792 switch ((l = lex(e, &expr))) {
1793 case LEX_NUMBER:
1794 number_push(e, e->number);
1795 previous = NULL;
1796 firstop = 0;
1797 break;
1798 case LEX_OPERATOR: {
1799 const qoperations_t *op = e->op;
1800 if (CONFIG_Q_HIDE_FUNCS && op->hidden) {
1801 error(e, "unknown operator \"%s\"", op->name);
1802 goto end;
1803 }
1804 if (firstop || (previous && previous != e->rpar)) {
1805 if (e->op == e->minus) {
1806 op = e->negate;
1807 } else if (e->op->arity == 1) {
1808 /* do nothing */
1809 } else if (e->op != e->lpar) {
1810 assert(e->op);
1811 error(e, "invalid use of \"%s\"", e->op->name);
1812 goto end;
1813 }
1814 }
1815 shunt(e, op);
1816 previous = op;
1817 firstop = 0;
1818 break;
1819 }
1820 case LEX_END: break;
1821 default:
1822 error(e, "invalid symbol: %s", e->id);
1823 l = LEX_END;
1824 }
1825 }
1826 while (e->ops_count)
1827 if (op_eval(e) < 0 || e->error)
1828 break;
1829 if (e->numbers_count != 1) {
1830 error(e, "invalid expression: %d", e->numbers_count);
1831 return -1;
1832 }
1833 implies(e->error == 0, e->numbers_count == 1);
1834 end:
1835 return e->error == 0 ? 0 : -1;
1836 }
1837
1838
1839