#!/usr/bin/guile -s
!#

;*******************************************************************************
;
;    Derived Parameter Calculations
;    ==============================
;
;    Author:    Alex Measday
;
;    (1) Derived Parameter Lookup Tables (and functions).
;    (2) MNF Update Lists (and functions).
;    (3) Macro Definitions and Utility Functions.
;    (4) Derived Parameter Operators:
;        (a) Bit-wise operators
;        (b) Comparison operators
;        (c) Date functions
;        (d) Integral function
;        (e) Mathematical operators
;        (f) Trigonometric operators
;    (5) Derived Parameter Definition and Term Rewriting.
;    (6) Network Functions
;    (7) Debug Functions
;    (8) Spacecraft-Specific Initialization and Run.
;
;*******************************************************************************

(use-modules (ice-9 pretty-print))

;*******************************************************************************
;
;    Derived Parameter Lookup Tables
;    -------------------------------
;
;    DP-POINT-MAP - is a hash table used to map telemetry-/ground-point
;        mnemonics to "dp-point" objects.  Entries are made in this table
;        for both the derived parameters themselves and any telemetry or
;        ground points that the parameters use in their calculations.
;
;    DP-CALC-MAP - is a hash table used to map derived-parameter "dp-point"
;        objects to the expressions used to calculate the values of the
;        derived parameters.  This table only has entries for the derived
;        parameters themselves.
;
;    DP-CALC-LIST - is a simple list of the derived parameter mnemonics
;        in the order of their definitions.
;
;    DP-UNDEFINED-POINTS - is a list of undefined points.
;
;    DP-NO-TRIGGERS - is a list of derived parameters with no triggers.
;
;*******************************************************************************

(define *dp-point-map* (make-hash-table 1000))

(define *dp-calc-map* (make-hash-table 300))

