%% Initialisation
clear all
clc
close all

sim('modele_epaule.mdl')

%% Lecture des valeurs simulées
M(:,1)=zeros(size(angle.signals.values,1),1);   % Poignet
M(:,2)=angle.signals.values;                    % Epaule
M(:,3)=intensite.signals.values;
M(:,4)=intensite.signals.values;

%% Lecture du vecteur temps (dt non constant)
stot=angle.time;

for i=2:size(stot,1)
    dt(i-1)=stot(i)-stot(i-1); % Calcul des intervalles de temps dt
end

% dt de mesure non constant, statistiques
dt_mini=min(dt);
dt_maxi=max(dt);
dt_mean=mean(dt);

%% Rééchantillonage toutes les ms par interpolation (spline)
stotint=0:0.001:stot(end);
for i=1:size(M,2)
    Mes(:,i)=interp1(stot,M(:,i),stotint,'pchip');
end

%% Définition du seuil de démarrage
seuil=1*pi/180; % En radian

%% Epaule ou poignet intéressant ?
ecart1=abs(min(Mes(:,1))-max(Mes(:,1)));
ecart2=abs(min(Mes(:,2))-max(Mes(:,2)));
if ecart1<seuil && ecart2<seuil
    no=0;
else
    if ecart2>ecart1
        no=2;
    else
        no=1;
    end
end

%% début et fin de l'affichage des courbes (xlim, ylim)
if no==1
    i=1;
    while abs(Mes(i,no)-Mes(size(Mes,1),no))<seuil
        i=i+1;
    end
    dep=i-400;
    i=size(Mes,1);
    while abs(Mes(i,no)-Mes(size(Mes,1),no))<seuil
        i=i-1;
    end
    fin=i+400;
else
    i=1;
    while abs(Mes(i,no)-Mes(size(Mes,1),no))<seuil || abs(Mes(i,no))>5*pi/180
        i=i+1;
    end
    dep=i-400;
    i=size(Mes,1);
    while abs(Mes(i,no)-Mes(size(Mes,1),no))<seuil || abs(Mes(i,no))>5*pi/180
        i=i-1;
    end
    fin=i+400;
end
    
%% début et fin du 1er mouvement (aller)
if dep<1
    dep=1;
end
angleinit=Mes(dep,no);
anglefin=Mes(fin,no);

i=dep+1;
while abs(Mes(i,no)-angleinit)<seuil
    i=i+1;
end
dep1=i;

i=dep1+1;
while abs(Mes(i,no)-Mes(i-1,no))>0.00002*pi/180
    i=i+1;
end
fin1=i;

anglefin1=Mes(fin1,no);

%% début et fin calcul de la vitesse moyenne max (Vmoy et Vmax)
i=dep1+1;
while Mes(i,no)>(angleinit+(anglefin1+angleinit)*0.05)
    i=i+1;
end
depvmoy1=i; % début du calcul de la vitesse moyenne max.
i=dep1+1;
while Mes(i,no)>(anglefin1-(anglefin1+angleinit)*0.05)
    i=i+1;
end
finvmoy1=i; % fin du calcul de la vitesse moyenne max.

%% Calcul du temp de réponse (Tr)
[mini1,imini1]=min(Mes(dep1:fin1,no));
depas1=abs(mini1-Mes(fin1,no))*100/abs((Mes(fin1,no)-Mes(dep1,no)));

if depas1>5
    i=dep1+imini1+1;
    while Mes(i,no)<(anglefin1+(anglefin1+angleinit)*0.05)
        i=i+1;
    end
else
    i=dep1+1;
    while Mes(i,no)>(anglefin1-(anglefin1+angleinit)*0.05)
        i=i+1;
    end
end
fin105=i;
Tr1=(fin105-dep1)*0.001;

%% début et fin du 2e mouvement (retour)
i=fin1;
if i>size(Mes,1)
    i=size(Mes,1)-1;
end

while abs(Mes(i,no)-anglefin1)<seuil
    if i==size(Mes,1)
        break
    end
    i=i+1;
