#################################################
# RaSCL : Robotics & Sensorial Control Language #
#                                               #
#  http://www.slyware.com/projects_rascl.shtml  #
#      Simon Chudley (simon@slyware.com)        #
#                                               #
#################################################
#
# File    : libraries/lib_walking.rascl
# Summary : Common code for legged walking robots.
#
# Contains common functions for making legged robots walk.
# Some functions work either with four or six legs, some require
# six.



###
# Function : wave_gait_walk
# Args     : RaSCL list { bool_forward level_out normalised_height normalised_raise_factor
#                         speed normalised_stop_distance }
# Purpose  : This function implements wave gait walking. It should work with any number of legs.
#            Specify the direction (.true for forward) and the height. Specify whether the bot should start
#            and finish by leveling itself out (.true for level out). This also puts the bot into a
#            stagger leg position, which is the best for wave gait walking.
#            The raise factor defines how much the legs are lifted before being swung; depends
#            what your bot is walking over! This function will continue until the stop distance is met. The distance
#            is in terms of normalised X servo leg movement.
#            Before it completes, it puts the bot back into a brase position, if level_out is true.
# Pre-cnd  : All servos are calibrated.
# Post-cnd : It will walk until the stop distance is met.
# Returns  : -
# 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 wave_gait_walk)

    # Check for correct number of arguments
    (::call .check_arguments (list
                                 6
                                 {wave_gait_walk { <forward_bool> <levelout_bool> <height> <raise_factor> <speed> <stop_distance> }}
                                 func_args))

    # Arguments
    (::= .lForward (icar func_args 0))
    (::= .lLevelOut (icar func_args 1))
    (::= .lHeight (icar func_args 2))
    (::= .lRaiseFactor (icar func_args 3))
    (::= .lSpeed (icar func_args 4))
    (::= .lStopDistance (icar func_args 5))

    # Constants
    (::= .lSwingSpeedupFactor 3)
    (::if { lForward })
        (::= .lDirFactor gCALMAX)
    (::else)
        (::= .lDirFactor 0)
    (::endif)

    # Initialisation
    (::= .lLegsXList {} )
    (::= .lLegsYList {} )
    (::= .lTotalDistance 0)

    # Split the servo legs into two lists, one of X servos and one of Y servos
    (::foreach gBotLegs)

        (::= .lLegPair iItem)
        (::= .lLeftLeg (icar lLegPair 0))
        (::= .lRightLeg (icar lLegPair 1))
        (::= .lLegsXList (list (icar lLeftLeg 1) (icar lRightLeg 1) (unlist lLegsXList)))
        (::= .lLegsYList (list (icar lLeftLeg 0) (icar lRightLeg 0) (unlist lLegsYList)))

    (::endforeach)

    # Put the robot into a stagger position; this evenly spreads out the legs which
    # is best for stable wave gait walking
    (::if { lLevelOut })

        (::call .stagger_position (list lHeight lSpeed lRaiseFactor))

    (::endif)

    # While we should keep on going
    (::while { (< lTotalDistance lStopDistance) })

        # First, work out the servo that is nearest its calibration point
        (::= .lDistList {})

        # For each servo
        (::foreach lLegsXList)

            (::= .lEntry iItem)
            (::= .lServo (dynamic_cast (car lEntry)))
            (::= .lModifier (icar lEntry 1))

            # Get current position
            (::= .lPos (*lServo::getpos))

            # Remember how far from its calibration pos it is
            (::= .lDistList (carpush (unlist lModifier) lDistList))

        (::endforeach)

        # Adjustments depending on direction
        (::if { (== lDirFactor gCALMAX) })

            (::= .lDistMove (min lDistList))
            (::= .lTotalDistance (+ lTotalDistance lDistMove))

        (::else)

            (::= .lDistMove (- 0 (- gCALMAX (max lDistList))))
            (::= .lTotalDistance (+ lTotalDistance (- 0 lDistMove)))

        (::endif)

        # We should therefore move all legs by the smallest amount, until one legs hits its
        # calibration point
        (::= .lDistList {})
        (::= .lTimes {})
        (::= .lId 0)
        (::= .lMovingID -1)

        # For all legs X servos
        (::foreach lLegsXList)

            (::= .lEntry iItem)
            (::= .lServo (dynamic_cast (car lEntry)))
            (::= .lModifier (icar lEntry 1))

            # Get its current position
            (::= .lPos (*lServo::getpos))

            # Work out its new position
            (::= .lPos (- (unlist lModifier) lDistMove))

            # Is this the leg that has now hit its calibration point?
            (::if { (and (== -1 lMovingID) (== lPos (- gCALMAX lDirFactor) )) })

                # Remember both of its servos for later
                (::= .lXServo (dynamic_cast (car (icar lLegsXList lId))))
                (::= .lYServo (dynamic_cast (car (icar lLegsYList lId))))
                (::= .lXServoModifier (icar (icar lLegsXList lId) 1))
                (::= .lYServoModifier (icar (icar lLegsYList lId) 1))
                (::= .lMovingID lId)

            (::else)

                # Remember how far this one is now from its calibration pos
                (::= .lDistList (carpush lPos lDistList))

            (::endif)

            # If we have anywhere to move
            (::if { (> lDistMove 0) })

                # Move this servo and remember how long it is going to take
                (::= .lTimes (carpush (*lServo::setpos (list (unlist lModifier) lSpeed)) lTimes))

            (::endif)

            (::= .lId (+ lId 1))

        (::endforeach)

        # Wait for all the servos to catch up
        (sleep (max lTimes))
        (::= .lTimes {})

        # We now need to swing the leg that is at its minimum calibration point
        # Get its current Y position
        (::= .lPos (*lYServo::getpos))
        (::= .lOriginalY (unlist lYServoModifier))

        # Raise it a bit
        (::= .lPos (+ lOriginalY lRaiseFactor))
        (sleep (*lYServo::setpos (list (unlist lYServoModifier) (* lSwingSpeedupFactor lSpeed))))

        # Move it full deflection in the X axis, and remember how long it will take
        (::= .lPos lDirFactor)
        (::= .lTimes (carpush (*lXServo::setpos (list (unlist lXServoModifier) (* lSwingSpeedupFactor lSpeed))) lTimes))

        # Tell all the other servos to continue moving whilst this one is raised
        # We will move them all until one more hits its calibration point

        # Adjustments depending on direction
        (::if { (== lDirFactor gCALMAX) })

            (::= .lDistMove (min lDistList))
            (::= .lTotalDistance (+ lTotalDistance lDistMove))

        (::else)

            (::= .lDistMove (- 0 (- gCALMAX (max lDistList))))
            (::= .lTotalDistance (+ lTotalDistance (- 0 lDistMove)))

        (::endif)

        # If we have anywhere to move
        (::if { (> lDistMove 0) } })

            # Work out the next servo to hit its calibration point
            (::= .lId 0)

            # For all legs X servos
            (::foreach lLegsXList)

                # If it's not the one we're currently swinging the other way
                (::if { (not (== lId lMovingID)) })

                    (::= .lEntry iItem)
                    (::= .lServo (dynamic_cast (car lEntry)))
                    (::= .lModifier (icar lEntry 1))

                    # Get its current position
                    (::= .lPos (*lServo::getpos))

                    # Modify to its new position
                    (::= .lPos (- (unlist lModifier) lDistMove))

                    # Move this servo and remember how long it is going to take
                    (::= .lTimes (carpush (*lServo::setpos (list (unlist lModifier) lSpeed))) lTimes)

                (::endif)

                (::= .lId (+ lId 1))

            (::endforeach)

        (::endif)

        # Wait for all other legs to catch up
        (sleep (max lTimes))

        # Place the raised one back on the ground
        (::= .lPos lOriginalY)
        (sleep (*lYServo::setpos (list (unlist lYServoModifier) (* lSwingSpeedupFactor lSpeed))))

    (::endwhile)

    # Put the robot back into a brace position
    (::if { lLevelOut })

        (::call .brace_position (list lHeight lSpeed lRaiseFactor))

    (::endif)

    (::return .true)

