function [bc bb] = buoyCntrl( compensatorType, varargin )
%
% PURPOSE: Build up quick buoyancy control loop.
%
% INPUTS: 
%         
%
% OUTPUTS: 
%          
%
% UNITS:   All are in SI units.
%
% CHANGES: 9 June 2010.  Rob McEwen, initial.
%
  if( exist( 'compensatorType' ) == 0 )
    compensatorType = 'PID'
  end

  global LoopFig1 LoopFig2;

  if( isempty(varargin) | varargin{1} == 0 ) 
    displayPlots = 0;
  else
    displayPlots = varargin{1};
  end

  units;
  len = (7*12 + 6)*in2m;         %Vehicle length including propeller frustum.
%
% Mass properties:
%
% mp  = masspropLR(len, rho);
% M   = mp.m;                    %kg, vehicle mass including battery.
  M   = 133;                     %kg, vehicle mass including battery.
  kp  = 30;                      %cc/m
% ki  = .5;                      %cc/m/sec.
  ki  = 0;                       %cc/m/sec.
  kd  = 200;                     %cc/(m/sec).
  deltaB_M = 400*1e-6*rho*g;     %N, maximum buoyancy change with the
                                 %bladder full or (nearly) empty.
% deltaB_M = 0*rho*g;     
  db       = 10;                 %cc, Deadband in buoyancy driver.     
  depthdb  = 10;                 %m, Deadband in Vertical Control.     
  depthStep= 20;                 %m  Step input magnitude
  depthdb  = 0;                  %m, Deadband in Vertical Control.     
  depthStep= 5;                  %m  Step input magnitude
%
% Pump rate 1.75 was observed in 20101027T172241 data.  Increasing it doesn't
% significantly change the simulated limit cycle.
  vdotmax  = 1.75;               %cc/sec, maximum pump rate.
  hyst     = 3;                  %cc
  hyst     = 0;                  %cc
  leakrate = 0;                  %cc/sec, leak rate.bc
  Ts       = .8;                 %Sampling rate
% omegan   = .1;                 %rad/sec, actuator low-pass dynamics
  omegan   = 100;                %rad/sec, actuator low-pass dynamics
  zeta     = 1/sqrt(2);          %n/a, actuator low-pass dynamics
  V0       = 100;                %cc, entrained air volume
  V0       = 0;                  %cc, entrained air volume
  zA       = 10.08;              %m, one-atmosphere depth; 1.013e5/rho/g
%
% Hydrodynamic properties:
%
% setupLR;
% stabDerKnut;
  Zwabsw = -300;
  Zwabsw = -665;
  Zwdot  = -140;
%
% Switch for various tests:
%
%  1 - open-loop sinusoid
%  2 - open-loop actuator sine sweep.
%  3 - closed loop.
  olSw     = 3;
%
%  1 - Actuator included (buoyancy pump, hysteresis, etc.)
%  2 - Perfect actuator.  
  noAct    = 1;
%  
  if( omegan >= 50. )
    actnum = 1;
    actden = 1;
  else
    actnum     = omegan^2;
    actden     = [1 2*zeta*omegan omegan^2];
  end    
%
% 143 seconds is the observed period of the limit cycle in the tank,
% kp=60.  Set the (linear) viscous drag term to create the maximum force
% that occurs when the vehicle is oscillating with an amplitude of 1 m and
% period of 143 seconds.
% deltaB_M=-(2*pi/143)^2*Zwabsw;
%
% The plant model is:
% 
%     (M - Z_wdot) 2dot{z} 
%  +   2 sqrt(-Zwabsw deltaB_M) dot{z} 
%  =   deltaB
%
%  =   deltaV rho g.
%
% where deltaB is the control in Newtons, and deltaB_M is the maximum.  M is total mass.  
%
  if( deltaB_M == 0 )
    G    = rho*g/( M - Zwdot );
    tau  = 0;
    denG = [1 0 0];
  else    
%
% Putting this into the form G/( s(  tau s + 1 )  ) = Z/deltaV gives
%
% G   = ( rho g )/( 2 sqrt( -Zwabsw deltaB_M ) )
%
% tau = G ( M - Z_wdot ) 
%
    if( Zwabsw ~= 0 )
      G    = rho*g/(2*sqrt(-Zwabsw*deltaB_M));
    else
      G    = rho*g/(2*sqrt(deltaB_M));         %Set Zwabsw = -1.
    end
    tau  = G*( M - Zwdot )/rho/g;
    denG = [tau 1 0];
  end
%
% The 1e-6 puts deltaV in CC.
%
  Gcc = 10e-6*G;
  numG = Gcc;
  G1   = rho*g/1e6;                  %Convert from vol to force (cc to Newtons)
  G2   = 1/( M - Zwdot );            %Convert from force to acceleration
