function []=PlotRef(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
%   
%   PlotRef(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=['rgb']';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;
    aux=varargin{i};
    if isnumeric(color)
      aux=aux*norm(aux)^-1;
    end
    color=[aux;aux;aux];
  end
   i=i+1; 
end

[l c]=size(T);

if l~=c
    error('T is not a Transform (rotation) matrix')
    return;
else
    if l==3
        T(1:3,4)=[0 0 0]';
        T(4,:)=[ 0 0 0 1];
    end
end

if ~exist('world_ref') 
    world_ref=eye(4);
elseif isempty(world_ref)
    world_ref=eye(4);
end

if ~exist('tam')
    tam=10;
    dX=floor(tam/3);
else
    dX=floor(tam/3);
end

BASE = [ 0   1     0     0     0     0
          0     0     0   1     0     0
          0     0     0     0     0   1 ]*tam;
      

d=dX;
tt=[-d -d -d 1]';
d=dX/3;
t1=[tam+d 0 0 1]';
t2=[0 tam+d 0 1]';
t3=[0 0 tam+d 1]';

local_base=world_ref*T*[BASE ; ones(1,length(BASE)) ];
t1=world_ref*T*t1;
t2=world_ref*T*t2;
t3=world_ref*T*t3;

hold on
h(1)=plot3(local_base(1,1:2),local_base(2,1:2),local_base(3,1:2),color(1,:),'linewidth',2);
h(2)=plot3(local_base(1,3:4),local_base(2,3:4),local_base(3,3:4),color(2,:),'linewidth',2);
h(3)=plot3(local_base(1,5:6),local_base(2,5:6),local_base(3,5:6),color(3,:),'linewidth',2);
hold off
h(4)=text(t1(1),t1(2),t1(3),'x','fontsize',8);
h(5)=text(t2(1),t2(2),t2(3),'y','fontsize',8);
h(6)=text(t3(1),t3(2),t3(3),'z','fontsize',8);
tt=world_ref*T*tt;

h(7)=text(tt(1)-0.1*tam,tt(2)-0.1*tam,tt(3)-0.1*tam,str,'fontsize',8);






