;;;;
;;;;
;;;; File: communication.lisp
;;;;
;;;; License: Apache License 2.0
;;;;
;;;; Contributors: Milan Cermak, milan.cermak@gmail.com
;;;;
;;;; Description: This file includes stuff relevant to communication
;;;;              in the NXTLisp project.
;;;;
;;;;


#|

 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)

;; define libbtnxtlisp CFFI wrappers

(define-foreign-library libbtnxtlisp
  (t (:default "libbtnxtlisp")))

;; ok, so what's the difference between load-foreign-library and use-foreign-library?
;; (load-foreign-library libbtnxtlisp)
(use-foreign-library libbtnxtlisp)

(defcfun "open_nxt_stream" :int
  (nxt-address :string) (channel :pointer))

(defcfun "close_nxt_stream" :int
  (socket :int))

(defcfun "send_msg" :int
  (socket :int)
  (msg :string)
  (length :int)
  (blocking :int)
  (timeout :int)
  (retries :int)
  (retries-interval :int))

(defcfun "recv_msg" :int
  (socket :int)
  (msg :string)
  (length :int)
  (blocking :int)
  (timeout :int)
  (retries :int)
  (retries-interval :int))



(defun decode-reply (reply type)
  "Decodes the received packet. Reply has to be an array."
  (let ((decoded))    
    (case type
      (:simple (if (eql #X00 (aref reply 4)) ; check the status byte
		   (setf decoded t)
		   (setf decoded nil)))
      (:raw (if (eql #X00 (aref reply 4))
		(progn
		  (loop for i from 5 to (1- fill-pointer reply) doing
			(push (aref reply i) decoded))
		  (setf decoded (reverse decoded)))
		(setf decoded nil)))
      (:ubyte (if (eql #X00 (aref reply 4))
		  (setf decoded (aref reply 5))
		  (setf decoded nil)))
      (:uword (if (eql #X00 (aref reply 4))
		  (setf decoded (+ (aref reply 5) (* (aref reply 6) 256))) ; little endian
		  (setf decoded nil)))
      (:ulong (if (eql #X00 (aref reply 4))
		  (setf decoded (+ (aref reply 5) (* (aref reply 6) 256)   ; little endian
				   (* (aref reply 7) 65536) (* (aref reply 8) 16777216)))
		  (setf decoded nil)))
      (:filename (if (eql #X00 (aref reply 4))
		   (progn
		     (setf decoded (make-string 20 :initial-element #\0))
		     (loop for ri from 5 to 24
			   and si from 0 doing
			   (setf (aref decoded si) (code-char (aref reply ri)))))
		   (setf decoded nil)))
      (otherwise (warn "decode-reply: unknown decode request")))
    decoded))

(defun send-packet (socket packet blocking timeout retries retries-interval)
  "Function handles packet sending through calling a C function.
Socket must be a file descriptor of an opened socket. Packet must
be a well-formed BT packet in the form of an array with :fill-pointer.
Returns t on successful sending or nil otherwise."
  (let ((length (fill-pointer packet)))
    (if (eql socket nil) ; if debugging packets
	packet           ; show what would be sent
	(with-foreign-string (msg packet)
	  (with-foreign-objects ((sd :int) (len :int) (blk :int)
				 (tout :int) (retr :int) (retr-i :int))
	     ;; set the foreign variables to correct values
	    (setf (mem-ref sd :int) socket
		  (mem-ref len :int) length
		  (mem-ref blk :int) (if blocking 0 1) ; as implemented in libbtnxtlisp
		  (mem-ref tout :int) timeout
		  (mem-ref retr :int) retries
		  (mem-ref retr-i :int) retries-interval)
	    ;; send packet (by calling foreign function) and store return value
	    (send-msg (mem-ref sd :int) msg (mem-ref len :int) (mem-ref blk :int)
		      (mem-ref tout :int) (mem-ref retr :int) (mem-ref retr-i :int)))))))

(defun recv-packet (socket length blocking timeout retries retries-interval)
  "Function receives packet from the NXT, usually a reply to a command. It
returns an array of (unsigned-byte 8) representing the packet."
  (let ((received 0)
	(retval (make-array length :element-type '(unsigned-byte 8) :fill-pointer 0)))
    (with-foreign-objects ((sd :int) (len :int) (blk :int) (tout :int)
			  (retr :int) (retr-i :int))
      ;; set foreign variables to correct values for the transmission
      (setf (mem-ref sd :int) socket
	    (mem-ref len :int) length
	    (mem-ref blk :int) (if blocking 0 1)
	    (mem-ref tout :int) timeout
	    (mem-ref retr :int) retries
	    (mem-ref retr-i :int) retries-interval)
      ;; msg serves as a buffer for the packet received from the NXT
      (with-foreign-object (msg :uchar length)
	(setf received (recv-msg (mem-ref sd :int) msg (mem-ref len :int)
				 (mem-ref blk :int) (mem-ref tout :int)
				 (mem-ref retr :int) (mem-ref retr-i :int)))
	(if (eql received -1)
	    (error "recv-packet: packet couldn't be received"))
	(dotimes (i received)
	  (vector-push (mem-aref msg :uchar i) retval))))
    retval))

(defmacro with-open-nxt-stream ((nxt-address port &rest keys) &body body)
  `(call-with-open-nxt-stream ,nxt-address ,port (lambda () ,@body) ,@keys))

(defun call-with-open-nxt-stream (nxt-address port function &key
				  (blocking *blocking-default*)
				  (reply *reply-default*)
				  (timeout *timeout-default*)
				  (retries *retries-default*)
				  (retries-interval *retries-interval-default*)
				  (default-answer *default-answer-default*))
  (let ((*blocking-default* blocking)
	(*reply-default* reply)
	(*timeout-default* timeout)
	(*retries-default* retries)
	(*retries-interval-default* retries-interval)
	(*default-answer-default* default-answer))    
    (let ((*standard-nxt-io* (if (eql 0 nxt-address)
				 0
				 (call-open-nxt-stream nxt-address port))))
      (if (< *standard-nxt-io* 0) ; couldn't open socket
	  *default-answer-default*
	  (progn
	    (unwind-protect
		 ;; give the NXT time to recover from the fact, that we have
		 ;; just established a connection (0.5 is an empirical value)
		 (sleep 0.5)
		 (funcall function)
	      (call-close-nxt-stream *standard-nxt-io*)))))))


(defun call-open-nxt-stream (nxt-address channel)
  "Calls foreign function open-nxt-stream."
  (with-foreign-string (adr nxt-address)
    (with-foreign-pointer (chan 1)
      (setf (mem-ref chan :int) channel)
      (open-nxt-stream adr chan))))

(defun call-close-nxt-stream (socket)
  "Calls foreign function close-nxt-stream."
  (with-foreign-object (sd :int)
    (setf (mem-ref sd :int) socket)
    (close-nxt-stream (mem-ref sd :int))))

;; EOF