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