end
dep2=i;

i=dep2+1;
while abs(Mes(i,no)-Mes(i-1,no))>0.00002*pi/180
    i=i+1;
end
fin2=i;

anglefin2=Mes(fin2,no);

%% début et fin calcul de la vitesse moyenne max (Vmoy et Vmax)
i=dep2+1;
while Mes(i,no)<(anglefin1-(anglefin2+anglefin1)*0.05)
    i=i+1;
end
depvmoy2=i;
i=dep2+1;
while Mes(i,no)<(anglefin2+(anglefin2+anglefin1)*0.05)
    i=i+1;
end
finvmoy2=i;

[maxi2,imaxi2]=max(Mes(dep2:fin2,no));
depas2=abs(maxi2-Mes(fin2,no))*100/abs((Mes(fin2,no)-Mes(dep2,no)));

%% Calcul du temp de réponse (Tr)
if depas2>5
    i=dep2+imaxi2+1;
    while Mes(i,no)>(anglefin2-(anglefin2+anglefin1)*0.05)
        i=i+1;
    end
else
    i=dep2+1;
    while Mes(i,no)<(anglefin2+(anglefin2+anglefin1)*0.05)
        i=i+1;
    end
end

fin205=i;
Tr2=(fin205-dep2)*0.001;

%% Calcul des vitesses moyennes et max (Vmoy et Vmax)
[v1max,xv1m]=max(abs(gradient(Mes(depvmoy1:finvmoy1,no),0.001))*180/pi);
[v2max,xv2m]=max(abs(gradient(Mes(depvmoy2:finvmoy2,no),0.001))*180/pi);
xv1max=xv1m+depvmoy1;
xv2max=xv2m+depvmoy2;
v1mean=abs(mean(gradient(Mes(depvmoy1:finvmoy1,no),0.001))*180/pi);
v2mean=abs(mean(gradient(Mes(depvmoy2:finvmoy2,no),0.001))*180/pi);
xv1mean=double(int16((finvmoy1+depvmoy1)/2));
xv2mean=double(int16((finvmoy2+depvmoy2)/2));

%% Nom des fichiers de sauvegarde
heure=clock;
date=[num2str(heure(1,1)),'-',num2str(heure(1,2)),'-',num2str(heure(1,3)),'-',num2str(heure(1,4)),'h-',num2str(heure(1,5)),'m']

%% Détection d'un fichier non intéressant
if no==0
    errordlg('Rien d''intéressant dans les données. Vous avez démarré l''exécution du programme trop tard.','Données mesurées quasi-constantes');

