;;;;
;;;;
;;;; File: protocol-commands.lisp
;;;;
;;;; License: Apache License 2.0
;;;;
;;;; Contributors: Milan Cermak, milan.cermak@gmail.com
;;;;
;;;; Description: All commands specified in the NXT Communications Protocol,
;;;;              NXT Direct Commands and related code.
;;;;
;;;;


#|

 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.

|#




(in-package :nxt)


;;;
;;; Communications protocol (normal commands)
;;;

(defun open-read-command (filename &key
			  (stream *standard-nxt-io*) (blocking *blocking-default*)
			  (reply *reply-default*) (timeout *timeout-default*)
			  (retries *retries-default*) (retries-interval *retries-interval-default*)
			  (default-answer *default-answer-default*))
  "System command OPEN READ COMMANND. Argument description:
filename - an ASCIIZ [15.3 chars] + null termination string

Reply should not be disabled for this command."
  (assert (arrayp filename) (filename)
	  "open-read-command: filename is not an array/string: ~A" filename)
  (let ((msg (make-initial-system-packet 24 reply #X80)))
    (push-string filename 20 msg)
    (create-packet msg)
    (if (eql stream 0)
	msg
	(let ((answer (send-packet stream msg blocking timeout retries retries-interval)))
	  (cond
	    ((< answer 0) default-answer)
	    ((null reply) answer)
	    (t (decode-reply (recv-packet stream 10 blocking timeout retries retries-interval)
			     :raw)))))))

(defun open-write-command (filename filesize &key
			   (stream *standard-nxt-io*) (blocking *blocking-default*)
			   (reply *reply-default*) (timeout *timeout-default*)
			   (retries *retries-default*) (retries-interval *retries-interval-default*)
			   (default-answer *default-answer-default*))
  "System command OPEN WRITE COMMAND. Argument description:
filename - an ASCIIZ [15.3 chars] + null termination string
filesize - file size; max 2^32-1 bytes

Reply should not be disabled for this command."
  (assert (arrayp filename) (filename)
	  "open-write-command: filename is not an array/string: ~A" filename)
  (assert (and (numberp filesize) (<= 0 filesize 4294967295)) (filesize)
	  "open-write-command: filesize is not a number or has an invalid value [0 - 4294967295]: ~A" filesize)
  (let ((msg (make-initial-system-packet 28 reply #X81)))
    (push-string filename 20 msg)
    (push-4bytes filesize msg)
    (create-packet msg)
    (if (eql stream 0)
	msg
	(let ((answer (send-packet stream msg blocking timeout retries retries-interval)))
	  (cond
	    ((< answer 0) default-answer)
	    ((null reply) answer)
	    (t (decode-reply (recv-packet stream 6 blocking timeout retries retries-interval)
			     :ubyte)))))))

(defun read-command (handle bytes-to-read &key
		     (stream *standard-nxt-io*) (blocking *blocking-default*)
		     (reply *reply-default*) (timeout *timeout-default*)
		     (retries *retries-default*) (retries-interval *retries-interval-default*)
		     (default-answer *default-answer-default*))
  "System command READ COMMAND. Argumets description:
handle - the handle number
bytes-to-read - number of data to read (in bytes); max 65535

Reply should not be disabled for this command."
  (assert (numberp handle) (handle)
	  "read-command: handle is not a number: ~A" handle)
  (assert (and (numberp bytes-to-read) (<= 0 bytes-to-read 65535)) (bytes-to-read)
	  "read-command: bytes-to-read is not a number or has an invalid value [0 - 65535]: ~A" bytes-to-read)
  (let ((msg (make-initial-system-packet 7 reply #X82)))
    (vector-push handle msg)
    (push-2bytes bytes-to-read msg)
    (create-packet msg)
    (if (eql stream 0)
	msg
	(let ((answer (send-packet stream msg blocking timeout retries retries-interval)))
	  (cond
	    ((< answer 0) default-answer)
	    ((null reply) answer)
	    (t (decode-reply (recv-packet stream (+ bytes-to-read 8) blocking timeout
					  retries retries-interval)
			     :raw)))))))

(defun write-command (handle data &key
		      (stream *standard-nxt-io*) (blocking *blocking-default*)
		      (reply *reply-default*) (timeout *timeout-default*)
		      (retries *retries-default*) (retries-interval *retries-interval-default*)
		      (default-answer *default-answer-default*))
  "System command WRITE COMMAND. Arguments description:
handle - the handle number
data - array of data to be written into FLASH"
  (assert (numberp handle) (handle)
	  "write-command: handle is not a number: ~A" handle)
  (assert (arrayp data) (data)
	  "write-command: data is not an array: ~A" data)
  (let ((msg (make-initial-system-packet (+ (length data) 5) reply #X83)))
    (vector-push handle msg)
    (dotimes (c (length data))
      (vector-push (aref data c) msg))
    (create-packet msg)
    (if (eql stream 0)
	msg
	(let ((answer (send-packet stream msg blocking timeout retries retries-interval)))
	  (cond
	    ((< answer 0) default-answer)
	    ((null reply) answer)
	    (t (decode-reply (recv-packet stream 8 blocking timeout retries retries-interval)
			     :raw)))))))

(defun close-command (handle &key
		      (stream *standard-nxt-io*) (blocking *blocking-default*)
		      (reply *reply-default*) (timeout *timeout-default*)
		      (retries *retries-default*) (retries-interval *retries-interval-default*)
		      (default-answer *default-answer-default*))
  "System command CLOSE COMMAND. Argument description:
handle - the handle number."
  (assert (numberp handle) (handle)
	  "close-command: handle is not a number: ~A" handle)
  (let ((msg (make-initial-system-packet 5 reply #X84)))
    (vector-push handle msg)
    (create-packet)
    (if (eql stream 0)
	msg
	(let ((answer (send-packet stream msg blocking timeout retries retries-interval)))
	  (cond
	    ((< answer 0) default-answer)
	    ((null reply) answer)
	    (t (decode-reply (recv-packet stream 6 blocking timeout retries retries-interval)
			     :ubyte)))))))

(defun delete-command (filename &key
		       (stream *standard-nxt-io*) (blocking *blocking-default*)
		       (reply *reply-default*) (timeout *timeout-default*)
		       (retries *retries-default*) (retries-interval *retries-interval-default*)
		       (default-answer *default-answer-default*))
  "System command DELETE COMMAND. Argument description:
filename - an ASCIIZ [15.3 chars] null terminated string"
  (assert (arrayp filename) (filename)
	  "delete-command: filename is not an array/string: ~A" filename)
  (let ((msg (make-initial-system-packet 24 reply #X85)))
    (push-string filename 20 msg)
    (create-packet msg)
    (if (eql stream 0)
	msg
	(let ((answer (send-packet stream msg blocking timeout retries retries-interval)))
	  (cond
	    ((< answer 0) default-answer)
	    ((null reply) answer)
	    (t (decode-reply (recv-packet stream 25 blocking timeout retries retries-interval)
			     :filename)))))))

(defun find-first (filename &key
		   (stream *standard-nxt-io*) (blocking *blocking-default*)
		   (reply *reply-default*) (timeout *timeout-default*)
		   (retries *retries-default*) (retries-interval *retries-interval-default*)
		   (default-answer *default-answer-default*))
  "System command FIND FIRST. Argument description:
filename - an ASCIIZ [15.3 chars] null terminated string with extensions (see Wildcard in
Lego Communication protocol)

Reply should not be disabled for this command."
  (assert (arrayp filename) (filename)
	  "find-first: filename is not an array/string: ~A" filename)
  (let ((msg (make-initial-system-packet 24 reply #X86)))
    (push-string filename 20 msg)
    (create-packet msg)
    (if (eql stream 0)
	msg
	(let ((answer (send-packet stream msg blocking timeout retries retries-interval)))
	  (cond
	    ((< answer 0) default-answer)
	    ((null reply) answer)
	    (t (decode-reply (recv-packet stream 30 blocking timeout retries retries-interval)
			     :raw)))))))

(defun find-next (handle &key
		  (stream *standard-nxt-io*) (blocking *blocking-default*)
		  (reply *reply-default*) (timeout *timeout-default*)
		  (retries *retries-default*) (retries-interval *retries-interval-default*)
		  (default-answer *default-answer-default*))
  "System command FIND NEXT. Argument description:
handle - the handle number from a previous find file or from find-first command

Reply should not be disabled for this command."
  (assert (numberp handle) (handle)
	  "find-next: handle is not a number: ~A" handle)
  (let ((msg (make-initial-system-packet 5 reply #X87)))
    (vector-push handle msg)
    (create-packet msg)
    (if (eql stream 0)
	msg
	(let ((answer (send-packet stream msg blocking timeout retries retries-interval)))
	  (cond
	    ((< answer 0) default-answer)
	    ((null reply) answer)
	    (t (decode-reply (recv-packet stream 30 blocking timeout retries retries-interval)
			     :raw)))))))

(defun get-firmware-version (&key
			     (stream *standard-nxt-io*) (blocking *blocking-default*)
			     (reply *reply-default*) (timeout *timeout-default*)
			     (retries *retries-default*) (retries-interval *retries-interval-default*)
			     (default-answer *default-answer-default*))
  "System command GET FIRMWARE VERSION.

Reply should not be disabled for this command."
  (let ((msg (make-initial-system-packet 4 reply #X88)))
    (create-packet msg)
    (if (eql stream 0)
	msg
	(let ((answer (send-packet stream msg blocking timeout retries retries-interval)))
	  (cond
	    ((< answer 0) default-answer)
	    ((null reply) answer)
	    (t (decode-reply (recv-packet stream 9 blocking timeout retries retries-interval)
			     :raw)))))))

(defun open-write-linear-command (filename filesize &key
				  (stream *standard-nxt-io*) (blocking *blocking-default*)
				  (reply *reply-default*) (timeout *timeout-default*)
				  (retries *retries-default*) (retries-interval *retries-interval-default*)
				  (default-answer *default-answer-default*))
  "System command OPEN WRITE LINEAR COMMAND. Argument description:
filename - an ASCIIZ [15.3 chars] null terminated string
filesize - file size; max 2^32-1 bytes

Reply should not be disabled for this command."
  (assert (arrayp filename) (filename)
	  "open-write-linear-command: filename is not an array/string: ~A" filename)
  (assert (and (numberp filesize) (<= 0 filesize 4294967295)) (filesize)
	  "open-write-linear-command: filesize is not a number or has an invalid value [0 - 4294967295]: ~A" filesize)
  (let ((msg (make-initial-system-packet 28 reply #X89)))
    (push-string filename 20 msg)
    (push-4bytes filesize msg)
    (create-packet msg)
    (if (eql stream 0)
	msg
	(let ((answer (send-packet stream msg blocking timeout retries retries-interval)))
	  (cond
	    ((< answer 0) default-answer)
	    ((null reply) answer)
	    (t (decode-reply (recv-packet stream 6 blocking timeout retries retries-interval)
			     :ubyte)))))))

(defun open-read-linear-command (filename &key
				 (stream *standard-nxt-io*) (blocking *blocking-default*)
				 (reply *reply-default*) (timeout *timeout-default*)
				 (retries *retries-default*) (retries-interval *retries-interval-default*)
				 (default-answer *default-answer-default*))
  "System command OPEN READ LINEAR COMMAND (internal command). Argument description:
filename - an ASCIIZ [15.3 chars] null terminated string

Reply should not be disabled for this command."
  (assert (arrayp filename) (filename)
	  "open-read-linear-command: filename is not an array/string: ~A" filename)
  (let ((msg (make-initial-system-packet 24 reply #X8A)))
    (push-string filename 20 msg)
    (create-packet msg)
    (if (eql stream 0)
	msg
	(let ((answer (send-packet stream msg blocking timeout retries retries-interval)))
	  (cond
	    ((< answer 0) default-answer)
	    ((null reply) answer)
	    (t (decode-reply (recv-packet stream 9 blocking timeout retries retries-interval)
			     :ulong)))))))

(defun open-write-data-command (filename filesize &key
				(stream *standard-nxt-io*) (blocking *blocking-default*)
				(reply *reply-default*) (timeout *timeout-default*)
				(retries *retries-default*) (retries-interval *retries-interval-default*)
				(default-answer *default-answer-default*))
  "System command OPEN WRITE DATA COMMAND. Argument description:
filename - an ASCIIZ [15.3 chars] null terminated string
filesize - file size; max 2^32-1 bytes

Reply should not be disabled for this command."
  (assert (arrayp filename) (filename)
	  "open-write-data-command: filename is not an array/string: ~A" filename)
  (assert (and (numberp filesize) (<= 0 filesize 4294967295)) (filesize)
	  "open-write-data-command: filesize is not a number or has an invalid value [0 - 4294967295]: ~A" filesize)
  (let ((msg (make-initial-system-packet 28 reply #X8B)))
    (push-string filename 20 msg)
    (push-4bytes filesize msg)
    (create-packet msg)
    (if (eql stream 0)
	msg
	(let ((answer (send-packet stream msg blocking timeout retries retries-interval)))
	  (cond
	    ((< answer 0) default-answer)
	    ((null reply) answer)
	    (t (decode-reply (recv-packet stream 6 blocking timeout retries retries-interval)
			     :ubyte)))))))

(defun open-append-data-command (filename &key
				 (stream *standard-nxt-io*) (blocking *blocking-default*)
				 (reply *reply-default*) (timeout *timeout-default*)
				 (retries *retries-default*) (retries-interval *retries-interval-default*)
				 (default-answer *default-answer-default*))
  "System command OPEN APPEND DATA COMMAND. Argument description:
filename - an ASCIIZ [15.3 chars] null terminated string

Reply should not be disabled for this command."
  (assert (arrayp filename) (filename)
	  "open-append-data-command: filename is not an array/string: ~A" filename)
  (let ((msg (make-initial-system-packet 24 reply #X8C)))
    (push-string filename 20 msg)
    (create-packet msg)
    (if (eql stream 0)
	msg
	(let ((answer (send-packet stream msg blocking timeout retries retries-interval)))
	  (cond
	    ((< answer 0) default-answer)
	    ((null reply) answer)
	    (t (decode-reply (recv-packet stream 10 blocking timeout retries retries-interval)
			     :raw)))))))

(defun set-brick-name-command (name &key
			       (stream *standard-nxt-io*) (blocking *blocking-default*)
			       (reply *reply-default*) (timeout *timeout-default*)
			       (retries *retries-default*) (retries-interval *retries-interval-default*)
			       (default-answer *default-answer-default*))
  "System command SET BRICK NAME COMMAND. Argument description:
name - max 15 character string"
  (assert (and (arrayp name) (<= (length name) 15)) (name)
	  "set-brick-name-command: name is not an array/string or too long [max 15]: ~A" name)
  (let ((msg (make-initial-system-packet 20 reply #X98)))
    (push-string name 15 msg)
    (create-packet msg)
    (if (eql stream 0)
	msg
	(let ((answer (send-packet stream msg blocking timeout retries retries-interval)))
	  (cond
	    ((< answer 0) default-answer)
	    ((null reply) answer)
	    (t (decode-reply (recv-packet stream 5 blocking timeout retries retries-interval)
			     :simple)))))))

(defun get-device-info (&key
			(stream *standard-nxt-io*) (blocking *blocking-default*)
			(reply *reply-default*) (timeout *timeout-default*)
			(retries *retries-default*) (retries-interval *retries-interval-default*)
			(default-answer *default-answer-default*))
  "System command GET DEVICE INFO.

Reply should not be disabled for this command."
  (let ((msg (make-initial-system-packet 4 reply #X9B)))
    (create-packet msg)
    (if (eql stream 0)
	msg
	(let ((answer (send-packet stream msg blocking timeout retries retries-interval)))
	  (cond
	    ((< answer 0) default-answer)
	    ((null reply) answer)
	    (t (decode-reply (recv-packet stream 35 blocking timeout retries retries-interval)
			     :raw)))))))

(defun delete-user-flash (&key
			  (stream *standard-nxt-io*) (blocking *blocking-default*)
			  (reply *reply-default*) (timeout *timeout-default*)
			  (retries *retries-default*) (retries-interval *retries-interval-default*)
			  (default-answer *default-answer-default*))
  "System command DELETE USER FLASH.

Reply should not be disabled for this command."
  (let ((msg (make-initial-system-packet 4 reply #XA0)))
    (create-packet msg)
    (if (eql stream 0)
	msg
	(let ((answer (send-packet stream msg blocking timeout retries retries-interval)))
	  (cond
	    ((< answer 0) default-answer)
	    ((null reply) answer)
	    (t (decode-reply (recv-packet stream 5 blocking timeout retries retries-interval)
			     :simple)))))))

(defun poll-command-length (buffer &key
			    (stream *standard-nxt-io*) (blocking *blocking-default*)
			    (reply *reply-default*) (timeout *timeout-default*)
			    (retries *retries-default*) (retries-interval *retries-interval-default*)
			    (default-answer *default-answer-default*))
  "System command POLL COMMAND LENGTH. Argument description:
buffer - can take two discreet values:
 #X00 - poll buffer
 #X01 - high speed buffer

Reply should not be disabled for this command."
  (assert (and (numberp buffer) (or (= buffer #X00) (= buffer #X01))) (buffer)
	  "poll-command-length: buffer is not a number or has an invalid value [#X00, #X01]: ~A" buffer)
  (let ((msg (make-initial-system-packet 5 reply #XA1)))
    (vector-push buffer msg)
    (create-packet msg)
    (if (eql stream 0)
	msg
	(let ((answer (send-packet stream msg blocking timeout retries retries-interval)))
	  (cond
	    ((< answer 0) default-answer)
	    ((null reply) answer)
	    ;; no decode-reply here, third byte in the reply packet doesn't
	    ;; represent a status/error byte, complain at Lego
	    (t (recv-packet stream 7 blocking timeout retries retries-interval)))))))

(defun poll-command (buffer length &key
		     (stream *standard-nxt-io*) (blocking *blocking-default*)
		     (reply *reply-default*) (timeout *timeout-default*)
		     (retries *retries-default*) (retries-interval *retries-interval-default*)
		     (default-answer *default-answer-default*))
  "System command POLL COMMAND. Argument description:
buffer - can take two discreet values:
 #X00 - poll buffer
 #X01 - high speed buffer

length - command length

Reply should not be disabled for this command."
  (assert (and (numberp buffer) (or (= buffer #X00) (= buffer #X01))) (buffer)
	  "poll-command: buffer is not a number or has an invalid value [#X00, #X01]: ~A" buffer)
  (assert (numberp length) (length)
	  "poll-command: length is not a number: ~A" length)
  (let ((msg (make-initial-system-packet 6 reply #XA2)))
    (vector-push buffer msg)
    (vector-push length msg)
    (create-packet msg)
    (if (eql stream 0)
	msg
	(let ((answer (send-packet stream msg blocking timeout retries retries-interval)))
	  (cond
	    ((< answer 0) default-answer)
	    ((null reply) answer)
	    ;; no decode-reply here, third byte in the reply packet doesn't
	    ;; represent a status/error byte, complain at Lego
	    (t (recv-packet stream 67 blocking timeout retries retries-interval)))))))


;;;
;;; Direct commands
;;;

(defun start-program (filename &key
		      (stream *standard-nxt-io*) (blocking *blocking-default*)
		      (reply *reply-default*) (timeout *timeout-default*)
		      (retries *retries-default*) (retries-interval *retries-interval-default*)
		      (default-answer *default-answer-default*))
  "Direct command STARTPROGRAM; filename has to be an ASCIIZ string
with maximum size [15.3] char + null terminator."
  (assert (arrayp filename) (filename)
	  "start-program: filename is not an array/string: ~A" filename)
  (let ((msg (make-initial-direct-packet 24 reply #X00)))
    (push-string filename 20 msg)
    (create-packet msg)
    (if (eql stream 0)
	msg   ; debugging packets
	(let ((answer (send-packet stream msg blocking timeout retries retries-interval)))
	  (cond
	    ((< answer 0) default-answer) ; sending failed
	    ((null reply) answer)         ; no reply requested
	    (t (decode-reply (recv-packet stream 5 blocking timeout retries retries-interval)
			     :simple)))))))

(defun stop-program (&key
		     (stream *standard-nxt-io*) (blocking *blocking-default*)
		     (reply *reply-default*) (timeout *timeout-default*)
		     (retries *retries-default*) (retries-interval *retries-interval-default*)
		     (default-answer *default-answer-default*))
  "Direct command STOPPROGRAM."
  (let ((msg (make-initial-direct-packet 4 reply #X01)))
    (create-packet msg)
    (if (eql stream 0)
	msg
	(let ((answer (send-packet stream msg blocking timeout retries retries-interval)))
	  (cond
	    ((< answer 0) default-answer)
	    ((null reply) answer)
	    (t (decode-reply (recv-packet stream 5 blocking timeout retries retries-interval)
			     :simple)))))))

(defun direct-play-soundfile (loop? filename &key
		       (stream *standard-nxt-io*) (blocking *blocking-default*)
 		       (reply *reply-default*) (timeout *timeout-default*)
		       (retries *retries-default*) (retries-interval *retries-interval-default*)
		       (default-answer *default-answer-default*))
  "Direct command PLAYSOUNDFILE; loop? has to be a boolean; filename has to be an
ASCIIZ string with maximum size [15.3] char + null terminator."
  (assert (arrayp filename) (filename)
	  "play-soundfile: filename is not an array/string: ~A" filename)
  (let ((msg (make-initial-direct-packet 25 reply #X02)))
    (vector-push (if loop? #X01 #X00) msg)
    (push-string filename 20 msg)
    (create-packet msg)    
    (if (eql stream 0)
	msg
	(let ((answer (send-packet stream msg blocking timeout retries retries-interval)))
	  (cond
	    ((< answer 0) default-answer)
	    ((null reply) answer)
	    (t (decode-reply (recv-packet stream 5 blocking timeout retries retries-interval)
			     :simple)))))))

(defun direct-play-tone (frequency duration &key
		  (stream *standard-nxt-io*) (blocking *blocking-default*)
		  (reply *reply-default*) (timeout *timeout-default*)
		  (retries *retries-default*) (retries-interval *retries-interval-default*)
		  (default-answer *default-answer-default*))
  "Direct command PLAYTONE. Frequency (hz)
has to be in range 200 - 14000; duration (ms) isn't specified (probably max
65535)."
  (assert (and (numberp frequency) (<= 200 frequency 14000)) (frequency)
	 "play-tone: frequency is not a number or out of range [200-14000]: ~A" frequency)
  (assert (and (numberp duration) (<= 0 duration 65535)) (duration)
	  "play-tone: duration is not a number or out of range [200-14000]: ~A" duration)
  (let ((msg (make-initial-direct-packet 8 reply #X03)))
    (push-2bytes frequency msg)
    (push-2bytes duration msg)
    (create-packet msg)
    (if (eql stream 0)
	msg
	(let ((answer (send-packet stream msg blocking timeout retries retries-interval)))
	  (cond
	    ((< answer 0) default-answer)
	    ((null reply) answer)
	    (t (decode-reply (recv-packet stream 5 blocking timeout retries retries-interval)
			     :simple)))))))

(defun set-output-state (output-port power mode regulation turn-ratio run-state tacho-limit
			 &key
			 (stream *standard-nxt-io*) (blocking *blocking-default*)
			 (reply *reply-default*) (timeout *timeout-default*)
			 (retries *retries-default*) (retries-interval *retries-interval-default*)
			 (default-answer *default-answer-default*))
  "Direct command SETOUTPUTSTATE.
Argument description:

output-port has to be in range 0 to 2 inclusive or #XFF meaning 'all'

power has to be in range -100 to 100 inclusive

mode byte recognises 3 discreet values:
 #X01 - turn on the specified motor
 #X02 - use run/break instead of run/float in PWM
 #X04 - turn on the regulation

regulation byte recognises 3 discreet values:
 #X00 - no regulation will be enabled
 #X01 - power control will be enabled on specified output
 #X02 - synchronisation will be enabled (needs enabled on two outputs)

turn-ration has to be in range -100 to 100 inclusive

run-state byte recognises 4 discreet values:
 #X00 - output will be idle
 #X10 - output will ramp-up
 #X20 - output will be running
 #X40 - output will ramp-down

tacho-limit has to be in range 0 - 2^32-1 inclusive; 0 is a special value - run forever"

  (assert (and (numberp output-port) (or (<= 0 output-port 2) (= output-port #XFF)))
	  (output-port)
	  "set-output-state: output-port is not a number or has an invalid value [0-2; #XFF]: ~A" output-port)
  (assert (and (numberp power) (<= -100 power 100))
	  (power)
	  "set-output-state: power is not a number or has an invalid value [-100 - 100]: ~A" power)
  (assert (and (numberp mode) (or (= mode #X01) (= mode #X02) (= mode #X04)))
	  (mode)
	  "set-output-state: mode is not a number or has an invalid value [#X01, #X02, #X04]: ~A" mode)
  (assert (and (numberp regulation) (or (= regulation #X00) (= regulation #X01) (= regulation #X02)))
	  (regulation)
	  "set-output-state: regulation is not a number or has an invalid value [#X00, #X01, #X02]: ~A" regulation)
  (assert (and (numberp turn-ratio) (<= -100 turn-ratio 100))
	  (turn-ratio)
	  "set-output-state: turn-ration is not a number or has an invalid value [-100 - 100]: ~A" turn-ratio)
  (assert (and (numberp run-state) (or (= run-state #X00) (= run-state #X10) (= run-state #X20) (= run-state #X40)))
	  (run-state)
	  "set-output-state: run-state is not a number or has an invalid value [#X00, #X10, #X20, #X40]: ~A" run-state)
  (assert (and (numberp tacho-limit) (<= 0 tacho-limit 4294967295))
	  (tacho-limit)
	  "set-output-state: tacho-limit is not a number or has an invalid value [0 - 4294967295]: ~A" tacho-limit)
  (let ((msg (make-initial-direct-packet 15 reply #X04)))
    (vector-push output-port msg)
    (vector-push power msg)
    (vector-push mode msg)
    (vector-push regulation msg)
    (vector-push turn-ratio msg)
    (push-4bytes tacho-limit msg)
    (create-packet msg)
    (if (eql stream 0)
	msg
	(let ((answer (send-packet stream msg blocking timeout retries retries-interval)))
	  (cond
	    ((< answer 0) default-answer)
	    ((null reply) answer)
	    (t (decode-reply (recv-packet stream 5 blocking timeout retries retries-interval)
			     :simple)))))))

(defun set-input-mode (input-port sensor-type sensor-mode &key
			    (stream *standard-nxt-io*) (blocking *blocking-default*)
			    (reply *reply-default*) (timeout *timeout-default*)
			    (retries *retries-default*) (retries-interval *retries-interval-default*)
			    (default-answer *default-answer-default*))
  "Direct command SETINPUTMODE.
Argument description:

input-port - has to be in range 0 to 3 inclusive

sensor-type recognises following values:
 #X00 - no sensor
 #X01 - switch
 #X02 - temperature
 #X03 - reflection
 #X04 - angle
 #X05 - light active
 #X06 - light inactive
 #X07 - sound dB
 #X08 - sound dBA
 #X09 - custom
 #X0A - lowspeed
 #X0B - lowspeed 9V
 #X0C - no of sensor types

sensor-mode recognises following values:
 #X00 - raw mode
 #X20 - boolean mode
 #X40 - transition cnt mode
 #X60 - period counter mode
 #X80 - pct full scale mode
 #XA0 - celsius mode
 #XC0 - fahrenheit mode
 #XE0 - angle step mode
 #X1F - slope mask
 #XE0 - mode mask"
  (assert (and (numberp input-port) (<= 0 input-port 3)) (input-port)
	  "set-input-mode: input-port is not a number or has an invalid value [0 - 3]: ~A" input-port)
  (assert (and (numberp sensor-type) (or (= sensor-type #X00) (= sensor-type #X01)
					 (= sensor-type #X02) (= sensor-type #X03)
					 (= sensor-type #X04) (= sensor-type #X05)
					 (= sensor-type #X06) (= sensor-type #X07)
					 (= sensor-type #X08) (= sensor-type #X09)
					 (= sensor-type #X0A) (= sensor-type #X0B)
					 (= sensor-type #X0C)))
	  (sensor-type)
	  "set-input-mode: sensor-type is not a number or has an invalid value [#X00 - #X0C]: ~A" sensor-type)
  (assert (and (numberp sensor-mode) (or (= sensor-mode #X00) (= sensor-mode #X20)
					 (= sensor-mode #X40) (= sensor-mode #X60)
					 (= sensor-mode #X80) (= sensor-mode #XA0)
					 (= sensor-mode #XC0) (= sensor-mode #XE0)
					 (= sensor-mode #X1F) (= sensor-mode #XE0)))
	  (sensor-mode)
	  "set-input-mode: sensor-mode is not a number or has an invalid value: ~A" sensor-mode)
  (let ((msg (make-initial-direct-packet 7 reply #X05)))
    (vector-push input-port msg)
    (vector-push sensor-type msg)
    (vector-push sensor-mode msg)
    (create-packet msg)
    (if (eql stream 0)
	msg
	(let ((answer (send-packet stream msg blocking timeout retries retries-interval)))
	  (cond
	    ((< answer 0) default-answer)
	    ((null reply) answer)
	    (t (decode-reply (recv-packet stream 5 blocking timeout retries retries-interval)
			     :simple)))))))

(defun get-output-state (output-port &key
			 (stream *standard-nxt-io*) (blocking *blocking-default*)
			 (reply *reply-default*) (timeout *timeout-default*)
			 (retries *retries-default*) (retries-interval *retries-interval-default*)
			 (default-answer *default-answer-default*))
  "Direct command GETOUTPUTSTATE. Argument description:
output-port - has to be in range 0 to 2 inclusive

Function returns a list of bytes. First element of the list is the output port byte (Byte 3 in
Lego Direct commands protocol.) It is up to the user to handle the values correctly. For
additional information about byte values, see Lego Direct commands protocol."
  (assert (and (numberp output-port) (<= 0 output-port 2)) (output-port)
	  "get-output-state: output-port is not a number or has an invalid value [0 - 2]: ~A" output-port)
  (let ((msg (make-initial-direct-packet 5 reply #X06)))
    (vector-push output-port msg)
    (create-packet msg)
    (if (eql stream 0)
	msg
	(let ((answer (send-packet stream msg blocking timeout retries retries-interval)))
	  (cond
	    ((< answer 0) default-answer)
	    ((null reply) answer)
	    (t (decode-reply (recv-packet stream 27 blocking timeout retries retries-interval)
			     :raw)))))))

(defun get-input-values (input-port &key
			 (stream *standard-nxt-io*) (blocking *blocking-default*)
			 (reply *reply-default*) (timeout *timeout-default*)
			 (retries *retries-default*) (retries-interval *retries-interval-default*)
			 (default-answer *default-answer-default*))
  "Direct command GETINPUTVALUES. Argument description:
input-port - haf to be in range 0 - 3 inclusive

Function returns a list of bytes. First element of the list is the input port byte (Byte 3 in
Lego Direct commands protocol.) It is up to the user to handle the values correctly. For
additional information about byte values, see Lego Direct commands protocol."
  (assert (and (numberp input-port) (<= 0 input-port 3)) (input-port)
	  "get-input-values: input-port is not a number or has an invalid value [0 - 3]: ~A" input-port)
  (let ((msg (make-initial-direct-packet 5 reply #X07)))
    (vector-push input-port msg)
    (create-packet msg)
    (if (eql stream 0)
	msg
	(let ((answer (send-packet stream msg blocking timeout retries retries-interval)))
	  (cond
	    ((< answer 0) default-answer)
	    ((null reply) answer)
	    (t (decode-reply (recv-packet stream 18 blocking timeout retries retries-interval)
			     :raw)))))))

(defun reset-input-scaled-value (input-port &key
				 (stream *standard-nxt-io*) (blocking *blocking-default*)
				 (reply *reply-default*) (timeout *timeout-default*)
				 (retries *retries-default*) (retries-interval *retries-interval-default*)
				 (default-answer *default-answer-default*))
  "Direct command RESETINPUTSCALEDVALUE. Argument description:
input-port - has to be in range 0 to 3 inclusive"
  (let ((msg (make-initial-direct-packet 5 reply #X08)))
    (vector-push input-port msg)
    (create-packet msg)
    (if (eql stream 0)
	msg
	(let ((answer (send-packet stream msg blocking timeout retries retries-interval)))
	  (cond
	    ((< answer 0) default-answer)
	    ((null reply) answer)
	    (t (decode-reply (recv-packet stream 5 blocking timeout retries retries-interval)
			     :simple)))))))

(defun message-write (inbox msg-len msg-data &key
		      (stream *standard-nxt-io*) (blocking *blocking-default*)
		      (reply *reply-default*) (timeout *timeout-default*)
		      (retries *retries-default*) (retries-interval *retries-interval-default*)
		      (default-answer *default-answer-default*))
  "Direct command MESSAGEWRITE. Arguments description:
inbox - the inbox number; has to be in range 0 to 9
msg-len - message length including the terminating null; message can't be longer than 59 bytes
msg-data - message (of length msg-len); treated as a string, must be null terminated"
  (assert (and (numberp inbox) (<= 0 inbox 9)) (inbox)
	  "message-write: inbox is not a nubmer or has an invalid value [0 - 9]: ~A" inbox)
  (assert (and (numberp msg-len) (<= 0 msg-len 59)) (msg-len)
	  "message-write: msg-len is not a number or has an invalid value [0 - 59]: ~A" msg-len)
  (assert (arrayp msg-data) (msg-data)
	  "message-write: msg-data is not a array/string")
  (let ((msg (make-initial-direct-packet (+ msg-len 6) reply #X09)))
    (vector-push inbox msg)
    (vector-push msg-len msg)
    (loop for i from 0 to (1- msg-len) doing (vector-push (aref msg-data i) msg))
    (create-packet msg)
    (if (eql stream 0)
	msg
	(let ((answer (send-packet stream msg blocking timeout retries retries-interval)))
	  (cond
	    ((< answer 0) default-answer)
	    ((null reply) answer)
	    (t (decode-reply (recv-packet stream 5 blocking timeout retries retries-interval)
			     :simple)))))))

(defun reset-motor-position (output-port relative? &key
			     (stream *standard-nxt-io*) (blocking *blocking-default*)
			     (reply *reply-default*) (timeout *timeout-default*)
			     (retries *retries-default*) (retries-interval *retries-interval-default*)
			     (default-answer *default-answer-default*))
  "Direct command RESETMOTORPOSITION. Arguments description:
output-port - has to be in range 0 to 2 inclusive
relative? - boolean; TRUE: position relative to last movement, FALSE: absolute position"
  (assert (and (numberp output-port) (<= 0 output-port 2)) (output-port)
	  "reset-motor-position: output-port is not a number or has an invalid value [0 - 2]: ~A" output-port)
  (let ((msg (make-initial-direct-packet 6 reply #X0A)))
    (vector-push output-port msg)
    (vector-push (if relative? #X01 #X00) msg)
    (create-packet msg)
    (if (eql stream 0)
	msg
	(let ((answer (send-packet stream msg blocking timeout retries retries-interval)))
	  (cond
	    ((< answer 0) default-answer)
	    ((null reply) answer)
	    (t (decode-reply (recv-packet stream 5 blocking timeout retries retries-interval)
			     :simple)))))))

(defun get-battery-level (&key
			  (stream *standard-nxt-io*) (blocking *blocking-default*)
			  (reply *reply-default*) (timeout *timeout-default*)
			  (retries *retries-default*) (retries-interval *retries-interval-default*)
			  (default-answer *default-answer-default*))
  "Direct command GETBATTERYLEVEL."
  (let ((msg (make-initial-direct-packet 4 reply #X0B)))
    (create-packet msg)
    (if (eql stream 0)
	msg
	(let ((answer (send-packet stream msg blocking timeout retries retries-interval)))
	  (cond
	    ((< answer 0) default-answer)
	    ((null reply) answer)
	    (t (decode-reply (recv-packet stream 7 blocking timeout retries retries-interval)
			     :uword)))))))


(defun stop-sound-playback (&key
			    (stream *standard-nxt-io*) (blocking *blocking-default*)
			    (reply *reply-default*) (timeout *timeout-default*)
			    (retries *retries-default*) (retries-interval *retries-interval-default*)
			    (default-answer *default-answer-default*))
  "Direct command STOPSOUNDPLAYBACK."
  (let ((msg (make-initial-direct-packet 4 reply #X0C)))
    (create-packet msg)
    (if (eql stream 0)
	msg
	(let ((answer (send-packet stream msg blocking timeout retries retries-interval)))
	  (cond
	    ((< answer 0) default-answer)
	    ((null reply) answer)
	    (t (decode-reply (recv-packet stream 5 blocking timeout retries retries-interval)
			     :simple)))))))

(defun keep-alive (&key
		   (stream *standard-nxt-io*) (blocking *blocking-default*)
		   (reply *reply-default*) (timeout *timeout-default*)
		   (retries *retries-default*) (retries-interval *retries-interval-default*)
		   (default-answer *default-answer-default*))
  "Direct command KEEPALIVE."
  (let ((msg (make-initial-direct-packet 4 reply #X0D)))
    (create-packet msg)
    (if (eql stream 0)
	msg
	(let ((answer (send-packet stream msg blocking timeout retries retries-interval)))
	  (cond
	    ((< answer 0) default-answer)
	    ((null reply) answer)
	    (t (decode-reply (recv-packet stream 9 blocking timeout retries retries-interval)
			     :ulong)))))))

(defun ls-get-status (port &key
		      (stream *standard-nxt-io*) (blocking *blocking-default*)
		      (reply *reply-default*) (timeout *timeout-default*)
		      (retries *retries-default*) (retries-interval *retries-interval-default*)
		      (default-answer *default-answer-default*))
  "Direct command LSGETSTATUS. Argument description:
port - has to be in range 0 to 3 inclusive"
  (assert (and (numberp port) (<= 0 port 3)) (port)
	  "ls-get-status: port is not a number or out of range [0 - 3]: ~A" port)
  (let ((msg (make-initial-direct-packet 5 reply #X0E)))
    (vector-push port msg)
    (create-packet msg)
    (if (eql stream 0)
	msg
	(let ((answer (send-packet stream msg blocking timeout retries retries-interval)))
	  (cond
	    ((< answer 0) default-answer)
	    ((null reply) answer)
	    (t (decode-reply (recv-packet stream 6 blocking timeout retries retries-interval)
			     :ubyte)))))))

(defun ls-write (port tx-len rx-len data &key
		 (stream *standard-nxt-io*) (blocking *blocking-default*)
		 (reply *reply-default*) (timeout *timeout-default*)
		 (retries *retries-default*) (retries-interval *retries-interval-default*)
		 (default-answer *default-answer-default*))
  "Direct command LSWRITE. Arguments description:
port - has to be in range 0 to 3 inclusive
tx-len - tx data length (in bytes)
rx-len - rx data length (in bytes)
data - data; length is limited to 16 bytes per command"
  (assert (and (numberp port) (<= 0 port 3)) (port)
	  "ls-write: port is not a number or has an invalid value [0 - 2]: ~A" port)
  (assert (numberp tx-len) (tx-len) ; not sure about the limit so I don't test it
	  "ls-write: tx-len is not a number: ~A" tx-len)
  (assert (numberp rx-len) (rx-len)
	  "ls-write: rx-len is not a number: ~A" rx-len)
  (assert (arrayp data) (data)
	  "ls-write: data has to be an array")
  (let ((msg (make-initial-direct-packet (+ tx-len 7) reply #X0F)))
    (vector-push port msg)
    (vector-push tx-len msg)
    (vector-push rx-len msg)
    (dotimes (i tx-len)
      (vector-push (aref data i) msg))
    (create-packet msg)
    (if (eql stream 0)
	msg
	(let ((answer (send-packet stream msg blocking timeout retries retries-interval)))
	  (cond
	    ((< answer 0) default-answer)
	    ((null reply) answer)
	    (t (decode-reply (recv-packet stream 5 blocking timeout retries retries-interval)
			     :simple)))))))

(defun ls-read (port &key
		(stream *standard-nxt-io*) (blocking *blocking-default*)
		(reply *reply-default*) (timeout *timeout-default*)
		(retries *retries-default*) (retries-interval *retries-interval-default*)
		(default-answer *default-answer-default*))
  "Direct command LSREAD. Argument description:
port - has to be in range 0 to 3 inclusive"
  (assert (and (numberp port) (<= 0 port 3)) (port)
	  "ls-read: port is not a number or has an invalid value [0 - 3]: ~A" port)
  (let ((msg (make-initial-direct-packet 5 reply #X0F)))
    (vector-push port msg)
    (create-packet msg)
    (if (eql stream 0)
	msg
	(let ((answer (send-packet stream msg blocking timeout retries retries-interval)))
	  (cond
	    ((< answer 0) default-answer)
	    ((null reply) answer)
	    (t (decode-reply (recv-packet stream 22 blocking timeout retries retries-interval)
			     :raw)))))))

(defun get-current-program-name (&key
				 (stream *standard-nxt-io*) (blocking *blocking-default*)
				 (reply *reply-default*) (timeout *timeout-default*)
				 (retries *retries-default*) (retries-interval *retries-interval-default*)
				 (default-answer *default-answer-default*))
  "Direct command GETCURRENTPROGRAMNAME."
  (let ((msg (make-initial-direct-packet 4 reply #X11)))
    (create-packet msg)
    (if (eql stream 0)
	msg
	(let ((answer (send-packet stream msg blocking timeout retries retries-interval)))
	  (cond
	    ((< answer 0) default-answer)
	    ((null reply) answer)
	    (t (decode-reply (recv-packet stream 25 blocking timeout retries retries-interval)
			     :filename)))))))

(defun message-read (remote-inbox local-inbox remove? &key
		     (stream *standard-nxt-io*) (blocking *blocking-default*)
		     (reply *reply-default*) (timeout *timeout-default*)
		     (retries *retries-default*) (retries-interval *retries-interval-default*)
		     (default-answer *default-answer-default*))
  "Direct command MESSAGEREAD. Arguments description:
remote-inbox - has to be in range 0 to 19 inclusive
local-inbox - has to be in range 0 to 9 inclusive
remove - boolean; TRUE clears message from remote-inbox

For additional information and semantics see the Lego Direct Commands protocol."
  (assert (and (numberp remote-inbox) (<= 0 remote-inbox 19)) (remote-inbox)
	  "message-read: remote-inbox is not a number or has an invalid value [0 - 19]: ~A" remote-inbox)
  (assert (and (numberp local-inbox) (<= 0 local-inbox 9)) (local-inbox)
	  "message-read: local-inbox is not a number or has an invalid value [0 - 9]: ~A" local-inbox)
  (let ((msg (make-initial-direct-packet 7 reply #X13)))
    (vector-push remote-inbox msg)
    (vector-push local-inbox msg)
    (vector-push (if remove? #X01 #X00) msg)
    (create-packet msg)
    (if (eql stream 0)
	msg
	(let ((answer (send-packet stream msg blocking timeout retries retries-interval)))
	  (cond
	    ((< answer 0) default-answer)
	    ((null reply) answer)
	    (t (decode-reply (recv-packet stream 66 blocking timeout retries retries-interval)
			     :raw)))))))

;;;
;;; various helper functions
;;;


(defun push-string (string maxlength vector)
  "Appends string to the tail of the vector. Lenght has to be the maximal possible
length of the string without null terminator. Empty slots are filled with #\0."
  (dotimes (c (length string))
    (vector-push (char-code (aref string c)) vector))
  (dotimes (foo (- maxlength (length string))) ; zero padding
    (vector-push #X00 vector)) 
  (vector-push #X00 vector)) ; null termination

(defun push-2bytes (number vector)
  (multiple-value-bind (lsb msb) (dec-to-2bytes number)
    (vector-push lsb vector)
    (vector-push msb vector)))

(defun push-4bytes (number vector)
  (multiple-value-bind (one two three four) (dec-to-4bytes number)
    (vector-push one vector)
    (vector-push two vector)
    (vector-push three vector)
    (vector-push four vector)))

(defun dec-to-2bytes (number)
  "Converts a decimal number (max 65535) to its representation in 2 bytes.
Returns multiple values - least significant byte and most significant byte." 
  (cond
    ((< number 256) (values number 0))
    ((eql 0 (rem number 256)) (values 0 (truncate (/ number 256))))
    (t (values (rem number 256) (truncate (/ number 256))))))

(defun dec-to-4bytes (number)
  "Converts a decimal number (max 4294967295) to its little-endian representation
in 4 bytes. Returns four multiple values (each byte as one)."
  (cond
    ((< number 256) (values number 0 0 0))
    ((eql 0 (rem number 16777216)) (values 0 0 0 (truncate (/ number 16777216))))
    ((eql 0 (rem number 65536)) (values 0 0 (truncate (/ number 65536)) 0))
    ((eql 0 (rem number 256)) (values 0 (truncate (/ number 256)) 0 0))
    ;; crazy! maybe there's a better way but I'm not in a thinking mood today
    (t (let* ((one (rem number 256))
	      (two (rem (/ (- number one) 256) 256))
	      (three (rem (/ (- (/ (- number one) 256) two) 256) 256))
	      (four (truncate (/ (- (/ (- (/ (- number one) 256) two) 256) three) 256))))
	 (values one two three four)))))

(defun make-initial-system-packet (size reply command)
  "Creates a system command packet with correct initial contents (reply and command bytes)."
  (let ((packet (make-array size :element-type '(unsigned-byte 8)
			    :initial-element 0 :fill-pointer 2)))
    (vector-push (if reply #X01 #X81) packet)
    (vector-push command packet)
    packet))

(defun make-initial-direct-packet (size reply command)
  "Creates a direct command packet of correct length and fills in reply and command bytes."
  (let ((packet	(make-array size :element-type '(unsigned-byte 8)
		     :initial-element 0 :fill-pointer 2)))
    (vector-push (if reply #X00 #X80) packet)
    (vector-push command packet)
    packet))

(defun create-packet (vector)
  "Forms a legal BT packet by inserting the length of the message
to the first two bytes of the packet. For additional information,
consult the Lego Communication protocol."
  ;; len holds the length of the whole packet which will be transmited
  ;; by Bluetooth, that is with 2 header bytes representing packet length
  (let ((len (fill-pointer vector)))
    (setf (fill-pointer vector) 0)
    (push-2bytes (- len 2) vector)     ; push header bytes
    (setf (fill-pointer vector) len)))