function [mytf] = dsa_tf2(w_list,amp_list,sval,datestamp)
% function [mytf] = dsa_1102(w_list,amp_list,sval,datestamp)
% ---------------------(March 19, 2002)---------------------
% ----- implements automatic downsampling (for low frequencies)
% w_list : optional vector of INPUT FREQUENCY values at which to find TF
% amp_list : optional vector of INPUT AMPLITUDE values at which SINE
%          output will sweep.  (If amp_list is a scalar, the same 
%          amplitude will be used at ALL frequencies...)
% sval : optional string value for the plot (e.g. 'r*' would plot
%          red *'s at points on figure(1)
% datestamp: optional 4th argument (not shown above).
%          If a non-zero 4th argument is used in calling the
%          function dsa_tf, the program will 'time stamp' the
%          output figure.  This will NOT WORK unless you have
%          the time-stamping MATLAB function 'get_date.m'
%          as well, however!!!
%-----------------------------------------------------------------------
% OUTPUTs: The output is an (N x 3) matrix. 'N' is the length of w_list.
%    Column 1:  Returned FREQUENCY list (w_list values)
%    Column 2:  GAIN, as the ratio of channel2/channel1 (not in db)
%    Column 3:  PHASE, in degrees
%-----------------------------------------------------------------------
% NOTES: 
%  (1) You must be running a SIMULINK model on the ds1102 board
%      for the MATLAB function dsa_tf() to work, and the simulink
%      model must include a special block called 'Dynamic Signal Analyzer'
%  (2) This program will overwrite figures 1 and 2 (in matlab)!!
%      Before running dsa_tf(), make sure you do not have images/plots 
%      in either figure which should not be destroyed. 

%-----------------------------------------------------------------------
% Section 0: Define number of samples and default siggen amplitude.
if ~exist('datestamp')
    do_date=0;
else
    do_date=(datestamp~=0);
end
N=10000;      % N: number of points to sample. 
ds=1;         % downsampling of 1, unless otherwise spec'd?
w_rad=0;      % indicates frequencies are NOT in rad/sec by default (Hz default)
dsa_A=.1;     % default AMPLITUDE of Swept Sine from DSA
exitval=0;    % program will exit if this is non-zero.
%-----------------------------------------------------------------------
% Section 1: Make sure all frequencies and amplitudes are set for this run:
fprintf(1,'\nNote: Make sure your (DSA) model is built and running.\n');
fprintf(1,'      This program will erase any images or plots in figure 1 and figure 2.\n\n');
fprintf(1,'      If you would like to STOP this program now, enter ''q'' to quit,\n');
isok=input('      otherwise, just hit enter to continue : ','s');
if strcmpi(isok,'q')
    fprintf(1,'\nOK, bye.\n')
else   
    if ~exist('sval') % default symbol for Bode plot
        sval='bo-';
    end
    if ~exist('w_list')
        w0=1:(1/10):3.0;
        w_use=10.^w0;
        fprintf('\nRunning %d points.\n  [Range = %.2f [Hz] (min) to %.2f [Hz] (max)] \n',length(w_use),min(w_use),max(w_use));
    else
        is_rad=input('\nThe list of frequencies you entered was in:\n HERTZ (h) or RAD/SEC (r) [default to Hertz]? ','s');  
        if strcmpi(is_rad,'r')
            fprintf('\n--> using RAD/SEC\n');
            w_use=(1/(2*pi))*w_list;
            w_rad=1;
        elseif strcmpi(is_rad,'h')
            fprintf('\n--> using HERTZ\n');
            w_use=w_list;
        else
            fprintf('\n...hmmm, I don''t understand, so I''m going to default and use HERTZ.\n');
            w_use=w_list;   
        end
    end   
    if ~exist('amp_list')
        A=dsa_A;           % initial SINE WAVE amplitude
        amp_list=A+(0*w_use);
    else
        if length(amp_list)<length(w_use)
            fprintf('Using %.4f as SINE WAVE amplitude for frequencies after %.4f Hz...\n',...
                amp_list(length(amp_list)), w_use(length(amp_list)));          
            for foo=(length(amp_list)+1):length(w_use)
                amp_list(foo)=amp_list(foo-1);  % take last value
            end
        end
    end
    %-----------------------------------------------------------------------
    % Section 2: Get dSPACE board parameter addresses with mlib().
    mlib('SelectBoard','ds1102');
    %   mtrc31('SelectBoard','ds1102');
    %   amp_addr=mlib('GetSimAddr','P[Model Root/Dynamic Signal Analyzer/Swept Sine.Amplitude]');
    %   freq_addr=mlib('GetSimAddr','P[Model Root/Dynamic Signal Analyzer/Swept Sine.Frequency]');
[amp_freq]=mlib('GetTrcVar',...
        {'Model Root/Dynamic\nSignal Analyzer/Swept\nSine/Amplitude';...
            'Model Root/Dynamic\nSignal Analyzer/Swept\nSine/Frequency'}); % MB 8/13/04
    phi_last=0; B=1; 
    %-----------------------------------------------------------------------
    % Section 3: Begin outputting the frame for a TABLE to the matlab screen.
    fprintf('\n----------------------------------------- :: -----------------------\n');
    fprintf('_________Frequency_________   _SineAmp__  ::  __GAIN___      _PHASE_\n');
    fprintf('   Hertz        [rad/sec]                 ::     db          Degrees\n');
    fprintf('----------------------------------------- :: -----------------------\n');
    %-----------------------------------------------------------------------
    % Section 4: Run through all requested FREQUENCY values; find gain and phase at each.
    f1=figure(1); set(f1,'Position',[465,220,450,520]);
    f2=figure(2); clf; set(f2,'Position',[10,220,450,520]);
    patch([-.5 1.5 1.5 -.5 -.5],[-.5 -.5 1.5 1.5 -.5],[0 0 0])
    patch([0 0 1 1 0],[0 1 1 0 0],[0 1 0])
    axis([-.5 1.5 -.5 1.5])
    t1=text(.5,.65,'COLLECTING'); set(t1,'HorizontalAlignment','center');
    t1=text(.5,.5,'FIRST'); set(t1,'HorizontalAlignment','center');
    t1=text(.5,.35,'DATA SET...'); set(t1,'HorizontalAlignment','center');
    axis off
    u1=uicontrol(2,'Position',[10 10 150 30],...
        'String','HIT to BREAK',...
        'Callback','stopval=1;',...
        'Enable','on','Value',5);
    
    i=1;
    while (i<=length(w_use)) & (exitval==0)
        ds = 1; % reset for new point...
        w=w_use(i);      % frequency for this data set   
        A=amp_list(i);   % NOTE: amplitude must be 'reasonable'...
        % USER must visually check that OUTPUT is not saturated!!!
        fprintf('%9.4f     [%10.4f]    %9.6f   ::  ',w,2*pi*w,A);
        drawnow;
        %      mlib('WriteD',freq_addr,w); mlib('WriteD',amp_addr,A);
        mlib('Write', amp_freq, 'Data', {A;w});   % KAL 3/19/02
        tic                                 % give system some time to settle...
        while (toc<(10/w)) & (get(u1,'Value')>1)
            drawnow
        end  
        %---------------------------------------------------------------------
        % Section 4-1: Here is the actual procedure to get gain and phase at
        % a particular frequency.  This would be more elegantly implemented
        % as a separate function.  Instead, it is included within this loop
        % so that the dynamic signal analyzer can be run from a SINGLE FILE.
        % (Otherwise, the user has to worry about having all files needed.)
        %k=[0:(N-1)]'; 
        %      A=mlib('Readd',amp_addr); w=mlib('Readd',freq_addr); T=1/w;
        %      %mtrc31('SelectBoard','ds1102');
        %      y_addr=mtrc31('GetAddr','rti B[Model Root/Dynamic Signal Analyzer/channel1]',...
        %         'rti B[Model Root/Dynamic Signal Analyzer/channel2]',...
        %         'rti B[Model Root/Dynamic Signal Analyzer/Swept Sine]');
        %      mtrc31('TraceVars',y_addr);
        %      samp_per=mlib('GetSimAddr','Task Info/Timer Task 1/sampleTime');
        %      dt=mlib('Readd',samp_per);
        
        y_addr=mlib('GetTrcVar',...
            {'Model Root/Dynamic\nSignal Analyzer/channel1/Out1';...
                'Model Root/Dynamic\nSignal Analyzer/channel2/Out1';...
                'Model Root/Dynamic\nSignal Analyzer/Swept\nSine/Out1'}); % MB 8/13/04
        mlib('Set', 'TraceVars', y_addr); % MB 8/13/04
        samp_per=mlib('GetTrcVar',...
            {'Task Info/Timer Task 1/sampleTime'}); % MB 8/13/04
        dt=mlib('Read',samp_per)
        % KAL 3/13/02
        ncyc=floor(w*dt*N);
        % Use an INTEGRAL number of sine waves!!
        Nlast=ceil(ncyc/(w*dt));
        % want to subtract off takeofflast fraction of last data point...
        takeofflast=(Nlast-(ncyc/(w*dt)));
        if ncyc>10                   % Display no more than this number of sine waves
            'more than 10 cycles'
            Nplot=round(10/(w*dt));
            % for the user to observe during the run...
        elseif ncyc==0  % then there is not even one cycle
            ncyc=1;
            per_cycle=1/(w*dt);
            'Nplot=Nlast'
            ds=ceil(per_cycle/N); % downsampling needed to get one cycle
            Nlast=ceil(ncyc/(ds*w*dt));
            % want to subtract off takeofflast fraction of last data point...
            takeofflast=(Nlast-(ncyc/(ds*w*dt)));
            while Nlast>N
                ds=ds+1;
                Nlast=ceil(ncyc/(ds*w*dt));
                % want to subtract off takeofflast fraction of last data point...
                takeofflast=(Nlast-(ncyc/(ds*w*dt)));
            end
            Nplot=Nlast;
        else
            Nplot=Nlast;
        end
        tic
        while (toc<5) & (get(u1,'Value')>1) % settle time pause...
            drawnow
        end
        if (get(u1,'Value')>1)
            %         % !@!!! set frame in desired way!!!
            %         mtrc31('SetFrame',[],1,0,N);
            %         %mtrc31('SetFrame',dt,1,0,dt*(N));
            %         mtrc31('SetTrigger',y_addr(3,:),0,1);
            %         mtrc31('LockProgram');
            %         mtrc31('StartCapture');
            %         while mtrc31('CaptureState')~=0
            % BELOW: modification for new mlib() syntax:
            if ~exist('ds')
                ds=1;
            end
            
            %Nlast
                  
            mlib('Set', ...
                'Downsampling', ds, ...
                'Delay', 0, ...
                'NumSamples', Nlast*ds); % KAL 3/13/02 ??? is this right??
            mlib('Set','Trigger', 'On',...
                'TriggerVariable', y_addr(3),...
                'TraceVars', y_addr,...
                'TriggerLevel', 0,...
                'TriggerEdge', 'rising');   % KAL 3/13/02
            mlib('LockProgram');  % KAL 3/13/02
            mlib('StartCapture');  % KAL 3/13/02
            while mlib('CaptureState')~=0
                drawnow;
            end
            %          my_data=mtrc31('FetchData');
            my_data= mlib('FetchData');  % KAL 3/13/02
           
            % Take an INTEGRAL number of sine waves, total:
            k=[0:(Nlast-1)]';
            ncyc=floor(w*dt*Nlast);
            %            ncyc=floor(w*dt*N)
            %            % Use an INTEGRAL number of sine waves!!
            %            Nlast=ceil(ncyc/(w*dt))
            %            countlast=1-(Nlast-(ncyc/(w*dt)));
            
            y_out=my_data(2,1:Nlast);
            y_in=my_data(1,1:Nlast);
            t_out=dt*[1:Nlast];
            %         mtrc31('UnlockProgram');
            mlib('UnlockProgram');
        end         
        set(u1,'Enable','off');  % do not allow a break until new data presented
        if (get(u1,'Value')<1)
            i=max(1,(i-1));
            w=w_use(i);      % frequency for this data set   
            A=amp_list(i);   % NOTE: amplitude must be 'reasonable'...
            % USER must visually check that OUTPUT is not saturated!!!
            fprintf(1,'\nBREAK: going back to previous frequency.\n');
            fprintf(1,' * Use COCKPIT to reset Amplitude.\n');
            fprintf(1,' * USE TRACE to view resulting sine waves.\n');
            fprintf(1,'Restart DSA by hitting RESTART button in figure 2.\n');
            %fprintf('%9.4f     [%10.4f]    %9.6f   ::  ',w,2*pi*w,A);
            drawnow;
            %         mlib('Writed',freq_addr,w); mlib('Writed',amp_addr,A);
            mlib('Write', amp_freq, 'Data', {A;w});   % KAL 3/19/02
            startval=0; exitval=0;
            subplot(3,1,3); cla; 
            patch([0 30 30 0 0],[2 2 7 7 2],[0 1 0]);
            axis off; axis([0 30 2 7]); 
            text(.5,6,'You can use COCKPIT and TRACE to reset');
            my_str=num2str(w_use(i),'%.1f');
            text(.5,5,['Amplitude and then CONTINUE at ' my_str ' Hz...']);
            text(.5,4,'...or you can EXIT to completely rerun DSA.');
            text(.5,3,'[choose EXIT or CONTINUE]'); 
            set(u1,'String','HIT to EXIT','Callback','exitval=1;');
            u2=uicontrol('Position',[200 10 150 30],'Value',5,...
                'String','CONTINUE','Callback','startval=1;');
            set(u1,'Enable','on','Value',5);
            while (get(u1,'Value')~=0) & (get(u2,'Value')~=0)
                drawnow;  % wait for user to hit a button...
            end
            if get(u1,'Value')==0
                exitval=1;     % exit the dsa
            else
                startval=1;    % restart the dsa
                %            A=mlib('Readd',amp_addr);
                A=mlib('Read',amp_freq(1));
                amp_list=A+(0*amp_list);  % reset amp_list for current and future freqs
                figure(2); clf; 
                patch([-.5 1.5 1.5 -.5 -.5],[-.5 -.5 1.5 1.5 -.5],[0 0 0])
                patch([0 0 1 1 0],[0 1 1 0 0],[0 1 0])
                axis([-.5 1.5 -.5 1.5])
                t1=text(.5,.65,'COLLECTING'); set(t1,'HorizontalAlignment','center');
                t1=text(.5,.5,[my_str ' Hz']); set(t1,'HorizontalAlignment','center');
                t1=text(.5,.35,'DATA SET...'); set(t1,'HorizontalAlignment','center');
                axis off
                u1=uicontrol(2,'Position',[10 10 150 30],...
                    'String','HIT to BREAK',...
                    'Callback','stopval=1;',...
                    'Enable','on','Value',5);
                
                
            end
        else
            % user did NOT request a break, so analyze this data set:
            if exist('y_out')
                if (max(y_out)<0.95) & (min(y_out)>-.95) & (max(y_in)<.95) & (min(y_out)>-.95)
                    warncolor=[1 1 0];                       % Amplitude not 'saturated'. OK to use this data.
                else
                    warncolor=[1 0 0];
                end
                %         y_sin=(dt*(w*2*pi)*(k))';
                y_sin=((w*2*pi)*t_out);       % Ideal SINE at this freq
                nmax=length(y_sin); tk=takeofflast;  % subtract off a tiny bit...
                Bc=(2/(length(y_out)))*(sum((y_out).*cos(y_sin))-tk*y_out(nmax)*cos(y_sin(nmax)));
                Bs=(2/(length(y_out)))*(sum((y_out).*sin(y_sin))-tk*y_out(nmax)*sin(y_sin(nmax)));
                %Bc=(2/(length(y_out)))*sum((y_out).*cos(y_sin)); %-tk*y_out(nmax)*cos(y_sin(nmax)));
                %Bs=(2/(length(y_out)))*sum((y_out).*sin(y_sin)); %-tk*y_out(nmax)*sin(y_sin(nmax)));
                B_=sqrt(Bc^2+Bs^2);           % Amplitude of OUTPUT at this freq
                Ac=(2/(length(y_out)))*(sum((y_in).*cos(y_sin))-tk*y_in(nmax)*cos(y_sin(nmax)));
                As=(2/(length(y_out)))*(sum((y_in).*sin(y_sin))-tk*y_in(nmax)*sin(y_sin(nmax)));
                %Ac=(2/(length(y_out)))*sum((y_in).*cos(y_sin)); %-tk*y_in(nmax)*cos(y_sin(nmax)));
                %As=(2/(length(y_out)))*sum((y_in).*sin(y_sin)); %-tk*y_in(nmax)*sin(y_sin(nmax)));
                A_=sqrt(Ac^2+As^2);           % Amplitude of INPUT at this freq (check)
                Gain=B_/A_;
                phi_out=atan2(Bc,Bs);
                phi_in=atan2(Ac,As);   
                phi=phi_out-phi_in;      % Difference in phase (input->output) (rad)
                figure(2); clf
                subplot(311)
                plot(t_out(1:Nplot),y_out(1:Nplot),'r.');
                title_str=['RED: channel2 [' num2str(ncyc) ' cycles]'];
                hold on; grid on; title(title_str); %xlabel('seconds')
                subplot(312)
                plot(t_out(1:Nplot),y_in(1:Nplot),'b.');   
                hold on; grid on; title('BLUE: channel1 (input)'); xlabel('seconds')
                subplot(313)
                patch([0 30 30 0 0],[2 2 7 7 2],warncolor);
                axis off; axis([0 30 2 7]); 
                text(.5,6,'Check that the OUTPUTS above are :');
                text(1.5,5,'* NOT SATURATED.');
                text(1.5,4,'* Not buried in noise.');
                text(.5,3,'otherwise, BREAK and run with new AMPLITUDEs.'); 
                u1=uicontrol(2,'Position',[10 10 150 30],...
                    'String','HIT to BREAK',...
                    'Callback','stopval=1;',...
                    'Enable','on', 'Value',5);
                stopval=0;   % insure 'stopval' is reset to show 'OK' state
                %else
                %   A=A*3/4;   % REDUCE INPUT AMPLITUDE AND RETAKE DATA!
                %   fprintf('Reducing input amplitude from %5.3f volts to %5.3f volts...\n',(4/3)*A,A);
                %   mlib('SelectBoard','ds1102');
                %   mlib('WriteF',amp_addr,A);
                %end   
                %end
            end
            if phi>phi_last
                while (phi-phi_last)>(pi)
                    phi=phi-(2*pi);
                end
            else
                while (phi-phi_last)<(-pi)
                    phi=phi+(2*pi);
                end
            end
            
            mytf(i,:)=[w Gain phi];
            fprintf(1,'%8.3f     %9.2f\n',20*log10(Gain),(180/pi)*phi);
            figure(1)
            subplot(2,1,1); semilogx(mytf(1:i,1),20*log10(mytf(1:i,2)),sval); hold on;
            %semilogx(mytf(1:i,1),20*log10(mytf(1:i,2)),'-'); 
            axis auto; grid on;
            xlabel('Frequency (Hertz)'); 
            ylabel('Gain (db)'); title('Transfer Function (channel2/channel1)');
            subplot(2,1,2); semilogx(mytf(1:i,1),(180/pi)*mytf(1:i,3),sval);hold on;   
            %semilogx(mytf(1:i,1),(180/pi)*mytf(1:i,3),'-'); 
            axis auto; grid on;
            if do_date==1
                now_val=datevec(now);
                xtemp=num2str(now_val(5));
                if length(xtemp)<2
                    xtemp=['0' xtemp];
                end
                sout=[num2str(now_val(2)) '/' num2str(now_val(3)) '/' num2str(now_val(1))];
                sout=[sout '  ' num2str(now_val(4)) ':' xtemp];
                s_xlabel=sout;
            else
                s_xlabel=' ';
            end
            s_xlabel2='                             ';
            s_xlabel2=s_xlabel2(1:length(s_xlabel));
            xlabel([s_xlabel2 '  Frequency (Hertz)  ' s_xlabel]); 
            ylabel('Phase (Degrees)');
            %if ((180/pi)*(max(mytf(1:i,3))-min(mytf(1:i,3))))>80
            %   set(gca,'YTick',[45*floor((180/pi)*min(mytf(1:i,3))):45:45*ceil((180/pi)*max(mytf(1:i,3)))]);   
            %elseif (((180/pi)*(max(mytf(1:i,3))-min(mytf(1:i,3))))>40) & (get(gca,'YTickMode')=='manual')
            %   set(gca,'YTick',[15*floor((180/pi)*min(mytf(1:i,3))):15:15*ceil((180/pi)*max(mytf(1:i,3)))]);   
            %end
            drawnow
            phi_last=phi;
            figure(2)  % PUT FIGURE 2 ON TOP TO FORCE USER TO LOOK AT SINE WAVE DATA
            i=i+1;   
        end
        %if startval==0
        %   i=max(1,(i-1));
        %end
        
    end  
    if exitval==0
        if w_rad==1
            fprintf(1,'\nThe TF created has 3 columns:  Frequency (rad/sec), Magnitude (absolute), Phase (radians)\n')
            mytf(:,1)=(2*pi)*mytf(:,1);
        else
            fprintf(1,'\nThe TF created has 3 columns:  Frequency (Hz), Magnitude (absolute), Phase (radians)\n')
        end
        fprintf(1,'To replot GAIN in Bode format:  semilogx(tf(:,1),20*log10(tf(:,2))\n');
        fprintf(1,'To replot PHASE in Bode format: semilogx(tf(:,1),(180/pi)*tf(:,3)\n');   
    else
        fprintf(1,'\n\n   **************************************************\n');
        fprintf(1,    '   *** Program exited by user during run.... bye. ***\n');
        fprintf(1,    '   **************************************************\n\n');
    end  % ends user query if 'ok' to continue
    %   mlib('Writed',freq_addr,w_use(1));
    %   mlib('Writed',amp_addr,0);   
    mlib('Write', amp_freq, 'Data', {0;w_use(1)});   % KAL 3/19/02   
end
%return
%function y = setstop()
%y=1;
%return