;;; ********************************************************************
;;; state.ss
;;; Susan Fox
;;; January 1, 2001

;;; Contains the State ADT, for keeping track of all the information
;;; about the robot's state, both low-level and high-level


;;; Requires utilities to be loaded, and the priority queue ADT,
;;; and the Message ADT
(if (not (top-level-bound? 'UTIL.SSLOADED))
    (load "util.ss"))
(if (not (top-level-bound? 'PQUEUE.SSLOADED))
    (load "pqueue.ss"))
(if (not (top-level-bound? 'MESSAGE.SSLOADED))
    (load "message.ss"))

(define STATE.SSLOADED #t)



;;; ********************************************************************


;;; The state contains information about the robot and its motors and
;;; sensors, and presumed position in the world.  It also contains information
;;; about the higher level reasoning of the robot: its current list
;;; of goals, its current plan and the step under consideration.

;;; The state also includes some information about the times at which
;;; various events occurred (such as localization, installation of a
;;; plan step, etc)

;;; The robot information derives from the robot state maintained by
;;; Saphira and accessed through the C/C++ code

;;; The plan information is relatively simple:  The current behavior in
;;; action, if any; the current high-level plan, if any; and the index
;;; in the sequence of plan steps of the current plan step.
;;; (Further details will be added if needed)

;;; The goal information is a priority queue of messages.  When updating
;;; the goal information the program looks at an input stream
;;; where new messages are posted in Scheme format, so that read works
;;; fine with them.


;;; Accessors for the three parts:
(define robot-part car)
(define plan-part cadr)
(define goal-part caddr)


;;; -------------------------------------------------------------------

;;; For simplicity of access within the ADT, the information is separated
;;; into three parts: robot state, plan state, and goal state

;;; init-state creates an initial state for the robot
;;;
(define init-state
  (lambda ()
    (list (init-robot-state)
      (init-plan-state)
      (init-goal-state))))



;;; update-state takes a state and creates a new state that has updated
;;; information.  It takes into account changes that happen in the world,
;;; not changes that the system chooses to make   UHOH:  problem with
;;; the fact that the priority queue is naturally mutative!!  New goals
;;; and changes to the queue won't cause re-evaluation
;;;
(define update-state
  (lambda (state)
    (list (update-robot-state (robot-part state))
          (update-plan-state (plan-part state))
	  (update-goal-state! (goal-part state)))))


;;; change-behaviors takes a state and a list of new behaviors
;;; and changes the state to contain the new behaviors
;;;
(define change-behaviors
  (lambda (state beh-list)
    (list (robot-part state)
      (new-behaviors (plan-part state) beh-list)
      (goal-part state))))


;;; -------------------------------------------------------------------
;;; Robot part operations, including some specialized accessors
;;; for this part




;;; init-robot-state accesses the robot's programming (in C/C++) to get
;;; the robot state information from it
;;;
(define init-robot-state
  (lambda ()
    "Not yet implemented"))



;;; update-robot-state reads information from the robot part of the
;;; program and updates its sonar readings, estimated location and
;;; motor values
;;;
(define update-robot-state
  (lambda (robot-data)
    "not yet implemented"))


;;; -------------------------------------------------------------------
;;; Plan part operations, including some specialized accessors

(define behaviors-of car)
(define plan-of cadr)
(define plan-step-of caddr)

;;; find the current plan step
;;;
(define get-plan-step
  (lambda (plan-state)
    (list-ref (plan-of plan-state) (plan-step-of plan-state))))


;;; new-behaviors takes a plan-part of the state and a list of new
;;; behaviors, and builds a new plan part with the new behaviors in
;;; place
;;;
(define new-behaviors
  (lambda (plan-part beh-list)
    (list beh-list
      (plan-of plan-part)
      (plan-step-of plan-part))))


;;; init-plan-state simply creates the plan portion of the state as
;;; empty -- no initial plan
;;;
(define init-plan-state
  (lambda ()
    (list #f    ;; no initial behavior set in place
          #f    ;; no current plan
	  #f    ;; no current plan step
	  )))

  
;;; update-plan-state checks to see if the current plan step has been
;;; achieved and, if so, changes to the next plan step.  Otherwise it
;;; does nothing
;;;
(define update-plan-state
  (lambda (plan-data)
    ...))


;;; -------------------------------------------------------------------
;;; Goal part operations



;;; init-goal-state examines the standard message stream to see if
;;; any messages have appeared and adds them to the (initially empty)
;;; priority queue
;;; Priorities for messages are computed by determining two factors:
;;;    the given priority, if any, and the given deadline, if any
;;;    These two values are combined with a formula given below
;;;
(define init-goal-state
  (lambda ()
    (let ([pqueue (init-queue)])
      (for-each
	(lambda (message)
	  (insert! message (compute-priority message) pqueue))
	(get-messages))
      pqueue)))
      

(define Deadline-cutoff 50)

;;; compute-priority computes the priority of a message from
;;; its given priority and its stated deadline.  The higher the
;;; urgency, the higher the priority.  The nearer the deadline,
;;; the higher the priority... For now the priority is the sum
;;; of the urgency and Deadline-cutoff minus the stated deadline
;;;
(define compute-priority
  (lambda (message)
    (+ (mess-urgency message)
       (let ([diff (- Deadline-cutoff (mess-deadline message))])
         (if (negative? diff)
	     0
	     diff)))))



;;; update-goal-state first checks to see if any existing goals have
;;; been achieved, or are currently achievable.  If so, then they are
;;; logged as successfully completed, and then deleted from
;;; the goal queue.  Next, it checks to see if any new messages have
;;; been posted, and adds them to the goal queue.
;;;
(define update-goal-state!
  (lambda (goal-part)
    (for-each (lambda (message)
		(insert! message (compute-priority message) goal-part))
      (get-messages))
    goal-part))