camera(t,x,u,flag,P)

function [sys,x0,str,ts] = camera(t,x,u,flag,P)
%
% modified 11/30/2006 - Randy Beard
% modified 05/04/2007 - Randy Beard

switch flag,

  %%%%%%%%%%%%%%%%%%
  % Initialization %
  %%%%%%%%%%%%%%%%%%
  case 0,
    [sys,x0,str,ts] = mdlInitializeSizes(P);

  %%%%%%%%%%
  % Update %
  %%%%%%%%%%
  case 2,                                                
    sys = mdlUpdate(t,x,u,P); 

  %%%%%%%%%%
  % Output %
  %%%%%%%%%%
  case 3,                                                
    sys = mdlOutput(t,x,u,P);

  %%%%%%%%%%%%%
  % Terminate %
  %%%%%%%%%%%%%
  case 9,                                                
    sys = []; % do nothing

  %%%%%%%%%%%%%%%%%%%%
  % Unexpected flags %
  %%%%%%%%%%%%%%%%%%%%
  otherwise
    error(['unhandled flag = ',num2str(flag)]);
end

%end dsfunc

%
%=======================================================================
% mdlInitializeSizes
% Return the sizes, initial conditions, and sample times for the S-function.
%=======================================================================
%
function [sys,x0,str,ts] = mdlInitializeSizes(P)

sizes = simsizes;
sizes.NumContStates  = 0;
M = size(P.drogue.markers,1);  % number of identifying dots
sizes.NumDiscStates  = 1+...     % initialize
                       M+... % camera display handle
                       3*M;    % camera data (eps_x, eps_y, eps_s)
sizes.NumOutputs     = 3*M;    % camera data (eps_x, eps_y, eps_s)
sizes.NumInputs      = 9+... % drogue states
                       9;    % mav states
sizes.DirFeedthrough = 1;
x0 = zeros(sizes.NumDiscStates,1);
sizes.NumSampleTimes = 1;


sys = simsizes(sizes);

str = [];
ts  = [P.mav.T_cam 0]; 

% end mdlInitializeSizes
%
%=======================================================================
% mdlUpdate
% Handle discrete state updates, sample time hits, and major time step
% requirements.
%==============================2=========================================
%
function sys = mdlUpdate(t,x,uu,P)
  M = size(P.drogue.markers,1);     % number of markers on drogue
  initialize        = x(1);         % initial graphics
  NN = 1;
  fig_cam           = x(1+NN:M+NN);     % target in camera frame
  
  NN = 0;
  drogue.n     = uu(1+NN);
  drogue.e     = uu(2+NN);
  drogue.d     = uu(3+NN);
  drogue.ndot  = uu(4+NN);
  drogue.edot  = uu(5+NN);
  drogue.ddot  = uu(6+NN);
  drogue.phi   = uu(7+NN);
  drogue.theta = uu(8+NN);
  drogue.psi   = uu(9+NN);
  NN = NN+9;
  mav.n    = uu(1+NN);
  mav.e    = uu(2+NN);
  mav.d    = uu(3+NN);
  mav.psi  = uu(4+NN);
  mav.V    = uu(5+NN);
  mav.phi  = uu(6+NN);
  mav.gam  = uu(7+NN);
  mav.az   = uu(8+NN);
  mav.el   = uu(9+NN);

  % get pixel location for each drogue marker
  for i=1:M,
    tmp = Rot_v_to_b(drogue.phi, drogue.theta, drogue.psi)'*P.drogue.markers(i,:)';
    dot.n = drogue.n + tmp(1);
    dot.e = drogue.e + tmp(2);
    dot.d = drogue.d + tmp(3);  
    [eps_x(i), eps_y(i), eps_size(i)] = cameraProjection(mav, dot, P);
  end

  % plot target in the camera
  if initialize==0, 
    % initialize the plot
    initialize = 1;
    
    figure(2), clf, hold on
    
    % plot camera view 
    h=subplot('position',[0.1, 0.1, 0.8, 0.8]);
    tmp = P.cam.pix/2;
    set(h,'XAxisLocation','top','XLim',[-tmp,tmp],'YLim',[-tmp,tmp]);
    axis ij
    hold on
    for i=1:M,
        fig_cam(i) = blobPlot([eps_x(i), eps_y(i)], eps_size(i), [], 'red', 'normal');
    end
    xlabel('px (pixels)')
    ylabel('py (pixels)')
    title('Camera View')

  else  % do this at every time step
    for i=1:M,
        blobPlot([eps_x(i), eps_y(i)], eps_size(i), fig_cam(i));
    end
  end
  
