;;;;
;;;;
;;;; File: remote-commands.lisp
;;;;
;;;; License: Apache License 2.0
;;;;
;;;; Contributors: Milan Cermak, milan.cermak@gmail.com
;;;;
;;;; Description: Commands for the "remote" NXTLisp. Inspired by the RCXLisp;
;;;;              I tried to perserve backward compatibility.
;;;;
;;;;


#|

 Copyright 2007 Milan Cermak

 Licensed under the Apache License, Version 2.0 (the "License");
 you may not use this file except in compliance with the License.
 You may obtain a copy of the License at

  http://www.apache.org/licenses/LICENSE-2.0

 Unless required by applicable law or agreed to in writing, software
 distributed under the License is distributed on an "AS IS" BASIS,
 WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 See the License for the specific language governing permissions and
 limitations under the License.

|#

;; TODO: add asserts to set-sensor-state (and possibly other functions); asserts are
;;       now in protocol-commands.lisp, but functions here call them, so correct input
;;       is provided, but users will (mostly) call remote functions -> asserts needed!
;;
;;       check how are strings (filenames, IDs, ...) handled, if an argument has to be
;;       an array of unsigned-bytes or a simple string will suffice
;;
;;       what values should sensor return? according to the Lego specs, GETINPUTVALUES
;;       returns a lot. I think, there should be a way for the user to select what return
;;       value he wants.

(in-package :nxt)

;; notes:
;;
;; skipped: all clock related functions, *-rcx-thread, *current-program, *timer
;;
;; done: play-tone (a direct command)
;;       play-system-sound-file (play-system-file in rcxlisp; a direct command wrapper)
;;       battery-power (simplified get-battery-level direct command)
;;       start-nxt-program (a wrapper around start-program direct command)
;;       shutdown (a wrapper around stop-program direct command)
;;       alivep (internaly uses get-battery-level direct command)
;;       firmware (returns firmware and protocol versions)
;;       change-nxt-id (a wrapper around set-brick-name-command)
;;       set-sensor-state (uses set-input-mode direct command and
;;                         reset-input-scaled-value if clear flag is set)
;;       sensor (type and mode are possible &optional arguments, maybe not the
;;               best solution)
                           

;; differences between RCX and NXT:
;; play-system-sound-file plays a sound file in NXT's memory, possibly in a loop
;; start-nxt-program takes and string as an program name; rcx requires a number
;; shutdown does not take :infinity as a valid delay value
;; alivep uses a direct command (get-battery-level) as a helper to ping the NXT
;; firmware has no keystring argument; returns firmware and protocol major and minor versions
;; change-nxt-id takes a string as argument (not a number)
;; set-sensor-state has no slope argument


(EXPORT '(var set-var message send-message sensor effector set-effector-state 
          read-rcx-executable))

