%% Bend the Knees %%

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

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

% setup vars
t = 0 : 1 : END;

% set positions
pos_lhip = [ 0, 0, -0.083, 0, 0 ];
pos_rhip = [ 0, 0, 0, 0, 0 ];

pos_lknee = [ 0, 0, 0, 0, 0 ];
pos_rknee = [ 0, 0, 0, 0, 0 ];

pos_lankle = [ 0, 0, 0, 0, 0 ];
pos_rankle = [ 0, 0, 0, 0, 0 ];


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

interpts_lhip   = interp1(t, pos_lhip, querypts_hip, 'spline')';
interpts_lknee  = interp1(t, pos_lknee, querypts_knee, 'spline')';
interpts_lankle = interp1(t, pos_lankle, querypts_ankle, 'spline')';

interpts_rhip   = interp1(t, pos_rhip, querypts_hip, 'spline')';
interpts_rknee  = interp1(t, pos_rknee, querypts_knee, 'spline')';
interpts_rankle = interp1(t, pos_rankle, querypts_ankle, 'spline')';

ret_LHip   = [querypts_hip', interpts_lhip];
ret_LKnee  = [querypts_knee', interpts_lknee];
ret_LAnkle = [querypts_ankle', interpts_lankle];

ret_RHip    = [querypts_hip', interpts_rhip];
ret_RKnee   = [querypts_knee', interpts_rknee];
ret_RAnkle = [querypts_ankle', interpts_rankle];

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

% figure % Both Hips
% plot(t, pos_hip, 'r', querypts_hip, interpts_hip, ':.');

% figure % Both Knees
% plot(t, pos_knee, 'b', querypts_knee, interpts_knee, ':.');

% figure % Both Ankles
% plot(t, pos_ankle, 'g', querypts_ankle, interpts_ankle, ':.');