%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%                                                            %%%
%%%   Programa de Tratamento de dados Sensor SHARP com FANUC   %%%
%%%                                                            %%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% Limpar dados existentes
clc;
clear all;
close all;

% Iniciar ficheiro

file = importdata('/home/carpinteiro/workingcopies/lar4/src/sensors/optoelectric/data/ULTIMATE_CALIBRATION/calibre_2.txt');


% Passar tensão
 file(:,2)=file(:,2)*5/1023;
%file(:,3)=file(:,3)+708;



% Variáveis vazias

Pontos=[];
Desv=[];
PntMeDesv=[];
PntMaDesv=[];
Media=[];
Moda=[];
Mediana=[];
MediaGeo=[];
Hist=[];
Dist=[]; % valores de x e y trocados para obter distância em função da tensão!!!

% Calcular Estatisticas

for n=1:size(file,1)/500-20;
    
   Pontos=[Pontos ; (10+(n-1))*ones(500,1) , file(1+(500*(n-1)):500+(500*(n-1)),2)]; 
   Media=[Media mean(file(1+(500*(n-1)):500+(500*(n-1)),2))];
   Mediana=[Mediana median(file(1+(500*(n-1)):500+(500*(n-1)),2))];
   MediaGeo=[MediaGeo geomean(file(1+(500*(n-1)):500+(500*(n-1)),2))];
   Moda=[Moda mode(file(1+(500*(n-1)):500+(500*(n-1)),2))];
   Desv=[Desv (sqrt(sum((file(1+(500*(n-1)):500+(500*(n-1)),2)).^2)/500 - mean(file(1+(500*(n-1)):500+(500*(n-1)),2)).^2))];
   PntMeDesv=[PntMeDesv Media(n)-Desv(n)];
   PntMaDesv=[PntMaDesv Media(n)+Desv(n)];
   Hist=[Hist;file(1+(500*(n-1)):500+(500*(n-1)),2)'];
   Dist=[Dist 100+(n-1)*10];

end

% Valores obtidos

figure('position',[100 50 600 600],'color',[1 1 1])
plot(Pontos(:,1)*10,Pontos(:,2),'.r');
hold on
plot(Dist,Media,'.b','LineWidth',2)
errorbar(Dist,Media,PntMaDesv-Media,'k');
grid on
%axis ([9.5,80.5,0,4])
xlabel('Distancia (mm)');
ylabel('Voltagem (V)');
title('Valores obtidos de 10 em 10 mm com o robo Fanuc');
export_fig Pontos.pdf -pdf

% Desenhar Exponencial 2    
    figure('position',[100 50 600 600],'color',[1 1 1])
    hold on
    fitType = 'exp2';
    [myFit,G] = fit(Media',Dist',fitType);
    yfit=myFit.a*exp(myFit.b*Media)+myFit.c*exp(myFit.d*Media); %# evaluate the curve fit over x
    plot(Media,Dist,'*m')
    plot(Media,yfit,'-b')
    
%     xx=linspace(0.3,2.9,16000);
%     yy=myFit.a*exp(myFit.b*xx)+myFit.c*exp(myFit.d*xx);
% 
%     plot(xx,yy);
    errorbar(Media,Dist,PntMaDesv-Media,'k');
    grid on
    %axis square
    ylabel('Distancia (mm)');
    xlabel('Voltagem (V)');
    rr=G.rsquare;
    rm=(G.rmse^2); 
    str1 = sprintf('R quadrado : %d \n EQM : %d',rr,rm);
    text(3.1,550,str1)
    title('Exponencial 2 para medidas de 10 em 10 mm com o robo Fanuc');
    export_fig('Exp2.pdf','-pdf')


%% Histogramas

for n=1:size(file,1)/500;
    
    figure('position',[100 50 600 600],'color',[1 1 1])
    hist(Hist(n,:),25)
    hold on
    plot([Media(n) Media(n)],[0 500],'-r')
    plot([Mediana(n) Mediana(n)],[0 500],'-k')
    plot([Moda(n) Moda(n)],[0 500],'-b')
    plot([MediaGeo(n) MediaGeo(n)],[0 500],'-g')
    grid on
    %axis square
    ylabel('Numero de medidas');
    xlabel('Voltagem (V)');
    title(sprintf('Histograma para %d cm leituras de 10 em 10 mm com FANUC',n+9));
    export_fig(sprintf('Hist%d.pdf',9+n),'-pdf')
end

%% Desenhar polinómio de aproximação de ordem 2 até 6
 for n=2:1:6;
    
    figure('position',[100 50 600 600],'color',[1 1 1])
    hold on
    
    fitType = sprintf('poly%d',n);
    [myFit,G] = fit(Media',Dist',fitType);
    if n==2
            Pol=[myFit.p1 myFit.p2 myFit.p3];
        elseif n==3
            Pol=[myFit.p1 myFit.p2 myFit.p3 myFit.p4];
        elseif n==4
            Pol=[myFit.p1 myFit.p2 myFit.p3 myFit.p4 myFit.p5];
        elseif n==5
            Pol=[myFit.p1 myFit.p2 myFit.p3 myFit.p4 myFit.p5 myFit.p6];    
        elseif n==6
            Pol=[myFit.p1 myFit.p2 myFit.p3 myFit.p4 myFit.p5 myFit.p6 myFit.p7];
    end
                
    
    yfit=polyval(Pol,Media); %# evaluate the curve fit over x
    plot(Media,Dist,'*m')
    plot(Media,yfit,'-b')

    grid on
    %axis square
    ylabel('Distancia (mm)');
    xlabel('Voltagem (V)');
    rr=G.rsquare;
    rm=(G.rmse^2); 
    str1 = sprintf('R quadrado : %d \n EQM : %d',rr,rm);
    text(3.1,550,str1)
    errorbar(Media,Dist,PntMaDesv-Media,'k');
    title(sprintf('Polinomio ordem %d para medidas de 10 em 10 mm com o robo Fanuc',n));


%     xx=linspace(0.3,2.9,16000);
%     yy=polyval(Pol,xx); 
% 
%     plot(xx,yy);
    
    export_fig(sprintf('Pol%d.pdf',n),'-pdf')

 end
%% Desenhar exponenciais
% Desenhar Exponencial 1
    figure('position',[100 50 600 600],'color',[1 1 1])
    hold on
    fitType = 'exp1';
    [myFit,G] = fit(Media',Dist',fitType);
    yfit=myFit.a*exp(myFit.b*Media); %# evaluate the curve fit over x
    plot(Media,Dist,'*m')
    plot(Media,yfit,'-b')
    
%     xx=linspace(0.3,2.9,16000);
%     yy=myFit.a*exp(myFit.b*xx);
% 
%     plot(xx,yy);
    errorbar(Media,Dist,PntMaDesv-Media,'k');
    grid on
    %axis square
    ylabel('Distancia (mm)');
    xlabel('Voltagem (V)');
    rr=G.rsquare;
    rm=(G.rmse^2); 
    str1 = sprintf('R quadrado : %d \n EQM : %d',rr,rm);
    text(3.1,550,str1)
    title('Exponencial 1 para medidas de 10 em 10 mm com o robo Fanuc');
    export_fig('Exp1.pdf','-pdf')
    
    
% Desenhar Exponencial 2    
    figure('position',[100 50 600 600],'color',[1 1 1])
    hold on
    fitType = 'exp2';
    [myFit,G] = fit(Media',Dist',fitType);
    yfit=myFit.a*exp(myFit.b*Media)+myFit.c*exp(myFit.d*Media); %# evaluate the curve fit over x
    plot(Media,Dist,'*m')
    plot(Media,yfit,'-b')
    
%     xx=linspace(0.3,2.9,16000);
%     yy=myFit.a*exp(myFit.b*xx)+myFit.c*exp(myFit.d*xx);
% 
%     plot(xx,yy);
    errorbar(Media,Dist,PntMaDesv-Media,'k');
    grid on
    %axis square
    ylabel('Distancia (mm)');
    xlabel('Voltagem (V)');
    rr=G.rsquare;
    rm=(G.rmse^2); 
    str1 = sprintf('R quadrado : %d \n EQM : %d',rr,rm);
    text(3.1,550,str1)
    title('Exponencial 2 para medidas de 10 em 10 mm com o robo Fanuc');
    export_fig('Exp2.pdf','-pdf')
    
    %% Desenhar o seno
    
    
    
    
    
    file_f = importdata('/home/carpinteiro/workingcopies/lar4/src/sensors/optoelectric/data/ULTIMATE_SENO/Fanuc2.txt');
    file_s = importdata('/home/carpinteiro/workingcopies/lar4/src/sensors/optoelectric/data/ULTIMATE_SENO/Sensor2.txt');
    
    KKK=0;
    
    % Guardar sinal e tempo
    sinal=file_s(:,2);
    time=file_s(:,1)-file_s(1,1);
    % Passar de mm para mm!
    file_f(:,1)=file_f(:,1)-file_f(1,1)+KKK;
    file_f(:,2)=file_f(:,2);
    
    % Passar os pontos para os valores conforme a curva de calibração!
    file_s(:,2)=file_s(:,2)*5/1023;
    file_s(:,2)=myFit.a*exp(myFit.b*file_s(:,2))+myFit.c*exp(myFit.d*file_s(:,2));
    file_s(:,1)=file_s(:,1)-file_s(1,1);
    
    %%
     % Desenhar Parte
     
     file_sf=interp1(file_s(:,1),file_s(:,2),file_f(:,1));
     
     [r2 rmse]=rsquare(file_f(:,2),file_sf);
     
    figure('position',[100 50 600 600],'color',[1 1 1])
    hold on
    plot(file_f(500:2200,1),file_f(500:2200,2),'b');
    str1 = sprintf('R quadrado : %d \n EQM : %d',r2,rmse);
    text(11,750,str1)
    plot(file_s(500:2200,1),file_s(500:2200,2),'r.');
    
     %pwelch(file_s(500:3500,2),[],[],[],1/(file_s(501,1)-file_s(500,1)),'twosided'); 
    
    grid on
    %axis square
    ylabel('Posicao (mm)');
    xlabel('Tempo (s)');
     title('Movimento circular para simular seno e leitura do sensor nesse movimento');
    export_fig('Seno_Parte_Graf.pdf','-pdf')
    
    %
    
    file_sf=interp1(file_s(500:2200,1),file_s(500:2200,2),file_f(500:2200,1));
     
     [r2 rmse]=rsquare(file_f(500:2200,2),file_sf);
     
    figure('position',[100 50 600 600],'color',[1 1 1])
    hold on
    plot(file_f(500:2200,1),file_f(500:2200,2),'b');
    str1 = sprintf('R quadrado : %d \n EQM : %d',r2,rmse);
    %text(20,280,str1)
    plot(file_s(500:2200,1),file_s(500:2200,2),'r.');
    
     %pwelch(file_s(500:3500,2),[],[],[],1/(file_s(501,1)-file_s(500,1)),'twosided'); 
    
    grid on
    axis([14 24 90 300]) 
    ylabel('Posicao (mm)');
    xlabel('Tempo (s)');
     title('Parte do movimento circular para simular seno e leitura do sensor nesse movimento');
    export_fig('Seno_Parte_Graf2.pdf','-pdf')
    
%%

    % Desenhar sinal lido directamente
    
    figure('position',[100 50 600 600],'color',[1 1 1])
    hold on
    plot(time,sinal,'b*-');
    grid on
    %axis square
    ylabel('Voltagem (V)');
    xlabel('Tempo (s)');
    title('Sinal');
    xport_fig('Sinal.pdf','-pdf')



%% Verificar as frequencias dos sinais lidos

    figure('position',[100 50 600 600],'color',[1 1 1])
    hold on
    pwelch(sinal,[],[],[],1/(time(2)-time(1)),'twosided'); 

    grid on
    export_fig('Frequencias.pdf','-pdf')
    
    %% Aplicar filtro
    close all
    % ordem filtro , valor de inicio, valor de fim, valor do sample
    %d = fdesign.bandstop('N,F3dB1,F3dB2',10,1,5,100);
    d=fdesign.lowpass('N,Fc',10,4,40);
    Hd = design(d,'butter');
    fvtool(Hd);
    
    figure;
    yfilter = filter(Hd,file_s(:,2));
    freq = 0:(2*pi)/length(x):pi;
    plot(file_s(:,1),file_s(:,2));
    hold on;
    grid on
    title('Filtrados com um filtro rejeita banda');
    plot(file_s(:,1),yfilter,'r');
    xlabel('Tempo (s)'); ylabel('Distancia (cm)');
    legend('Original Signal','Bandstop Signal');
    export_fig('Filtrado.pdf','-pdf')
    
    
    


