!7

cmodule RunVectorSDK > DynPrintLogAndEnabled {
  switchable bool printImages;
  switchable bool keepControl;
  switchable double checkTouchedInterval = 30.0; // seconds. also for checking connection liveness
  S toSay;
  
  transient Process process;
  transient PrintStream out;
  transient WithTimestamp<Bool> isBeingTouched;
  
  start-thread {
    dm_registerAs_direct vectorSDK();
    dm_reloadOnFieldChange enabled();
    dm_onFieldChange keepControl(r { if (keepControl) requestControl(); else releaseControl(); });
    
    doEvery(checkTouchedInterval, r {
      checkBeingTouched();
      restartIfNoEvents();
    });
    
    _startSDK();
  }
  
  void _startSDK q {
    cleanMeUp_process();
    if (!enabled) ret;
    //File script = userDir("vector-sdk/cam-stream-to-stdout.py");
    File script = userDir("vector-sdk/control-from-java-v2.py");
    assertFileExists(script);
    makeExecutable(script);

    print("Calling " + script);
    process = Runtime.getRuntime().exec(platformQuoteOpt(script));
    print("SDK process started");
    print("PID: " + getOpt(process, "pid"));
  
    drainErrorStreamToConsole(process, line -> enabled, lambda0 enter);
    out = new PrintStream(process.getOutputStream());
    
    isBeingTouched = WithTimestamp(null);
    
    dm_startThread("SDK Input", r {
      DataInputStream in = new(process.getInputStream());
      S line;
      while ((line = in.readLine()) != null) {
        if (enabled) {
          vmBus_send vectorSDK_gotLine(module(), line);
          if (eqic(line, "Connected to vector")) {
            vmBus_send vectorSDK_connected(module());
            if (keepControl)
              requestControl();
            checkBeingTouched();
          }
          if (printImages || !startsWith(line, "img:"))
            print("Got line: " + takeFirst(100, line));
          if (eq(line, "Unknown error"))
            _startSDK();
          if (startsWith(line, "touch:"))
            isBeingTouched = WithTimestamp(match("true", afterColon(line)));
        }
      }
    });
  }

  void cleanMeUp_process {
    if (process != null) {
      print("Stopping SDK process");
      process.destroy/*Forcibly*/();
      process = null;
      print("Stopped SDK process, hopefully");
    }
  }
  
  visual centerAndSouthWithMargin(super,
    westAndCenterWithMargins(
      dm_checkBox keepControl(),
      dm_textFieldAndSubmit_q toSay(r { say(toSay) }, buttonText := "Say")));

  // API
  
  void eval(S pythonCode) {
    if (out == null || empty(pythonCode)) ret;
    out.println(jsonEncode(pythonCode));
    out.flush();
  }
  
  void evalWithControl(S pythonCode) {
    if (keepControl)
      ret with eval(pythonCode);
    print("evalWithControl: " + pythonCode);
    eval("with anki_vector.Robot() as robot2:\n" +
      indent(4, jreplace(pythonCode, "robot", "robot2")));
  }

  // returns true if (probably) successful
  bool say(S s) {
    if (out == null) false;
    s = newLinesToSpaces_trim(s);
    if (empty(s)) true;
    print("Making Vector say: " + s);
    // with release_control, there is "unknown error" after first utterance
    /*eval(autoUnindent(linesLL(
      "robot.conn.request_control(timeout=5.0)",
      "robot.behavior.say_text(" + pythonQuote(s) + ")",
      //"robot.conn.release_control()"
    )));*/
    /*eval("with anki_vector.Robot() as robot2:\n" +
      "    robot2.behavior.say_text(" + pythonQuote(s) + ")");*/
    evalWithControl("robot.behavior.say_text(" + pythonQuote(s) + ")");
    true;
  }
  
  void playWAV(File wav) {
    print("Playing: " + wav);
    if (!is16KMonoWAVFile(wav)) {
      print("Converting to 16k");
      File wav2 = javaxCachesDir("vector.wav");
      deleteFile(wav2);
      ffmpeg_toMonoAudio_16k(wav, wav2);
      if (!fileExists(wav2)) fail("Conversion failed");
      wav = wav2;
    }
    int volume = 75;
    evalWithControl("robot.audio.stream_wav_file(" + pythonQuote(f2s(wav)) + ", " + volume + ")";
  }
  
  void startCamStream {
    eval("camFeedOn()");
  }
  
  void checkBeingTouched() {
    eval([[print("touch: " + str(robot.touch.last_sensor_reading.is_being_touched))]]);
  }
  
  // request control in main connection. currently not sure how to undo
  void requestControl() {
    eval("robot.conn.request_control()");
  }
  
  // Not working in tests?
  void releaseControl() {
    eval("robot.conn.release_control()");
  }
  
  void restartIfNoEvents() {
    if (isBeingTouched != null && isBeingTouched.olderThanSeconds(checkTouchedInterval*2.0)) {
      isBeingTouched = WithTimestamp(null);
      _startSDK();
    }
  }
}