(::endfunc)


###
# Function : set_bot_height
# Args     : RaSCL list { normalised_height_position speed }
# Purpose  : This raises your bot to the specified noralised position.
#            Note it doesn't do anything special if the legs are all at different heights
#            initially!
# Pre-cnd  : All servos are calibrated.
# Post-cnd : The robot will be at the height you specified.
#            All servo operations will have complete by the time this function returns.
#            If any of the servos were invalid, false is returned.
# Returns  : -
# 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_bot_height)

    # Check for correct number of arguments
    (::call .check_arguments (list
                                 2
                                 {set_bot_height { <height> <speed> }}
                                 func_args))

    # Arguments
    (::= .lHeight (icar func_args 0))
    (::= .lSpeed (icar func_args 1))

    # Set all legs to the same height to start
    (::= .lTimes {})
    (::foreach gBotLegs)

        (::= .lLegPair iItem)
        (::foreach lLegPair)

            # Get leg Y servo
            (::= .lLeg iItem)
            (::= .lLegY (icar lLeg 0))
            (::= .lLegYServo (dynamic_cast (icar lLegY 0)))
            (::= .lLegYModifier (icar lLegY 1))
            (::= .lPos lHeight)

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

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

        (::endforeach)

    (::endforeach)

    # Wait for the height adjustment to take place
    (sleep (max lTimes))

