///
/// eForth1 Servo Demo
///
/// Select one of the demos, feed it into ef_setup(example#) 
///   
#include <Servo.h>
#include <eForth1.h>

#define DEMO demo3       /** selected, demo1, demo2, or demo3 */

/// Demo1: swing servo #3
PROGMEM const char demo1[] = 
": sv3 3 0 CALL ;\n"  // call C function #0 (see below) to control servo #3
"45 sv3 200 DELAY 135 sv3 300 DELAY 90 sv3\n"
;
/// Demo2: move all servos in sync
PROGMEM const char demo2[] = 
": sync 7 FOR DUP I 0 CALL NEXT DROP ;\n"     // sync 8 servos
": rnd CLOCK DROP ABS 180 MOD ;\n"            // randomize servo angle
": sweep rnd sync ;\n"                        // 8 servos in action
"' sweep 500 0 TMISR\n"                       // make sweep as ISR[0] at 500ms period
"1 TIMER\n"                                   // timer on
;
/// Demo3: move servo arms each with different amplitude
PROGMEM const char demo3[] =
"CREATE w 8 ALLOT\n"                                         // array[8] to keep angles
": xx ( -- ) 7 FOR I DUP 1+ 10 * SWAP w + C! NEXT ; xx\n"    // init each angle
": w@ ( n -- c w ) 90 SWAP w + C@ ;\n"                       // fetch servo angle by id
": ang= ( ph n -- ) SWAP OVER w@ ROT 1 CALL SWAP 0 CALL ;\n" // set servo angle
"VARIABLE x\n"                                               // step control
": step 7 FOR DUP I ang= NEXT DROP ;\n"                      // set all servo angles
": swing x @ DUP 1+ x ! step ;\n"                            // 1 timer tick forward
"' swing 100 0 TMISR\n"                                      // swing every 100ms as timer ISR[0]
"1 TIMER\n"                                                  // turn timer on
;

Servo sv[8];
/// servo arm angle setting ( angle servo# -- )
void servo() {
  int p = vm_pop();  ///> servo#
  int a = vm_pop();  ///> angle
  sv[p].write(a);
}
/// sinusoidal wave arm position calc ( center amp phase -- angle )
void cyclic() {
  const float PERIOD PROGMEM = 2.0 * PI / 256.0;  ///> default 256 step per cycle
  float v = sin(PERIOD * vm_pop());               ///> directional vector
  float d = vm_pop() * v;                         ///> delta = amplitude * dir_vector
  int   r = static_cast<int>(vm_pop() + d + 0.5); ///> final arm angle
  vm_push(r);
}

void setup() {
  Serial.begin(115200);
  while (!Serial);

  for (int i=0; i<8; i++) {
    sv[i].attach(4 + i);
  }

  ef_setup(DEMO);           ///> initiate eForth1

  vm_cfunc(0, servo);       ///> register servo write function
  vm_cfunc(1, cyclic);      ///> register cyclic control function
}

void loop() {
  ef_run();                 ///> invoke eForth1 VM outer interpreter
}