(define *dp-calc-list* '())

(define *dp-undefined-points* '())

(define *dp-no-triggers* '())


;*******************************************************************************
;    dp-add-point - adds a mnemonic-to-point mapping to the *dp-point-map*
;        hash table.  If this is the first time the point has been added,
;        a request for updates is also registered with the Points service.
;*******************************************************************************

(define (dp-add-point mnemonic . request-flags)
    (let
        ((handle (hashq-get-handle *dp-point-map* mnemonic)))
        (if (not handle)
            (begin
                (hashq-set! *dp-point-map* mnemonic
                    (dp-make-point
                        mnemonic
                        (if (null? request-flags) #f (car request-flags))
                    )
                )
                (set! handle (hashq-get-handle *dp-point-map* mnemonic))
                (if (not (cdr handle))
                    (set! *dp-undefined-points*
                        (append *dp-undefined-points*
                            (list (symbol->string mnemonic)))
                    )
                )
            )
        )
        (if (not (cdr handle))
            (dp-errormsg (string-append "(dp-add-point) " (symbol->string mnemonic) " not found!"))
        )
        (cdr handle)
    )
)


;*******************************************************************************
;    dp-add-calc - adds a point-to-expression mapping to the dp-calc-map
;        hash table.  The mapping maps a derived parameter's "dp-point"
;        object to the parameter's derivation expression.  The function
;        returns #t if a new mapping was added and #f otherwise.
;*******************************************************************************

(define (dp-add-calc point expression)
    (if point
        (if (hashq-get-handle *dp-calc-map* point)
            (begin
                (dp-errormsg (string-append "(dp-add-calc) " (dp-point-name point) " has already been defined!"))
                #f
            )
            (begin
                (hashq-set! *dp-calc-map* point expression)
                #t
            )
        )
        #f
    )
)


;*******************************************************************************
;    dp-parameter - maps a derived parameter's mnemonic to its "dp-point"
;        object (using the *dp-point-map* table) and then to the parameter's
;        derivation expression (using the dp-calc-map table).  The return
;        value is a pair, (<point> . <expr>), consisting of the parameter's
;        "dp-point" and its derivation expression.
;*******************************************************************************

(define (dp-parameter mnemonic)
    (let
        ((handle (hashq-get-handle *dp-point-map* mnemonic)))
        (if handle
            (set! handle (hashq-get-handle *dp-calc-map* (cdr handle)))
        )
        handle
    )
)

;*******************************************************************************
;
;    MNF Update Lists
;    ----------------
;
;    DP-MNF-UPDATE-LISTS - is an array indexed by minor frame ID.  Each entry
;        in the array is a list of derived parameters to be computed when a
;        MNF of the given ID is received.  Each element of an update list is
;        is a pair (as returned by dp-parameter) of a parameter's "dp-point"
;        and its derivation expression.
;
;*******************************************************************************

(define *dp-mnf-update-lists* (make-vector 64 '()))

(define *dp-slice-points* '())

(define *dp-no-trigger-points* '())

(define *dp-num-points-set* 0)
(define *dp-num-points-not-set* 0)

(define *dp-frame-timestamp* #f)


;*******************************************************************************
;    dp-mnf-add-update - adds one or more derived parameters (identified by
;        their mnemonic) to the list of updates for a particular MNF ID.
;*******************************************************************************

(define (dp-mnf-add-update id mnemonics)
    (let
        ((handles '()))
        (if (not (list? mnemonics))
            (set! mnemonics (list mnemonics))
        )
        (for-each
            (lambda (mnemonic)
                (let
                    ((handle (dp-parameter mnemonic)))
                    (if handle
                        (set! handles (append handles (list handle)))
                        (dp-errormsg (string-append "(dp-mnf-add-update) Invalid mnemonic: " (symbol->string mnemonic)))
                    )
                )
            )
            mnemonics
        )
        (if (not (null? handles))
            (vector-set! *dp-mnf-update-lists* id
                (append (vector-ref *dp-mnf-update-lists* id) handles))
        )
    )
)


(define DP-FRESH-MASK  #xC0000000)
(define DP-FRESH-FLAGS #x00000000)
(define DP-STALE-MASK  #x80000000)
(define DP-STALE-FLAGS #x80000000)


(define (dp-evaluate point expression timestamp)
    (let
        ((result (false-if-exception (primitive-eval expression))))
        (if (and
                result
                (false-if-exception
                    (dp-point-set! point result #f timestamp DP-FRESH-MASK DP-FRESH-FLAGS)
                )
            )
            (begin
                (set! *dp-num-points-set* (1+ *dp-num-points-set*))
;               (dp-debugmsg (string-append "(dp-evaluate) " (dp-point-name point) " = " (object->string result)))
            )
            (begin
                (set! *dp-num-points-not-set* (1+ *dp-num-points-not-set*))
                (dp-errormsg (string-append "+++++ Error setting " (dp-point-name point)))
                (if (zero? (logand DP-STALE-MASK (dp-point-system-flags point)))
                    (dp-point-set! point #f #f timestamp DP-STALE-MASK DP-STALE-FLAGS)
                )
            )
        )
    )
)


(define (dp-evaluate-on-mnf id)
    (if (and (number? id) (<= -1 id (vector-length *dp-mnf-update-lists*)))
        (for-each
            (lambda (handle)
                (if (pair? handle)
                    (begin
                        (dp-debugmsg (string-append "Evaluating " (dp-point-name (car handle)) " {"))
                        (dp-evaluate (car handle) (cdr handle) *dp-frame-timestamp*)
                        (dp-debugmsg (string-append "}  Evaluated " (dp-point-name (car handle))))
                    )
                )
            )
            (vector-ref *dp-mnf-update-lists* id)
        )
    )
)

;*******************************************************************************
;    Macro Definitions and Utility Functions
;*******************************************************************************


;*******************************************************************************
;    dp-benchmark -
;*******************************************************************************

(define (dp-make-benchmark)
    (let
        ((start-time '(0 0 0 0 0))
         (stop-time #f))

        (define (current)
            (let ((tms (times)))
                (list (tms:clock tms) (tms:utime tms) (tms:stime tms)
                      (tms:cutime tms) (tms:cstime tms))
            )
        )
        (define (elapsed)
            (if stop-time
                (map-in-order - stop-time start-time)
                (map-in-order - (current) start-time)
            )
        )
        (define (rate num-items)
            (/ num-items (elapsed))
        )
        (define (start)
            (set! stop-time #f)
            (set! start-time (current))
        )
        (define (stop)
            (set! stop-time (current))
        )

        (lambda arguments
            (apply
                (case (car arguments)
                    ((current) current)
                    ((elapsed) elapsed)
                    ((rate) rate)
                    ((start) start)
                    ((stop) stop)
                    (else (error "Invalid method!"))
                )
                (cdr arguments)
            )
        )
    )
)


;*******************************************************************************
;    dp-multi-op - returns the result of applying an operator to multiple
;        numeric operands.  The operand list is scanned and operands which are
;        points are replaced by their values.  Invalid points are filtered out
;        and the operator is only applied to numbers and valid points.
;*******************************************************************************

(define (dp-multi-op operator operands)
    (let
        ((expression (list operator)))
        (for-each
            (lambda (number)
                (if number
                    (set! expression (append expression (list number)))
                )
            )
            operands
        )
        (primitive-eval expression)
    )
)

;*******************************************************************************
;    Bit-Wise Operators
;*******************************************************************************

(define (dp-and . operands)
    (false-if-exception (logand #xFFFFFFFF (apply logand operands)))
)


(define (dp-or . operands)
    (dp-multi-op logior operands)
)


(define (dp-not value mask)
    (logand (lognot value) mask)
)

;*******************************************************************************
;    Comparison Operators
;*******************************************************************************

(define (dp-eq . operands)
    (false-if-exception (if (apply = operands) 1 0))
)


(define (dp-ge . operands)
    (false-if-exception (if (apply >= operands) 1 0))
)


(define (dp-gt . operands)
    (false-if-exception (if (apply > operands) 1 0))
)


(define (dp-le . operands)
    (false-if-exception (if (apply <= operands) 1 0))
)


(define (dp-lt . operands)
    (false-if-exception (if (apply < operands) 1 0))
)

;*******************************************************************************
;    Date Functions
;*******************************************************************************

(define dp-julian-day-1970 2440587.5)
(define dp-seconds-per-day (* 60.0 60.0 24.0))


(define (dp-cjday)
    (if *dp-frame-timestamp*
        (+  dp-julian-day-1970
            (/ (+ (car *dp-frame-timestamp*)
                  (/ (cdr *dp-frame-timestamp*) 1000000.0))
               dp-seconds-per-day)
        )
        #f
    )
)


(define (dp-cyday)
    (if *dp-frame-timestamp*
        (+ 1 (tm:yday (gmtime (car *dp-frame-timestamp*))))
        #f
    )
)


(define (dp-chour scale)
    (if *dp-frame-timestamp*
        (let*
            ((frame-gmt
                (gmtime (car *dp-frame-timestamp*)))
             (seconds-in-day
                (+
                    (* 60.0 (+ (tm:min frame-gmt) (* 60.0 (tm:hour frame-gmt))))
                    (tm:sec frame-gmt)
                ))
            )
            (* scale (/ seconds-in-day dp-seconds-per-day))
        )
        #f
    )
)

;*******************************************************************************
;    Integral Function
;*******************************************************************************

(define (dp-make-integ)
    (let
        ((start-value 0)
         (integral 0.0))
        (lambda (start argument)
            (if (> start 0)
                (set! integral (+ argument (if (> start-value 0) integral 0.0)))
            )
            (set! start-value start)
            integral
        )
    )
)

;*******************************************************************************
;    Mathematical Operators
;*******************************************************************************

(define (dp-first . operands)
    (let
        ((first-valid #f))
        (for-each
            (lambda (number)
                (if (not first-valid)
                    (set! first-valid number)
                )
            )
            operands
        )
        first-valid
    )
)


(define (dp-idiv dividend divisor)
    (truncate (/ dividend divisor))
)


(define (dp-max . operands)
    (dp-multi-op max operands)
)


(define (dp-min . operands)
    (dp-multi-op min operands)
)


(define (dp-mod dividend divisor)
    (let
        ((result (dp-smod dividend divisor)))
        (if (and result (negative? result))
            (+ result divisor)
            result
        )
    )
)


(define (dp-nth index . operands)
    (list-ref operands (inexact->exact index))
)


(define (dp-smod dividend divisor)
    (if (positive? divisor)
        (- dividend (* (dp-idiv dividend divisor) divisor))
        #f
    )
)

;*******************************************************************************
;    Trigonometric Operators
;*******************************************************************************

(define PI (acos -1.0))
(define DEG2RAD (/ PI 180.0))
(define RAD2DEG (/ 180.0 PI))


(define-macro dp-to-degrees
    (lambda (angle)
        `(* RAD2DEG ,angle)
    )
)


(define-macro dp-to-radians
    (lambda (angle)
        `(* DEG2RAD ,angle)
    )
)


(define (dp-cos angle)
    (cos (dp-to-radians angle))
)


(define (dp-sin angle)
    (sin (dp-to-radians angle))
)


(define (dp-tan angle)
    (tan (dp-to-radians angle))
)


(define (dp-acos value)
    (dp-to-degrees (acos value))
)


(define (dp-asin value)
    (dp-to-degrees (asin value))
)


(define (dp-atan value)
    (dp-to-degrees (atan value))
)

;*******************************************************************************
;    dp-rewrite
;*******************************************************************************

(define term-replacements '(
    (+		. +)
    (-		. -)
    (*		. *)
    (/		. /)
    (AND	. dp-and)
    (OR		. dp-or)
    (NOT	. dp-not)
    (>		. dp-gt)
    (<		. dp-lt)
    (=		. dp-eq)
    (COS	. dp-cos)
    (SIN	. dp-sin)
    (TAN	. dp-tan)
    (ACOS	. dp-acos)
    (ASIN	. dp-asin)
    (ATAN	. dp-atan)
    (SQRT	. sqrt)
    (MOD	. dp-mod)
    (ABS	. abs)
    (INT	. truncate)
    (MIN	. dp-min)
    (MAX	. dp-max)
    (APPLY	. dp-apply)
    (FIRST	. dp-first)
    (NTH	. dp-nth)
    (INTEG	. dp-integ)
    (SIGMA	. +)
    (CJDAY	. dp-cjday)
    (CYDAY	. dp-cyday)
    (CHOUR	. dp-chour)
;	Functions below are not used in the current P_CALCUL.TXT.
    (CJDAT	. dp-cjdat)
    (CUMUL	. dp-cumul)
    (DELTA	. dp-delta)
    (DIV	. dp-idiv)
    (EXP	. exp)
    (LN		. log)
    (LOG	. log10)
    (POW	. expt)
    (SMOD	. dp-smod)
    (STATUS	. dp-status)
))


(define (dp-rewrite expression)
    (if (not (list? expression))
        (let
            ((rewrite (assq expression term-replacements)))
            (cond
                (rewrite	; Replace P_CALCUL functions by dp-* functions?
                    (if (eq? (cdr rewrite) 'dp-integ)
                        (dp-make-integ)
                        (cdr rewrite)
                    )
                )
                ((symbol? expression)
                    (if (eq? #\$ (string-ref (symbol->string expression) 0))
				; Convert "$<hexdigits>" to number.
                        (string->number
                            (substring (symbol->string expression) 1)
                            16
                        )
				; Must be a reference to a point.
                        (if (dp-add-point expression)
                            (list 'dp-point-get (dp-add-point expression))
                            #f
                        )
                    )
                )
			; Numbers, operators, etc.
                (else expression)
            )
        )
        (list 'false-if-exception (map-in-order dp-rewrite expression))
    )
)

;*******************************************************************************
;    dp-define
;*******************************************************************************

;    Request flags for derived parameters only - don't send updates!

(define  dp-request-flags (logior #x0001 #x0002 #x0004 #x0040))

(define (dp-define mnemonic description expression)
    (if (dp-add-calc (dp-add-point mnemonic dp-request-flags)
                     (dp-rewrite expression))
        (begin
            (set! *dp-calc-list* (append *dp-calc-list* (list mnemonic)))
            (dp-point-set! (dp-make-point mnemonic) #f #f #f DP-STALE-MASK DP-STALE-FLAGS)
        )
    )
)

;*******************************************************************************
;    Network functions.
;*******************************************************************************

(define (net-addr-of host)
    (let
        ((entry (false-if-exception (gethost (if host host (gethostname))))))
        (if entry
            (car (hostent:addr-list entry))
            host
        )
    )
)


(define (net-port-of service . protocol)
    (let
        ((entry
            (false-if-exception
                (getserv service (if (null? protocol) "tcp" (car protocol)))))
        )
        (cond
            (entry (servent:port entry))
            ((number? service) service)
            ((string? service) (string->number service))
            (else service)
        )
    )
)


(define (tcp-listen service . backlog)
    (let
        ((ep-port (socket AF_INET SOCK_STREAM 0)))
        (setsockopt ep-port SOL_SOCKET SO_REUSEADDR 1)
        (bind ep-port AF_INET INADDR_ANY (net-port-of service))
        (listen ep-port (if (null? backlog) 99 (car backlog)))
        (dp-debugmsg (string-append "(tcp-listen) Listening on " (object->string ep-port)))
        ep-port
    )
)


(define (tcp-answer lp-port)
    (let
        ((ep-port (car (accept lp-port))))
        (dp-debugmsg (string-append "(tcp-anser) Answered " (object->string ep-port) " on " (object->string lp-port)))
        ep-port
    )
)


(define (tcp-call server)
    (let
        ((service #f)
         (host #f)
         (ep-port (socket AF_INET SOCK_STREAM 0))
        )
        (if (not (string? server))
            (set! server (object->string server))
        )
        (set! service (car (string-split server #\@)))
        (set! host (cdr (string-split server #\@)))
        (set! host (if (null? host) #f (car host)))
        (connect ep-port AF_INET (net-addr-of host) (net-port-of service "tcp"))
        ep-port
    )
)

;*******************************************************************************
;    Debug Functions
;*******************************************************************************

(define *dp-calc-map-pretty* #f)


(define (dp-build-calc-map-pretty)
    (set! *dp-calc-map-pretty*
        (call-with-output-string
            (lambda (p)
                (pretty-print (hash-fold acons '() *dp-calc-map*) p)
            )
        )
    )
)


(define *dp-mnf-update-lists-pretty* #f)


(define (dp-build-mnf-update-lists-pretty)
    (set! *dp-mnf-update-lists-pretty*
        (call-with-output-string
            (lambda (p)
                (do ((i 0 (1+ i)))
                    ((>= i (vector-length *dp-mnf-update-lists*)))
                    (let
                        ((updates (vector-ref *dp-mnf-update-lists* i)))
                        (if (not (null? updates))
                            (begin
                                (display "---- MNF " p)
                                (write-line i p)
                                (write-line
                                    (map-in-order
                                        (lambda (point)
                                            (dp-point-name (car point))
                                        )
                                        updates
                                    )
                                    p
                                )
                            )
                        )
                    )
                )
            )
        )
    )
)


(define (dp-dump-info . to-port)
    (set! to-port (if to-port (car to-port) (current-output-port)))
    (write-line "=========================" to-port)
    (write-line "Parameter Calculations:" to-port)
    (if (not *dp-calc-map-pretty*) (dp-build-calc-map-pretty))
    (write-line *dp-calc-map-pretty* to-port)
    (write-line "=========================" to-port)
    (write-line "MNF Update Lists:" to-port)
    (if (not *dp-mnf-update-lists-pretty*) (dp-build-mnf-update-lists-pretty))
    (write-line *dp-mnf-update-lists-pretty* to-port)
    (write-line "=========================" to-port)
    (write-line "Derived Parameters with No Triggers:" to-port)
    (for-each
        (lambda (mnemonic)
            (write-line (string-append "    " mnemonic) to-port)
        )
        (sort *dp-no-triggers* string-ci<?)
    )
    (write-line "=========================" to-port)
    (write-line "Undefined Points:" to-port)
    (for-each
        (lambda (mnemonic)
            (write-line (string-append "    " mnemonic) to-port)
        )
        (sort *dp-undefined-points* string-ci<?)
    )
    (write-line "=========================" to-port)
    (write-line "Slice-Triggered Points:" to-port)
    (for-each
        (lambda (handle)
            (write-line (string-append "    " (dp-point-name (car handle))) to-port)
        )
        *dp-slice-points*
    )
    (write-line "=========================" to-port)
    (display "    # of points set: " to-port)
    (write-line *dp-num-points-set* to-port)
    (display "# of points not set: " to-port)
    (write-line *dp-num-points-not-set* to-port)
    (display "         Frame Time: " to-port)
    (write-line *dp-frame-timestamp* to-port)
    (write-line "=========================" to-port)
    (write-line (gc-stats) to-port)
    (write-line "=========================" to-port)
)


(define my-listening-port (tcp-listen 10203))


(dp-iox-onio (dp-make-iox) my-listening-port
    (lambda (callback reason lp-port)
        (let
            ((ep-port (tcp-answer lp-port)))
            (false-if-exception (dp-dump-info ep-port))
            (close-port ep-port)
        )
    )
)

;*******************************************************************************
;    Load the derived parameter definitions.
;*******************************************************************************

(load-from-path "dp_mission.scm")

(dp-debugmsg (string-append "Total # of Points: " (number->string (hash-fold (lambda (key value seed) (1+ seed)) 0 *dp-point-map*))))

(dp-debugmsg "Running the ORB ...")

(dp-orb-run (dp-make-orb))