quickjs/src/pid_fuzzy.c

   1 #include "a.h"
   2 #include "a/pid_fuzzy.h"
   3
   4 static JSClassID liba_pid_fuzzy_class_id;
   5
   6 static void liba_pid_fuzzy_finalizer(JSRuntime *rt, JSValue val)
   7 {
   8     a_pid_fuzzy *self = (a_pid_fuzzy *)JS_GetOpaque(val, liba_pid_fuzzy_class_id);
   9     union
  10     {
  11         a_float const *p;
  12         a_float *o;
  13     } u;
  14     js_free_rt(rt, ((void)(u.p = self->me), u.o));
  15     js_free_rt(rt, ((void)(u.p = self->mec), u.o));
  16     js_free_rt(rt, ((void)(u.p = self->mkp), u.o));
  17     js_free_rt(rt, ((void)(u.p = self->mki), u.o));
  18     js_free_rt(rt, ((void)(u.p = self->mkd), u.o));
  19     js_free_rt(rt, a_pid_fuzzy_block(self));
  20     js_free_rt(rt, self);
  21 }
  22
  23 static JSClassDef liba_pid_fuzzy_class = {"pid_fuzzy", .finalizer = liba_pid_fuzzy_finalizer};
  24
  25 static JSValue liba_pid_fuzzy_ctor(JSContext *ctx, JSValueConst new_target, int argc, JSValueConst *argv)
  26 {
  27     (void)argc;
  28     (void)argv;
  29     JSValue proto, clazz = JS_UNDEFINED;
  30     a_pid_fuzzy *const self = (a_pid_fuzzy *)js_mallocz(ctx, sizeof(a_pid_fuzzy));
  31     if (!self) { return JS_EXCEPTION; }
  32     self->pid.summax = +A_FLOAT_INF;
  33     self->pid.summin = -A_FLOAT_INF;
  34     self->pid.outmax = +A_FLOAT_INF;
  35     self->pid.outmin = -A_FLOAT_INF;
  36     self->kp = self->pid.kp = 1;
  37     self->op = a_fuzzy_equ;
  38     a_pid_fuzzy_init(self);
  39     proto = JS_GetPropertyStr(ctx, new_target, "prototype");
  40     if (JS_IsException(proto)) { goto fail; }
  41     clazz = JS_NewObjectProtoClass(ctx, proto, liba_pid_fuzzy_class_id);
  42     JS_FreeValue(ctx, proto);
  43     if (JS_IsException(clazz)) { goto fail; }
  44     JS_SetOpaque(clazz, self);
  45     return clazz;
  46 fail:
  47     js_free(ctx, self);
  48     JS_FreeValue(ctx, clazz);
  49     return JS_EXCEPTION;
  50 }
  51
  52 static JSValue liba_pid_fuzzy_get(JSContext *ctx, JSValueConst this_val, int magic)
  53 {
  54     a_pid_fuzzy *const self = (a_pid_fuzzy *)JS_GetOpaque2(ctx, this_val, liba_pid_fuzzy_class_id);
  55     if (!self) { return JS_EXCEPTION; }
  56     double x;
  57     switch (magic)
  58     {
  59     case 0: return JS_NewUint32(ctx, self->block);
  60     case 1: x = (double)self->kp; break;
  61     case 2: x = (double)self->ki; break;
  62     case 3: x = (double)self->kd; break;
  63     case 4: x = (double)self->pid.summax; break;
  64     case 5: x = (double)self->pid.summin; break;
  65     case 6: x = (double)self->pid.outmax; break;
  66     case 7: x = (double)self->pid.outmin; break;
  67     case 8: x = (double)self->pid.out; break;
  68     case 9: x = (double)self->pid.fdb; break;
  69     case 10: x = (double)self->pid.err; break;
  70     case 11: return JS_NewUint32(ctx, self->order);
  71     default: return JS_UNDEFINED;
  72     }
  73     return JS_NewFloat64(ctx, x);
  74 }
  75
  76 static int liba_pid_fuzzy_block_(JSContext *ctx, a_pid_fuzzy *self, unsigned int block)
  77 {
  78     void *ptr = a_pid_fuzzy_block(self);
  79     if (block > self->block)
  80     {
  81         ptr = js_realloc(ctx, ptr, A_PID_FUZZY_BLOCK(block));
  82         if (!ptr) { return ~0; }
  83     }
  84     a_pid_fuzzy_set_block(self, ptr, block);
  85     return 0;
  86 }
  87
  88 static JSValue liba_pid_fuzzy_set(JSContext *ctx, JSValueConst this_val, JSValueConst val, int magic)
  89 {
  90     a_pid_fuzzy *const self = (a_pid_fuzzy *)JS_GetOpaque2(ctx, this_val, liba_pid_fuzzy_class_id);
  91     if (!self) { return JS_EXCEPTION; }
  92     a_u32 u;
  93     if (magic == 0)
  94     {
  95         if (JS_ToUint32(ctx, &u, val)) { return JS_EXCEPTION; }
  96         if (liba_pid_fuzzy_block_(ctx, self, (unsigned int)u)) { return JS_EXCEPTION; }
  97         return JS_UNDEFINED;
  98     }
  99     double x;
 100     if (JS_ToFloat64(ctx, &x, val)) { return JS_EXCEPTION; }
 101     switch (magic)
 102     {
 103     case 1: self->pid.kp = self->kp = (a_float)x; break;
 104     case 2: self->pid.ki = self->ki = (a_float)x; break;
 105     case 3: self->pid.kd = self->kd = (a_float)x; break;
 106     case 4: self->pid.summax = (a_float)x; break;
 107     case 5: self->pid.summin = (a_float)x; break;
 108     case 6: self->pid.outmax = (a_float)x; break;
 109     case 7: self->pid.outmin = (a_float)x; break;
 110     default: break;
 111     }
 112     return JS_UNDEFINED;
 113 }
 114
 115 static JSValue liba_pid_fuzzy_op(JSContext *ctx, JSValueConst this_val, int argc, JSValueConst *argv)
 116 {
 117     (void)argc;
 118     a_pid_fuzzy *const self = (a_pid_fuzzy *)JS_GetOpaque2(ctx, this_val, liba_pid_fuzzy_class_id);
 119     if (!self) { return JS_EXCEPTION; }
 120     a_u32 op;
 121     if (JS_ToUint32(ctx, &op, argv[0])) { return JS_EXCEPTION; }
 122     a_pid_fuzzy_set_op(self, (unsigned int)op);
 123     return JS_UNDEFINED;
 124 }
 125
 126 static JSValue liba_pid_fuzzy_rule(JSContext *ctx, JSValueConst this_val, int argc, JSValueConst *argv)
 127 {
 128     (void)argc;
 129     a_pid_fuzzy *const self = (a_pid_fuzzy *)JS_GetOpaque2(ctx, this_val, liba_pid_fuzzy_class_id);
 130     if (!self) { return JS_EXCEPTION; }
 131     union
 132     {
 133         a_float const *p;
 134         a_float *o;
 135     } u;
 136     a_u32 row;
 137     a_u32 len = 0;
 138     a_u32 order = 0;
 139     JSValue res = JS_UNDEFINED;
 140     if (JS_IsArray(ctx, argv[0]))
 141     {
 142         if (js_array_length(ctx, argv[0], &order)) { goto fail; }
 143         res = js_concat(ctx, argv[0]);
 144         if (js_array_length(ctx, res, &len)) { goto fail; }
 145         if (len)
 146         {
 147             a_float *const me = (a_float *)js_realloc(ctx, ((void)(u.p = self->me), u.o), sizeof(a_float) * len);
 148             if (!me) { goto fail; }
 149             self->me = me;
 150             if (js_array_num_get(ctx, res, me, len)) { goto fail; }
 151         }
 152         JS_FreeValue(ctx, res);
 153         res = JS_UNDEFINED;
 154     }
 155     if (JS_IsArray(ctx, argv[1]))
 156     {
 157         if (js_array_length(ctx, argv[1], &row) || row != order) { goto fail; }
 158         res = js_concat(ctx, argv[1]);
 159         if (js_array_length(ctx, res, &len)) { goto fail; }
 160         if (len)
 161         {
 162             a_float *const mec = (a_float *)js_realloc(ctx, ((void)(u.p = self->mec), u.o), sizeof(a_float) * len);
 163             if (!mec) { goto fail; }
 164             self->mec = mec;
 165             if (js_array_num_get(ctx, res, mec, len)) { goto fail; }
 166         }
 167         JS_FreeValue(ctx, res);
 168         res = JS_UNDEFINED;
 169     }
 170     if (JS_IsArray(ctx, argv[2]))
 171     {
 172         if (js_array_length(ctx, argv[2], &row) || row != order) { goto fail; }
 173         res = js_concat(ctx, argv[2]);
 174         if (js_array_length(ctx, res, &len)) { goto fail; }
 175         if (len)
 176         {
 177             a_float *const mkp = (a_float *)js_realloc(ctx, ((void)(u.p = self->mkp), u.o), sizeof(a_float) * len);
 178             if (!mkp) { goto fail; }
 179             self->mkp = mkp;
 180             if (js_array_num_get(ctx, res, mkp, len)) { goto fail; }
 181         }
 182         JS_FreeValue(ctx, res);
 183         res = JS_UNDEFINED;
 184     }
 185     if (JS_IsArray(ctx, argv[3]))
 186     {
 187         if (js_array_length(ctx, argv[3], &row) || row != order) { goto fail; }
 188         res = js_concat(ctx, argv[3]);
 189         if (js_array_length(ctx, res, &len)) { goto fail; }
 190         if (len)
 191         {
 192             a_float *const mki = (a_float *)js_realloc(ctx, ((void)(u.p = self->mki), u.o), sizeof(a_float) * len);
 193             if (!mki) { goto fail; }
 194             self->mki = mki;
 195             if (js_array_num_get(ctx, res, mki, len)) { goto fail; }
 196         }
 197         JS_FreeValue(ctx, res);
 198         res = JS_UNDEFINED;
 199     }
 200     if (JS_IsArray(ctx, argv[4]))
 201     {
 202         if (js_array_length(ctx, argv[4], &row) || row != order) { goto fail; }
 203         res = js_concat(ctx, argv[4]);
 204         if (js_array_length(ctx, res, &len)) { goto fail; }
 205         if (len)
 206         {
 207             a_float *const mkd = (a_float *)js_realloc(ctx, ((void)(u.p = self->mkd), u.o), sizeof(a_float) * len);
 208             if (!mkd) { goto fail; }
 209             self->mkd = mkd;
 210             if (js_array_num_get(ctx, res, mkd, len)) { goto fail; }
 211         }
 212         JS_FreeValue(ctx, res);
 213     }
 214     self->order = order;
 215     return JS_UNDEFINED;
 216 fail:
 217     JS_FreeValue(ctx, res);
 218     return JS_UNDEFINED;
 219 }
 220
 221 static JSValue liba_pid_fuzzy_block(JSContext *ctx, JSValueConst this_val, int argc, JSValueConst *argv)
 222 {
 223     (void)argc;
 224     a_pid_fuzzy *const self = (a_pid_fuzzy *)JS_GetOpaque2(ctx, this_val, liba_pid_fuzzy_class_id);
 225     if (!self) { return JS_EXCEPTION; }
 226     a_u32 block;
 227     if (JS_ToUint32(ctx, &block, argv[0])) { return JS_EXCEPTION; }
 228     if (liba_pid_fuzzy_block_(ctx, self, block)) { return JS_EXCEPTION; }
 229     return JS_UNDEFINED;
 230 }
 231
 232 static JSValue liba_pid_fuzzy_kpid(JSContext *ctx, JSValueConst this_val, int argc, JSValueConst *argv)
 233 {
 234     (void)argc;
 235     a_pid_fuzzy *const self = (a_pid_fuzzy *)JS_GetOpaque2(ctx, this_val, liba_pid_fuzzy_class_id);
 236     if (!self) { return JS_EXCEPTION; }
 237     double args[] = {0, 0, 0};
 238     for (unsigned int i = 0; i < A_LEN(args); ++i)
 239     {
 240         if (JS_ToFloat64(ctx, &args[i], argv[i])) { return JS_EXCEPTION; }
 241     }
 242     a_pid_fuzzy_kpid(self, (a_float)args[0], (a_float)args[1], (a_float)args[2]);
 243     return JS_UNDEFINED;
 244 }
 245
 246 static JSValue liba_pid_fuzzy_zero(JSContext *ctx, JSValueConst this_val, int argc, JSValueConst *argv)
 247 {
 248     (void)argc;
 249     (void)argv;
 250     a_pid_fuzzy *const self = (a_pid_fuzzy *)JS_GetOpaque2(ctx, this_val, liba_pid_fuzzy_class_id);
 251     if (!self) { return JS_EXCEPTION; }
 252     a_pid_fuzzy_zero(self);
 253     return JS_UNDEFINED;
 254 }
 255
 256 static JSValue liba_pid_fuzzy_run(JSContext *ctx, JSValueConst this_val, int argc, JSValueConst *argv)
 257 {
 258     (void)argc;
 259     a_pid_fuzzy *const self = (a_pid_fuzzy *)JS_GetOpaque2(ctx, this_val, liba_pid_fuzzy_class_id);
 260     if (!self) { return JS_EXCEPTION; }
 261     double args[] = {0, 0};
 262     for (unsigned int i = 0; i < A_LEN(args); ++i)
 263     {
 264         if (JS_ToFloat64(ctx, &args[i], argv[i])) { return JS_EXCEPTION; }
 265     }
 266     return JS_NewFloat64(ctx, (double)a_pid_fuzzy_run(self, (a_float)args[0], (a_float)args[1]));
 267 }
 268
 269 static JSValue liba_pid_fuzzy_pos(JSContext *ctx, JSValueConst this_val, int argc, JSValueConst *argv)
 270 {
 271     (void)argc;
 272     a_pid_fuzzy *const self = (a_pid_fuzzy *)JS_GetOpaque2(ctx, this_val, liba_pid_fuzzy_class_id);
 273     if (!self) { return JS_EXCEPTION; }
 274     double args[] = {0, 0};
 275     for (unsigned int i = 0; i < A_LEN(args); ++i)
 276     {
 277         if (JS_ToFloat64(ctx, &args[i], argv[i])) { return JS_EXCEPTION; }
 278     }
 279     return JS_NewFloat64(ctx, (double)a_pid_fuzzy_pos(self, (a_float)args[0], (a_float)args[1]));
 280 }
 281
 282 static JSValue liba_pid_fuzzy_inc(JSContext *ctx, JSValueConst this_val, int argc, JSValueConst *argv)
 283 {
 284     (void)argc;
 285     a_pid_fuzzy *const self = (a_pid_fuzzy *)JS_GetOpaque2(ctx, this_val, liba_pid_fuzzy_class_id);
 286     if (!self) { return JS_EXCEPTION; }
 287     double args[] = {0, 0};
 288     for (unsigned int i = 0; i < A_LEN(args); ++i)
 289     {
 290         if (JS_ToFloat64(ctx, &args[i], argv[i])) { return JS_EXCEPTION; }
 291     }
 292     return JS_NewFloat64(ctx, (double)a_pid_fuzzy_inc(self, (a_float)args[0], (a_float)args[1]));
 293 }
 294
 295 static JSCFunctionListEntry const liba_pid_fuzzy_proto[] = {
 296     JS_PROP_STRING_DEF("[Symbol.toStringTag]", "a.pid.fuzzy", 0),
 297     JS_CGETSET_MAGIC_DEF("block", liba_pid_fuzzy_get, liba_pid_fuzzy_set, 0),
 298     JS_CGETSET_MAGIC_DEF("kp", liba_pid_fuzzy_get, liba_pid_fuzzy_set, 1),
 299     JS_CGETSET_MAGIC_DEF("ki", liba_pid_fuzzy_get, liba_pid_fuzzy_set, 2),
 300     JS_CGETSET_MAGIC_DEF("kd", liba_pid_fuzzy_get, liba_pid_fuzzy_set, 3),
 301     JS_CGETSET_MAGIC_DEF("summax", liba_pid_fuzzy_get, liba_pid_fuzzy_set, 4),
 302     JS_CGETSET_MAGIC_DEF("summin", liba_pid_fuzzy_get, liba_pid_fuzzy_set, 5),
 303     JS_CGETSET_MAGIC_DEF("outmax", liba_pid_fuzzy_get, liba_pid_fuzzy_set, 6),
 304     JS_CGETSET_MAGIC_DEF("outmin", liba_pid_fuzzy_get, liba_pid_fuzzy_set, 7),
 305     JS_CGETSET_MAGIC_DEF("out", liba_pid_fuzzy_get, NULL, 8),
 306     JS_CGETSET_MAGIC_DEF("fdb", liba_pid_fuzzy_get, NULL, 9),
 307     JS_CGETSET_MAGIC_DEF("err", liba_pid_fuzzy_get, NULL, 10),
 308     JS_CGETSET_MAGIC_DEF("order", liba_pid_fuzzy_get, NULL, 11),
 309     JS_CFUNC_DEF("op", 1, liba_pid_fuzzy_op),
 310     JS_CFUNC_DEF("rule", 5, liba_pid_fuzzy_rule),
 311     JS_CFUNC_DEF("set_block", 1, liba_pid_fuzzy_block),
 312     JS_CFUNC_DEF("kpid", 3, liba_pid_fuzzy_kpid),
 313     JS_CFUNC_DEF("zero", 0, liba_pid_fuzzy_zero),
 314     JS_CFUNC_DEF("run", 2, liba_pid_fuzzy_run),
 315     JS_CFUNC_DEF("pos", 2, liba_pid_fuzzy_pos),
 316     JS_CFUNC_DEF("inc", 2, liba_pid_fuzzy_inc),
 317 };
 318
 319 int js_liba_pid_fuzzy_init(JSContext *ctx, JSModuleDef *m)
 320 {
 321     JS_NewClassID(&liba_pid_fuzzy_class_id);
 322     JS_NewClass(JS_GetRuntime(ctx), liba_pid_fuzzy_class_id, &liba_pid_fuzzy_class);
 323
 324     JSValue const proto = JS_NewObject(ctx);
 325     JS_SetPropertyFunctionList(ctx, proto, liba_pid_fuzzy_proto, A_LEN(liba_pid_fuzzy_proto));
 326
 327     JSValue const clazz = JS_NewCFunction2(ctx, liba_pid_fuzzy_ctor, "pid_fuzzy", 3, JS_CFUNC_constructor, 0);
 328     JS_SetConstructor(ctx, clazz, proto);
 329     JS_SetClassProto(ctx, liba_pid_fuzzy_class_id, proto);
 330
 331     JS_DefinePropertyValueStr(ctx, clazz, "CAP", JS_NewUint32(ctx, A_PID_FUZZY_CAP), 0);
 332     JS_DefinePropertyValueStr(ctx, clazz, "CAP_ALGEBRA", JS_NewUint32(ctx, A_PID_FUZZY_CAP_ALGEBRA), 0);
 333     JS_DefinePropertyValueStr(ctx, clazz, "CAP_BOUNDED", JS_NewUint32(ctx, A_PID_FUZZY_CAP_BOUNDED), 0);
 334     JS_DefinePropertyValueStr(ctx, clazz, "CUP", JS_NewUint32(ctx, A_PID_FUZZY_CUP), 0);
 335     JS_DefinePropertyValueStr(ctx, clazz, "CUP_ALGEBRA", JS_NewUint32(ctx, A_PID_FUZZY_CUP_ALGEBRA), 0);
 336     JS_DefinePropertyValueStr(ctx, clazz, "CUP_BOUNDED", JS_NewUint32(ctx, A_PID_FUZZY_CUP_BOUNDED), 0);
 337     JS_DefinePropertyValueStr(ctx, clazz, "EQU", JS_NewUint32(ctx, A_PID_FUZZY_EQU), 0);
 338
 339     return JS_SetModuleExport(ctx, m, "pid_fuzzy", clazz);
 340 }