% determine_force_connection
% This m file will determine which foot is in contact with which force
% plate by searching the COP data and comparing the distance of the COP to
% the CAL marker on the left and right foot. If three plates are used, then
% forces have to be concatenated, as OpenSIM only permits 2 external forces
% to be applied

if n == 10 % then only one plate is used
    COP1x=grf_complete(:,8)*1000;
    COP1z=grf_complete(:,10)*1000;
    Fy1 = grf_complete(:,3);
    foot_on_plate1 = find(Fy1>0);

    COP1xz_HS=[mean(COP1x(foot_on_plate1)) mean(COP1z(foot_on_plate1))];

    RCALx_HS_t1 = spline(marker_time,RCALx,time_foot_on_plate1);
    RCALz_HS_t1 = spline(marker_time,RCALz,time_foot_on_plate1);
    RCALxz_t1 = [RCALx_HS_t1 RCALz_HS_t1];

    LCALx_HS_t1 = spline(marker_time,LCALx,time_foot_on_plate1);
    LCALz_HS_t1 = spline(marker_time,LCALz,time_foot_on_plate1);
    LCALxz_t1 = [LCALx_HS_t1 LCALz_HS_t1];

    if norm(COP1xz_HS - RCALxz_t1) < norm(COP1xz_HS - LCALxz_t1)
        % then the first force plate is connected to the Right Foot
        trc_fname
        Force1_connection = 'R'
    else %  the first force plate is connected to the Left Foot
        trc_fname
        Force1_connection = 'L'
    end

end

if n == 19 % then two force plates are present in the trial
    COP1x=grf_complete(:,14)*1000;
    COP1z=grf_complete(:,16)*1000;
    COP2x=grf_complete(:,17)*1000;
    COP2z=grf_complete(:,19)*1000;
    % determine heel strikes
    Fy1 = grf_complete(:,3);
    foot_on_plate1 = find(Fy1>0);
    time_foot_on_plate1 = grf_complete(foot_on_plate1(1));

     Fy2 = grf_complete(:,9);
     foot_on_plate2 = find(Fy2>0);
     time_foot_on_plate2 = grf_complete(foot_on_plate2(1));

    % find mean COP when foot is on the plate
    COP1xz_HS=[mean(COP1x(foot_on_plate1)) mean(COP1z(foot_on_plate1))];
    COP2xz_HS=[mean(COP2x(foot_on_plate2)) mean(COP2z(foot_on_plate2))];

    % get CAL marker data for same time point
    RCALx_HS_t1 = spline(marker_time,RCALx,time_foot_on_plate1);
    RCALz_HS_t1 = spline(marker_time,RCALz,time_foot_on_plate1);
    RCALx_HS_t2 = spline(marker_time,RCALx,time_foot_on_plate2);
    RCALz_HS_t2 = spline(marker_time,RCALz,time_foot_on_plate2);

    RCALxz_t1 = [RCALx_HS_t1 RCALz_HS_t1];
    RCALxz_t2 = [RCALx_HS_t2 RCALz_HS_t2];

    LCALx_HS_t1 = spline(marker_time,LCALx,time_foot_on_plate1);
    LCALz_HS_t1 = spline(marker_time,LCALz,time_foot_on_plate1);
    LCALx_HS_t2 = spline(marker_time,LCALx,time_foot_on_plate2);
    LCALz_HS_t2 = spline(marker_time,LCALz,time_foot_on_plate2);

    LCALxz_t1 = [LCALx_HS_t1 LCALz_HS_t1];
    LCALxz_t2 = [LCALx_HS_t2 LCALz_HS_t2];

    % find the linear distance between the COP values and the left
    % and right CAL markers

    if norm(COP1xz_HS - RCALxz_t1) < norm(COP1xz_HS - LCALxz_t1)
        % then the first force plate is connected to the Right Foot
        trc_fname
        Force1_connection = 'R'
        Force2_connection = 'L'
    else %  the first force plate is connected to the Left Foot
        trc_fname
        Force1_connection = 'L'
        Force2_connection = 'R'
    end

                if norm(COP2xz_HS - RCALxz_t2) < norm(COP2xz_HS - LCALxz_t2)
                    % then the second force plate is connected to the Right Foot
                    Force1_connection = 'R';
                    Force2_connection = 'L';
                else %  the first force plate is connected to the Left Foot
                    Force1_connection = 'R';
                    Force2_connection = 'L';
                end
    
end

if n == 28 % then three force plates are present in the trial...this should only occur during a walking trial
    % we know then that the first set of force plate data is the
    % large (middle) plate. So we need to append data from the
    % second and third plates, as OpenSIM only accepts two force inputs, one for each segment

    COP1x=grf_complete(:,20)*1000;
    COP1z=grf_complete(:,22)*1000;

    % determine heel strikes
    Fy1 = grf_complete(:,3);
    foot_on_plate1 = find(Fy1>0);
    time_foot_on_plate1 = grf_complete(foot_on_plate1(1));

    Fy2 = grf_complete(:,9);
    foot_on_plate2 = find(Fy2>0);
    time_foot_on_plate2 = grf_complete(foot_on_plate2(1));

    Fy3 = grf_complete(:,15);
    foot_on_plate3 = find(Fy3>0);
    time_foot_on_plate3 = grf_complete(foot_on_plate3(1));

    if time_foot_on_plate3 < time_foot_on_plate2
        middle_point = foot_on_plate3(end)+10;

        % append two force plates into one set
        combined_forces = [grf_complete(1:middle_point,14:19);grf_complete(middle_point+1:end,8:13)];
        combined_COP = [grf_complete(1:middle_point,26:28);grf_complete(middle_point+1:end,23:25)];
        % now append the end of force plate data 2 onto force plate
        % data 3
        new_grf_complete=[grf_complete(:,1:7) combined_forces grf_complete(:,20:22) combined_COP];
        grf_complete = new_grf_complete;
    else
        middle_point = foot_on_plate2(end)+10

        % append two force plates into one set
        combined_forces = [grf_complete(1:middle_point,8:13);grf_complete(middle_point+1:end,14:19)];
        combined_COP = [grf_complete(1:middle_point,23:25);grf_complete(middle_point+1:end,26:28)];
        % now append the end of force plate data 2 onto force plate
        % data 3
        new_grf_complete=[grf_complete(:,1:7) combined_forces grf_complete(:,20:22) combined_COP];
        grf_complete = new_grf_complete;
    end

    % find mean COP when foot is on the plate
    COP1xz_HS=[mean(COP1x(foot_on_plate1)) mean(COP1z(foot_on_plate1))];

    % get CAL marker data for same time point
    RCALx_HS_t1 = spline(marker_time,RCALx,time_foot_on_plate1);
    RCALz_HS_t1 = spline(marker_time,RCALz,time_foot_on_plate1);
    RCALxz_t1 = [RCALx_HS_t1 RCALz_HS_t1];

    LCALx_HS_t1 = spline(marker_time,LCALx,time_foot_on_plate1);
    LCALz_HS_t1 = spline(marker_time,LCALz,time_foot_on_plate1);
    LCALxz_t1 = [LCALx_HS_t1 LCALz_HS_t1];

    % find the linear distance between the COP values and the left
    % and right CAL markers

    if norm(COP1xz_HS - RCALxz_t1) < norm(COP1xz_HS - LCALxz_t1)
        % then the first force plate is connected to the Right Foot
        trc_fname
        Force1_connection = 'R'
        Force2_connection = 'L'
    else %  the first force plate is connected to the Left Foot
        trc_fname
        Force1_connection = 'L'
        Force2_connection = 'R'
    end
end