function flatCOP = calcFlattenedCOP(tsol, xsol, contactInfo, vParams,vToe)

buildParamVariableList; %requires vParams and vToe


flatCOP = [];
flatCOP.param = [((K1cx-K2ax)-K1bx) ((K1cy-K2ay)-K1by) (K1cz-K1bz) rH rF K1crz]; 
flatCOP.paramNames = {'meta_x-heel-x', 'meta_y-heel_y', 'meta_z-heel-z',...
                      'heel radius','meta radius','mtj_ang'};

if(K2az ~= 0)
   disp('ERROR: Flat COP assumes meta tarsal joint is on the frame!'); 
end
                  
flatCOP.heel.fo  = zeros(length(tsol),3); % Force on disk in the gnd. frame
flatCOP.heel.rpm = zeros(length(tsol),3); % COP of heel in disk frame
flatCOP.meta.fo  = zeros(length(tsol),3); % Force ...
flatCOP.meta.rpm = zeros(length(tsol),3); % COP of meta in disk frame
flatCOP.fo = zeros(length(tsol),3); %Total GRF
flatCOP.cop= zeros(length(tsol),3); %Flat cop location with 0,0 as the heel
                                    %center


epsRoot = eps^0.5;

for i = 1:1:length(tsol)

%%
%turn the arrays into named variables
%%    
    %State Information
    x    = xsol(i,8);
    y    = xsol(i,9);
    z    = xsol(i,10);
    th   = xsol(i,11);
    zeta = xsol(i,12);
    eta   = xsol(i,13);
    xi  = xsol(i,14);
        
    %Contact Information
    HFx = contactInfo(i,1);
    HFy = contactInfo(i,2);
    HFz = contactInfo(i,3);
    Hcopx  = contactInfo(i,4);
    Hcopy  = contactInfo(i,5);
    Hcopz  = contactInfo(i,6);
    
    FFx = contactInfo(i,7);
    FFy = contactInfo(i,8);
    FFz = contactInfo(i,9);
    Fcopx  = contactInfo(i,10);
    Fcopy  = contactInfo(i,11);
    Fcopz  = contactInfo(i,12);
    
    calcPosVecsRotMatrices;
    
    %%
    %Get heel loads in the ground frame.
    %Calculate vector from heel center to heel cop in the heel frame
    %%
    flatCOP.heel.fo(i,:) = [HFx HFy HFz];       
    rpmH0 =  [Hcopx Hcopy Hcopz] - rHeel;   
    rpmH  =  (RMHeel*rpmH0')';
    flatCOP.heel.rpm(i,:) = rpmH;
    
    %%
    %Do the same for the metatarsal contact
    %%
    flatCOP.meta.fo(i,:) = [FFx FFy FFz];
    rpmF0 = [Fcopx Fcopy Fcopz] - rForeFoot;
    rpmF = (RMForeFoot*rpmF0')';
    flatCOP.meta.rpm(i,:) = rpmF;
    
    %%
    %Now the whole foot
    %%
    
    flatCOP.fo(i,:) = flatCOP.heel.fo(i,:) + flatCOP.meta.fo(i,:);
    copH = rpmH;
    copF = rpmF + flatCOP.param(1:3);
    copFlat = (copH.*HFz + copF.*FFz)./(epsRoot + HFz+FFz);
    flatCOP.cop(i,:) = copFlat;    
    
end