function [h1]=PlotCam(T,world_ref,tam,varargin)
% 
%   [h1]=PlotCam(T,world_ref,tam,varargin)
%     Plot the camera with the orientation/translation given by T  
%     If the world_reference of the T is not reference to Matlab (0,0,0)
%  
%   Input T, worlf_ref, tam, and othe option like string and color
%   
%   PlotCam(eye(4),eye(4),10,'color',[1 0 0],'string','something','color',[ r g b])
%
%   
%   Return all the handlers which can be deleted
%

i=1;
color=[52 217 160]*norm([52 217 160])^-1;str='';
while i<length(varargin)
  if isequal(varargin{i},'string')
    i=i+1;
    str=varargin{i};
  end
  if isequal(varargin{i},'color')
    i=i+1;
    color=varargin{i};
    if isnumeric(color)
      color=color*norm(color)^-1;
    end
  end
   i=i+1; 
end

if ~exist('world_ref')
    world_ref=eye(4);
else
    if isempty(world_ref)
        world_ref=eye(4);
    else
        [l c]=size(world_ref);
        if (l==c) && (l==1)
            tam=world_ref;
            world_ref=eye(4);
        end
    end
end

if ~exist('tam')
    tam=150;
end

BASE = [ 0   tam     0     0     0     0
          0     0     0   tam     0     0
          0     0     0     0     0   tam ];
scale=1/3;
IP = [ -tam*scale  tam*scale  tam*scale  -tam*scale  -tam*scale
        -tam*scale  -tam*scale   tam*scale   tam*scale  -tam*scale
        tam  tam  tam  tam  tam ];      


d=tam/10;
tt=[-d -d -d 1]';

% referencial da camara
BASE= world_ref*T*[ BASE ; ones(1,length(BASE)) ];
BASEk=BASE(1:3,:);

% imagem do ccd
IP= world_ref*T*[ IP ; ones(1,length(IP))];
IP=IP(1:3,:);
IPk = reshape([IP;BASEk(:,1)*ones(1,5);IP],3,15);


hold on
p1 = struct('vertices',IPk','faces',[1 4 2;2 4 7;2 7 10;2 10 1]);
h1(1) = patch(p1,'facecolor',color,'EdgeColor', 'r');

p2 = struct('vertices',IPk','faces',[1 10 7;7 4 1]);
h1(7) = patch(p2,'facecolor',[236 171 76]/255,'EdgeColor', 'none');

% referencial com uma cor
% plot3(BASEk(1,:),BASEk(2,:),BASEk(3,:),'b-','linewidth',2);
%referencial rgb-> xyz
h1(2)=plot3(BASEk(1,1:2),BASEk(2,1:2),BASEk(3,1:2),'r','linewidth',2);
h1(3)=plot3(BASEk(1,3:4),BASEk(2,3:4),BASEk(3,3:4),'g','linewidth',2);
h1(4)=plot3(BASEk(1,5:6),BASEk(2,5:6),BASEk(3,5:6),'b','linewidth',2);

h1(5)=plot3(IPk(1,:),IPk(2,:),IPk(3,:),'r-','linewidth',1);

h1(6)=text(BASEk(1,2),BASEk(2,2),BASEk(3,2),'x','fontsize',8);
tt=world_ref*T*tt;
h1(8)=text(tt(1),tt(2),tt(3),str,'fontsize',10);
hold off