(::endfunc)




###
# Function : stagger_position
# Args     : RaSCL list { normalised_height_position speed normalised_raise_factor }
# Purpose  : This puts your legged robot into a staggered leg position. A staggered leg
#            position is where starting from the left front leg (which is at its minimum X
#            position), the legs are evenly spread out in the order of middle left, rear left,
#            front right etc.
#            You should put your bot into this position prior to wave gait walking.
#            This function initially moves all the legs so they are the height specified (and
#            level), and then individually moves each leg to its final position (lifting, swinging
#            and replacing).
#            It raises the legs by the specified raise factor.
#            It should work with 4, 6, 8 etc legs, as it just evenly spaces them out.
# Pre-cnd  : All servos are calibrated.
# Post-cnd : The robot will be at the height you specified, with the legs in the stagger position.
#            All servo operations will have complete by the time this function returns.
# Returns  : -
# 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 stagger_position)

    # Check for correct number of arguments
    (::call .check_arguments (list
                                 3
                                 {stagger_position { <height> <speed> <raise_factor> }}
                                 func_args))

    # Arguments
    (::= .lHeight (icar func_args 0))
    (::= .lSpeed (icar func_args 1))
    (::= .lRaiseFactor (icar func_args 2))

    # Initialisation
    (::= .lOffsetInc  (/ gCALMAX (* (count gBotLegs) 2)))
    (::= .lOffset (- gCALMAX (/ lOffsetInc 2)))
    (::= .lLOffset 200)
    (::= .lROffset 100)

    # Make sure the bot is level to start
    (::call .set_bot_height (list lHeight lSpeed))

    # For all the legs
    (::foreach gBotLegs)

        # For each pair
        (::= .lId 0)
        (::= .lLegPair iItem)
        (::foreach lLegPair)

            (::= .lLeg iItem)

            # Y servo
            (::= .lLegY (icar lLeg 0))
            (::= .lLegYServo (dynamic_cast (icar lLegY 0)))
            (::= .lLegYModifier (icar lLegY 1))

            # X servo
            (::= .lLegX (icar lLeg 1))
            (::= .lLegXServo (dynamic_cast (icar lLegX 0)))
            (::= .lLegXModifier (icar lLegX 1))

            # Check for valid servo pointers
            (::call .test_rascl_pointer (list .Resource<Servo> lLegYServo))
            (::call .test_rascl_pointer (list .Resource<Servo> lLegXServo))

            # If left leg
            (::if { (== lId 0) })

                (::= .lOffset lLOffset)
            (::else)

                (::= .lOffset lROffset)
            (::endif)

            # Raise the leg
            (::= .lPos (+ lHeight lRaiseFactor))
            (sleep (*lLegYServo::setpos (list (unlist lLegYModifier) lSpeed)))

            # Swing it to its new position
            (::= .lPos lOffset)
            (sleep (*lLegXServo::setpos (list (unlist lLegXModifier) lSpeed)))

            # Replace it back on the floor
            (::= .lPos lHeight)
            (sleep (*lLegYServo::setpos (list (unlist lLegYModifier) lSpeed)))

            (::= .lId (+ lId 1))

        (::endforeach)

        # Position of next leg
        (::= .lLOffset (- lLOffset lOffsetInc))
        (::= .lROffset (- lROffset lOffsetInc))

    (::endforeach)

(::endfunc)