;;; -*- lexical-binding: t -*-
;;
;; this file:
;;   https://dataswamp.org/~incal/terror-3/ev3-motor-straight.el

(require 'ev3-motor)
(require 'ev3-motor-stop)

(defun get-speed-high ()
  (let ((speed-left  (get-motor-speed 'left))
        (speed-right (get-motor-speed 'right)) )
    (if (< speed-left speed-right)
        speed-right
      speed-left) ))

(defun get-speed-low ()
  (let ((speed-left  (get-motor-speed 'left))
        (speed-right (get-motor-speed 'right)) )
    (if (< speed-left speed-right)
        speed-left
      speed-right) ))

(let*((speed-def  100)
      (stop-speed   0))

  (defun get-speed-forward-inc (&optional min mod)
    (let ((speed-min (or min speed-def))
          (speed-mod (or mod speed-def))
          (speed     (get-speed-high)) )
      (if (< speed speed-min)
          speed-min
        (+ speed speed-mod) )))

  (declare-function get-speed-forward-inc nil)

  (defun get-speed-forward-dec (&optional min mod)
    (let ((speed-min (or min    speed-def))
          (speed-mod (or mod (- speed-def)))
          (speed     (get-speed-high)) )
      (if (< speed speed-min)
          stop-speed
        (+ speed speed-mod) )))

  (declare-function get-speed-forward-dec nil)

  (defun get-speed-reverse-inc (&optional min mod)
    (let ((speed-min (or min (- speed-def)))
          (speed-mod (or mod (- speed-def)))
          (speed     (get-speed-low)) )
      (if (< speed-min speed)
          speed-min
        (+ speed speed-mod) )))

  (declare-function get-speed-reverse-inc nil)

  (defun get-speed-reverse-dec (&optional min mod)
    (let ((speed-min (or min (- speed-def)))
          (speed-mod (or mod    speed-def))
          (speed     (get-speed-low)) )
      (if (< speed-min speed)
          stop-speed
        (+ speed speed-mod) )))

  (declare-function get-speed-reverse-dec nil) )

(defun ev3-forward-dwim ()
  (interactive)
  (set-motor-both
   (if (or (still-p) (moving-forward-p))
       (get-speed-forward-inc)
     (get-speed-reverse-dec) )))

(defun ev3-reverse-dwim ()
  (interactive)
  (set-motor-both
   (if (moving-forward-p)
       (get-speed-forward-dec)
     (get-speed-reverse-inc) )))

(defun ev3-forward-inc ()
  (interactive)
  (set-motor-both (get-speed-forward-inc) ))

(defun ev3-reverse-inc ()
  (interactive)
  (set-motor-both (get-speed-reverse-inc) ))

(provide 'ev3-motor-straight)