#!/data/languages/clisp/bin/clisp -ansi -q -Kfull
;;;; -*- mode:lisp; coding:iso-8859-1 -*-
;;;;FILE:              dump-net-fdm
;;;;LANGUAGE:          common lisp (clisp)
;;;;SYSTEM:            UNIX
;;;;    This script dumps net-fdm UDP traffic to the port 5510.
;;;;    Launch fgfs with: ::
;;;;       fgfs_period=20
;;;;       slaveIP=localhost
;;;;       fgfs --native-fdm=socket,out,${fgfs_period},${slaveIP},5510,udp  ...
;;;;    Launch dump-net-fdm in a terminal:
;;;;       xterm -e ./dump-net-fdm
;;;;    <PJB> Pascal J. Bourguignon
;;;;    2011-04-07 <PJB> Created.
;;;;    Copyright Pascal J. Bourguignon 2002 - 2002
;;;;    This script is free software; you can redistribute it and/or
;;;;    modify it under the terms of the GNU  General Public
;;;;    License as published by the Free Software Foundation; either
;;;;    version 2 of the License, or (at your option) any later version.
;;;;    This script is distributed in the hope that it will be useful,
;;;;    but WITHOUT ANY WARRANTY; without even the implied warranty of
;;;;    General Public License for more details.
;;;;    You should have received a copy of the GNU General Public
;;;;    License along with this library; see the file COPYING.LIB.
;;;;    If not, write to the Free Software Foundation,
;;;;    59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.

