#################################################
# RaSCL : Robotics & Sensorial Control Language #
#                                               #
#  http://www.slyware.com/projects_rascl.shtml  #
#      Simon Chudley (simon@slyware.com)        #
#                                               #
#################################################
#
# File    : libraries/lib_servos.rascl
# Summary : Common code for manipulating servos.
#
# Contains common functions for manipulating servos.




##################
## SERVO OBJECT ##
##################

###
# Function : Resource<Servo>create (Servo constructor)
# Args     : servo_id
# Purpose  : Initiates a specialised abstract servo representation of a resource.
# Pre-cnd  : -
# Post-cnd : Servo member variables will be initiated.
#
(::complex_function Resource<Servo> create)

   (->set .mId func_args)
   (->set .mPosition  0)
   (->set .mCalibration {})

(::endfunc)



###
# Function : Resource<Servo>setpos_abs
# Args     : RaSCL list { real_position speed }
# Purpose  : This sets the servo to a real position (not normalised). It has no protection
#            over whether the specified position is valid, or outside the servos calibration.
#            Consider using Resource<Servo>setpos or Resource<Servo>setpos_real.
# Pre-cnd  : The Servo has been added to a resource container that has correctly implemented
#            the encode_servo_command and servo_move_time methods.
# Post-cnd : Servo will be moving (or moved) to the specified position.
# Returns  : Returns the time (in miliseconds) it will take to move the servo (given its
#            last known position) if successful.
# Throws   : ServoCommandFailedException : If the command could not be encoded and written
#            InvalidParametersException  : If invalid parameters are given
#
(::complex_function Resource<Servo> setpos_abs)

    # Check for correct number of arguments
    (::call .check_arguments (list
                                 2
                                 {Resource<Servo>setpos_abs expects { <real_position> <speed> }}
                                 func_args))

    # Split Arguments
    (::= .lPos (icar func_args 0))    # Absolute target position
    (::= .lSpeed (icar func_args 1))  # Speed

    # Can't continue if we're not added to a resource container!
    (::if { (not (isptr (->get .mParent))) })

        (::= .lId (->get .mId))
        (::throw .ServoCommandFailedException (cdrpush lId {No parent controller exists for servo id:}))

    (::endif)

    # Member mParent holds a RaSCL pointer to our parent controller, but we need to dynamically cast it
    # to get a RaSCL pointer to the specific type of controller
    (::= .lController (dynamic_cast (->get .mParent)))

    # Ask controller how long it will take to execute this command
    (::= .lTime (*lController::servo_move_time (list (abs (- (->get .mPosition) lPos)) lSpeed)))

    # Ask our parent controller to encode this servo command
    (::= .lCommand (*lController::encode_servo_command (list (->get .mId) lPos lSpeed)))

    # Send command if we encoded it successfully
    (::if { (and (> (count lCommand) 0) (*lController::write lCommand)) })

        # Update the new position
        (->set .mPosition lPos)

        # Return the time taken
        (::return lTime)

    (::endif)

    # If we get here, something went wrong
    (::= .lId (->get .mId))
    (::throw .ServoCommandFailedException (cdrpush lId {Unable to encode and send command to servo id:}))

(::endfunc)



