%% 3D actuation of all joints %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

END  = 4;
GAIN = 1;

GAIN_HIP   = GAIN;
GAIN_KNEE  = GAIN;
GAIN_ANKLE = GAIN;

% setup timeline, query points
t = 0 : 1 : END;
q = 0 : 0.001: END;

% vars starting with tr_ are "tracks", or arrays of unprocessed positions
% over a period of time

% TRACKS: Left Leg Joints X, Y, Z
tr_LHipY = [0, 0, 0, 0, 0]; % positive: move foot back
tr_LHipX = [0, 0, 0, 0, 0]; % pos: move foot inward
tr_LHipZ = [0, 0, 0, 0, 0]; % pos: move knee left (c-clockw)

tr_LKneeY = [0, 0, 0, 0, 0];
tr_LKneeX = [0, 0, 0, 0, 0];
tr_LKneeZ = [0, 0, 0, 0, 0];

tr_LAnkleY = [0, 0, 0, 0, 0];
tr_LAnkleX = [0, 0, 0, 0, 0];
tr_LAnkleZ = [0, 0, 0, 0, 0];

% TRACKS: Right Leg Joints X, Y, Z
tr_RHipY = [0, 0, 0, 0, 0]; % positive: move leg back
tr_RHipX = [0, 0, 0, 0, 0]; % pos: mv foot in
tr_RHipZ = [0, 0, 0, 0, 0]; % pos: mv c-clockw

tr_RKneeY = [0, 0, 0, 0, 0];
tr_RKneeX = [0, 0, 0, 0, 0];
tr_RKneeZ = [0, 0, 0, 0, 0];

tr_RAnkleY = [0, 0, 0, 0, 0];
tr_RAnkleX = [0, 0, 0, 0, 0];
tr_RAnkleZ = [0, 0, 0, 0, 0];

% interp's, left leg
it_LHipY = interp1(t, tr_LHipY, q, 'spline')';
it_LHipZ = interp1(t, tr_LHipZ, q, 'spline')';
it_LHipX = interp1(t, tr_LHipX, q, 'spline')';

it_LKneeY = interp1(t, tr_LKneeY, q, 'spline')';
it_LKneeZ = interp1(t, tr_LKneeZ, q, 'spline')';
it_LKneeX = interp1(t, tr_LKneeX, q, 'spline')';

it_LAnkleY = interp1(t, tr_LAnkleY, q, 'spline')';
it_LAnkleZ = interp1(t, tr_LAnkleZ, q, 'spline')';
it_LAnkleX = interp1(t, tr_LAnkleX, q, 'spline')';

% interp's, right leg
it_RHipY = interp1(t, tr_RHipY, q, 'spline')';
it_RHipZ = interp1(t, tr_RHipZ, q, 'spline')';
it_RHipX = interp1(t, tr_RHipX, q, 'spline')';

it_RKneeY = interp1(t, tr_RKneeY, q, 'spline')';
it_RKneeZ = interp1(t, tr_RKneeZ, q, 'spline')';
it_RKneeX = interp1(t, tr_RKneeX, q, 'spline')';

it_RAnkleY = interp1(t, tr_RAnkleY, q, 'spline')';
it_RAnkleZ = interp1(t, tr_RAnkleZ, q, 'spline')';
it_RAnkleX = interp1(t, tr_RAnkleX, q, 'spline')';

%% RETURN VALUES %%

% Left Leg
ret_LHipY = [q', it_LHipY];
ret_LHipZ = [q', it_LHipZ];
ret_LHipX = [q', it_LHipX];

ret_LKneeY = [q', it_LKneeY];
ret_LKneeZ = [q', it_LKneeZ];
ret_LKneeX = [q', it_LKneeX];

ret_LAnkleY = [q', it_LAnkleY];
ret_LAnkleZ = [q', it_LAnkleZ];
ret_LAnkleX = [q', it_LAnkleX];

%%%%%%%%%%%%%%%%%%%%

% Right Leg
ret_RHipY = [q', it_RHipY];
ret_RHipZ = [q', it_RHipZ];
ret_RHipX = [q', it_RHipX];

ret_RKneeY = [q', it_RKneeY];
ret_RKneeZ = [q', it_RKneeZ];
ret_RKneeX = [q', it_RKneeX];

ret_RAnkleY = [q', it_RAnkleY];
ret_RAnkleZ = [q', it_RAnkleZ];
ret_RAnkleX = [q', it_RAnkleX];