%Combine all processing files
%Heather Beem Aug 2010
%Updated Spring 2011- use chord instead of chord_eff for Cl calculations
%Includes new calibration info from different force sensor ...
    %(installed Dec 2011, which is after these particular experiments were done)

clear all;
close all;
clc;

%Model Characteristics
lambda = 0; %sweepback angle, deg CHANGE THIS FOR DIFF ANGLES
mass = 1.4414; %measured with weigh scale (kg)
U = 0.2; %tow speed (m/s)
chord = 0.0693; %m
chord_eff = chord/cos(lambda*pi/180); %adjust for sweep angle, convert to rad
span_eff = 0.43; %m CHANGE THIS FOR DIFF ANGLES
forcehz = 1000; %sampling rate of force meas


%% Calibration
C = [-0.54078   -0.02116   1.00555 -32.46314   0.16251  32.45768; ...
    -0.36244  39.01850  -0.46187 -18.80360   0.70820 -18.84513; ...
    18.54507  -0.75908  18.85107  -0.52706  18.75407  -0.84758; ...
    -0.17990  0.12059 -32.64874   0.82334  32.25039  -1.49116; ...
    37.18211  -1.56733 -18.80921   0.54439 -18.38546   0.80521; ...
    0.26372 -19.18453   0.31216 -18.71633 0.02841 -18.72425];
    % Factory Calibration in lbf/volt or in-lbf/volt

C(1,:) = C(1,:)./17.5315918109945;
C(2,:) = C(2,:)./17.5315918109945;
C(3,:) = C(3,:)./5.60414235116549;
C(4,:) = C(4,:)./8.52944252054268;
C(5,:) = C(5,:)./8.52944252054268;
C(6,:) = C(6,:)./8.20975505707158; %Scale factors

C = [C(1:3,:)*4.44822; C(4:6,:)*0.11298];  
    % Convert calibration units to N and N-m

%% Create filter for raw force data 
% %(BUTTERWORTH)
% Wp=1/(forcehz/2); %original: 10/500
% Ws=5/(forcehz/2); %original: 15/500
% Rp=3;
% Rs=-6;
% %Determine order of filter and effective cutoff freq
% [N, Wn] = buttord(Wp, Ws, Rp, Rs);
% %Calculate filter's TF coefficients
% [B,A] = butter(N,Wn);

% (CHEBYSHEV)
Wp = 1.5/(forcehz/2);
n = 1;
R = 3;
[B,A] = cheby1(n,R,Wp);
%% Define DAQ Info

% Define how the data was read in 
servocycle = 203.959*10^-6;
gatherperiod = 10;
gathertime = servocycle*gatherperiod;


%% Process Position Data (Loop)

%-----------Read in files---------------
files_pos = dir('*.txt'); %create directory of all the .txt files
hexpathname = 'C:\Users\Heather\My Dropbox\Research\Sweepback\Sweep_MATLAB\Combine Files\0_Water';
    
for i_pos = 1:length(files_pos)
%---- Convert position from hex------------
hexfilename = ['output_3_' num2str(i_pos) '_' num2str(i_pos) '.txt'];
motdata = hexinterp(hexpathname,hexfilename,2,[0 0])*0.000326; 
%encoder counts to meters (not quite....)

    
    %Adjust scaling (above constant is incorrect)
    motyposinit = mean(motdata(500:1000,2));
    motyposfin = mean(motdata(4000:5000,2));
    motydelta = abs(motyposinit - motyposfin);
    actualydelta = (2.00590+3.50310)*0.0254; % 5.5 in = 0.1399m
    motscale = actualydelta/motydelta;
    motdata = motdata*motscale;
  
    %Assign values
    mottime_all = 0:gathertime:(length(motdata)-1)*gathertime;
    motxpos_all = motdata(:,1);
    motypos_all = motdata(:,2);
    mothz = 1/(mean(diff(mottime_all)));



%Resample motion freq to force freq (1000 Hz)
motxpos_all = resample(motxpos_all,round(forcehz),round(mothz));
motypos_all = resample(motypos_all,round(forcehz),round(mothz));
mottime_all = resample(mottime_all,round(forcehz),round(mothz));
mottime_all = mottime_all';