(defvar *last-sensor-type* nil
  "If sensor is called with no modifier arguments, *last-sensor-type* is checked
to obtain sensor type. It also maintains backward compatibility with RCXLisp.")

(defvar *last-sensor-mode* nil
  "If sensor is called with no modifier arguments, *last-sensor-mode* is checked
to obtain sensor mode. It also maintains backward compatibility with RCXLisp.")

(defun update-last-sensor-type (port sensor-type)
  "Updates the *last-sensor-type* variable according to last call of set-sensor-state."
  (if (or (eql *last-sensor-type* nil) ; port has not been used yet (not in list)
	  (not (let ((used nil)) ; there's gotta be a better way...
		 (dolist (i *last-sensor-type*)
		   (if (eql (car i) port)
		       (setf used t)))
		 used)))
      (push (list port sensor-type) *last-sensor-type*) 
      (loop for element in *last-sensor-type*           ; port has been used, update
	    when (eql (car element) port)
	    return (setf (cdr element) sensor-type))))

(defun update-last-sensor-mode (port sensor-mode)
  "Updates the *last-sensor-mode* variable according to last call of set-sensor-state."
  (if (or (eql *last-sensor-mode* nil) ; port has not been used yet (not in list) 
	  (not (let ((used nil)) 
		 (dolist (i *last-sensor-mode*)
		   (if (eql (car i) port)
		       (setf used t)))
		 used)))
      (push (list port sensor-mode) *last-sensor-mode*) 
      (loop for element in *last-sensor-mode*           ; port has been used, update
	    when (eql (car element) port)
	    return (setf (cdr element) sensor-mode))))

(defun extract-type (port)
  "Returns the last set type of sensor on port."
  (loop for element in *last-sensor-type*
	when (eql (car element) port)
	return (cdr element)))

(defun extract-mode (port)
  "Returns the last set mode of sensor on port."
  (loop for element in *last-sensor-mode*
	when (eql (car element) port)
	return (cdr element)))


(defun play-tone (frequency duration &optional (stream *standard-nxt-io*))
  "Plays a tone on the NXT. Frequency is in Hz (200-14000), duration in ms."
  (direct-play-tone frequency duration :stream stream))

(defun play-system-sound-file (loop? filename &optional stream)
  "Plays a soundfile <filename> on the NXT. If loop? if t, soundfile is played in a loop."
  (direct-play-soundfile (loop? filename :stream stream)))

(defun battery-power (&optional (stream *standard-nxt-io*))
  "Return NXT's current battery voltage in mV."
  (get-battery-level :stream stream))

(defun start-nxt-program (program &optional (stream *standard-nxt-io*))
  "Starts a program on the NXT brick. Argument must be a [15.3] filename with null
termination."
  (start-program (program :stream stream)))

(defun shutdown (&optional (delay 0) (stream *standard-nxt-io*))
  "Tells the NXT to stop execution of the current program after <delay> minutes."
  (sleep (* 60 delay))
  (stop-program :stream stream))

(defun alivep (&optional (stream *standard-nxt-io*))
  "Pings the NXT to see if a reply comes back."
  ;; get-battery-level is just a random function, in fact any could be used (altough
  ;; this one makes the best sense); it is sent to the brick to see if a reply comes back
  (get-battery-level :stream stream :reply t))

(defun firmware (&optional (stream *standard-nxt-io*))
  "Returns four distinct values: major and minor firmware number and
major and minor protocol version."
  (let ((raw-reply (get-firmware-version :stream stream)))
    (values (aref 8 raw-reply)   ; major firmware version
	    (aref 7 raw-reply)   ; minor firmware version
	    (aref 6 raw-reply)   ; major protocol version 
	    (aref 5 raw-reply)))); minor protocol version

(defun change-nxt-id (id &optional (stream *standard-nxt-io*))
  "Changes the ID (name) of the NXT brick. The change is immediately visible
on the brick. ID has to be a string of max 15 characters."
  (set-brick-name-command (id :stream stream)))

(defun set-sensor-state (port &key (type :no-sensor)(mode :raw)
			 (clear nil) (stream *standard-nxt-io*))
  "Configures a sensor attached to port <port>. For description of valid values
for <type> and <mode> consult the NXTLisp manual or SETINPUTMODE direct command
specified in Lego Direct Commands communication protocol."
  (let ((sensor-type (case type
		       (:no-sensor #x00)
		       (:switch #x01)
		       (:temperature #x02)
		       (:reflection #x03)
		       (:angle #x04)
		       (:light-active #x05)
		       (:light-inactive #x06)
		       (:sound-db #x07)
		       (:sound-dba #x08)
		       (:custom #x09)
		       (:lowspeed #x0A)
		       (:lowspeed-9V 0x0B)
		       (:no-of-sensor-types #x0C)))
	(sensor-mode (case mode
		       (:raw #x00)
		       (:boolean #x20)
		       (:transition-cnt #x40)
		       (:period-counter #x60)
		       (:pct-full-scale #x80)
		       (:celsius #xA0)
		       (:fahrenheit #xC0)
		       (:angle-steps #xE0)
		       (:slope-mask #x1F)
		       (:mode-mask #xE0)))) ; has the same value as :angle-steps in docs (?)
    (set-input-mode port sensor-type sensor-mode :stream stream)
    (if clear (reset-input-scaled-value port)) ; if the clear flag is set, reset the counter
    ;; see the manual or documentation in the source code for description
    (update-last-sensor-type port sensor-type)
    (update-last-sensor-mode port sensor-mode)))


;; TODO: should the type/mode handling be this way? rethink
;;       document and add asserts to type and mode arguments
;;       if type is temperature
(defun sensor (port &optional (type :raw type-supplied-p)
	       (mode :raw mode-supplied-p) (stream *standard-nxt-io*))
  "Retrieves values from the sensor plugged in <port> port."
  (let ((raw-packet (get-input-values port))
	;; if type or mode was not supplied, use values set
	;; in last call to set-sensor-state
	(sensor-type (if type-supplied-p
			 type
			 (if (eql (extract-type port) nil)
			     :raw
			     (extract-type port))))
	(sensor-mode (if mode-supplied-p
			 mode
			 (if (eql (extract-mode port) nil)
			     :raw
			     (extract-type port)))))
    ;; let's decode the packet according to type/mode
    (if (not (aref raw-packet 1))
	(warn "sensor: Packet marked as not valid."))
    ;; sensor-type has higher priority to sensor-mode
    (case sensor-type
      (:angle (extract-number (aref raw-packet 9)
			      (aref raw-packet 10)
			      :sword))
      ((or :light-active
	   :light-inactive) (extract-number (aref raw-packet 7)
					    (aref raw-packet 8)
					    :uword))
      (:temperature (extract-number (aref raw-packet 9) ; the same as :celsius/:fahrenheit
				    (aref raw-packet 10)
				    :sword))
      (t (case sensor-mode
	   ((or :boolean :transition-cnt :period-counter
		:celsius :fahrenheit) (extract-number (aref raw-packet 9)
						      (aref raw-packet 10)
						      :sword))
	   (t (extract-number (aref raw-packet 5)
			      (aref raw-packet 6)
			      :uword)))))))

(defun effector (effector feature &optional (stream *standard-nxt-io*))
  "Returns the current value of <feature> for <effector>."
  ;; there probably will be a problem with backward compatibility
  ;; I'm not sure what can be done with get-output-state direct command
  )

(defun set-effector-state (effector feature value &optional (stream *standard-nxt-io*))
  "TODO: document")

(defun extract-number (byte-1 byte-2 mode)
  "Return a number constructed from 2 bytes, either a signed word or unsigned word."
  (case mode
    (:uword (+ byte-1 (* byte-2 256)))
    (:sword (if (< byte-2 127) ; possitive?
		(+ byte-1 (* byte-2 256))
		(1+ (lognot (+ byte-1 (* byte-2 256))))))))