sys = [initialize; fig_cam; eps_x'; eps_y'; eps_size'];


%end mdlUpdate

%=======================================================================
% mdlOutput
%==============================2=========================================
%
function sys = mdlOutput(t,x,uu,P)
  M = size(P.drogue.markers,1);
 
  sys = x(1+M+1:end);
  

%=======================================================================
% cameraProjection
% project the target onto the camera
%==============================2=========================================
function [eps_x, eps_y, eps_size] = cameraProjection(mav, target, P)

  % target in the vehicle frame
  p_obj_v = [target.n; target.e; target.d] - [mav.n; mav.e; mav.d];
  
  % tranform to the camera frame
  R_v_b = Rot_v_to_b(mav.phi,mav.gam,mav.psi);  % vehicle to body
  R_b_g = Rot_b_to_g(mav.az,mav.el);            % body to gimbal
  R_g_c = [...                                  % gimbal to camera
      0, 1, 0;...
      0, 0, 1;...
      1, 0, 0];
  p_obj_c = (R_g_c * R_b_g * R_v_b) * p_obj_v;
    
  % convert to pixel location
  if p_obj_c(3)<.1,
      eps_x=-9999;
      eps_y=-9999;
      eps_size = 0;
  else
    eps_x =  P.cam.f*(p_obj_c(1)/(p_obj_c(3)))+P.cam.pixelnoise*randn;
    eps_y =  P.cam.f*(p_obj_c(2)/(p_obj_c(3)))+P.cam.pixelnoise*randn;
    eps_size = P.cam.f*(P.drogue.marker_size/(p_obj_c(3)))+P.cam.pixelnoise*randn;
  end
  
  % snap output of camera to -9999 if outside field of view
  tmp = P.cam.pix/2+eps_size;
  if eps_x<-tmp | eps_x>tmp | eps_y<-tmp | eps_y>tmp,
      eps_x = -9999;
      eps_y = -9999;
      eps_size = 0;
  end
  
%=======================================================================
% blobPlot
%  draw circle 
%==============================2=========================================
function handle=blobPlot(z, size, handle, color, mode);

  th = 0:.1:2*pi;
  X = z(1)+ size*cos(th);
  Y = z(2)+ size*sin(th);

  if isempty(handle),
    handle = fill(X, Y, color, 'EraseMode', mode);
  else
    set(handle,'XData',X,'YData',Y);
    drawnow
  end


  
%%%%%%%%%%%%%%%%%%%%%%%
function R = Rot_v_to_b(phi,theta,psi);
% Rotation matrix from body coordinates to vehicle coordinates

Rot_v_to_v1 = [...
    cos(psi), sin(psi), 0;...
    -sin(psi), cos(psi), 0;...
    0, 0, 1;...
    ];
    
Rot_v1_to_v2 = [...
    cos(theta), 0, -sin(theta);...
    0, 1, 0;...
    sin(theta), 0, cos(theta);...
    ];
    
Rot_v2_to_b = [...
    1, 0, 0;...
    0, cos(phi), sin(phi);...
    0, -sin(phi), cos(phi);...
    ];
    
R = Rot_v2_to_b * Rot_v1_to_v2 * Rot_v_to_v1;

%%%%%%%%%%%%%%%%%%%%%%%
function R = Rot_b_to_g(az,el);
% Rotation matrix from body coordinates to gimbal coordinates
Rot_b_to_g1 = [...
    cos(az), sin(az), 0;...
    -sin(az), cos(az), 0;...
    0, 0, 1;...
    ];

Rot_g1_to_g = [...
    cos(el), 0, -sin(el);...
    0, 1, 0;...
    sin(el), 0, cos(el);...
    ];

R = Rot_g1_to_g * Rot_b_to_g1;