RTOE =squeeze(RTOE);
RHEE =squeeze(RHEE);
RMT5 =squeeze(RMT5);
lTOE =squeeze(LTOE);
LHEE =squeeze(LHEE);
LMT5 =squeeze(LMT5);

for i = 1:length(ZMP)/3;
    %----- compute support polygon  RIGHT---------
    pts = [RTOE(i,1:2);RHEE(i,1:2);RMT5(i,1:2)];
    middle_pt=(RTOE(i,1:2) - RMT5(i,1:2))./2;
    pt1 = RHEE(i,1:2) + middle_pt;
    pt2 = RHEE(i,1:2) - middle_pt;
    pt3 = RHEE(i,1:2) + [-middle_pt(2) middle_pt(1)];
    pt4 = RHEE(i,1:2) + [middle_pt(2) -middle_pt(1)];

    final_pt1 = pt1 + (pt3 - pt1)/2;
    final_pt2 = pt2 + (pt4 - pt2)/2;
    final_pt1r = final_pt1;
    final_pt2r = final_pt2;

    h1 = figure(1);
    plot([RTOE(i,1);RMT5(i,1);final_pt2r(1);final_pt1r(1);RTOE(i,1)],...
            [RTOE(i,2);RMT5(i,2);final_pt2r(2);final_pt1r(2);RTOE(i,2)])
        hold on
    plot(ZMP(1,i),ZMP(2,i),'k*')

    %----- compute support polygon  LEFT---------
    pts = [LTOE(i,1:2);LHEE(i,1:2);LMT5(i,1:2)];
    middle_pt=(LTOE(i,1:2) - LMT5(i,1:2))./2;
    pt1 = LHEE(i,1:2) + middle_pt;
    pt2 = LHEE(i,1:2) - middle_pt;
    pt3 = LHEE(i,1:2) + [middle_pt(2) -middle_pt(1)];
    pt4 = LHEE(i,1:2) + [-middle_pt(2) middle_pt(1)];
    final_pt1 = pt1 + (pt3 - pt1)/2;
    final_pt2 = pt2 + (pt4 - pt2)/2;

     plot([LTOE(i,1);LMT5(i,1);final_pt2(1);final_pt1(1);LTOE(i,1)],...
            [LTOE(i,2);LMT5(i,2);final_pt2(2);final_pt1(2);LTOE(i,2)],'r')
    axis equal
    axis([min(ZMP(1,:)) max(ZMP(1,:)) min(ZMP(2,:)) max(ZMP(2,:))])
    legend('Right foot', 'ZMP','Left foot');
     pause(0.05)
    hold off
    
    %---- Check whether zmp is on the support polygon ------
    sp_polygon_R(1,:) = [RTOE(i,1);RMT5(i,1);final_pt2r(1);final_pt1r(1)];
    sp_polygon_R(2,:) = [RTOE(i,2);RMT5(i,2);final_pt2r(2);final_pt1r(2)];
    sp_polygon_L(1,:) = [LTOE(i,1);LMT5(i,1);final_pt2(1);final_pt1(1)];
    sp_polygon_L(2,:) = [LTOE(i,2);LMT5(i,2);final_pt2(2);final_pt1(2)];
    warning('off');
    is_on_sp_polygon_R(i) = tsearch(sp_polygon_R(1,:),sp_polygon_R(2,:),[1 2 3;1 3 4],ZMP(1,i),ZMP(2,i));
    is_on_sp_polygon_L(i) = tsearch(sp_polygon_L(1,:),sp_polygon_L(2,:),[1 2 3;1 3 4],ZMP(1,i),ZMP(2,i));

    % MAKE MOVIE
    Movie_frames(i) = getframe(h1);
end
    movie2avi(Movie_frames,'ZMP_support_polygon_movie.avi')
