% BPC Sensing Matrix
clc
clear
close all

run /users/swinkels/deploy/MatlabVirgoTools/trunk/startup.m
format short

%% Channels


    gps = [1462194492 1462194603 1462194722 1462194871]; % 
    len = 90;
    sensors = {'INJ_BPL_QF_h_norm', 'INJ_BPL_QF_v_norm','INJ_BPL_QN_h_norm', 'INJ_BPL_QN_v_norm' };
    actuators = {'INJ_BPL_PZT_EIB_M1BH_CORR','INJ_BPL_PZT_EIB_M1BV_CORR', 'INJ_BPL_PZT_LB_M14H_CORR', 'INJ_BPL_PZT_LB_M14V_CORR'};

%% Acquire 4x4 matrix of sensors data and 1x4 vector of actuators data

    for m=1:length(actuators)
        for n=1:length(sensors)    
            eh(m,n,:) = getChannel(sensors{n},gps(m),len);
        end
   
      nh(m,:) = getChannel(actuators{m},gps(m),len);
    end
%% FFT, TF and driving matrix parameter

f_line = 2;
c_th = 0.4;

fft_length = 36;
fs =10e3;
navg = len*2/fft_length;
k= round(log2(fft_length * fs));
NFFT = 2^k;

npoints = len*fs;
npt = 2^floor(log(2*npoints/navg)/log(2));



%% Compute Mag Phase and Coherence matrices

close all
clear cdh tfdh
 for m=1:length(actuators)   
        for n=1:length(sensors)
            
            [cdh(m,n,:),f] = mscohere(squeeze(eh(m,n,:)),squeeze(nh(m,:)), blackman(npt),[],npt,fs); 
            [tfdh(m,n,:),f] = tfestimate(squeeze(nh(m,:)),squeeze(eh(m,n,:)), blackman(npt),[],npt,fs);

            figure(1)
            subplot(length(actuators),length(sensors),4*(m-1)+n)
            loglog(f,abs(squeeze(tfdh(m,n,:))))
            xlim([0.9,5])
            grid on
            title(sensors{n})
            
            figure(2)
            subplot(length(actuators),length(sensors),4*(m-1)+n)
            semilogx(f,(angle(squeeze(tfdh(m,n,:)))))
            xlim([0.9,5])
            grid on
            title(sensors{n}, 'Interpreter', 'none')

            figure(3)
            subplot(length(actuators),length(sensors),4*(m-1)+n)
            semilogx(f,(squeeze(cdh(m,n,:))))
            xlim([0.9,5])
            grid on
            title(sensors{n}, 'Interpreter', 'none')

        end
  end
%% Compute the driving matrix

[g,idx] = min(abs(f-f_line));

sensing_matrix = zeros(length(sensors),length(actuators));

for m=1:length(actuators)   
        for n=1:length(sensors)
            Mh(m,n) = abs(squeeze(tfdh(m,n,idx)));
            Ph(m,n) = angle(squeeze(tfdh(m,n,idx)));
            Ch(m,n) = squeeze(cdh(m,n,idx));
            if (Ch(m,n)>c_th)
                if abs(Ph(m,n)) < 0.9*pi
                    sensing_matrix(m,n)=Mh(m,n);
                else
                    sensing_matrix(m,n)=-Mh(m,n);
                end
            end
        end
end

sensing_matrix
driving_matrix = inv(sensing_matrix)'

 
 %% Check the driving matrix, I multiplied each sensor for the weight of each actuator

clear pxx
diag_sensing = zeros(length(sensors),length(actuators), len*fs);
 
diag_sensing(1,1,:) = driving_matrix(1,1)*squeeze(eh(1,1,:))+driving_matrix(2,1)*squeeze(eh(2,1,:))+driving_matrix(3,1)*squeeze(eh(3,1,:))+driving_matrix(4,1)*squeeze(eh(4,1,:));
diag_sensing(1,2,:) = driving_matrix(1,1)*squeeze(eh(1,2,:))+driving_matrix(2,1)*squeeze(eh(2,2,:))+driving_matrix(3,1)*squeeze(eh(3,2,:))+driving_matrix(4,1)*squeeze(eh(4,2,:));
diag_sensing(1,3,:) = driving_matrix(1,1)*squeeze(eh(1,3,:))+driving_matrix(2,1)*squeeze(eh(2,3,:))+driving_matrix(3,1)*squeeze(eh(3,3,:))+driving_matrix(4,1)*squeeze(eh(4,3,:));
diag_sensing(1,4,:) = driving_matrix(1,1)*squeeze(eh(1,4,:))+driving_matrix(2,1)*squeeze(eh(2,4,:))+driving_matrix(3,1)*squeeze(eh(3,4,:))+driving_matrix(4,1)*squeeze(eh(4,4,:));