%% Courbe de la réponse du poignet
elseif no==1
    figure; hold on
    plot(stotint(:),Mes(:,no)*180/pi,'.')
    plot(stotint(depvmoy1:finvmoy1),Mes(depvmoy1:finvmoy1,no)*180/pi,'r.')
    plot(stotint(depvmoy2:finvmoy2),Mes(depvmoy2:finvmoy2,no)*180/pi,'r.')
    text(stotint(dep1),Mes(dep1,no)*180/pi,'\downarrow','FontSize',10,'VerticalAlignment','bottom')
    text(stotint(fin105),Mes(fin105,no)*180/pi,['\downarrow Tr5%=',num2str(Tr1,'%1.2f'),'s'],'FontSize',10,'VerticalAlignment','bottom')
    text(stotint(dep2),Mes(dep2,no)*180/pi,'\uparrow','FontSize',10,'VerticalAlignment','top')
    text(stotint(fin205),Mes(fin205,no)*180/pi,['\uparrow Tr5%=',num2str(Tr2,'%1.2f'),'s'],'FontSize',10,'VerticalAlignment','top')
    text(stotint(xv1max),Mes(xv1max,no)*180/pi,['  Vmax=',num2str(v1max,'%4.0f'),'°/s'],'FontSize',10,'VerticalAlignment','top','color','red')
    text(stotint(xv2max),Mes(xv2max,no)*180/pi,['  Vmax=',num2str(v2max,'%4.0f'),'°/s'],'FontSize',10,'VerticalAlignment','top','color','red')
    text(stotint(xv1mean),Mes(xv1mean,no)*180/pi,['  Vmoy=',num2str(v1mean,'%4.0f'),'°/s'],'FontSize',10,'VerticalAlignment','top','HorizontalAlignment','right','color','red')
    text(stotint(xv2mean),Mes(xv2mean,no)*180/pi,['  Vmoy=',num2str(v2mean,'%4.0f'),'°/s'],'FontSize',10,'VerticalAlignment','top','HorizontalAlignment','right','color','red')
    text(stotint(imini1+dep1),Mes(imini1+dep1,no)*180/pi,['D%=',num2str(depas1,'%2.1f'),'%'],'FontSize',10,'VerticalAlignment','top')
    text(stotint(imaxi2+dep2),Mes(imaxi2+dep2,no)*180/pi,['D%=',num2str(depas2,'%2.1f'),'%'],'FontSize',10,'VerticalAlignment','bottom')
    xlim([stotint(dep),stotint(fin)])
    plot([stotint(dep),stotint(fin)],[(anglefin1-(anglefin1+angleinit)*0.05)*180/pi,(anglefin1-(anglefin1+angleinit)*0.05)*180/pi],'r-')
    plot([stotint(dep),stotint(fin)],[(anglefin1+(anglefin1+angleinit)*0.05)*180/pi,(anglefin1+(anglefin1+angleinit)*0.05)*180/pi],'r-')
    plot([stotint(dep),stotint(fin)],[(anglefin2-(anglefin2+anglefin1)*0.05)*180/pi,(anglefin2-(anglefin2+anglefin1)*0.05)*180/pi],'r-')
    plot([stotint(dep),stotint(fin)],[(anglefin2+(anglefin2+anglefin1)*0.05)*180/pi,(anglefin2+(anglefin2+anglefin1)*0.05)*180/pi],'r-')
    xlabel('Temps (s)')
    ylabel('RWristYaw (°)')
    saveas(gcf,['RWristYaw_',date,'.png'],'png')
    
%% Courbe d'intensité du poignet
    figure;hold on;
    subplot(2,1,1)
    plot(stotint(:),Mes(:,1)*180/pi,'.');xlim([stotint(dep),stotint(fin)])
    xlabel('Temps (s)')
    ylabel('RWristYaw (°)')
    subplot(2,1,2)
    plot(stotint(:),Mes(:,3),'r.');xlim([stotint(dep),stotint(fin)])
        [imax,timax]=max(abs(Mes(dep1:fin1,3)));
    text(stotint(timax+dep1),Mes(timax+dep1,3),['imax=',num2str(imax,'%0.2f'),'A'],'FontSize',10,'VerticalAlignment','bottom')
    [imax,timax]=max(abs(Mes(dep2:fin2,3)));
    text(stotint(timax+dep2),Mes(timax+dep2,3),['imax=',num2str(imax,'%0.2f'),'A'],'FontSize',10,'VerticalAlignment','bottom')

    xlabel('Temps (s)')
    ylabel('Intensité moteur poignet (A)')
    saveas(gcf,['RWristYaw_Current_',date,'.png'],'png')

