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

(require 'ev3-motor)

(defsubst xor-local (cond1 cond2)
  (cond ((not cond1) cond2)
        ((not cond2) cond1) ))

(let ((step 100))

  (defun get-rotate-speed (way)
    (let ((speed (abs (get-motor-speed way))) )
      (if (< speed step)
          step
        (+ speed step) )))

  (declare-function get-rotate-speed nil)

  (defun ev3-rotate-dec (way)
    "Reduces the robot's rotation in the direction WAY.
WAY is either `left' or `right'."
    (unless (member way '(left right))
      (error "Not rotating that way (bogus argument)") )
    (let ((speed (get-rotate-speed way)))
      (if (eq way 'left)
          (progn
            (set-motor-left  (- speed step))
            (set-motor-right (+ speed step)) )
        (when (eq way 'right)
          (set-motor-left  (+ speed step))
          (set-motor-right (- speed step)) ))))

  (declare-function ev3-rotate-dec nil) )

;; rotate

(defun ev3-rotate (way)
  (let ((rotate-speed (get-rotate-speed way)))
    (if (eq way 'left)
        (progn
          (set-motor-left (- rotate-speed))
          (set-motor-right   rotate-speed) )
      (set-motor-left     rotate-speed)
      (set-motor-right (- rotate-speed)) )))

(defun ev3-rotate-left  ()
  (interactive)
  (ev3-rotate 'left) )

(defun ev3-rotate-right ()
  (interactive)
  (ev3-rotate 'right) )

;; is the droid rotating?

(defun rotates-left-p ()
  (and (> 0 (get-motor-speed 'left))
       (< 0 (get-motor-speed 'right)) ))

(defun rotates-right-p ()
  (and (< 0 (get-motor-speed 'left))
       (> 0 (get-motor-speed 'right)) ))

(defun rotates-p (&optional way)
  (if (not way)
      (xor-local (rotates-left-p)
                 (rotates-right-p) )
    (if (eq way 'left)
        (rotates-left-p)
      (when (eq way 'right)
          (rotates-right-p) ))))

;; rotate intuitively

(defun ev3-rotate-left-dwim ()
  (interactive)
  (if (rotates-p 'right)
      (ev3-rotate-dec 'right)
    (ev3-rotate 'left) ))

(defun ev3-rotate-right-dwim ()
  (interactive)
  (if (rotates-p 'left)
      (ev3-rotate-dec 'left)
    (ev3-rotate 'right) ))

(provide 'ev3-motor-rotate)