%
% Now form the compensator transfer function
%
  if( strcmpi( compensatorType, 'PID' ) )
    %
    % The (non-causal) PID compensator t.f. is
    %
    % K(s) = (kd s^2 + kp s + ki) / s
    %
    numK = [ kd kp ki ];
    denK = [1 0];
  elseif( strcmpi( compensatorType, 'PD' ))  
    numK  = [kd kp];
    denK  = 1;
  elseif( strcmpi( compensatorType, 'PI' ))  
    numK  = [kp ki];
    denK  = [1 0];
  else
    numK = kp;
    denK = 1;
    if( ~strcmpi( compensatorType, 'P' ))  
      fprintf('Unrecognized compensator type ''%s''.\n',compensatorType);
      fprintf('Using constant kp = %.2f m/rad instead.\n',kp);
      compensatorType = 'P';
    end
  end  
%
% Compute the loop tf assuming the actuator (buoyancy pump) has unity
% transfer function:
  Gs = tf( numG, denG);
  Ks = tf( numK, denK);
  Ls = series(Ks, Gs);


  position = get(0,'screensize');
  npposition(1:2) = position(1:2) + round([position(3)*.02 position(4)*.04]);
  npposition(3:4) = round([position(3)*.95 position(4)*.90]);
  position = npposition;

  if( exist('fontsize') ~= 1 ) fontsize = 14; end
  if( exist('fontweight') ~= 1 ) fontweight = 'normal'; end

  if(displayPlots)
    %
    % First figure.  
    %
    % if exist( 'LoopFig1' ) ~= 1
    if( isempty(LoopFig1) )
      LoopFig1 = figure;
      figure(LoopFig1);
      %      winpos( position );
    else 
      figure( LoopFig1 ); 
    end
    set(gcf,'position',position)


    set( LoopFig1, 'name', ...
		   'Open Loop',...
		   'DeleteFcn', 'clear LoopFig1' );
    clf
    
    nichols(Ls)
    p1hand = gca;
    grid on;
  end  

  fprintf('The open loop transfer function is:\n')
  Gs
  fprintf('The closed loop transfer function is:\n')
  Gcl= feedback( Ls, 1 )
  fprintf('It has closed loop roots with time constants:\n')
  [roots(Gcl.den{:})  -1./real(roots(Gcl.den{:}))]
  if(  sum(imag(roots(Gcl.den{:}))  ~=  0)  ) 
    damp(Gcl) 
    fprintf('The period is:\n')
    2*pi./imag(roots(Gcl.den{:}))
  end
  
%
% Now Compute loop transfer function from Steve's diagram that includes the
% actuator EZ servo feedback
%
% L(s) = ( 1 + G*K )/s
%  
%      = ( 1 + N/D )/s
%  
%      = ( D + N)/(D s)
%
  N = conv(numK, numG);
  D = conv(denK, denG);
  
  numL = polyadd(D,N);
% numL = N;                %Try removing the inner motor controller loop
  denL = [D 0];
  
  L = tf(numL, denL);
  if( displayPlots)
    %
    % Figure.  
    %
    % if exist( 'LoopFig2' ) ~= 1
    if( isempty(LoopFig2) )
      LoopFig2 = figure;
      figure(LoopFig2);
      %      winpos( position );
    else 
      figure( LoopFig2 ); 
    end
    set(gcf,'position',position)


    set( LoopFig2, 'name', ...
		   'Open Loop',...
		   'DeleteFcn', 'clear LoopFig2' );
    clf
    
    nichols(L)
    grid on
  end

  bc.Zwabsw     = Zwabsw;
  bc.kp         = kp;
  bc.kd         = kd;
  bc.G1         = G1;
  bc.G2         = G2;
  bc.denG       = denG;
  bc.kp_vloop   = 1;
  bc.vdotmax    = vdotmax;  
  bc.leakrate   = leakrate;
  bc.db         = db;
  bc.depthdb    = depthdb;
  bc.depthStep  = depthStep;
  bc.Ts         = Ts;
  bc.actnum     = actnum;
  bc.actden     = actden;
  bc.olSw       = olSw;
  bc.noAct      = noAct;
  bc.hyst       = hyst;
  bc.zA         = zA;
  bc.rhogV0     = rho*g*V0/1e6;

  
  bb.compensatorType = compensatorType;
  bb.tau        = tau;
  bb.Gs         = Gs;
  bb.Ks         = Ks;
  bb.Ls         = Ls;
  bb.Gcl        = Gcl;
  bb.L          = L;
  bb.deltaB_M   = deltaB_M;

%  simout = sim('buoyancy',bc)
  
  
  