###
# Function : Resource<Servo>setpos_real
# Args     : RaSCL list { real_position speed }
# Purpose  : This sets the servo to a real position (not normalised). It adds the protection
#            that if calibration information has been supplied for this servo, it will not
#            let you move it outside its calibration range. If the servo is not calibrated, it
#            provides no protection and behaves like Resource<Servo>setpos_abs.
# Pre-cnd  : The Servo has been added to a resource container that has correctly implemented
#            the encode_servo_command and servo_move_time methods. Servo calibration information has
#            been correctly set-up.
# Post-cnd : Servo will be moving (or moved) to the specified position.
# Returns  : Returns the time (in miliseconds) it will take to move the servo (given its
#            last known position) if successful.
# Throws   : ServoCommandFailedException : If the command could not be encoded and written
#            InvalidParametersException  : If invalid parameters are given
#
(::complex_function Resource<Servo> setpos_real)

    # Check for correct number of arguments
    (::call .check_arguments (list
                                 2
                                 {Resource<Servo>setpos_real expects { <real_position> <speed> }}
                                 func_args))

    # Arguments
    (::= .lPos (icar func_args 0))    # Absolute target position
    (::= .lSpeed (icar func_args 1))  # Speed
    (::= .lId (->get .mId))           # Servo ID
    (::= .lCal (->get .mCalibration)) # Calibration settings

    # Any calibration settings?
    (::if { (> (count lCal) 0) })

        # Get calibration values
        (::= .lMin (icar lCal 0))
        (::= .lMax (icar lCal 1))

        # Servo limit protection
        (::if { (< lPos lMin) })

            (log {WARNING: Attempting to set servo id:} lId {to out-of-range value:} lPos {min:} lMin)
            (::= .lPos lMin)

        (::else)

            (::if { (> lRealPos lMax) })

                (log {WARNING: Attempting to set servo id:} lId {to out-of-range value:} lPos {max:} lMax)
                (::= .lPos lMax)

            (::endif)

        (::endif)

    (::else)

        (log {WARNING: No calibration information available for servo id:} lId)

    (::endif)

    # Execute this command
    (::return (->setpos_abs (list lPos lSpeed)))

(::endfunc)



###
# Function : Resource<Servo>setpos
# Args     : RaSCL list { normalised_position speed }
# Purpose  : Using the pre-set normalisation and calibration settings for this
#            servo, this moves the servo to the specified position at the specified speed.
# Pre-cnd  : The Servo has been added to a resource container that has correctly implemented
#            the encode_servo_command and servo_move_time methods. Servo calibration information has
#            been correctly set-up.
# Post-cnd : Servo will be moving (or moved) to the specified position.
# Returns  : Returns the time (in miliseconds) it will take to move the servo (given its
#            last known position) if successful.
# Throws   : ServoCommandFailedException : If the command could not be encoded and written
#            ServoNotCalibratedException : If the servo has not been calibrated
#            InvalidParametersException  : If invalid parameters are given
#
(::complex_function Resource<Servo> setpos)

    # Check for correct number of arguments
    (::call .check_arguments (list
                                 2
                                 {Resource<Servo>setpos expects { <normalised_position> <speed> }}
                                 func_args))

    # Arguments
    (::= .lNormPos (icar func_args 0))  # Target normalised position
    (::= .lSpeed (icar func_args 1))    # Speed
    (::= .lId (->get .mId))             # Servo ID
    (::= .lCal (->get .mCalibration))   # Calibration settings

    # Limit check on normalised value
    (::if { (< lNormPos 0) })

        (log {WARNING: Attempting to set servo id:} lId {to out-of-range normalised value:} lNormPos {min: 0})
        (::= .lNormPos 0)

    (::else)

        (::if { (> lNormPos gCALMAX) })

            (log {WARNING: Attempting to set servo id:} lId {to out-of-range normalised value:} lNormPos {max:} gCALMAX)
            (::= .lNormPos gCALMAX)

        (::endif)

    (::endif)

    # Convert the normalised position to a real one
    (::if { (> (count lCal) 0) })

        # Get calibration values and convert
        (::= .lMin (icar lCal 0))
        (::= .lStep (icar lCal 2))
        (::= .lNormPos (+ lMin (* lStep lNormPos)))

    (::else)

        (::throw .ServoNotCalibratedException (cdrpush lId {No calibration information set for servo id:}))

    (::endif)

    # Set the real absolute position, and return the result
    (::return (->setpos_abs (list lNormPos lSpeed)))

(::endfunc)



###
# Function : Resource<Servo>getpos
# Args     : -
# Purpose  : Using the pre-set normalisation and calibration settings for this
#            servo, this returns the normalised last known position of the servo.
# Pre-cnd  : Servo calibration information has been correctly set-up.
# Post-cnd : -
# Returns  : Returns the last known normalised position.
# Throws   : ServoNotCalibratedException : If the servo has not been calibrated
#
(::complex_function Resource<Servo> getpos)

    # Get state
    (::= .lCal (->get .mCalibration))   # Calibration settings
    (::= .lPos (->get .mPosition))      # Last known position

    # Convert the real position to a normalised one
    (::if { (> (count lCal) 0) })

        # Get calibration values, convert and return
        (::= .lMin (icar lCal 0))
        (::= .lStep (icar lCal 2))
        (::return (/ (- lPos lMin) lStep))

    (::else)

        (::= .lId (->get .mId))
        (::throw .ServoNotCalibratedException (cdrpush lId {No calibration information set for servo id:}))

    (::endif)

