#! /usr/bin/env python
# -*- encoding: UTF-8 -*-

"""Example: Use ALMath"""

import almath

"""
This file shows how to use ALMath library in Python to clip
NAO footsteps to correct moves (avoiding too big steps and collision).
Use clipFootStep to clip your desired pose.
"""


# Various parameters describing step characteristics.
minFootSeparation = 0.088
minStepX = - 0.04
maxStepX = 0.08
maxStepY = 0.16
maxStepTheta = 0.35

def clipFootStepOnGaitConfig(footMove, isLeftSupport):
    ''' Clip the foot move so that it does not exceed the maximum
        size of steps.
        footMove is an almath.Pose2D (x, y, theta position).
        isLeftSupport must be set to True if the move is on the right leg
        (the robot is supporting itself on the left leg).
    '''

    def clipFloat(minValue, maxValue, value):
        ''' Clip value between two extremes. '''
        clipped = value
        if (clipped < minValue):
            clipped = minValue
        if (clipped > maxValue):
            clipped = maxValue
        return clipped

    # Clip X.
    clippedX = clipFloat(minStepX, maxStepX, footMove.x)
    footMove.x = clippedX

    # Clip Y.
    if not isLeftSupport:
        clippedY = clipFloat(minFootSeparation, maxStepY, footMove.y)
    else:
        clippedY = clipFloat(-maxStepY, - minFootSeparation, footMove.y)
    footMove.y = clippedY

    # Clip Theta.
    clippedTheta = clipFloat(-maxStepTheta, maxStepTheta, footMove.theta)
    footMove.theta = clippedTheta


def clipFootStepWithEllipse(footMove):
    ''' Clip the foot move inside an ellipse defined by the foot's dimansions.
        footMove is an almath.Pose2D (x, y, theta position).
    '''

    # Apply an offset to have Y component of foot move centered on 0.
    if (footMove.y < -minFootSeparation):
        footMove.y = footMove.y + minFootSeparation
    elif (footMove.y > minFootSeparation):
        footMove.y = footMove.y - minFootSeparation
    else:
        return

    # Clip the foot move to an ellipse using ALMath method.
    if footMove.x >= 0:
        almath.clipFootWithEllipse(maxStepX, maxStepY - minFootSeparation, footMove)
    else:
        almath.clipFootWithEllipse(minStepX, maxStepY - minFootSeparation, footMove)

    # Correct the previous offset on Y component.
    if footMove.y >=0:
        footMove.y  = footMove.y + minFootSeparation
    else:
        footMove.y = footMove.y - minFootSeparation


def clipFootStepToAvoidCollision(footMove, isLeftSupport):
    """ Clip the foot move to avoid collision with the other foot.
        footMove is an almath.Pose2D (x, y, theta position).
        isLeftSupport must be set to True if the move is on the right leg
        (the robot is supporting itself on the left leg).
    """

    # Bounding boxes of NAO's feet.
    rFootBox = [almath.Position2D(0.11, 0.038),    # rFootBoxFL
                almath.Position2D(0.11, -0.050),   # rFootBoxFR
                almath.Position2D(-0.047, -0.050), # rFootBoxRR
                almath.Position2D(-0.047, 0.038)]  # rFootBoxRL
    lFootBox = [almath.Position2D(0.11, 0.050),    # lFootBoxFL
                almath.Position2D(0.11, -0.038),   # lFootBoxFR
                almath.Position2D(-0.047, -0.038), # lFootBoxRR
                almath.Position2D(-0.047,  0.050)] # lFootBoxRL

    # Use ALMath method.
    almath.avoidFootCollision(lFootBox, rFootBox, isLeftSupport, footMove)

def clipFootStep(footMove, isLeftSupport):
    ''' Clipping functions to avoid any warnings and undesired effects
        when sending the footsteps to ALMotion.
        footMove is an almath.Pose2D (x, y, theta position)
        isLeftSupport must be set to True if the move is on the right leg
        (the robot is supporting itself on the left leg).
    '''

    # First clip the foot move with gait config.
    clipFootStepOnGaitConfig(footMove, isLeftSupport)
    # Then clip it on an ellipse.
    clipFootStepWithEllipse(footMove)
    # Then make sure you do not have any collision.
    clipFootStepToAvoidCollision(footMove, isLeftSupport)