"""
    Authors: Amin Arriaga, Eduardo Lopez
    Project: Graduation Thesis: GIAdog

    This file contains a function that adds directionality to gaits.

"""

import numpy as np
from __env__ import LEG_SPAN

def direction_deltas(delta_t, ftg_freqs, ftg_phases, command_dir, turn_dir):
    """
        Function to add directionality to gaits.

        Notes:
        The constants might be changed to be more general.

    
    """
    turn_dir = turn_dir[0]
    delta = np.zeros([4, 3])
    for i in range(4):
        # Position delta
        x_delta =  np.cos(command_dir) * delta_t * ftg_phases[i][1] * \
                    ftg_freqs[i] * LEG_SPAN * 1.7
        y_delta =  np.sin(command_dir) * delta_t * ftg_phases[i][1] * \
                        ftg_freqs[i] * LEG_SPAN * 1.02
        position_delta = np.array([x_delta, y_delta, 0])


        # Rotation delta
        theta = np.pi/4
        if   i == 0: phi_arc = -theta
        elif i == 1: phi_arc = -(np.pi - theta)
        elif i == 2: phi_arc = theta
        elif i == 3: phi_arc = (np.pi - theta)

        
        
        x_delta =-  np.cos(phi_arc) * delta_t * ftg_phases[i][1] * turn_dir \
                        * ftg_freqs[i] * LEG_SPAN * 0.68
        y_delta =-  np.sin(phi_arc) * delta_t * ftg_phases[i][1] * turn_dir \
                        * ftg_freqs[i] * LEG_SPAN * 0.68

        rotation_delta = np.array([x_delta, y_delta, 0])
        
        delta[i] =  (position_delta + rotation_delta)
    
    return delta