Km     = 0.0218;        % motor constant, Nm/A
Rm     = 20.2;          % winding resistance, Ohm
alpha  = 0.001888;      % motor position (mm) = alpha*motor position (rad), in mm/rad 
Kpitch = 0.055;         % pitch correction constant, A/rad
i_0    = 0.03;          % nominal current at 0 pitch, A
i_cutoff  = 0.02;       % nominal current at cutoff, A
omega_nominal = 419.4;  % nominal motor speed, rad/s
sigma = [0.34 0.0025 0.0035 0.00021];   % residual std
NN = 5;

file_list = dir('2019-04-02-15iterations/*.hdf5');

maxresidual = zeros(length(file_list),4);
allsigma = zeros(length(file_list),4);
for k = 1:length(file_list)
    if (k == 1) || (k == 10)
        continue
    end
    filename = strcat(file_list(k).folder,'\',file_list(k).name);
    [time,voltage, current, strpot, encoder, ~,~,~,~,~,~,~,~,~,pitch] = read_hdf_file(filename);
    
    [time,indx] = unique(time); % remove duplicates
    voltage = voltage(indx); current = current(indx); strpot = strpot(indx); encoder = encoder(indx); pitch = pitch(indx);
    
    [voltage_V,current_A,strpot_mm,encoder_rad] = convert_to_physical_units(voltage,current,strpot,encoder);
    
    endofleg = find(diff(time) > 0.75);   % end of leg
    k1 = 2:(endofleg(1)-2);
    k2 = (endofleg(1)+2):(endofleg(2)-2);

    %% encoder - stringpot
    r0 = alpha * encoder_rad - strpot_mm;

    %% omega_el - omega_mech
    omega_el = (voltage_V - Rm*current_A)/Km; 
    omega_mech = [diff(encoder_rad) ./ diff(time) 0];
    r1 = alpha * (omega_el(k1) - omega_mech(k1));
    r1 = [r1  alpha * (omega_el(k2) - omega_mech(k2))];
     
    %% current - nominal
    r2 = current_A(k1) - nominal_current(pitch(k1),1);% %% R2 = delta between measured current and nominal current
    r2 = [r2 current_A(k2) - nominal_current(pitch(k2),-1)];
 
    %% mechanical speed - nominal
    r3 = alpha * (omega_mech(k1) - omega_nominal);
    r3 = [r3 alpha * (omega_mech(k1) - omega_nominal)];
    
    maxresidual(k,:) = [max(abs(r0))  max(abs(r1))  max(abs(r2))  max(abs(r3))];
    allsigma(k,:) = [std(r0)  std(r1)  std(r2)  std(r3)];
    
    fprintf("file %d | max residual: %f %f %f %f \n",k, maxresidual(k,:))
end