% figure
% plot(motypos_all)
% title('Foil Position', 'FontSize',16)


%----Determine frames of sweep motion (detect decrease in position data)---
for i_mot = 2000:4000
    if motypos_all(i_mot) == max(motypos_all(2000:4000)); %reach max value
            i_mot_max = i_mot; %time of max position
            mot_sweep_start_indiv = i_mot_max;
    end
end

for i_mot = 3000:5000
        if motypos_all(i_mot) < -0.0889 && motypos_all(i_mot+ 1) < -0.0889 ...
            && motypos_all(i_mot+ 2)< -0.0889 %reaches certain value
           mot_sweep_end_indiv = i_mot; %end of sweep (From when motor trigger starts)               
           break
        end                 
end

    % Write start and end of sweep (from motor) to single big matrix
    mot_sweep_start(i_pos,1) = mot_sweep_start_indiv;
    mot_sweep_end(i_pos,1) = mot_sweep_end_indiv;

%----Convert position to acceleration (calculate inertia data)------------ 

motyvel_all = cdiff(motypos_all).*forcehz;
% figure
% plot(mottime(1:length(motyvel)),motyvel)

motyvel_all = filtfilt(B,A,motyvel_all);
% figure
% plot(mottime(1:length(motyvel)),motyvel)

motyacc_all = cdiff(motyvel_all).*forcehz;
% figure
% plot(mottime(1:length(motyacc)),motyacc) 
 
motyacc_all = filtfilt(B,A,motyacc_all);
% figure
% plot(mottime(1:length(motyacc)),motyacc)
   
%Cut off data before sweep motion 
    motypos = motypos_all(mot_sweep_start_indiv:length(motypos_all));
    motyvel = motyvel_all(mot_sweep_start_indiv:length(motypos_all)-1);
    motyacc = motyacc_all(mot_sweep_start_indiv:length(motypos_all)-2);
    motxpos = motxpos_all(mot_sweep_start_indiv:length(motypos_all));
    mottime = mottime_all(mot_sweep_start_indiv:length(motypos_all)-1);
    
        % Scale position measurements
        countsy = (max(motypos) - min(motypos))/2;    % counts          
        % Scale velocity and acceleration measurements in the y direction
        % Calculate the intertia in the y direction.
        foilyvel = motyvel/countsy*chord;                % Meters/Second
        foilyacc = motyacc/countsy*chord;                 % Meters/Second^2
        foilyinertia_indiv = mass*foilyacc;                 % Newtons
   
    
% figure
% plot(foilyinertia_indiv(1:mot_sweep_end_indiv))
% title('Indiv Inertia Force', 'FontSize',16)

%Combine inertia force into one big cell (set of indiv matrices)
foilyinertia{i_pos} = foilyinertia_indiv;

%Save lengths of each runs' sweep time into one matrix
length_inertiamatrix(i_pos) = length(foilyinertia_indiv); 


end
%     All Time:
%     figure
%     plot(mottime_all(1:9440), motypos_all(1:9440), mottime_all(1:9440),...
%         motyvel_all(1:9440), mottime_all(1:9440), motyacc_all(1:9440))
%     legend('pos','vel','acc')
%     During Sweep:
    figure
    plot(mottime_all(mot_sweep_start(i_pos):mot_sweep_end(i_pos)), ...
        motypos_all(mot_sweep_start(i_pos):mot_sweep_end(i_pos)),...
        mottime_all(mot_sweep_start(i_pos):mot_sweep_end(i_pos)),...
        motyvel_all(mot_sweep_start(i_pos):mot_sweep_end(i_pos)),...
        mottime_all(mot_sweep_start(i_pos):mot_sweep_end(i_pos)),...
        motyacc_all(mot_sweep_start(i_pos):mot_sweep_end(i_pos)))
    legend('pos','vel','acc')
    xlabel('Time(sec)')
    ylabel('m, m/s, m/s^2')

%-------------------Average all runs together------------------------------
length_smallest = min(length_inertiamatrix); 
%use smallest sweep time. may all be diff sizes

foilyinertiasum = 0;
for j_pos=1:length(files_pos) %Sum all together

    foilyinertiasum = foilyinertiasum + ...
        foilyinertia{j_pos}(1:length_smallest);
     