diag_sensing(2,1,:) = driving_matrix(1,2)*squeeze(eh(1,1,:))+driving_matrix(2,2)*squeeze(eh(2,1,:))+driving_matrix(3,2)*squeeze(eh(3,1,:))+driving_matrix(4,2)*squeeze(eh(4,1,:));
diag_sensing(2,2,:) = driving_matrix(1,2)*squeeze(eh(1,2,:))+driving_matrix(2,2)*squeeze(eh(2,2,:))+driving_matrix(3,2)*squeeze(eh(3,2,:))+driving_matrix(4,2)*squeeze(eh(4,2,:));
diag_sensing(2,3,:) = driving_matrix(1,2)*squeeze(eh(1,3,:))+driving_matrix(2,2)*squeeze(eh(2,3,:))+driving_matrix(3,2)*squeeze(eh(3,3,:))+driving_matrix(4,2)*squeeze(eh(4,3,:));
diag_sensing(2,4,:) = driving_matrix(1,2)*squeeze(eh(1,4,:))+driving_matrix(2,2)*squeeze(eh(2,4,:))+driving_matrix(3,2)*squeeze(eh(3,4,:))+driving_matrix(4,2)*squeeze(eh(4,4,:));

diag_sensing(3,1,:) = driving_matrix(1,3)*squeeze(eh(1,1,:))+driving_matrix(2,2)*squeeze(eh(2,1,:))+driving_matrix(3,2)*squeeze(eh(3,1,:))+driving_matrix(4,3)*squeeze(eh(4,1,:));
diag_sensing(3,2,:) = driving_matrix(1,3)*squeeze(eh(1,2,:))+driving_matrix(2,2)*squeeze(eh(2,2,:))+driving_matrix(3,2)*squeeze(eh(3,2,:))+driving_matrix(4,3)*squeeze(eh(4,2,:));
diag_sensing(3,3,:) = driving_matrix(1,3)*squeeze(eh(1,3,:))+driving_matrix(2,2)*squeeze(eh(2,3,:))+driving_matrix(3,2)*squeeze(eh(3,3,:))+driving_matrix(4,3)*squeeze(eh(4,3,:));
diag_sensing(3,4,:) = driving_matrix(1,3)*squeeze(eh(1,4,:))+driving_matrix(2,2)*squeeze(eh(2,4,:))+driving_matrix(3,2)*squeeze(eh(3,4,:))+driving_matrix(4,3)*squeeze(eh(4,4,:));

diag_sensing(4,1,:) = driving_matrix(1,4)*squeeze(eh(1,1,:))+driving_matrix(2,4)*squeeze(eh(2,1,:))+driving_matrix(3,4)*squeeze(eh(3,1,:))+driving_matrix(4,4)*squeeze(eh(4,1,:));
diag_sensing(4,2,:) = driving_matrix(1,4)*squeeze(eh(1,2,:))+driving_matrix(2,4)*squeeze(eh(2,2,:))+driving_matrix(3,4)*squeeze(eh(3,2,:))+driving_matrix(4,4)*squeeze(eh(4,2,:));
diag_sensing(4,3,:) = driving_matrix(1,4)*squeeze(eh(1,3,:))+driving_matrix(2,4)*squeeze(eh(2,3,:))+driving_matrix(3,4)*squeeze(eh(3,3,:))+driving_matrix(4,4)*squeeze(eh(4,3,:));
diag_sensing(4,4,:) = driving_matrix(1,4)*squeeze(eh(1,4,:))+driving_matrix(2,4)*squeeze(eh(2,4,:))+driving_matrix(3,4)*squeeze(eh(3,4,:))+driving_matrix(4,4)*squeeze(eh(4,4,:));


  for m=1:length(actuators)   
        for n=1:length(sensors)
            [pxx(m,n,:),f] = pwelch(detrend(squeeze(diag_sensing(m,n,:)),'linear'),blackman(NFFT),NFFT/2,NFFT,fs);
            
            figure(4)
            subplot(length(actuators),length(sensors),4*(m-1)+n)
            semilogx(f,squeeze(sqrt(pxx(m,n,:))))
            grid on
            xlim([0.5,100])
            ylim([0,0.005])
            if m == 1
                title(sensors{n}, 'Interpreter', 'none')
            end
            if n == 1
                ylabel(actuators{m}, 'Interpreter', 'none')
            end
            if m == 4
                xlabel('Frequency [Hz]')
            end
        end
 end