(load "~/.clisprc.lisp")
(require "rawsock")
(push #P"~/src/lisp/fgfs/" asdf:*central-registry*)
(asdf-load :com.ogamita.fgfs.dump-net-fdm)
(use-package :com.ogamita.fgfs.net-fdm-24)

(defun square (x) (* x x))

(defun print-fdm (fdm)
  (with-accessors ((version net-fdm-version)
                   (padding net-fdm-padding)
                   (longitude net-fdm-longitude)
                   (latitude net-fdm-latitude)
                   (altitude net-fdm-altitude)
                   (agl net-fdm-agl)
                   (phi net-fdm-phi)
                   (theta net-fdm-theta)
                   (psi net-fdm-psi)
                   (alpha net-fdm-alpha)
                   (beta net-fdm-beta)
                   (phidot net-fdm-phidot)
                   (thetadot net-fdm-thetadot)
                   (psidot net-fdm-psidot)
                   (vcas net-fdm-vcas)
                   (climb-rate net-fdm-climb-rate)
                   (v-north net-fdm-v-north)
                   (v-east net-fdm-v-east)
                   (v-down net-fdm-v-down)
                   (v-wind-body-north net-fdm-v-wind-body-north)
                   (v-wind-body-east net-fdm-v-wind-body-east)
                   (v-wind-body-down net-fdm-v-wind-body-down)
                   (A-X-pilot net-fdm-A-X-pilot)
                   (A-Y-pilot net-fdm-A-Y-pilot)
                   (A-Z-pilot net-fdm-A-Z-pilot)
                   (stall-warning net-fdm-stall-warning)
                   (slip-deg net-fdm-slip-deg)
                   (num-engines net-fdm-num-engines)
                   (eng-state net-fdm-eng-state)
                   (rpm net-fdm-rpm)
                   (fuel-flow net-fdm-fuel-flow)
                   (fuel-px net-fdm-fuel-px)
                   (egt net-fdm-egt)
                   (cht net-fdm-cht)
                   (mp-osi net-fdm-mp-osi)
                   (tit net-fdm-tit)
                   (oil-temp net-fdm-oil-temp)
                   (oil-px net-fdm-oil-px)
                   (num-tanks net-fdm-num-tanks)
                   (fuel-quantity net-fdm-fuel-quantity)
                   (num-wheels net-fdm-num-wheels)
                   (wow net-fdm-wow)
                   (gear-pos net-fdm-gear-pos)
                   (gear-steer net-fdm-gear-steer)
                   (gear-compression net-fdm-gear-compression)
                   (cur-time net-fdm-cur-time)
                   (warp net-fdm-warp)
                   (visibility net-fdm-visibility)
                   (elevator net-fdm-elevator)
                   (elevator-trim-tab net-fdm-elevator-trim-tab)
                   (left-flap net-fdm-left-flap)
                   (right-flap net-fdm-right-flap)
                   (left-aileron net-fdm-left-aileron)
                   (right-aileron net-fdm-right-aileron)
                   (rudder net-fdm-rudder)
                   (nose-wheel net-fdm-nose-wheel)
                   (speedbrake net-fdm-speedbrake)
                   (spoilers net-fdm-spoilers)) fdm
    (format t "~Cc~A"
            (code-char 27)
            (format nil "
long    ~10,6F alti ~8,2F      #engi ~D        Engines
lati    ~10,6F agl  ~8,2F      estat   ~D        ~D        ~D        ~D
                   climb  ~7,3F     rpm   ~7,2F ~7,2F ~7,2F ~7,2F
alpha   ~7,3F    stall   ~3,0F        flow  ~7,2F ~7,2F ~7,2F ~7,2F
speed  ~6,1F                         fpres ~7,2F ~7,2F ~7,2F ~7,2F
wind    ~7,3F                       egt   ~7,2F ~7,2F ~7,2F ~7,2F
time  ~12F                    cht   ~7,2F ~7,2F ~7,2F ~7,2F
warp  ~12F                    mposi ~7,2F ~7,2F ~7,2F ~7,2F
visi  ~7,1F                         tit   ~7,2F ~7,2F ~7,2F ~7,2F
  eleva ~7,3F    etrim ~7,3F      oilt  ~7,2F ~7,2F ~7,2F ~7,2F
  lflap ~7,3F    rflap ~7,3F      oilp  ~7,2F ~7,2F ~7,2F ~7,2F
  laile ~7,3F    raile ~7,3F                    Fuel Tanks
  ruder ~7,3F    spoil ~7,3F      #tanks ~D
  nosew ~7,3F    brake ~7,3F      fuelq ~7,2F ~7,2F ~7,2F ~7,2F
#wheels ~D
wow    ~7,2F ~7,2F ~7,2F
gpos   ~7,2F ~7,2F ~7,2F
gsteer ~7,2F ~7,2F ~7,2F
gcompr ~7,2F ~7,2F ~7,2F
                    longitude altitude num-engines
                    latitude agl           (aref eng-state 0) (aref eng-state 2) (aref eng-state 2) (aref eng-state 3)
                    climb-rate             (aref rpm 0) (aref rpm 2) (aref rpm 2) (aref rpm 3)
                    alpha stall-warning    (aref fuel-flow 0) (aref fuel-flow 2) (aref fuel-flow 2) (aref fuel-flow 3)
                    (sqrt (+ (square v-north) (square v-east) (square v-down))) (aref fuel-px 0) (aref fuel-px 2) (aref fuel-px 2) (aref fuel-px 3)
                    (sqrt (+ (square v-wind-body-north) (square v-wind-body-east) (square v-wind-body-down))) (aref egt 0) (aref egt 2) (aref egt 2) (aref egt 3)
                    cur-time     (aref cht 0) (aref cht 2) (aref cht 2) (aref cht 3)
                    warp         (aref mp-osi 0) (aref mp-osi 2) (aref mp-osi 2) (aref mp-osi 3)
                    visibility   (aref tit 0) (aref tit 2) (aref tit 2) (aref tit 3)
                    elevator elevator-trim-tab   (aref oil-temp 0) (aref oil-temp 2) (aref oil-temp 2) (aref oil-temp 3)
                    left-flap right-flap         (aref oil-px 0) (aref oil-px 2) (aref oil-px 2) (aref oil-px 3)
                    left-aileron right-aileron
                    rudder spoilers  num-tanks
                    nose-wheel speedbrake  (aref fuel-quantity 0) (aref fuel-quantity 1) (aref fuel-quantity 2) (aref fuel-quantity 3)
                    (aref wow 0) (aref wow 1) (aref wow 2)
                    (aref gear-pos 0) (aref gear-pos 1) (aref gear-pos 2)
                    (aref gear-steer 0) (aref gear-steer 1) (aref gear-steer 2)
                    (aref gear-compression 0) (aref gear-compression 1) (aref gear-compression 2)
                    ;; slip-deg
                    ;; A-X-pilot A-Y-pilot A-Z-pilot
                    ;; phi theta psi
                    ;; beta phidot thetadot psidot vcas

;; (print-fdm (make-net-fdm))

(defmacro check-errors (message &body body)
  (let ((verr (gensym))
        (vmsg (gensym)))
    `(let ((,vmsg ,message))
           (multiple-value-prog1 (progn ,@body)
             (format t "~&~A succeeded.~%" ,vmsg))
         (error (,verr)
           (format t "~%~A: ~A~%" ,vmsg ,verr)
           (throw 'errors))))))

(catch 'errors
  (let* ((port 5510)
         (socket   (rawsock:socket :inet :dgram :ipproto-ip))
         (address  (rawsock:make-sockaddr
                    :inet (vector (ldb (byte 8 8) port)
                                  (ldb (byte 8 0) port)
                                  0 0 0 0 0 0 0 0 0 0 0 0)))
         (remote  (rawsock:make-sockaddr
                   :inet (vector 0 0 0 0 0 0 0 0 0 0 0 0 0 0)))
         (buffer (make-net-fdm-buffer))
         (fdm    (make-net-fdm)))
           (check-errors "bind" (rawsock:bind socket address))
           ;; (check-errors "listen" (rawsock:sock-listen socket))
              (check-errors (format nil "(recvfrom ~A)" socket)
                            (rawsock:recvfrom socket buffer remote))
              (deserialize fdm buffer)
              (print-fdm fdm))))
    (rawsock:sock-close socket)))

;;;; THE END ;;;;