end


foilyinertiaavg = foilyinertiasum/length(files_pos); %take average
figure
plot (mottime(1:9000), foilyinertiaavg(1:9000));
title('Averaged Inertia Force', 'FontSize',16)
xlabel('Time (sec)','FontSize',16)
ylabel('Fy (N)','FontSize',16) 
hold off

dlmwrite ('Inertia0.xls',foilyinertiaavg(1:2000))

%% Process Force Data
%-----------Read in files--------------------------------------------------
files = dir('*.lvm'); %create directory of all the .lvm files
%---------Run loop for all files-------------------------------------------
for i_force=1:length(files) 


    data = load(files(i_force).name);
    
    voltage = data(:,1:6);  % Columns 1-6 are voltages for Fx,Fy,Fz,Mx,My,Mz
    velocity = data(:,9)/6.6824;  % Velocity should be in column 9
    U = abs(mean(velocity(15000:19000)));
    forcedata = C*voltage';  % Forces in N and N-m, need to transpose the voltage matrix to get it in the right form
    forcedata = forcedata'; %Flip signs so that positive lift is positive value

    
    forcedata = forcedata(:,2); %only Fy
    forcedata = forcedata - mean(forcedata(1:1000)); %subtract initial value (first 1 sec)

    t = dlmread('t.csv'); % load t data
    t = t(1:length(data),:);
    
  
    %Filter each run FIRST
    forcedata_filter = filtfilt(B,A,forcedata);
%     plot(t,forcedata_filter,t,data(:,8)./10, t, velocity) %scale motor trigger so easier to see
%     legend('Measured Force (Filtered)', 'Motor Trigger', 'Carriage Velocity')
%     xlabel('Time (sec)')
    
%------Detect jump in motor trigger----------------
    for j_force = 1:length(data)

        if data(j_force+1,8) - data(j_force,8) < -2.5 && data(j_force+2,8)< 1  %jumps down
            force_motor_start = j_force+1;  %when motor trigger starts (but not yet sweep)
            break
        end        
        
    end
%------Calculate time of sweep---------------------
    force_sweep_start = force_motor_start + mot_sweep_start(i_force); 
        %sweep starts ~3 sec after motor trigger
        %get the mot values from different cell (Process Position Data).
    force_sweep_end = force_motor_start + mot_sweep_end(i_force);
    
    force_sweep_settle = force_sweep_end + 2500; %time beyond end of sweep. use to 
        %see force settle to zero again. CHANGE IF USE CHORD_EFF INSTEAD
   
%------Combine Fy data into one big cell-----------
    force_sweep{i_force} = forcedata_filter(force_sweep_start:force_sweep_end);
    force_sweep_extend{i_force} = forcedata_filter(force_sweep_start:force_sweep_settle);
    
%------Set beginning values to zero--------------------------------
    force_sweep{i_force} = force_sweep{i_force} - force_sweep{i_force}(1);
    force_sweep_extend{i_force} = force_sweep_extend{i_force} - force_sweep_extend{i_force}(1);

%-----Save lengths of each runs' sweep time into one matrix--------
    length_forcematrix(i_force) = length(force_sweep{i_force});     
    length_forcematrix_extend(i_force) = length(force_sweep_extend{i_force});

    
end
% figure; hold on;
% plot(force_sweep{1});plot(force_sweep{2});plot(force_sweep{3});
% plot(force_sweep{4});plot(force_sweep{5});plot(force_sweep{6});
% plot(force_sweep{7});plot(force_sweep{8});plot(force_sweep{9});
% plot(force_sweep{10});plot(force_sweep{11});plot(force_sweep{12});
% plot(force_sweep{13});plot(force_sweep{14});plot(force_sweep{15});
% title('Fy for all runs: 0 deg')
% xlabel('Time (ms)')
% ylabel('Fy (N)')


%-------Average force values during the sweep---------
length_smallest_force = min(length_forcematrix); %use smallest sweep time
length_smallest_force_extend = min(length_forcematrix_extend);

forcesum = 0;
for j_force=1:length(files) %Sum all together
    forcesum = forcesum + force_sweep{j_force}(1:length_smallest_force);
     
end

