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

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

% Iniciar ficheiro

file0 = importdata('/home/carpinteiro/workingcopies/lar4/src/sensors/optoelectric/data/Calibração sensor 0 int/Leitura0.txt');
file1 = importdata('/home/carpinteiro/workingcopies/lar4/src/sensors/optoelectric/data/Calibração sensor 1 int/Leitura1.txt');

% Passar tensão
file0(:,2)=file0(:,2)*5/1023;
file1(:,2)=file1(:,2)*5/1023;

% Variáveis vazias

Pontos0=[];
Desv0=[];
PntMeDesv0=[];
PntMaDesv0=[];
Media0=[];
Moda0=[];
Mediana0=[];
MediaGeo0=[];
Hist0=[];
Dist0=[]; % valores de x e y trocados para obter distância em função da tensão!!!

% Calcular Estatisticas

for n=1:size(file0,1)/500;
    
   Pontos0=[Pontos0 ; (10+(n-1))*ones(500,1) , file0(1+(500*(n-1)):500+(500*(n-1)),2)]; 
   Media0=[Media0 mean(file0(1+(500*(n-1)):500+(500*(n-1)),2))];
   Mediana0=[Mediana0 median(file0(1+(500*(n-1)):500+(500*(n-1)),2))];
   MediaGeo0=[MediaGeo0 geomean(file0(1+(500*(n-1)):500+(500*(n-1)),2))];
   Moda0=[Moda0 mode(file0(1+(500*(n-1)):500+(500*(n-1)),2))];
   Desv0=[Desv0 (sqrt(sum((file0(1+(500*(n-1)):500+(500*(n-1)),2)).^2)/500 - mean(file0(1+(500*(n-1)):500+(500*(n-1)),2)).^2))];
   PntMeDesv0=[PntMeDesv0 Media0(n)-Desv0(n)];
   PntMaDesv0=[PntMaDesv0 Media0(n)+Desv0(n)];
   Hist0=[Hist0;file0(1+(500*(n-1)):500+(500*(n-1)),2)'];
   Dist0=[Dist0 10+(n-1)];

end

% Valores obtidos

figure('position',[100 50 600 600],'color',[1 1 1])
plot(Pontos0(:,1),Pontos0(:,2),'.r');
hold on
plot(Dist0,Media0,'.b','LineWidth',2)
plot(Dist0,PntMeDesv0,'.k','LineWidth',2)
plot(Dist0,PntMaDesv0,'.k','LineWidth',2)
grid on
axis ([9.5,80.5,0,4])
xlabel('Distancia (cm)');
ylabel('Voltagem (V)');
title('Valores Lidos de 1 em 1 cm com FANUC');
export_fig Pontos0.pdf -pdf

%%

% Variáveis vazias

Pontos1=[];
Desv1=[];
PntMeDesv1=[];
PntMaDesv1=[];
Media1=[];
Moda1=[];
Mediana1=[];
MediaGeo1=[];
Hist1=[];
Dist1=[]; % valores de x e y trocados para obter distância em função da tensão!!!

% Calcular Estatisticas

for n=1:size(file0,1)/500;
    
   Pontos1=[Pontos1 ; (10+(n-1))*ones(500,1) , file1(1+(500*(n-1)):500+(500*(n-1)),2)]; 
   Media1=[Media1 mean(file1(1+(500*(n-1)):500+(500*(n-1)),2))];
   Mediana1=[Mediana1 median(file1(1+(500*(n-1)):500+(500*(n-1)),2))];
   MediaGeo1=[MediaGeo1 geomean(file1(1+(500*(n-1)):500+(500*(n-1)),2))];
   Moda1=[Moda1 mode(file1(1+(500*(n-1)):500+(500*(n-1)),2))];
   Desv1=[Desv1 (sqrt(sum((file1(1+(500*(n-1)):500+(500*(n-1)),2)).^2)/500 - mean(file1(1+(500*(n-1)):500+(500*(n-1)),2)).^2))];
   PntMeDesv1=[PntMeDesv1 Media1(n)-Desv1(n)];
   PntMaDesv1=[PntMaDesv1 Media1(n)+Desv1(n)];
   Hist1=[Hist1;file1(1+(500*(n-1)):500+(500*(n-1)),2)'];
   Dist1=[Dist1 10+(n-1)];

end

% Valores obtidos

figure('position',[100 50 600 600],'color',[1 1 1])
plot(Pontos1(:,1),Pontos1(:,2),'.r');
hold on
plot(Dist1,Media1,'.b','LineWidth',2)
plot(Dist1,PntMeDesv1,'.k','LineWidth',2)
plot(Dist1,PntMaDesv1,'.k','LineWidth',2)
grid on
axis ([9.5,80.5,0,4])
xlabel('Distancia (cm)');
ylabel('Voltagem (V)');
title('Valores Lidos de 1 em 1 cm com FANUC');
export_fig Pontos1.pdf -pdf

%%

%% Desenhar o seno
    
    % Usar segunda exponencial com a mediana
    figure('position',[100 50 600 600],'color',[1 1 1])
    hold on
    fitType = 'exp2';
    myFit0 = fit(Mediana0',Dist0',fitType);
    yfit0=myFit0.a*exp(myFit0.b*Mediana0)+myFit0.c*exp(myFit0.d*Mediana0); %# evaluate the curve fit over x
    plot(Mediana0,Dist0,'*m')
    plot(Mediana0,yfit0,'-g')
    xx=linspace(0.3,2.9,16000);
    yy=myFit0.a*exp(myFit0.b*xx)+myFit0.c*exp(myFit0.d*xx);

    plot(xx,yy);
    
    grid on
   % axis square
    ylabel('Distancia (cm)');
    xlabel('Voltagem (V)');
    title('Exponencial 2 para Mediana para medidas de 1 em 1 cm com FANUC');
    export_fig('Exp_Med_Usada0.pdf','-pdf')
    
    % Usar segunda exponencial com a mediana
    figure('position',[100 50 600 600],'color',[1 1 1])
    hold on
    fitType = 'exp2';
    myFit1 = fit(Mediana1',Dist1',fitType);
    yfit1=myFit1.a*exp(myFit1.b*Mediana1)+myFit1.c*exp(myFit1.d*Mediana1); %# evaluate the curve fit over x
    plot(Mediana1,Dist1,'*m')
    plot(Mediana1,yfit1,'-g')
    xx=linspace(0.3,2.9,16000);
    yy=myFit1.a*exp(myFit1.b*xx)+myFit1.c*exp(myFit1.d*xx);

    plot(xx,yy);
    
    grid on
   % axis square
    ylabel('Distancia (cm)');
    xlabel('Voltagem (V)');
    title('Exponencial 2 para Mediana para medidas de 1 em 1 cm com FANUC');
    export_fig('Exp_Med_Usada1.pdf','-pdf')
    
    
    
    file_f = importdata('/home/carpinteiro/workingcopies/lar4/src/sensors/optoelectric/data/Distancia/dados vazio/Fanuc.txt');
    file_s1 = importdata('/home/carpinteiro/workingcopies/lar4/src/sensors/optoelectric/data/Distancia/dados vazio/Sensor0.txt');
    file_s2 = importdata('/home/carpinteiro/workingcopies/lar4/src/sensors/optoelectric/data/Distancia/dados vazio/Sensor1.txt');
    

    % Passar de mm para cm!
    file_f(:,1)=file_f(:,1)-file_f(1,1);
    file_f(:,2)=file_f(:,2)./10;
    
    % Passar os pontos para os valores conforme a curva de calibração!
    file_s1(:,2)=file_s1(:,2)*5/1023;
    file_s1(:,2)=myFit0.a*exp(myFit0.b*file_s1(:,2))+myFit0.c*exp(myFit0.d*file_s1(:,2));
    file_s1(:,1)=file_s1(:,1)-file_s1(1,1);
    
    file_s2(:,2)=file_s2(:,2)*5/1023;
    file_s2(:,2)=myFit1.a*exp(myFit1.b*file_s2(:,2))+myFit1.c*exp(myFit1.d*file_s2(:,2));
    file_s2(:,1)=file_s2(:,1)-file_s2(1,1);
    
    
    % Desenhar
    figure('position',[100 50 600 600],'color',[1 1 1])
    hold on
    plot(file_f(:,1),file_f(:,2),'b');
    plot(file_s2(:,1),file_s2(:,2),'g*-');
    plot(file_s1(:,1),file_s1(:,2),'r*-');
    
    grid on
    %axis square
    ylabel('Posicao (cm)');
    xlabel('Tempo (s)');
    title('Fanuc movimento circular para simular seno e leitura do sensor nesse movimento');
    export_fig('Sensor_Fanuc.pdf','-pdf')