%% Courbe de la réponse de l'épaule
elseif no==2
    figure; hold on
    plot(stotint(:),Mes(:,2)*180/pi,'.')
    plot(stotint(depvmoy1:finvmoy1),Mes(depvmoy1:finvmoy1,no)*180/pi,'r.')
    plot(stotint(depvmoy2:finvmoy2),Mes(depvmoy2:finvmoy2,no)*180/pi,'r.')
    text(stotint(dep1),Mes(dep1,2)*180/pi,'\downarrow','FontSize',10,'VerticalAlignment','bottom')
    text(stotint(fin105),Mes(fin105,2)*180/pi,['\downarrow Tr5%=',num2str(Tr1,'%1.2f'),'s'],'FontSize',10,'VerticalAlignment','bottom')
    text(stotint(dep2),Mes(dep2,2)*180/pi,'\uparrow','FontSize',10,'VerticalAlignment','top')
    text(stotint(fin205),Mes(fin205,2)*180/pi,['\uparrow Tr5%=',num2str(Tr2,'%1.2f'),'s'],'FontSize',10,'VerticalAlignment','top')
    text(stotint(xv1max),Mes(xv1max,no)*180/pi,['  Vmax=',num2str(v1max,'%4.0f'),'°/s'],'FontSize',10,'VerticalAlignment','top','color','red')
    text(stotint(xv2max),Mes(xv2max,no)*180/pi,['  Vmax=',num2str(v2max,'%4.0f'),'°/s'],'FontSize',10,'VerticalAlignment','top','color','red')
    text(stotint(xv1mean),Mes(xv1mean,no)*180/pi,['  Vmoy=',num2str(v1mean,'%4.0f'),'°/s'],'FontSize',10,'VerticalAlignment','top','HorizontalAlignment','right','color','red')
    text(stotint(xv2mean),Mes(xv2mean,no)*180/pi,['  Vmoy=',num2str(v2mean,'%4.0f'),'°/s'],'FontSize',10,'VerticalAlignment','top','HorizontalAlignment','right','color','red')
    text(stotint(imini1+dep1),Mes(imini1+dep1,2)*180/pi,['D%=',num2str(depas1,'%2.1f'),'%'],'FontSize',10,'VerticalAlignment','top')
    text(stotint(imaxi2+dep2),Mes(imaxi2+dep2,2)*180/pi,['D%=',num2str(depas2,'%2.1f'),'%'],'FontSize',10,'VerticalAlignment','bottom')
    xlim([stotint(dep),stotint(fin)])
    plot([stotint(dep),stotint(fin)],[(anglefin1-(anglefin1+angleinit)*0.05)*180/pi,(anglefin1-(anglefin1+angleinit)*0.05)*180/pi],'r-')
    plot([stotint(dep),stotint(fin)],[(anglefin1+(anglefin1+angleinit)*0.05)*180/pi,(anglefin1+(anglefin1+angleinit)*0.05)*180/pi],'r-')
    plot([stotint(dep),stotint(fin)],[(anglefin2-(anglefin2+anglefin1)*0.05)*180/pi,(anglefin2-(anglefin2+anglefin1)*0.05)*180/pi],'r-')
    plot([stotint(dep),stotint(fin)],[(anglefin2+(anglefin2+anglefin1)*0.05)*180/pi,(anglefin2+(anglefin2+anglefin1)*0.05)*180/pi],'r-')
    xlabel('Temps (s)')
    ylabel('RShoulderPitch (°)')
    title('Réponse de commande de l''épaule')
    saveas(gcf,['RShoulderPitch_',date,'.png'],'png')
    
%% Courbe d'intensité de l'épaule
    figure;hold on;
    subplot(2,1,1)
    plot(stotint(:),Mes(:,2)*180/pi,'.');xlim([stotint(dep),stotint(fin)])
    xlabel('Temps (s)')
    ylabel('RShoulderPitch (°)')
    subplot(2,1,2)
    plot(stotint(:),Mes(:,4),'r.');xlim([stotint(dep),stotint(fin)])
    [imax,timax]=max(abs(Mes(dep1:fin1,4)));
    text(stotint(timax+dep1),Mes(timax+dep1,4),['imax=',num2str(imax,'%0.2f'),'A'],'FontSize',10,'VerticalAlignment','bottom')
    [imax,timax]=max(abs(Mes(dep2:fin2,4)));
    text(stotint(timax+dep2),Mes(timax+dep2,4),['imax=',num2str(imax,'%0.2f'),'A'],'FontSize',10,'VerticalAlignment','bottom')
    xlabel('Temps (s)')
    ylabel('Intensité moteur épaule(A)')
    saveas(gcf,['RShoulderPitch_Current_',date,'.png'],'png')
end

%% Sauvegarde des résultats numériques.
save(['Donnees_',date])