forcesum_extend = 0;
for j_force_extend=1:length(files) %Sum all together
    forcesum_extend = forcesum_extend + ...
        force_sweep_extend{j_force_extend}(1:length_smallest_force_extend);
     
end

forceavg = forcesum/length(files); %take average
% figure
% plot(t(1:length(forceavg)), forceavg)
% title('Averaged Fy (Measured)')

forceavg_extend = forcesum_extend/length(files); %take average
% figure
% plot(t(1:length(forceavg_extend)), forceavg_extend)
% title('Averaged Fy (Measured)')

        
%-----------------------------Calculate Lift-------------------------------
        Lift_meas = forceavg; %Set beginning to equal zero?
        t_sweep = t(force_sweep_start:force_sweep_end,:);
        t_sweep = t_sweep - t_sweep(1);
        tstar = t_sweep*U/chord; %Nondimensionalize time (standard way)
        t_nondim = t_sweep/(length(t_sweep)/1000); %Nondim time wrt plunge (t/T)
        
      
%         figure
%         plot(tstar(1:length(Lift_meas)),Lift_meas)
%         title('Fy Measured during Sweep','FontSize',12)
%         xlabel('Nondim Time (tU/c)','FontSize',16)
%         ylabel('Fy (N)','FontSize',16)  
    
    
        Lift_meas_extend = forceavg_extend; 
              
        t_sweep_extend = t(force_sweep_start:force_sweep_settle,:);
        t_sweep_extend = t_sweep_extend - t_sweep_extend(1);
        tstar_extend = t_sweep_extend*U/chord; 
                %Nondimensionalize time (standard way)
                
        t_nondim_extend = t_sweep_extend/(length(t_sweep)/1000); 
                %Nondim time wrt plunge
        
%% Combine Tare and Force Measurements  
    %--------------Sweep Motion Only------------------------------
    Lift = Lift_meas + foilyinertiaavg(1:length(Lift_meas)); %Newtons 
    
    Lift = Lift-Lift(1); %set beginning value to zero
    
    figure
    plot(t(1:length(forceavg)), Lift_meas, t(1:length(forceavg)), ...
        foilyinertiaavg(1:length(forceavg)), t(1:length(forceavg)), ...
        Lift(1:length(forceavg)))
    legend('All','Inertia','Hydro')
    title('0 deg (During Sweep)','FontSize',16)
    xlabel('Time (sec)','FontSize',16)
    ylabel('Force (N)','FontSize',16)
    
    CL = Lift/(0.5*1000*U.^2*chord*span_eff); 
    figure
    plot(t_nondim(1:length(CL)),CL)
    title('Lift Coefficient: 0 deg (During Sweep)', 'FontSize',16) 
    xlabel('t/T','FontSize',16)
    ylabel('Cl','FontSize',16)
    
       
    dlmwrite('Cl0.xls', CL);
    dlmwrite('tstar0.xls', t_nondim(1:length(CL)));
    
    %------------------------Beyond Sweep Motion--------------------------
    Lift_extend = Lift_meas_extend + foilyinertiaavg(1:length(Lift_meas_extend)); 
    Lift_extend = Lift_extend-Lift_extend(1);
    
    figure
    plot(t(1:length(Lift_extend)), ...
        Lift_meas_extend(1:length(Lift_extend)), ...
        t(1:length(Lift_extend)), ...
        foilyinertiaavg(1:length(Lift_extend)),...
        t(1:length(Lift_extend)), ...
        Lift_extend(1:length(Lift_extend)))
    legend('All','Inertia','Hydro')
    title('Lift Coefficient: 0 deg', 'FontSize',16)
    xlabel('Time (sec)','FontSize',16)
    ylabel('Force (N)','FontSize',16)        

    
    CL_extend = Lift_extend/(0.5*1000*U.^2*chord*span_eff); 
    figure
    plot(t_nondim_extend(1:length(CL_extend)),CL_extend)
    title('Lift Coefficient: 0 deg', 'FontSize',16) 
    xlabel('t/T','FontSize',16)
    ylabel('Cl','FontSize',16)
    
        
    dlmwrite('Cl0_extend.xls', CL_extend);
    dlmwrite('tstar0_extend.xls', t_nondim_extend(1:length(CL_extend)));
