added TODO note
[laserbrain_demo] / src / mparser.cc
1 #include <stdio.h>
2 #include <stdarg.h>
3 #include <assert.h>
4 #include <string>
5 #include <map>
6 #include <stack>
7 #include "mparser.h"
8 #include "machine.h"
9
10 enum ValueType { VAL_NUM, VAL_VEC, VAL_STR };
11 struct Value {
12         ValueType type;
13         std::string s;
14         float v[4];
15 };
16
17 struct ParserState;
18
19 typedef bool (*Func)(ParserState*);
20
21 struct FuncDesc {
22         Func func;
23         int arity;
24 };
25
26 struct ParserState {
27         FILE *fp;
28         const char *fname;
29         int nline, nerrors;
30         Machine *mcn;
31
32         int nextc;
33         int savedc;
34
35         std::stack<Value> val;
36         std::map<std::string, Value> var;
37         std::map<std::string, FuncDesc> func;
38
39         std::map<std::string, Gear*> gears;
40         std::vector<Gear*> cur_gear;
41         Motor *cur_motor;
42 };
43
44 #define MAX_ID_LEN      31
45
46 static bool machine(ParserState *ps);
47 static bool expect(ParserState *ps, char c);
48 static bool statement(ParserState *ps);
49 static bool expression(ParserState *ps);
50 static bool term(ParserState *ps);
51 static bool vector(ParserState *ps);
52 static bool function(ParserState *ps, const char *name);
53
54 static char *get_ident(ParserState *ps, char *buf, int bsz);
55 static bool get_number(ParserState *ps, float *num);
56 static char *get_string(ParserState *ps, char *buf, int bsz);
57
58 static bool nextchar(ParserState *ps);
59 static void putback(ParserState *ps, char c);
60
61 static void errmsg(ParserState *ps, const char *fmt, ...);
62 static void expected(ParserState *ps, const char *str);
63
64 static void set_var(ParserState *ps, const char *name, const Value &value);
65 static bool have_var(ParserState *ps, const char *name);
66 static const Value &get_var(ParserState *ps, const char *name);
67
68 static void set_func(ParserState *ps, const char *name, Func func, int nargs = 0);
69 static bool have_func(ParserState *ps, const char *name);
70 static Func get_func(ParserState *ps, const char *name);
71 static int get_func_arity(ParserState *ps, const char *name);
72
73 static Gear *begin_gear(ParserState *ps);
74 static bool end_gear(ParserState *ps);
75 static Gear *this_gear(ParserState *ps);
76 static Gear *find_gear(ParserState *ps, const char *name);
77 static void update_gear_vars(ParserState *ps, Gear *gear);
78 static bool set_gear_var(Gear *gear, const char *name, const Value &val);
79
80 static void update_motor_vars(ParserState *ps, Motor *motor);
81 static bool set_motor_var(ParserState *ps, Motor *motor, const char *name, const Value &val);
82
83 // built-in functions
84 static bool func_adjacent(ParserState *ps);
85 static bool func_coaxial(ParserState *ps);
86
87
88 bool parse_machine(Machine *mcn, const char *fname)
89 {
90         FILE *fp = fopen(fname, "rb");
91         if(!fp) return false;
92
93         ParserState pstate;
94         pstate.fname = fname;
95         pstate.nline = 1;
96         pstate.nerrors = 0;
97         pstate.fp = fp;
98         pstate.mcn = mcn;
99         pstate.nextc = 0;
100         pstate.savedc = -1;
101         pstate.cur_motor = 0;
102
103         // init built-in function table
104         set_func(&pstate, "adjacent", func_adjacent, 2);
105         set_func(&pstate, "coaxial", func_coaxial, 2);
106
107         nextchar(&pstate);
108         bool res = machine(&pstate);
109         fclose(fp);
110         return res;
111 }
112
113 static bool machine(ParserState *ps)
114 {
115         char tok[MAX_ID_LEN + 1];
116         if(strcmp(get_ident(ps, tok, sizeof tok), "machine") != 0) {
117                 expected(ps, "machine");
118                 return false;
119         }
120         expect(ps, '{');
121
122         while(ps->nextc != '}') {
123                 if(!statement(ps)) {
124                         return false;
125                 }
126         }
127         expect(ps, '}');
128         return true;
129 }
130
131 static bool expect(ParserState *ps, char c)
132 {
133         if(c != ps->nextc) {
134                 char buf[2] = {0, 0};
135                 buf[0] = c;
136                 expected(ps, buf);
137                 return false;
138         }
139         nextchar(ps);
140         return true;
141 }
142
143 static bool statement(ParserState *ps)
144 {
145         char id[MAX_ID_LEN + 1];
146         if(!get_ident(ps, id, sizeof id)) {
147                 return false;
148         }
149
150         if(strcmp(id, "gear") == 0) {
151                 if(!expect(ps, '{')) {
152                         return false;
153                 }
154
155                 Gear *gear = begin_gear(ps);
156                 if(!gear) return false;
157
158                 update_gear_vars(ps, gear);
159
160                 while(ps->nextc != '}') {
161                         if(!statement(ps)) {
162                                 return false;
163                         }
164                 }
165                 expect(ps, '}');
166
167                 if(!end_gear(ps)) {
168                         return false;
169                 }
170                 return true;
171         }
172
173         if(strcmp(id, "motor") == 0) {
174                 if(!expect(ps, '{')) {
175                         return false;
176                 }
177                 if(ps->cur_motor) {
178                         errmsg(ps, "nested motors not allowed");
179                         return false;
180                 }
181
182                 ps->cur_motor = new Motor;
183                 update_motor_vars(ps, ps->cur_motor);
184
185                 while(ps->nextc != '}') {
186                         if(!statement(ps)) {
187                                 return false;
188                         }
189                 }
190                 expect(ps, '}');
191
192                 ps->mcn->add_motor(ps->cur_motor->drive, ps->cur_motor->speed);
193                 delete ps->cur_motor;
194                 ps->cur_motor = 0;
195                 return true;
196         }
197
198         if(ps->nextc == '=') {
199                 expect(ps, '=');
200                 if(!expression(ps)) {
201                         return false;
202                 }
203                 expect(ps, ';');
204
205                 set_var(ps, id, ps->val.top());
206                 ps->val.pop();
207                 return true;
208         }
209
210         expected(ps, "expression");
211         return false;
212 }
213
214 static bool expression(ParserState *ps)
215 {
216         return term(ps);        // TODO expand
217 }
218
219 static bool term(ParserState *ps)
220 {
221         if(ps->nextc == '+') {
222                 expect(ps, '+');
223                 return expression(ps);
224         }
225         if(ps->nextc == '-') {
226                 expect(ps, '-');
227                 if(!expression(ps)) {
228                         return false;
229                 }
230
231                 Value &v = ps->val.top();
232                 switch(v.type) {
233                 case VAL_VEC:
234                         v.v[3] = -v.v[3];
235                         v.v[2] = -v.v[2];
236                         v.v[1] = -v.v[1];
237                 case VAL_NUM:
238                         v.v[0] = -v.v[0];
239                         break;
240
241                 default:
242                         expected(ps, "number or vector after an unary -");
243                         return false;
244                 }
245                 return true;
246         }
247         if(isalpha(ps->nextc)) {
248                 char buf[MAX_ID_LEN];
249                 if(!get_ident(ps, buf, sizeof buf)) {
250                         return false;
251                 }
252
253                 if(ps->nextc == '(') {
254                         if(!function(ps, buf)) {
255                                 return false;
256                         }
257                 } else if(have_var(ps, buf)) {
258                         ps->val.push(get_var(ps, buf));
259                 } else {
260                         errmsg(ps, "unknown identifier: %s", buf);
261                         return false;
262                 }
263                 return true;
264         }
265         if(isdigit(ps->nextc)) {
266                 float num;
267                 if(!get_number(ps, &num)) {
268                         return false;
269                 }
270
271                 Value v;
272                 v.type = VAL_NUM;
273                 v.v[0] = num;
274                 ps->val.push(v);
275                 return true;
276         }
277         if(ps->nextc == '[') {
278                 return vector(ps);
279         }
280         if(ps->nextc == '"') {
281                 char buf[MAX_ID_LEN];
282                 if(!get_string(ps, buf, sizeof buf)) {
283                         return false;
284                 }
285
286                 Value v;
287                 v.type = VAL_STR;
288                 v.s = buf;
289                 ps->val.push(v);
290                 return true;
291         }
292
293         expected(ps, "term");
294         return false;
295 }
296
297 static bool vector(ParserState *ps)
298 {
299         int nelem;
300         if(!expect(ps, '[') ||
301                         !expression(ps) ||
302                         !expect(ps, ',') ||
303                         !expression(ps) ||
304                         !expect(ps, ',') ||
305                         !expression(ps)) {
306                 return false;
307         }
308
309         if(ps->nextc == ']') {
310                 expect(ps, ']');
311                 nelem = 3;
312         } else {
313                 if(!expect(ps, ',') || !expression(ps) || !expect(ps, ']')) {
314                         return false;
315                 }
316                 nelem = 4;
317         }
318
319         Value vec;
320         vec.type = VAL_VEC;
321         vec.v[3] = 1.0f;
322
323         for(int i=0; i<nelem; i++) {
324                 const Value &tmp = ps->val.top();
325                 if(tmp.type != VAL_NUM) {
326                         expected(ps, "numbers as vector elements");
327                         return false;
328                 }
329                 vec.v[nelem - i - 1] = tmp.v[0];
330                 ps->val.pop();
331         }
332
333         ps->val.push(vec);
334         return true;
335 }
336
337 static bool function(ParserState *ps, const char *name)
338 {
339         if(!expect(ps, '(')) {
340                 return false;
341         }
342
343         if(!have_func(ps, name)) {
344                 errmsg(ps, "unknown function: %s", name);
345                 return false;
346         }
347
348         int nargs = 0;
349         while(ps->nextc != ')') {
350                 if(nargs && !expect(ps, ',')) {
351                         return false;
352                 }
353                 if(!expression(ps)) {
354                         return false;
355                 }
356                 ++nargs;
357         }
358         expect(ps, ')');
359
360         int arity = get_func_arity(ps, name);
361         if(arity != nargs) {
362                 errmsg(ps, "function %s expects %d arguments", name, arity);
363                 return false;
364         }
365         Func func = get_func(ps, name);
366         assert(func);
367
368         if(!func(ps)) {
369                 errmsg(ps, "function %s failed", name);
370                 return false;
371         }
372         return true;
373 }
374
375 static char *get_ident(ParserState *ps, char *buf, int bsz)
376 {
377         char *ptr = buf;
378         if(!isalpha(ps->nextc)) {
379                 expected(ps, "identifier");
380                 return 0;
381         }
382         while(isalnum(ps->nextc) || ps->nextc == '_') {
383                 if(bsz > 1) {
384                         *ptr++ = ps->nextc;
385                         --bsz;
386                 }
387                 nextchar(ps);
388         }
389         *ptr = 0;
390         printf("get_ident -> \"%s\"\n", buf);
391         return buf;
392 }
393
394 static bool get_number(ParserState *ps, float *num)
395 {
396         char buf[256], *ptr = buf, *end = buf + sizeof buf;
397         bool found_point = false;
398
399         if(!isdigit(ps->nextc)) {
400                 expected(ps, "number");
401                 return false;
402         }
403
404         while(isdigit(ps->nextc) || (ps->nextc == '.' && !found_point)) {
405                 if(ptr < end) {
406                         *ptr++ = ps->nextc;
407                 }
408                 if(ps->nextc == '.') {
409                         found_point = true;
410                 }
411                 nextchar(ps);
412         }
413         *ptr = 0;
414
415         *num = atof(buf);
416         printf("get_number -> %f\n", *num);
417         return true;
418 }
419
420 static char *get_string(ParserState *ps, char *buf, int bsz)
421 {
422         char *ptr = buf;
423         if(!expect(ps, '"')) {
424                 return 0;
425         }
426
427         while(ps->nextc != -1 && ps->nextc != '"') {
428                 if(bsz > 1) {
429                         *ptr++ = ps->nextc;
430                         --bsz;
431                 }
432                 if((ps->nextc = fgetc(ps->fp)) == '\n') {
433                         ++ps->nline;
434                 }
435         }
436         *ptr = 0;
437
438         if(ps->nextc == -1) {
439                 return 0;
440         }
441         nextchar(ps);
442
443         printf("get_string -> \"%s\"\n", buf);
444         return buf;
445 }
446
447 static bool skip_line(ParserState *ps)
448 {
449         int c;
450         while((c = fgetc(ps->fp)) != -1 && c != '\n');
451         if(c != -1) {
452                 ps->nextc = fgetc(ps->fp);
453                 return true;
454         }
455         return false;
456 }
457
458 static bool nextchar(ParserState *ps)
459 {
460         if(ps->savedc != -1) {
461                 ps->nextc = ps->savedc;
462                 ps->savedc = -1;
463                 return true;
464         }
465
466         while((ps->nextc = fgetc(ps->fp)) != -1) {
467                 if(ps->nextc == '#') {
468                         if(!skip_line(ps)) {
469                                 return false;
470                         }
471                         ++ps->nline;
472                 }
473                 if(!isspace(ps->nextc)) {
474                         break;
475                 }
476                 if(ps->nextc == '\n') {
477                         ++ps->nline;
478                 }
479         }
480
481         /*if(ps->nextc != -1) {
482                 printf("DBG: nextchar -> %c\n", ps->nextc);
483         } else {
484                 printf("DBG: nextchar -> EOF\n");
485         }*/
486         return ps->nextc != -1;
487 }
488
489 static void putback(ParserState *ps, char c)
490 {
491         ps->savedc = ps->nextc;
492         ps->nextc = c;
493 }
494
495 static void errmsg(ParserState *ps, const char *fmt, ...)
496 {
497         fprintf(stderr, "%s line %d error: ", ps->fname, ps->nline);
498
499         va_list ap;
500         va_start(ap, fmt);
501         vfprintf(stderr, fmt, ap);
502         va_end(ap);
503
504         fputc('\n', stderr);
505 }
506
507 static void expected(ParserState *ps, const char *str)
508 {
509         errmsg(ps, "expected: %s", str);
510         ++ps->nerrors;
511 }
512
513 static void set_var(ParserState *ps, const char *name, const Value &value)
514 {
515         ps->var[name] = value;
516
517         Gear *gear = this_gear(ps);
518         if(gear) {
519                 set_gear_var(gear, name, value);
520         }
521
522         if(ps->cur_motor) {
523                 set_motor_var(ps, ps->cur_motor, name, value);
524         }
525 }
526
527 static bool have_var(ParserState *ps, const char *name)
528 {
529         return ps->var.find(name) != ps->var.end();
530 }
531
532 static const Value &get_var(ParserState *ps, const char *name)
533 {
534         return ps->var[name];
535 }
536
537 static void set_func(ParserState *ps, const char *name, Func func, int nargs)
538 {
539         FuncDesc fdesc;
540         fdesc.func = func;
541         fdesc.arity = nargs;
542         ps->func[name] = fdesc;
543 }
544
545 static bool have_func(ParserState *ps, const char *name)
546 {
547         return ps->func.find(name) != ps->func.end();
548 }
549
550 static Func get_func(ParserState *ps, const char *name)
551 {
552         return ps->func[name].func;
553 }
554
555 static int get_func_arity(ParserState *ps, const char *name)
556 {
557         return ps->func[name].arity;
558 }
559
560 static void print_value(const Value &val, FILE *fp)
561 {
562         switch(val.type) {
563         case VAL_NUM:
564                 fprintf(fp, "%f", val.v[0]);
565                 break;
566
567         case VAL_VEC:
568                 fprintf(fp, "[%f %f %f %f]", val.v[0], val.v[1], val.v[2], val.v[3]);
569                 break;
570
571         case VAL_STR:
572                 fprintf(fp, "\"%s\"", val.s.c_str());
573                 break;
574
575         default:
576                 fprintf(fp, "<invalid>");
577         }
578 }
579
580 // Gear-specific stuff
581
582 static Gear *begin_gear(ParserState *ps)
583 {
584         Gear *res = new Gear;
585         ps->cur_gear.push_back(res);
586         return res;
587 }
588
589 static bool end_gear(ParserState *ps)
590 {
591         if(ps->cur_gear.empty()) {
592                 errmsg(ps, "parser error: unbalanced end_gear");
593                 return false;
594         }
595         Gear *gear = ps->cur_gear.back();
596         ps->cur_gear.pop_back();
597
598         printf("DBG: end_gear: %s\n", gear->name.c_str());
599
600         if(gear->name.empty() || ps->gears[gear->name]) {
601                 char buf[32];
602                 sprintf(buf, "gear%04d", (int)ps->gears.size());
603                 gear->name = buf;
604         }
605         printf("DBG: adding gear: %s\n", gear->name.c_str());
606         ps->gears[gear->name] = gear;
607         ps->mcn->add_gear(gear);
608
609         if(!ps->cur_gear.empty()) {
610                 ps->cur_gear.back()->attach(gear);
611         }
612         return true;
613 }
614
615 static Gear *this_gear(ParserState *ps)
616 {
617         return ps->cur_gear.empty() ? 0 : ps->cur_gear.back();
618 }
619
620 static Gear *find_gear(ParserState *ps, const char *name)
621 {
622         // search progressively wider lexical scopes
623         std::vector<Gear*>::const_reverse_iterator it = ps->cur_gear.rbegin();
624         while(it != ps->cur_gear.rend()) {
625                 Gear *g = *it++;
626                 if(g->name == std::string(name)) {
627                         return g;
628                 }
629         }
630
631         return ps->gears[name];
632 }
633
634 static void update_gear_vars(ParserState *ps, Gear *gear)
635 {
636         std::map<std::string, Value>::const_iterator it = ps->var.begin();
637         while(it != ps->var.end()) {
638                 set_gear_var(gear, it->first.c_str(), it->second);
639                 ++it;
640         }
641 }
642
643 #define ASSERT_TYPE(v, t) \
644         do { \
645                 if((v).type != (t)) { \
646                         fprintf(stderr, "type mismatch while trying to set %s to value: ", name); \
647                         print_value(val, stderr); \
648                         fputc('\n', stderr); \
649                         return false; \
650                 } \
651         } while(0)
652
653 #define VVEC3(val) Vec3((val).v[0], (val).v[1], (val).v[2])
654 #define VVEC4(val) Vec4((val).v[0], (val).v[1], (val).v[2], (val).v[3])
655
656 static bool set_gear_var(Gear *gear, const char *name, const Value &val)
657 {
658         if(strcmp(name, "name") == 0) {
659                 ASSERT_TYPE(val, VAL_STR);
660                 gear->name = val.s;
661
662         } else if(strcmp(name, "position") == 0) {
663                 ASSERT_TYPE(val, VAL_VEC);
664                 gear->pos = VVEC3(val);
665
666         } else if(strcmp(name, "plane") == 0) {
667                 ASSERT_TYPE(val, VAL_VEC);
668                 gear->axis = VVEC3(val);
669                 gear->pdist = val.v[3];
670                 printf("setting plane eqn: %f %f %f  %f\n", val.v[0], val.v[1], val.v[2], val.v[3]);
671
672         } else if(strcmp(name, "thickness") == 0) {
673                 ASSERT_TYPE(val, VAL_NUM);
674                 gear->thickness = val.v[0];
675
676         } else if(strcmp(name, "teeth") == 0) {
677                 ASSERT_TYPE(val, VAL_NUM);
678                 gear->set_teeth(val.v[0]);      // set teeth and recalc radius
679
680         } else if(strcmp(name, "teeth_length") == 0) {
681                 ASSERT_TYPE(val, VAL_NUM);
682                 gear->teeth_length = val.v[0];
683
684         } else if(strcmp(name, "pitch") == 0) {
685                 ASSERT_TYPE(val, VAL_NUM);
686                 gear->set_teeth(gear->nteeth, val.v[0]); // set pitch and recalc radius
687
688         } else if(strcmp(name, "color") == 0) {
689                 ASSERT_TYPE(val, VAL_VEC);
690                 gear->color = VVEC3(val);
691
692         } else if(strcmp(name, "roughness") == 0) {
693                 ASSERT_TYPE(val, VAL_NUM);
694                 gear->roughness = val.v[0];
695
696         } else {
697                 return false;
698         }
699
700         return true;
701 }
702
703
704 // motor stuff
705
706 static void update_motor_vars(ParserState *ps, Motor *motor)
707 {
708         std::map<std::string, Value>::const_iterator it = ps->var.begin();
709         while(it != ps->var.end()) {
710                 set_motor_var(ps, motor, it->first.c_str(), it->second);
711                 ++it;
712         }
713 }
714
715 static bool set_motor_var(ParserState *ps, Motor *motor, const char *name, const Value &val)
716 {
717         if(strcmp(name, "drive") == 0) {
718                 ASSERT_TYPE(val, VAL_STR);
719                 Gear *gear = ps->gears[val.s];
720                 if(!gear) {
721                         errmsg(ps, "undefined gear: %s", val.s.c_str());
722                         return false;
723                 }
724                 int idx = ps->mcn->get_gear_index(gear);
725                 assert(idx >= 0);
726                 motor->drive = idx;
727
728         } else if(strcmp(name, "speed") == 0) {
729                 ASSERT_TYPE(val, VAL_NUM);
730                 motor->speed = val.v[0];
731
732         } else {
733                 return false;
734         }
735         return true;
736 }
737
738
739 // built-in functions
740 static bool func_adjacent(ParserState *ps)
741 {
742         Value val_angle = ps->val.top(); ps->val.pop();
743         Value val_gear_name = ps->val.top(); ps->val.pop();
744
745         if(val_gear_name.type != VAL_STR) {
746                 expected(ps, "gear name (string) as 1st arg to adjacent");
747                 return false;
748         }
749         if(val_angle.type != VAL_NUM) {
750                 expected(ps, "angle as 2nd arg to adjacent");
751                 return false;
752         }
753
754         Gear *gthis = this_gear(ps);
755         if(!gthis) {
756                 errmsg(ps, "adjacent: called outside of a gear block");
757                 return false;
758         }
759
760         Gear *gother = ps->gears[val_gear_name.s];
761         if(!gother) {
762                 errmsg(ps, "adjacent: gear \"%s\" not found", val_gear_name.s.c_str());
763                 return false;
764         }
765
766         Mat4 xform;
767         xform.rotation(deg_to_rad(val_angle.v[0]), gother->axis);
768         Vec3 dir = xform * Vec3(0, 1, 0);
769
770         float sum_radii = gthis->radius + gother->radius;
771         float avg_teeth_len = (gthis->teeth_length + gother->teeth_length) * 0.5;
772         Vec3 pos = gother->pos + dir * (sum_radii - avg_teeth_len * 0.75);
773
774         Value res;
775         res.type = VAL_VEC;
776         res.v[0] = pos.x;
777         res.v[1] = pos.y;
778         res.v[2] = pos.z;
779         ps->val.push(res);
780         return true;
781 }
782
783 static bool func_coaxial(ParserState *ps)
784 {
785         Value val_dist = ps->val.top(); ps->val.pop();
786         Value val_gear_name = ps->val.top(); ps->val.pop();
787
788         if(val_gear_name.type != VAL_STR) {
789                 expected(ps, "gear name (string) as 1st arg to func_coaxial");
790                 return false;
791         }
792         if(val_dist.type != VAL_NUM) {
793                 expected(ps, "stacking distance as 2nd arg to func_coaxial");
794                 return false;
795         }
796
797         Gear *gthis = this_gear(ps);
798         if(!gthis) {
799                 errmsg(ps, "coaxial: called outside of a gear block");
800                 return false;
801         }
802
803         Gear *gother = find_gear(ps, val_gear_name.s.c_str());
804         if(!gother) {
805                 errmsg(ps, "coaxial: gear \"%s\" not found", val_gear_name.s.c_str());
806                 return false;
807         }
808
809         float avg_thickness = (gthis->thickness + gother->thickness) * 0.5;
810         Vec3 pos = gother->pos + gother->axis * val_dist.v[0] * avg_thickness;
811
812         Value res;
813         res.type = VAL_VEC;
814         res.v[0] = pos.x;
815         res.v[1] = pos.y;
816         res.v[2] = pos.z;
817         ps->val.push(res);
818         return true;
819 }