% This script extracts the angular response of the QUAD suspension.
% This is based on Jeff's script; DARMmodel_ALS_20140313.m
% which is available in susSVN:
% /QUAD/Common/FilterDesign/HierarchicalControl
%
%
% 2014.Apr.25th

clear all
clc

set(0,'DefaultLineLineWidth',2)
set(0,'DefaultAxesFontSize',16)
set(0,'DefaultAxesXGrid','on')
set(0,'DefaultAxesYGrid','on')
set(0,'DefaultAxesZGrid','on')

%%
svnlocal = '/Users/izumikiwamu/Documents/SVN/';
svnDir.sus = [svnlocal 'susSVN/'];

toolsDir = [svnDir.sus 'Common/MatlabTools/'];
modelDir = [svnDir.sus 'QUAD/Common/MatlabTools/QuadModel_Production/'];
resultsDir = '../figure/';

figTag = '2014-04-25-ESDcheck';
author = ', K. Izumi';

susType = 'QUAD';
buildType = 'fiber';
plotFlag = false;
isDamped = true;

controlDOFs = {'L','P','Y'};

freqRange = [1e-1 100];
xTicks = [10.^(-1:2)];
freq = logspace(log10(freqRange(1)),log10(freqRange(2)),1024);

sampleFreq = 16384;
sampleTime = 1/sampleFreq;
digType = 'tustin';

             %L  P  Y  % From T1100595
nActs.quad = [2  1  2;... % M0
              4  4  4;... % L1
              4  4  4;... % L2
              4  4  4];   % L3      
          
                % L P     Y         % From T1100595
leverArms.quad = [1 0.078 0.120;... % [m] M0
                  1 0.065 0.065;... % [m] L1
                  1 0.071 0.071;... % [m] L2
                  1 0.140 0.140];   % [m] L3 
              
driveStages.modelName = {'top','uim','pum','tst'};
driveStages.channelName = {'M0','L1','L2','L3'};
driveStages.color = {[0.0 0.0 1.0],... % TOP
                     [0.0 0.7 0.0],... % UIM
                     [1.0 0.0 0.0],... % PUM
                     [0.0 0.7 0.7]};   % TST
              
% cdState = {'lp','lp1','acqOfflpOn','ln5mm'}; % Try to make the UIM useful
% cdState = {'lp','lp3','acqOfflpOn','ln5mm'};
cdState = {'acq','acq','acqOnlpOff','acq5mm'}; % Use largest drive force possible
% cdState = {'lp','lp3','acqOfflpOn','ln20mm'};
% cdState = {'acq','acq','acqOnlpOff','acq20mm'};               

dacGain = 20 / 2^18; % [V/ct] 18 bit.
aiGain  = 1;         % [V/V]

finalStage = 4;
iStage = finalStage; % let's focus on the final stage

urad = 1e-6;
figNum = 1;

%%
%addpath(modelDir,gwincDir,bscrqDir)
addpath(modelDir)

%% Define baseline damped model
[model,~,~,~] = generate_QUAD_Model_Production(freq,buildType,svnDir.sus,plotFlag,isDamped);

% pitch plant dof =2
plant(iStage).dof(2).c = model.dampedss(model.dampedout.(driveStages.modelName{finalStage}).disp.P,...
                                                   model.dampedin.(driveStages.modelName{iStage}).drive.P);
plant(iStage).dof(2).f = squeeze(model.dampedf(model.dampedout.(driveStages.modelName{finalStage}).disp.P,...
                                                   model.dampedin.(driveStages.modelName{iStage}).drive.P,:));

% yaw plant dof =3                                   
plant(iStage).dof(3).c = model.dampedss(model.dampedout.(driveStages.modelName{finalStage}).disp.Y,...
                                                   model.dampedin.(driveStages.modelName{iStage}).drive.Y);
plant(iStage).dof(3).f = squeeze(model.dampedf(model.dampedout.(driveStages.modelName{finalStage}).disp.Y,...
                                                   model.dampedin.(driveStages.modelName{iStage}).drive.Y,:));                                          
       
%% Create DAC, Driver, EUL2OSEM, OSEM2EUL, and output filter models 
addpath(toolsDir)
% get some numbers.
[~,~,osemModel,coilDriverModel]  = make_OSEM_filter_model(freq,susType,driveStages.channelName{iStage},cdState{iStage});

alpha = 4.2e-10; % actuator coefficient of all four sections in N/V^2 (T1100595-v4)
cnts2volt = dacGain  * aiGain * coilDriverModel.dcVoltageGain;
Vbias = cnts2volt * 125e3; % applied 125000 counts in the sus model
volt2torque = 2* alpha * Vbias * leverArms.quad(iStage, 2) * 2/pi; % 2/pi correction due to an integration over the arc (J.Kissel will update the document)

cnts2torque = cnts2volt * volt2torque; % from counts to torque. Assuming all the segments gets the same amount of voltage.
%% get the data (they are already calibrated in urad)
pitdata = '../data/pit_ESD_oplev_um.dat';
yawdata = '../data/yaw_ESD_oplev_um.dat';
P = importdata(pitdata);
Y = importdata(yawdata);

fp = P(:,1);
fy = Y(:,1);
pitMeasured = -P(:,2) - i*P(:,3);
yawMeasured = Y(:,2) + i*Y(:,3); % somehow the sign appeared to be opposite

LOCK2EUL = 2.5254; % this was set at 2.5254 for some reason when tfs were measured.
%%
figure(figNum);
figNames{figNum} = [resultsDir figTag '_PlantDriveChain.pdf'];
figNum = figNum + 1;
subplot(211)
ll = loglog(freq,abs([plant(4).dof(2).f .* cnts2torque/ urad...
                      plant(4).dof(3).f .* cnts2torque/ urad ]));
hold on
loglog(fp, abs(pitMeasured)./LOCK2EUL, '.',...
        'MarkerSize', 20,...
        'Color', 'c')
loglog(fy, abs(yawMeasured)./LOCK2EUL, '.',...
        'MarkerSize', 20, ...
         'Color', 'm')    

set(ll(1),'Color',driveStages.color{2})
set(ll(2),'Color',driveStages.color{3})
xlim(freqRange)
set(gca,'XTick',xTicks)
ylim([1e-9 1e-3])
set(gca,'YTick',10.^(-9:-3))
xlabel('Frequency [Hz]')
ylabel('Magnitude [urad/cnts]')
title('From LOCK output to angles with all Euler matrix elements set 1')
legend('Pit model (damped)', 'Yaw model (damped)', 'Pit measured', 'Yaw measure')

subplot(212)
ll = semilogx(freq,180/pi*angle([plant(4).dof(2).f...
                                 plant(4).dof(3).f]));
set(ll(1),'Color',driveStages.color{2})
set(ll(2),'Color',driveStages.color{3})
hold on;
semilogx(fp, 180/pi*angle(pitMeasured), '.',...
            'MarkerSize', 20, ...
             'Color', 'c');

semilogx(fy, 180/pi*angle(yawMeasured), '.',...
            'MarkerSize', 20, ...
             'Color', 'm');

xlim(freqRange)
set(gca,'XTick',xTicks)
ylim([-185 185])
set(gca,'YTick',-180:45:180)
xlabel('Frequency [Hz]')
ylabel('Phase [deg]')