(::endfunc)



###
# Function : Resource<Servo>getpos_real
# Args     : -
# Purpose  : Returns the real last known position of this servo.
# Pre-cnd  : -
# Post-cnd : Returns the last known real position.
#
(::complex_function Resource<Servo> getpos_real)

    # Return position
    (::return (->get .mPosition))

(::endfunc)



###
# Function : set_all_servos
# Args     : RaSCL list { normalised_position speed }
# Purpose  : Using the pre-set normalisation and calibration settings for the
#            servos, this sets all servos to the specified normalised position at
#            the specified speed.
# Pre-cnd  : All servo resources have been calibrated and generic_botinit.rascl has
#            normalised the calibration settings.
# Post-cnd : All servos will be set to the specified position.
# Returns  : Returns the time in milliseconds that it will take the servo with the
#            furthest distance to move to move to its new position, given its current
#            position.
# Throws   : ServoCommandFailedException : If a servo command could not be encoded and written
#            ServoNotCalibratedException : If a servo has not been calibrated
#            InvalidPointerException     : If an invalid servo pointer was given
#            InvalidParametersException  : If invalid parameters are given
#
(::*complex_function set_all_servos)

    # Check for correct number of arguments
    (::call .check_arguments (list
                                 2
                                 {set_all_servos expects { <normalised_position> <speed> }}
                                 func_args))
    # Params
    (::= .lPos (icar func_args 0))
    (::= .lSpeed (icar func_args 1))
    (::= .lTimes {})

    # For each servo
    (::foreach gBotServos)

        (::= .lEntry iItem)
        (::= .lServo (dynamic_cast (car lEntry)))

        # Check it is a valid servo pointer
        (::call .test_rascl_pointer (list .Resource<Servo> lServo))

        # Move servo, and add the returned time to the list
        (::= .lTimes (carpush (*lServo::setpos (list lPos lSpeed)) lTimes))

    (::endforeach)

    (::return (max lTimes))

(::endfunc)



###
# Function : center_all_servos
# Args     : RaSCL list { speed }
# Purpose  : Using the pre-set normalisation and calibration settings for the
#            servos, this sets all servos to their calibrated center positions.
# Pre-cnd  : All servo resources have been calibrated and generic_botinit.rascl has
#            normalised the calibration settings.
# Post-cnd : All servos will be moved (or moving) to their center positions.
# Returns  : Returns the time in milliseconds that it will take the servo with the
#            furthest distance to move to move to its new position, given its current
#            position.
# Throws   : ServoCommandFailedException : If a servo command could not be encoded and written
#            ServoNotCalibratedException : If a servo has not been calibrated
#            InvalidPointerException     : If an invalid servo pointer was given
#            InvalidParametersException  : If invalid parameters are given
#
(::*complex_function center_all_servos)

    # Check for correct number of arguments
    (::call .check_arguments (list
                                 1
                                 {center_all_servos { <speed> }}
                                 func_args))
    # Params
    (::= .lSpeed (icar func_args 0))
    (::= .lTimes {})

    # For each servo
    (::foreach gBotServos)

        (::= .lEntry iItem)
        (::= .lServo (dynamic_cast (car lEntry)))

        # Check it is a valid servo pointer
        (::call .test_rascl_pointer (list .Resource<Servo> lServo))

        # Calibrated center point for this servo
        (::= .lCenter (icar (*lServo::get .mCalibration) 3))

        # Move servo, and add the returned time to the list
        (::= .lTimes (carpush (*lServo::setpos (list lCenter lSpeed)) lTimes))

    (::endforeach)

    # Return the maximum time to wait for all servos to move
    (::return (max lTimes))

(::endfunc)