%% Bend the Knees %%

END  = 4; % total pts: END+1
GAIN = 3;

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

% setup vars
t = 0 : 1 : END;
% yeah im lost
pos_hip   = [0, -1, -3, -1, 0];
pos_knee  = [0, 1, 3, 1, 0];
pos_ankle = [0, 1, 3, 1, 0];

% generate graph lines
querypts_hip   = 0 : 0.001 : END;
querypts_knee  = 0 : 0.001 : END;
querypts_ankle = 0 : 0.001 : END;

interpts_hip   = interp1(t, pos_hip, querypts_hip, 'spline')';
interpts_knee  = interp1(t, pos_knee, querypts_knee, 'spline')';
interpts_ankle = interp1(t, pos_ankle, querypts_ankle, 'spline')';

ret_LHip   = [querypts_hip', interpts_hip];
ret_LKnee  = [querypts_knee', interpts_knee];
ret_LAnkle = [querypts_ankle', interpts_ankle];

ret_RHip    = [querypts_hip', interpts_hip];
ret_RKnee   = [querypts_knee', interpts_knee];
ret_RAnkle = [querypts_ankle', interpts_ankle];

% create graph
%figure
%plot(t, p,'o', querypts, interpts, ':.');