%% Reorder all points

clear all;close all;clc

load refData

mink=1;
maxk=length(data);

spacing=1.*pi/180;

for k=mink:maxk
 
    tg=[];
    rg=[];
    rl=[];
    cl=[];
    
    xa=data{k}.x;
    ya=data{k}.y;
    
    ra=sqrt(xa.*xa + ya.*ya);
    ta=atan2(ya,xa);

    %put t in a grid
    for h=1:length(xa)
        x=xa(h);
        y=ya(h);
        r=ra(h);
        t=ta(h);
        
        rb=spacing*ceil(t/spacing);
        lb=spacing*floor(t/spacing);
        
        if abs(rb-t)<abs(lb-t)
            t=rb;
        else
            t=lb;
        end
        
        if isempty(find(tg==t,1))
            tg=[tg t];
            rg(find(tg==t,1))=r;
            rl(find(tg==t,1))=data{k}.labels(h);
            cl(find(tg==t,1),:)=data{k}.colors(h,:);
        else
            %keep the closest return
            if rg(find(tg==t,1)) > r
                rg(find(tg==t,1))=r;
                rl(find(tg==t,1))=data{k}.labels(h);
                cl(find(tg==t,1),:)=data{k}.colors(h,:);
            end
        end
    end
    
    [tg, I] = sort(tg);
    rg=rg(I);
    rl=rl(I);
    
    for g=1:size(cl,2)
        cl(:,g)=cl(I,g);
    end
    
    data{k}.rx=rg.*cos(tg);
    data{k}.ry=rg.*sin(tg);
    data{k}.rl=rl;
    data{k}.cl=cl;
    
    disp(sprintf('... %.1f %%',(k-mink)/(maxk-mink)*100))
end

disp('saving ...')
% save refData data
disp('done')

%%

clear all;close all;clc

cd ~/Desktop/model
load refData
addpath icp_n
addpath ekfukf
addpath tls
run('csm/setall.m')

mk=182;
nk=160;
% nk=180;

figure('position',[1000 50 900 900])

L=30;

kp=nk;

rotp=eye(3);
transp=zeros(3,1);
tf=[0; 0; 0];
dt=1/50;

l=2.54;

init=1;

for k=nk:mk
    x=data{k}.rx;
    y=data{k}.ry;
    lab=data{k}.rl;
    col=data{k}.cl;
    
    for l=6:6
        pause(1)
        figure(1)
        clf
        set(gca, 'LooseInset', get(gca,'TightInset'))
        
        xc=x(lab==l);
        
        if isempty(xc); disp('No points in the current data'); break; end
        if length(xc)==1; continue; end
        
        yc=y(lab==l);
        cl=col(lab==l,:);
        
        xp=data{kp}.rx(data{kp}.rl==l);
        yp=data{kp}.ry(data{kp}.rl==l);
        
        if isempty(xp);continue;end
        
        
        %prior
        
%         prior=data{k}.prior;
        
        %colors
        cc=cl(1,:);
        ct=[0 0 0];
        ft=0.4;
        cp=cc*(1-ft)+ct*ft;
        
        pc=[xc;yc;zeros(1,length(xc))];
        pp=[xp;yp;zeros(1,length(xp))];
        
        spc=dpsimplify(pc',0.002);
%         spc=spc';
        spc=pc;
        
        spp=dpsimplify(pp',0.002);
%         spp=spp';
        spp=pp;
        
        plot(spc(1,:),spc(2,:),'marker','o','linestyle','none','markersize',5,'markeredgecolor',cc,'markerfacecolor',cc)
        
        axis([-5 20 -2 23]); axis square
        grid on
        hold on
        
        plot(spp(1,:),spp(2,:),'marker','o','linestyle','none','markersize',5,'markeredgecolor',cp,'markerfacecolor',cp)
        
        dx=diff(spc(1,:));
        dy=diff(spc(2,:));
        
        d=sqrt(dx.*dx+dy.*dy);
        
        s=[0 cumsum(d)];
        
        nPoints=round(sum(d)/0.2);
        
%         if(nPoints<10)
%             nPoints=round(sum(d)/0.1);
%         end
        
        is=linspace(0,sum(d),nPoints);
        xn=interp1(s,spc(1,:),is,'cubic');
        yn=interp1(s,spc(2,:),is,'cubic');

        ispc=[xn;yn;zeros(1,length(xn))];
%         ispc=spc;
%         ispc=pc;
        
        dx=diff(spp(1,:));
        dy=diff(spp(2,:));
        
        d=sqrt(dx.*dx+dy.*dy);
        
        s=[0 cumsum(d)];
        
        nPoints=round(sum(d)/0.2);
        
%         if(nPoints<10)
%             nPoints=round(sum(d)/0.1);
%         end
        
        is=linspace(0,sum(d),nPoints);
        xn=interp1(s,spp(1,:),is,'cubic');
        yn=interp1(s,spp(2,:),is,'cubic');
        
        ispp=[xn;yn;zeros(1,length(xn))];
%         ispp=spp;
%         ispp=pp;
        
        
        plot(ispc(1,:),ispc(2,:),'marker','o','linestyle','-','color',cc,'markersize',3,'markeredgecolor',cc,'markerfacecolor',cc)
        plot(ispp(1,:),ispp(2,:),'marker','o','linestyle','-','color',cp,'markersize',3,'markeredgecolor',cp,'markerfacecolor',cp)
        
        if ~init
            plot(prior(1,:),prior(2,:),'marker','o','linestyle','-','color',[0 0 1],'markersize',3,'markeredgecolor',[0 0 1],'markerfacecolor',[0 0 1])
        end
%         tf = MbICP_generic(ispc, ispp, [0,0,0], false,4);

        if init
            tf = MbICP_generic(ispc, ispp, [0,0,0], false,4);
            init=0;
        else
            tf = MbICP_generic(ispc, prior, [0,0,0], false,4);
            tf=priorTf+tf;
        end
        
        isppr=applyTransform(ispp(1:2,:),tf);
        
        [vk,rk,theta, phi, pk3 ,ccr]=estimation(ispp,isppr,dt,tf);
        
%         avgv=sum(vk)/length(vk)
%         realv=X(3,k)
%         avgr=sum(rk)/length(rk);
%         avgphi=asin((tf(3)*l)/(avgv*dt))
%         realphi=X(5,k)
        
        
%         theta(theta>pi)=theta(theta>pi)-pi;
%         avgtheta=sum(theta)/length(theta)
%         avgtheta=theta(1)
%         for h=1:size(
%             avgr=
%         end
        
        plot(ccr(1),ccr(2),'marker','o','markersize',20,'color',[1 0 0],'markeredgecolor',[1 0 0],'markerfacecolor',[1 0 0])
        
        prior=zeros(2,size(ispc,2));
        priorTf=tf;

        for h=1:size(ispc,2)
            r=sqrt( (ccr(1)-ispc(1,h))^2 + (ccr(2)-ispc(2,h))^2 );
            prior(1,h)=ccr(1)+r*cos(atan2(ispc(2,h)-ccr(2),ispc(1,h)-ccr(1))+tf(3));
            prior(2,h)=ccr(2)+r*sin(atan2(ispc(2,h)-ccr(2),ispc(1,h)-ccr(1))+tf(3));
        end
        
        plot(isppr(1,:),isppr(2,:),'marker','o','linestyle','-','color',[1 0 0],'markersize',3,'markeredgecolor',[1 0 0],'markerfacecolor',[1 0 0])
        plot(prior(1,:),prior(2,:),'marker','o','linestyle','-','color',cc,'markersize',3,'markeredgecolor',cc,'markerfacecolor',cc)
        
        break
        
        p=ispc;
        params.laser_ref.nrays=size(p,2);
        params.laser_ref.theta=atan2(p(2,:),p(1,:));
        params.laser_ref.readings=sqrt(p(1,:).*p(1,:)+p(2,:).*p(2,:));
        params.laser_ref.readings_valid=ones(1,size(p,2));
        params.laser_ref.estimate=[0;0;0];
        params.laser_ref.points=[p(1,:);p(2,:)];
        
%         ld.color='.b';
%         ld_plot(params.laser_ref,ld)
        
        p=isppr;
        params.laser_sens.nrays=size(p,2);
        params.laser_sens.theta=atan2(p(2,:),p(1,:));
        params.laser_sens.readings=sqrt(p(1,:).*p(1,:)+p(2,:).*p(2,:));
        params.laser_sens.readings_valid=ones(1,size(p,2));
        params.laser_sens.estimate=[0;0;0];
        params.laser_sens.points=[p(1,:);p(2,:)];
        
%         ld.color='.r';
%         ld_plot(params.laser_sens,ld)
        disp('\nstarting alignment\n\n')
        params.interactive=true;
        res = icp2(params);
        
%         params.laser_sens=
        % 	params_required(params, 'laser_sens');
% 	params_required(params, 'laser_ref');
        
        break
        pts=ispc;
        al=atan(pts(2,:)./pts(1,:));
        ao=atan(diff(pts(2,:))./diff(pts(1,:)));
        
        adif=abs(ao-al(1:end-1));
        adif(adif>pi/2)=pi-adif(adif>pi/2);
        adif=[adif adif(end)];
        
        mp=(pts(:,1:end-1)+pts(:,2:end))/2;
        
%         for h=1:size(mp,2)
%             text(mp(1,h),mp(2,h),sprintf('%.1f',adif(h)*180/pi))
%         end
        
        ispc(:,adif<40*pi/180)=[];
        
        plot(ispc(1,:),ispc(2,:),'linestyle','-','color',[1 0 0],'markersize',3,'markeredgecolor',[1 0 0],'markerfacecolor',[1 0 0])
%         plot(ispp(1,:),ispp(2,:),'marker','o','linestyle','-','color',cp,'markersize',3,'markeredgecolor',cp,'markerfacecolor',cp)
        
        plot([0 ispc(1,1)],[0 ispc(2,1)],'b')
%         a1=atan(ispc(2,1)/ispc(1,1));
%         a11=atan((spc(2,2)-spc(2,1))/(spc(1,2)-spc(1,1)));
%         da=abs(a11-a1);
        
%         text(ispc(1,1),ispc(2,1)+0.2,['oa: ' num2str(a11)]);
%         text(ispc(1,1),ispc(2,1)+0.5,['la: ' num2str(a1)]);
%         text(ispc(1,1),ispc(2,1)+0.8,['ia: ' num2str(da)]);
        
        plot([0 ispc(1,end)],[0 ispc(2,end)],'b')
        
        plot([0 ispp(1,1)],[0 ispp(2,1)],'r')
        plot([0 ispp(1,end)],[0 ispp(2,end)],'r')
        
%         text
        
        break
        
%         plot(pc(1,:),pc(2,:),'marker','o','linestyle','-','color',cc,'markersize',2,'markeredgecolor',cc,'markerfacecolor',cc)
%         plot(pp(1,:),pp(2,:),'marker','o','linestyle','-','color',cp,'markersize',2,'markeredgecolor',cp,'markerfacecolor',cp)
        
% %         plot(ispp(1,:),ispp(2,:),'marker','o','linestyle','-','color',cp,'markersize',3,'markeredgecolor',cp,'markerfacecolor',cp)

%         isppr=rotp*ispp+repmat(transp,1,size(ispp,2));

%         [rot, trans]=icp(pc,pp,50,'WorstRejection',0.001);
        [rot, trans]=icp(ispc,ispp,30,'WorstRejection',0.001);
        
%         if abs(acos(rot(1)))>10*pi/180
%             rot=eye(3);
%         end
        
%         rotp=rot;
%         transp=trans;

%         tp=rot*pp+repmat(trans,1,size(pp,2));
%         stpi=rot*ispp+repmat(trans,1,size(ispp,2));
        
%         [rot, trans]=icp(ispc,stpi,30,'WorstRejection',0.2);

%         tp=rot*pp+repmat(trans,1,size(pp,2));
        stp=rot*ispp+repmat(trans,1,size(ispp,2));
        
%         [rot, trans]=icp(ispc,stpi,20,'WorstRejection',0.05);
        
%         stp=rot*stpi+repmat(trans,1,size(stpi,2));
        
%         plot(tp(1,:),tp(2,:),'marker','o','linestyle','none','markersize',3,'markeredgecolor',[1 0 0],'markerfacecolor',[1 0 0])
%         plot(isppr(1,:),isppr(2,:),'marker','o','linestyle','-','color',[1 0 0],'markersize',4,'markeredgecolor',[1 0 0],'markerfacecolor',[1 0 0])
%         plot(stp(1,:),stp(2,:),'marker','o','linestyle','-','color',[1 .5 0],'markersize',2,'markeredgecolor',[1 .5 0],'markerfacecolor',[1 .5 0])
%         plot(stp(1,:),stp(2,:),'marker','o','linestyle','-','color',[1 .0 0],'markersize',2,'markeredgecolor',[1 .5 0],'markerfacecolor',[1 .5 0])
        
%         break
%         sp=dpsimplify(tp',0.1);
%         sp=sp';
%         
%         plot(sp(1,:),sp(2,:),'marker','o','linestyle','-','markersize',3,'markeredgecolor',[0 0 1],'markerfacecolor',[0 0 1])
        

        np=ispp(1:2,:)';
        no=LineNormals2D(np);
        nispp=no;
        anpp=atan2(no(:,2),no(:,1));
        
%         plot([np(:,1) np(:,1)-1*no(:,1)]',[np(:,2) np(:,2)-1*no(:,2)]','color',cp)
                
        np=ispc(1:2,:)';
        no=LineNormals2D(np);
        nispc=no;
        anpc=atan2(no(:,2),no(:,1));
        
%         plot([np(:,1) np(:,1)-1*no(:,1)]',[np(:,2) np(:,2)-1*no(:,2)]','color',cc)
        

%         break
        
%         da = tril( abs(abs(repmat(anpc,1,size(anpc,1)) bem por aí?
% 
% Podes perguntar ao Emilio qual é o gestor de terminais e janelas que ele tem isntaado.
% 
% Acho que o vou tentar usar isso
% 
% Abraço
% 
% Miguel  - repmat(anpc',size(anpc',2),1)) -pi/2),-1 );
%         da(logical(triu(ones(size(anpc,1)))))=nan;
        
        [mval, mpos]=min(da(:));
        [impos, jmpos] = ind2sub(size(da),mpos);
        
%         plot(ispc(1,impos),ispc(2,impos),'marker','o','color',[1 0 0],'markersize',6,'markeredgecolor',[1 0 0],'markerfacecolor',[1 0 0])
%         plot(ispc(1,jmpos),ispc(2,jmpos),'marker','o','color',[0 0 1],'markersize',6,'markeredgecolor',[0 0 1],'markerfacecolor',[0 0 1])

        s=linspace(-10,10,2);
        
        v1=[nispc(impos,:) 0];
        mr=[0 1 0; -1 0 0; 0 0 1];
        v1r=mr*v1';
        
        v2=[nispc(jmpos,:) 0];
        mr=[0 1 0; -1 0 0; 0 0 1];
        v2r=mr*v2';
        
        md1(1,:)=ispc(1,impos)+ v1r(1)*s;
        md1(2,:)=ispc(2,impos)+ v1r(2)*s;
        
        md2(1,:)=ispc(1,jmpos)+ v2r(1)*s;
        md2(2,:)=ispc(2,jmpos)+ v2r(2)*s;
        
%         plot(md1(1,:),md1(2,:),'r')
%         plot(md2(1,:),md2(2,:),'r')
        
        [xcc, ycc]=lineintersect(md1(:)',md2(:)');
        plot(xcc,ycc,'*r')
        
        dt=sqrt( pow2(ispc(1,:)-xcc) + pow2(ispc(2,:)-ycc));
        epc=ispc(:,dt<3);
        
        
        % for the past scan

        da = tril( abs(abs(repmat(anpp,1,size(anpp,1)) - repmat(anpp',size(anpp',2),1)) -pi/2),-1 );
        da(logical(triu(ones(size(anpp,1)))))=nan;
        
        [mval, mpos]=min(da(:));
        [impos, jmpos] = ind2sub(size(da),mpos);
        
%         plot(ispc(1,impos),ispc(2,impos),'marker','o','color',[1 0 0],'markersize',6,'markeredgecolor',[1 0 0],'markerfacecolor',[1 0 0])
%         plot(ispc(1,jmpos),ispc(2,jmpos),'marker','o','color',[0 0 1],'markersize',6,'markeredgecolor',[0 0 1],'markerfacecolor',[0 0 1])

        s=linspace(-10,10,2);
        
        v1=[nispp(impos,:) 0];
        mr=[0 1 0; -1 0 0; 0 0 1];
        v1r=mr*v1';
        
        v2=[nispp(jmpos,:) 0];
        mr=[0 1 0; -1 0 0; 0 0 1];
        v2r=mr*v2';
        
        md1(1,:)=ispp(1,impos)+ v1r(1)*s;
        md1(2,:)=ispp(2,impos)+ v1r(2)*s;
        
        md2(1,:)=ispp(1,jmpos)+ v2r(1)*s;
        md2(2,:)=ispp(2,jmpos)+ v2r(2)*s;
        
%         plot(md1(1,:),md1(2,:),'b')
%         plot(md2(1,:),md2(2,:),'b')
        
        [xcp, ycp]=lineintersect(md1(:)',md2(:)');
        plot(xcp,ycp,'*b')
        
        
        dt=sqrt( pow2(ispp(1,:)-xcp) + pow2(ispp(2,:)-ycp));
        epp=ispp(:,dt<3);

        
        epc=ispc;
        epp=ispp;
        
        %align ep
        
        rot=eye(3);
        trans=[xcc-xcp ycc-ycp 0]';
        
%         epp=rot*epp+repmat(trans,1,size(epp,2));
        
%         [rot, trans]=icp(epc,epp,50,'WorstRejection',0.0001);
        
%         tp=rot*pp+repmat(trans,1,size(pp,2));
        eppt=rot*epp+repmat(trans,1,size(epp,2));
        
%         plot(epc(1,:),epc(2,:),'marker','o','linestyle','-','color',[1 0 0],'markersize',3,'markeredgecolor',[1 0 0],'markerfacecolor',[1 0 0])
%         plot(epp(1,:),epp(2,:),'marker','o','linestyle','-','color',[0 0 1],'markersize',3,'markeredgecolor',[0 0 1],'markerfacecolor',[0 0 1])

        plot(eppt(1,:),eppt(2,:),'marker','o','linestyle','-','color',[1 .5 0],'markersize',5,'markeredgecolor',[1 .5 0],'markerfacecolor',[1 .5 0])
%         for h=1:size(ispc,2)
%             
%             for g=1:size(ispc,2)
%                 if h==g;continue;end
%                 
%                 adif = abs( abs(anpp-anpc) - pi/2 );
%                 
%                 
%             end
%         
%         end
        
        break
        
        
        
%         figure(2)
%         clf
%         hist(no)
        
%         iispc=[ispc; 
%         break
        
        figure(2)
        clf
        set(gca, 'LooseInset', get(gca,'TightInset'))

        [n, x]=hist(anpp,30);
        h=bar(x,n);
        set(h,'facecolor',cp,'barwidth',0.3)
        hold on
        
        [mv, ix]=max(n);
        
        plot(x(ix),mv,'*')
        
        maxpp=x(ix);
        
        [n, x]=hist(anpc,30);
        h=bar(x,n);
        set(h,'facecolor',cc,'barwidth',0.3)
        
        [mv, ix]=max(n);
        
        
        figure(3)
        clf
        
        set(gca, 'LooseInset', get(gca,'TightInset'))
        
        plot(abs(diff(anpp)),'color',cp,'marker','*');
        
        hold on
        
        plot(abs(diff(anpc)),'color',cc,'marker','*');
        
        
%       
%         figure(4)
%         clf
%         
%         set(gca, 'LooseInset', get(gca,'TightInset'))
%         
%         plot(anpp,'color',cp,'marker','*');
%         
%         hold on
%         
%         plot(anpc,'color',cc,'marker','*');
        
        break;
        
        plot(x(ix),mv,'*r')
        
        maxpc=x(ix);
        
        an=maxpc-maxpp;
        
        rot=[cos(an) -sin(an) 0; +sin(an) cos(an) 0; 0 0 1];
        
        rispp=rot*ispp;
        
        
        
        figure(1)
        plot(rispp(1,:),rispp(2,:),'marker','o','linestyle','-','color',[1 0 0],'markersize',3,'markeredgecolor',[1 0 0],'markerfacecolor',[1 0 0])
        
        np=rispp(1:2,:)';
        no=LineNormals2D(np);
        anpr=atan2(no(:,2),no(:,1));
        
        plot([np(:,1) np(:,1)-1*no(:,1)]',[np(:,2) np(:,2)-1*no(:,2)]','color',[1 0 0])
        
        figure(2)
        [n, x]=hist(anpr,30);
        h=bar(x,n);
        set(h,'facecolor',[0 1 0],'barwidth',0.3)
        hold on
        
        maxpr=x(ix);
        
        [clpr, ixpr]=min(anpr-maxpr);
        [clpc, ixpc]=min(anpc-maxpc);
        
        corrspr=ixpr;
        corrspc=ixpc;
        
        v=no(corrspr,:)
        pi=rispp(1:2,corrspr)
        pf=ispc(1:2,corrspc)
        
        figure(3)
        
        plot(pi(1),pi(2),'*'); hold on
        plot(pf(1),pf(2),'*r');
        
        s=linspace(0,2,10);
        
        pint = repmat(v',1,size(s,2)).*repmat(s,2,1);
        
        plot(pint(1,:)+pi(1),pint(2,:)+pi(2),'g');
        
        
%         fc=(pf-pi)./v'
        
%         
%         hold on
        
%         [n, x]=hist([an2' an1'],20);
%         h=bar(x,n);
%         set(h,'facecolor',[0 1 0],'barwidth',0.3)
        
        
%         figure
%         axis([-1.2 1.2 -1.2 1.2])
        
%         plot([zeros(size(normals(:,1))) 1*normals(:,1)]',[zeros(size(normals(:,2))) 1*normals(:,2)]')
        
        
%         [coeff,score] = princomp(normals);
        
%         for h=1:size(coeff,2)
%             plot([0 coeff(1,h)],[0 coeff(2,h)])
%         end
    end
    
    kp=k;
    hold off
    title(num2str(k))
    
    pause(0.5)
end

%% TLS tests

clear all;close all;clc

cd ~/Desktop/model
load refData
addpath icp_n
addpath tls
requireFEXpackage(31069)

%% polar lines

clear all;close all;clc
cd ~/Desktop/model

l1p=[1 0];
l2p=[1 pi/2];
l3p=[1.2 -2*pi/180];
l4p=[1.2 -2*pi/180+pi/2];

[l1(1,:), l1(2,:)]=linePolar2Cartesian(l1p(1),l1p(2),linspace(-2,2,2));
[l2(1,:), l2(2,:)]=linePolar2Cartesian(l2p(1),l2p(2),linspace(-2,2,2));
[l3(1,:), l3(2,:)]=linePolar2Cartesian(l3p(1),l3p(2),linspace(-2,2,2));
[l4(1,:), l4(2,:)]=linePolar2Cartesian(l4p(1),l4p(2),linspace(-2,2,2));

plot(l1(1,:),l1(2,:),'b')
hold on
axis([-2 2 -2 2]); grid on; axis square

plot(l2(1,:),l2(2,:),'b')

plot(l3(1,:),l3(2,:),'r')
plot(l4(1,:),l4(2,:),'r')


%% ekf usage trial

clear all;close all;clc

cd ~/Desktop/model
addpath ekfukf

% Measurement model and it's derivative
f_func = @ekf_nonholonomic_f;
df_dx_func = @ekf_nonholonomic_df_dx;
h_func = @ekf_nonholonomic_h;
% dh_dx_func = @ekf_sine_dh_dx;
% d2h_dx2_func = @ekf_sine_d2h_dx2;
% Estimate with EKF

% v_i=5;
l=2.5;
dt=1/50;

% v=ones(1,500)*v_i;
% phi=linspace(0,20,500);
% phi=phi*pi./180;
% theta=zeros(1,500);
% x=zeros(1,500);
% y=zeros(1,500);
% dphi=diff(phi);
% dphi=[dphi(1) dphi];

N=500;

X=zeros(6,N);
X(:,1)=[0 0 5 10*pi/180 0 1.0*pi/180]';

for k=1:N-1
    X(:,k+1)=f_func(X(:,k),[dt l]);
end

plot(X(1,:),X(2,:))
axis square;
title('path')

q1=0.05;

Y(1,:)=X(1,:)+q1*randn(1,N);
Y(2,:)=X(2,:)+q1*randn(1,N);

hold on

plot(Y(1,:),Y(2,:),'r*')

% M=X(:,1);
M=[0 0 0 0 0 0]';

% P=eye(6)*1e1;

P=[ 1e-2 0 0 0 0 0
    0 1e-2 0 0 0 0
    0 0 1e1 0 0 0
    0 0 0 1e1 0 0
    0 0 0 0 1e1 0
    0 0 0 0 0 1e2]*1e1;

Q=[ 1e1 0 0 0 0 0        %X
    0 1e1 0 0 0 0        %Y
    0 0 1e-2 0  0 0         %V
    0 0 0 1e-5 0 0        %T
    0 0 0 0 1e-4 0        %F
    0 0 0 0 0 1e-3]*1e-4; %dF

H= [1 0
    0 1
    0 0
    0 0
    0 0
    0 0]';

R=eye(2)*1e-1;

MM=zeros(size(M,1),size(Y,2));
PP=zeros(size(M,1),size(M,1),size(Y,2));
sig=zeros(size(M,1),size(Y,2));

MM(:,1)=M;
PP(:,:,1)=P;
Mo=M;

for k=2:size(Y,2) 
    [M,P] = ekf_predict1(M,P,df_dx_func,Q,f_func(M,[dt l]),eye(6),[dt l]);
    [M,P] = ekf_update1(M,P,Y(:,k),H,R,H*M,eye(2),[dt l]);
    
%     P=0.5*P+0.5*PP(:,:,k-1)
    %Apply constrains
    
%     d=abs(Mo-M);
    
%     if k<50
%         if d(4)>0.1
%             M(4)=(0.8*Mo(4)+0.2*M(4))/1;
%             M(4)
%         end
%     end
    
%         if d(6)>0.05
%             M(6)=(0.95*Mo(6)+0.05*M(6))/1;
%             M(4)
%             sig(6,k)=1;
%         end

    MM(4,1:k)=unwrap(MM(4,1:k));
    MM(5,1:k)=unwrap(MM(5,1:k));

    MM(:,k)   = M;
    PP(:,:,k) = P;
   
%     Mo=M;
    
%    error_ellipse(PP(1:2,1:2,k),M)
end

[SM1,SP1] = erts_smooth1(MM,PP,df_dx_func,Q,f_func,eye(6),[dt l],1);
% df_dx_func(MM,[dt l])*M

plot(MM(1,:),MM(2,:),'k','linewidth',2)

plot(SM1(1,:),SM1(2,:),'color',[51 102 204]/255,'linewidth',2)

set(gcf,'position',[1000 50 900 900])
set(gca, 'LooseInset', get(gca,'TightInset'))

figure('position',[1000 50 900 900])
set(gca, 'LooseInset', get(gca,'TightInset'))
plot(MM(3,:),'-*r')
hold on
plot(X(3,:))

plot(SM1(3,:),'color',[51 102 204]/255,'linewidth',2)

title('velocity')

figure('position',[1000 50 900 900])
set(gca, 'LooseInset', get(gca,'TightInset'))
plot(MM(4,:),'-*r')
hold on
plot(X(4,:))
plot(SM1(4,:),'color',[51 102 204]/255,'linewidth',2)
title('theta')

figure('position',[1000 50 900 900])
set(gca, 'LooseInset', get(gca,'TightInset'))

plot(MM(5,:),'-*r')
hold on
plot(X(5,:))
plot(SM1(5,:),'color',[51 102 204]/255,'linewidth',2)
title('phi')


figure('position',[1000 50 900 900])
set(gca, 'LooseInset', get(gca,'TightInset'))

plot(MM(6,:),'-*r')
hold on
plot(SM1(6,:),'color',[51 102 204]/255,'linewidth',2)
plot(X(6,:))

for k=1:size(sig,2)
    if sig(6,k)
        plot(k,MM(6,k),'*k')
    end
        
end

title('dphi')

% figure('position',[1000 50 900 900])
% set(gca, 'LooseInset', get(gca,'TightInset'))
% 
% for k=1:size(PP,3)
%     tra(k)=trace(PP(:,:,k));
% end
% 
% plot(tra,'k')
% 
% title('trace')

figure('position',[1000 50 900 900])
set(gca, 'LooseInset', get(gca,'TightInset'))

plot(abs(X(1,:)-MM(1,:)),'k')
hold on
plot(abs(X(2,:)-MM(2,:)),'r')
plot(hypot(X(1,:)-MM(1,:),X(2,:)-MM(2,:)),'b')

% E=[X(1,:)-MM(1,:);X(2,:)-MM(2,:)]



title('residual')

%% Geometric/Observation model

clear all;close all;clc
cd '~/Dropbox/Nonholonomic generic/model'
addpath ekfukf
path(path,genpath(pwd));%Get all derivated folders to path

% Measurement model and it's derivative
f_func = @ekf_nonholonomic_f;
df_dx_func = @ekf_nonholonomic_df_dx;
h_func = @ekf_nonholonomic_h;
% dh_dx_func = @ekf_sine_dh_dx;
% d2h_dx2_func = @ekf_sine_d2h_dx2;
% Estimate with EKF

N=100;
dt=1/50;
l=2.5;

fc=[1 0 0];
ec=[0 0 1];

X=zeros(6,N);
X(:,1)=[0 10 20 90*pi/180 -10*pi/180 -5*pi/180]';

for k=1:N-1
    X(:,k+1)=f_func(X(:,k),[dt l]);
end

L=3.2;
W=2;
l=2.5;
bp=0.3;

%Full
model=[-bp L-bp L-bp -bp -bp 
       -W/2 -W/2 W/2 W/2 -W/2];

%Bottom and left
% model=[ L-bp -bp -bp
%        W/2 W/2 -W/2];

%Left
% model=[ L-bp -bp
%        W/2 W/2 ];

%Bottom
% model=[ -bp -bp
%        -W/2 W/2 ];
   
%Right
% model=[ -bp L-bp
%        -W/2 -W/2 ];
   
%Top
% model=[ L-bp L-bp
%         W/2 -W/2 ];

%Top and right
% model=[ L-bp L-bp -bp
%         W/2 -W/2  -W/2];

    
d=sqrt(diff(model(1,:)).*diff(model(1,:)) + diff(model(2,:)).*diff(model(2,:)));
s=[0 cumsum(d)];

x=model(1,:);
y=model(2,:);

is=linspace(0,s(end),100);

xn=interp1(s,x,is,'linear');
yn=interp1(s,y,is,'linear');

model=[xn(1:end-1);yn(1:end-1)];

% model=model+randn(size(model))*0.05;
% model=[model [0 0]'];

f(2)=figure('position',[1000 50 900 900]);
set(gca, 'LooseInset', get(gca,'TightInset'))
hold on;grid on

f(1)=figure('position',[1000 50 900 900]);
set(gca, 'LooseInset', get(gca,'TightInset'))
hold on;grid on

% axis([-0.1 12 -1 4])

% plot(s,x,'*')
% plot(s,y,'*r')

% plot(is,xn,'-')
% plot(is,yn,'r-')

% axis([-5 30 -5 30])
grid on

sc=fc*(1-1/N)+ec*(1/N);
% patch(md(1,:),md(2,:),sc)
% plot(md(1,:),md(2,:),'*k')

PTS=zeros(2,size(model,2),N);

M=[0 0 0 0 0 0]';

% P=eye(6)*1e1;

P=[ 1e-2 0 0 0 0 0
    0 1e-2 0 0 0 0
    0 0 1e1 0 0 0
    0 0 0 1e1 0 0
    0 0 0 0 1e1 0
    0 0 0 0 0 1e2]*1e1;

Q=[ 1e1 0 0 0 0 0        %X
    0 1e1 0 0 0 0        %Y
    0 0 1e-2 0  0 0         %V
    0 0 0 1e-5 0 0        %T
    0 0 0 0 1e-4 0        %F
    0 0 0 0 0 1e-3]*1e-4; %dF

H= [0 0 0
    0 0 0
    1 0 0
    0 1 0
    0 0 1
    0 0 0]';


R=eye(3)*1e-1;

MM=zeros(size(M,1),size(X,2));
PP=zeros(size(M,1),size(M,1),size(X,2));

MM(:,1)=M;
PP(:,:,1)=P;

AT=zeros(4,4,size(X,2));

md=applyTransform(model,[X(1,1) X(2,1) X(4,1)]);
PTS(:,:,1)=md;

% break
pn=1;
pe=md(:,pn);
% pe=[X(1,1) X(2,1)]';
ccs=zeros(2,N);

mtheta=zeros(1,N);
avgtheta=zeros(1,N);

for k=2:N
    
    disp(['k: ' num2str(k)])
    
    figure(f(1))
    clf
    
    set(gca, 'LooseInset', get(gca,'TightInset'))
    hold on
    axis([-10 40 -10 40])
    grid on
    format long
    t=[X(1,k) X(2,k) X(4,k)];
    
    AT(:,:,k)=makeTransformMatrix(t);
    
    md=applyTransform(model,t);
    
    PTS(:,:,k)=md;
    
    %Generate observavel vehicle points
    pc=observabilityFilter(md,[0 0],70*pi/180);
    
%     for h=1:size(pc,2)
%         text(pc(1,h),pc(2,h),num2str(h))
%     end
    
%     plot(md(1,:),md(2,:),'*')
%     plot(pc(1,:),pc(2,:),'*r')
    
%     continue
%     break
    
%     pc=PTS(:,:,k);
    pp=PTS(:,:,k-1);
    
    pp=observabilityFilter(pp,[0 0],70*pi/180);
    
%     plot(pc(1,:),pc(2,:),'o','markersize',4,'color',[1 0 0],'MarkerFaceColor',[1 0 0])
%     plot(pp(1,:),pp(2,:),'*','markersize',4,'color',[0 0 1],'MarkerFaceColor',[0 0 1])
    
    plot(X(1,:),X(2,:))
    
    pc=dpsimplify(pc',0.5)';
    
    points=pc;
    
    d=sqrt(diff(points(1,:)).*diff(points(1,:)) + diff(points(2,:)).*diff(points(2,:)));
    s=[0 cumsum(d)];

    x=points(1,:);
    y=points(2,:);

    is=linspace(0,s(end),round(s(end)/0.1));

    xn=interp1(s,x,is,'linear');
    yn=interp1(s,y,is,'linear');
    
    pc=[xn;yn];
    
    pp=dpsimplify(pp',0.5)';
     
    points=pp;
    
    d=sqrt(diff(points(1,:)).*diff(points(1,:)) + diff(points(2,:)).*diff(points(2,:)));
    s=[0 cumsum(d)];

    x=points(1,:);
    y=points(2,:);

    is=linspace(0,s(end),round(s(end)/0.1));

    xn=interp1(s,x,is,'linear');
    yn=interp1(s,y,is,'linear');
    
    pp=[xn;yn];
    
    tf=MbICP_generic(pc,pp, [0,0,0], false,4);
    
%     AT(:,k)=ttas;
%     ATs=ATs+ttas
    
%     dx=tf(1);
%     dy=tf(2);
    
%     dtheta=tf(3);
%     dthetaReal=X(4,k)-X(4,k-1);

%     phi=
%     phiReal=X(5);
%     thetaReal=X(4,k);
    
   
%     Yp=[tf(1) tf(2) tf(3)]'
%     xp=[X(1,k)-X(1,k-1) X(2,k)-X(2,k-1) X(4,k)-X(4,k-1)]'
    
    
%     dxr=tf(1)*cos(X(4,k-1))-tf(2)*sin(X(4,k-1));
%     dyr=tf(1)*sin(X(4,k-1))+tf(2)*cos(X(4,k-1));
    
%     ctf=[dxr dyr tf(3)]';
%     ctf=tf;
%     hf=[dxr-xp(1); dyr-xp(2)]
    
%     hf=[cos(X(4,k-1)) -sin(X(4,k-1));sin(X(4,k-1)) cos(X(4,k-1))]*tf(1:2)
%     df=norm(Xp(1:2)-hf)
    
%     xf=[cos(X(4,k)-X(4,k-1)) -sin(X(4,k)-X(4,k-1));sin(X(4,k)-X(4,k-1)) cos(X(4,k)-X(4,k-1))]*Xp(1:2)
    
%     dtheta=X(4,k)
    
%     trf= [cos(dtheta) -sin(dtheta); sin(dtheta) cos(dtheta)] *Xp(1:2)
    
%     tf=[0.1958 0.0215 0.0137]
%     tf(2)=0.0215
%     Y=createMeasuremet(ctf(1),ctf(2),ctf(3),[dt l])';
%     Xr=[X(3,k) X(4,k) X(5,k)]';
    
%     [M,P] = ekf_predict1(M,P,df_dx_func,Q,f_func(M,[dt l]),eye(6),[dt l]);
%     [M,P] = ekf_update1(M,P,Y,H,R,H*M,eye(3),[dt l]);
    
%     MM(:,k)   = M;
%     PP(:,:,k) = P;
%     ptrans=[0.3672; 0; 0.0539];
    pt=applyTransform(pp,tf);
%     plot(pt(1,:),pt(2,:),'o','markersize',4,'color',[0 1 0],'MarkerFaceColor',[0 1 0])
  
%     plot([pp(1,:); pc(1,:)],[pp(2,:); pc(2,:)])

    [v,rk,~, phi, pk3 ,cc]=estimation(pp,pt,dt,tf);
    ccs(:,k)=cc;
%     vapx=sqrt( (X(1,k)-X(1,k-1))*(X(1,k)-X(1,k-1)) + (X(2,k)-X(2,k-1))*(X(2,k)-X(2,k-1)) )/dt;
    
%     vreal=(X(1,k)-X(1,k-1))/(cos(X(4,k-1))*cos(X(5,k-1))*dt);
    
    avgv=sum(v)/length(v);
%     avgv=v(1);
    
    avgphi=asin((tf(3)*l)/(avgv*dt));
    
%     r=l/tan(avgphi)
    
%     for g=1:5
%         avgv=avgv/sqrt(cos(avgphi));
%         avgphi=asin((tf(3)*l)/(avgv*dt));
%     end
    
    avgv;
    realv=X(3,k);
%     avgr=sum(rk)/length(rk);

    avgphi;
    realphi=X(5,k-1);
    thetaReal=X(4,k);
    
    avgp=[sum(pt(1,:))/size(pt,2);sum(pt(2,:))/size(pt,2)];
    
    if thetaReal>pi
        thetaReal=thetaReal-2*pi;
    end
    
    if thetaReal<-pi
        thetaReal=thetaReal+2*pi;
    end
    
    text(avgp(1),avgp(2),['thetaReal: ' num2str(thetaReal)])
    
    
    ashape=atan2(diff(pc(2,:)),diff(pc(1,:)));
    ind=kmeans(ashape,2,'emptyaction','singleton');
    
    a=zeros(4,1);
    a(1)=sum(ashape(ind==1))/length(ashape(ind==1));
    a(2)=sum(ashape(ind==2))/length(ashape(ind==2));
    
    if a(1)>0
        a(3)=a(1)-pi;
    else
        a(3)=a(1)+pi;
    end
    
    if a(2)>0
        a(4)=a(2)-pi;
    else
        a(4)=a(2)+pi;
    end
    
    avel=atan2(pt(2,:)-pp(2,:),pt(1,:)-pp(1,:));
    
    avel=meanangle(avel*180/pi)*pi/180;
    
    avgtheta(k)=avel;
    
    text(avgp(1),avgp(2)-0.6,['avl: ' num2str(avel)])
    
%     aerr=a-avel;
%     if a-avel > pi/2 - 10*pi/180
%         a=a-pi;
%     elseif a-avel < pi/2 - 10*pi/180
%         a=a+pi;
%     end
   
    plot(cc(1),cc(2),'r*')%Curvature center
%     plot(pk3(1,:),pk3(2,:),'*','markersize',6,'color',[0 1 1],'MarkerFaceColor',[0 1 1])
    

    points=pc;
    
    plot(points(1,:),points(2,:),'r')
    
    d=sqrt(diff(points(1,:)).*diff(points(1,:)) + diff(points(2,:)).*diff(points(2,:)));
    s=[0 cumsum(d)];

    x=points(1,:);
    y=points(2,:);

    is=linspace(0,s(end),round(s(end)/0.1));

    xn=interp1(s,x,is,'linear');
    yn=interp1(s,y,is,'linear');
    
    ipoints=[xn;yn];
    
    acenter=atan((cc(2)-ipoints(2,:))./(cc(1)-ipoints(1,:)));
    
    adiff=abs(abs(repmat(a,1,size(acenter,2))-repmat(acenter,4,1))-pi/2);
    val=min(min(adiff));
    
    if val<1e-2
        [r,c]=find(adiff==val);
    
        r=unique(r);
        
        if length(r)==2
            thetaPossible=a(r);
        elseif length(r)==1
            if r<3
                thetaPossible=[a(r);a(r+2)];
            else
                thetaPossible=[a(r);a(r-2)];
            end
        else
            thetaPossible=nan;
        end
        
        
        theta=thetaPossible( abs(thetaPossible-avel)<pi/2);
        if isempty(theta)
            theta=nan;
        end
        
        
        text(avgp(1),avgp(2)-1.2,['theta: ' num2str(theta)])
    else
        text(avgp(1),avgp(2)-1.2,['theta: error too large: ' num2str(val)])
        theta=nan;
    end
    
    
    mtheta(k)=theta;

    plot(ccs(1,2:k),ccs(2,2:k),'color',[0.5 0.5 0])
    
    %Geometric interpretation to allow a better under
    
    figure(f(2))
    clf
    axis([-3 5 -3 5])
    hold on
    set(gca, 'LooseInset', get(gca,'TightInset'))
    
    p=ipoints;
    
    if ~isnan(theta)
        orientation=theta;
    else
        orientation=avel;
    end
    
%     orientation=avel;
    
    p=applyTransform(p,[0 0 -orientation]);
    
   
    %Get preliminar vehicle limits and correct
    
    bl=min(p(1,:));
    p(1,:)=p(1,:)-bl;
    
    rl=min(p(2,:));
    p(2,:)=p(2,:)-rl;
    
    %Get corrected vehicle limits
    
    fl=max(p(1,:));
    ll=max(p(2,:));
    bl=min(p(1,:));
    rl=min(p(2,:));
    
    un=0.3;
    
    fc=[1 0 0];
    lc=[0 1 0];
    bc=[0 0 1];
    rc=[1 .5 0];
    
    square=[0 .2 .2 0; 0 0 .2 .2];
    
    sq=applyTransform(square,[-2.5 4.5 0]);
    patch(sq(1,:),sq(2,:),fc)
    text(mean(sq(1,:))+.2,mean(sq(2,:)),'Front')
    
    sq=applyTransform(sq,[0 -.3 0]);
    patch(sq(1,:),sq(2,:),lc)
    text(mean(sq(1,:))+.2,mean(sq(2,:)),'Left')
    
    sq=applyTransform(sq,[0 -.3 0]);
    patch(sq(1,:),sq(2,:),bc)
    text(mean(sq(1,:))+.2,mean(sq(2,:)),'Back')
    
    sq=applyTransform(sq,[0 -.3 0]);
    patch(sq(1,:),sq(2,:),rc)
    text(mean(sq(1,:))+.2,mean(sq(2,:)),'Right')
    
%     patch([-3 -2.5 -2.5 -3],[5 5 4.5 4.5],fc)
%     patch([-3 -2.5 -2.5 -3],[5 5 4.5 4.5],fc)
%     patch([-3 -2.5 -2.5 -3],[5 5 4.5 4.5],fc)
    
    fz=[fl-un  fl+un fl+un fl-un
        ll+un ll+un rl-un rl-un];
    
    lz=[bl-un fl+un fl+un bl-un
        ll+un ll+un ll-un ll-un];
    
    bz=[bl-un bl+un bl+un bl-un
        ll+un ll+un rl-un rl-un];
    
    rz=[bl-un fl+un fl+un bl-un
        rl+un rl+un rl-un rl-un];
    
    patch(fz(1,:),fz(2,:),fc,'facealpha',0.2);
    patch(lz(1,:),lz(2,:),lc,'facealpha',0.2)
    patch(bz(1,:),bz(2,:),bc,'facealpha',0.2)
    patch(rz(1,:),rz(2,:),rc,'facealpha',0.2)
    
    plot([bl fl fl bl bl],[ll ll rl rl ll],'k','linewidth',2)
    
    n=LineNormals2D(p')';
    
    reverse=1;
    plot([p(1,:); p(1,:)+reverse*n(1,:)],[p(2,:); p(2,:)+reverse*n(2,:)])
    
    anp=atan2(reverse*n(2,:),reverse*n(1,:));
    
%     anp(anp<0)=anp(anp<0)+2*pi;
%     anp(anp>pi)=anp(anp>pi)-pi;
    
    af=0;
    al=pi/2;
    ab=pi;
    ar=-pi/2;
    
    au=25*pi/180;
    
    FRONT=1;
    LEFT=2;
    RIGHT=3;
    BACK=4;
    
    classification=zeros(1,size(p,2));
    
    for h=1:size(p,2)
        
%         text(p(1,h)+0.5,p(2,h)-0.5,num2str(anp(h)))
        %Check if front point
        if p(1,h)>fl-un && p(1,h)<fl+un
            
            if angleAbsDiff(anp(h),af)<au
                classification(h)=FRONT;
            end
        end
        
        %Check if left point
        if p(2,h)>ll-un && p(2,h)<ll+un
            if angleAbsDiff(anp(h),al)<au
                classification(h)=LEFT;
            end
        end
        
        %Check if right point
        if p(2,h)>rl-un && p(2,h)<rl+un
            if angleAbsDiff(anp(h),ar)<au
                classification(h)=RIGHT;
            end
        end
        
        %Check if back point
        if p(1,h)>bl-un && p(1,h)<bl+un
            if angleAbsDiff(anp(h),ab)<au
                classification(h)=BACK;
            end
        end
        
        switch classification(h)
            case FRONT
                plot(p(1,h),p(2,h),'o','markersize',8,'color',fc,'markerfacecolor',fc)
                
            case LEFT
                plot(p(1,h),p(2,h),'o','markersize',8,'color',lc,'markerfacecolor',lc)
                
            case RIGHT
                plot(p(1,h),p(2,h),'o','markersize',8,'color',rc,'markerfacecolor',rc)
            case BACK
                plot(p(1,h),p(2,h),'o','markersize',8,'color',bc,'markerfacecolor',bc) 
                
            case 0
                plot(p(1,h),p(2,h),'o','markersize',8,'color',[0 0 0]) 
        end
    end
    
    
    
    
    
%     text(avgp(1),avgp(2)-1.2,['avl: ' num2str(avel)])
%     text(avgp(1),avgp(2)-1.8,['a2: ' num2str(a(2))])
    
    
%     acenter=atan((cc(2)-ipoints(2,:))./(cc(1)-ipoints(1,:)));
%     acenter(end)=[];
    
%     ashape=atan(diff(ipoints(2,:))./diff(ipoints(1,:)));
    
%     adiff=abs(abs(acenter-ashape)-pi/2);
    
%     [val, indx]=min(adiff);
    
%     plot(ipoints(1,:),ipoints(2,:),'k')
%     plot(ipoints(1,indx),ipoints(2,indx),'*r')
%     v=cc-ipoints(:,indx);
%     v=v/norm(v);
    
%     plot([cc(1) ipoints(1,indx) ipoints(1,indx)-v(1)*5],[cc(2) ipoints(2,indx) ipoints(2,indx)-v(2)*5],'r')
    
    
%     theta=ashape(indx)
%     thetaReal
    
%     thetaError=abs(theta-thetaReal)*180/pi
    
%     if val<1e-3
%         
%         plot(ipoints(1,indx),ipoints(2,indx),'*r')
%         text(ipoints(1,indx),ipoints(2,indx)-1,['min: ' num2str(val)])
%     end
    
    
%     for h=2:size(ipoints,2)
%         lacenter=atan((cc(2)-ipoints(2,h))/(cc(1)-ipoints(1,h)));
%         lashape=atan((ipoints(2,h)-ipoints(2,h-1))/(ipoints(1,h)-ipoints(1,h-1)));
%         
%         ladiff=abs(abs(lacenter-lashape)-pi/2);
%         
%         text(ipoints(1,h),ipoints(2,h),['lad: ' num2str(ladiff) ' ad: ' num2str(adiff(h))]);
%     end
    

%     plot([pc(1,:)' pc(1,:)'-1*normals(1,:)'],[pc(2,:)' pc(2,:)'+normals(2,:)'])
%     plot([pc pc+normals])
%     mp=sum(pc,2)/length(pc);
%     plot(mp(1),mp(2),'o','markersize',10,'color',[1 0.6 0],'MarkerFaceColor',[1 0.6 0])
    
%     for h=1:size(pk3,2)
%         text(pk3(1,h),pk3(2,h),num2str(v(h)))
%         text(pk3(1,h),pk3(2,h)-0.1,num2str(theta(h)*180/pi))
%     end


    pause(0.5)
end

figure('position',[1000 50 900 900]);
set(gca, 'LooseInset', get(gca,'TightInset'))
hold on;grid on

plot(mtheta*180/pi,'-ro','markersize',6,'markerfacecolor',[1 0 0]);

rt=X(4,:);
rt(rt>pi)=rt(rt>pi)-2*pi;
rt(rt<-pi)=rt(rt<-pi)+2*pi;

plot(rt*180/pi,'-o','markersize',4,'markerfacecolor',[0 0 1],'color',[0 0 1]);

plot(avgtheta*180/pi,'-o','markersize',4,'markerfacecolor',[0 0 0],'color',[0 0 0]);

figure('position',[1000 50 900 900]);
set(gca, 'LooseInset', get(gca,'TightInset'))
hold on;grid on
plot((rt-avgtheta)*180/pi,'k')


% fig=figure('position',[1000 50 900 900]);
% set(gca, 'LooseInset', get(gca,'TightInset'))
% hold on;grid on


%%

% dx=cos(t)*cos(f)*v*dt
% dy=sin(t)*cos(f)*v*dt
% dt=sin(f)*v*dt/l
clear all;close all;clc

syms theta phi v dt l dx dy dtheta

% f1=cos(theta)*cos(phi)*v*dt-dx
% f2=sin(theta)*cos(phi)*v*dt-dy
% f3=sin(phi)*v*dt/l-dtheta
% 
% s=solve(f1,f2,f3,theta,phi,v)

% f_func = @func1;

% theta = 0.0342 phi 0.1745 v=10

dt=0.05;
l=2.54;
dx=0.4901;
dy=.1030;
dtheta=-0.0252;

optimset('LargeScale','off','display','off');
fmincon(@func1,[0 0 0],[],[],[],[],[0 0 0],[pi/2 pi/2 20],[],[],dt,dx,dy,dtheta,l)

%%
clear all;close all;clc
cd ~/Desktop/model

syms dx phi dtheta l dt dy v theta


f1=cos(theta)*cos(phi)*v*dt-dx*cos(theta)+dy*sin(theta)
f2=sin(theta)*cos(phi)*v*dt-dx*sin(theta)-dy*cos(theta)
f3=sin(phi)*v*dt/l-dtheta

s=solve(f1,f2,f3,phi,v,theta)


f1=sin(acos(dx*tan(phi)/(dtheta*l)))*dtheta*l/tan(phi)-dy;
f2=dtheta*l/(sin(phi)*dt)-v;
f3=acos(dx*tan(phi)/(dtheta*l))-theta;
s=solve(f1,f2,f3,phi,v,theta);

v1=(sqrt(dtheta*dtheta*l*l/(dx*dx+dy*dy)+1)*sqrt(dx*dx+dy*dy))/dt;
v2=-(sqrt(dtheta*dtheta*l*l/(dx*dx+dy*dy)+1)*sqrt(dx*dx+dy*dy))/dt;

theta1=acos(dx/(sqrt(dx*dx+dy*dy)));
theta2=pi+acos(dx/(sqrt(dx*dx+dy*dy)));

phi1=atan(dtheta*l/(sqrt(dx*dx+dy*dy)));
phi2=-atan(dtheta*l/(sqrt(dx*dx+dy*dy)));

dt=1/50;
l=2.54;
dx= 0.196060085342831;
dy= 0.018822734952181;
dtheta=0.013673084855664;

%theta 0.109384678845310  0.095711593989644
%phi 0.174532925199433  0.174532925199435
%v 10 10

disp('v')
subs(v1)
subs(v2)

disp('theta')
subs(theta1)
subs(theta2)

disp('phi')
subs(phi1)
subs(phi2)

%%

clear all;close all;clc

cd ~/Desktop/model

syms dx phi dtheta l dt dy v theta

% f1 = cos(theta)*cos(theta)*v*dt-tx*cos(theta)+ty*sin(theta);
% f2 = sin(theta)*cos(theta)*v*dt-tx*sin(theta)-ty*cos(theta);
% f3 = sin(phi)*v*dt/l - ttheta;

f1=(sqrt(dtheta*dtheta*l*l/(dx*dx+dy*dy)+1)*sqrt(dx*dx+dy*dy))/dt -v
f2=atan(dtheta*l/(sqrt(dx*dx+dy*dy)))-phi
f3=dx*sin(theta)+dy*cos(theta)-sin(theta)*cos(phi)*v*dt

% f1=(dx - dy*tan(theta))/(cos(phi)*dt) - v
% f2=atan(dtheta*l/(sqrt(dx*dx+dy*dy)))-phi
% f3=dtheta*l/tan(phi)-dx-dy/tan(theta)

s=solve(f1,f2,f3,v,theta,phi)

%% Morais

clear all;close all;clc


p=[188 360; 1 74];

alpha=atan2(p(1,1)-p(1,2),p(2,2)-p(2,1));
r=p(1,1) *cos(alpha)+p(2,1)*sin(alpha);

plot(p(1,:),p(2,:)); hold on;axis square
axis([0 360 -200 200])

plot([0 r*cos(alpha)],[0 r*sin(alpha)],'r')
r=45;
alpha=0.977;
plot([0 r*cos(alpha)],[0 r*sin(alpha)],'r')


%% Circle geometry
clear all;close all;clc

fig=figure('position',[1000 50 900 900]);
set(gca, 'LooseInset', get(gca,'TightInset'))
hold on;grid on

axis([-1 15 -1 15])

arrow=[0 1
       0 0]*0.2;

% dx1=0.3982
% dy1=0.1633

% dx2=0.2313
% dy2=0.2741


x1i=7.373599437357463
y1i=4.867805573539854

x1f=7.740343387177802
y1f=5.093065095475790

x2i=8.995787078172736
y2i=8.213740336239994

x2f=9.180048828996059
y2f=8.521478783912174


dt=1/50;
dtheta=0.053861439893805
l=2.54

% arrowMod=applyTransform(arrow,[dx dy dtheta]);


plot([x1i x1f],[y1i y1f],'*r-')
plot([x2i x2f],[y2i y2f],'*b-')

text(x1i,y1i-0.2,'1i')
text(x2i,y2i-0.2,'2i')

v1=[x1f-x1i;y1f-y1i];
v1=v1/norm(v1);

md1=[x1i+x1f;y1i+y1f]/2;

v1i=[-v1(2); 1*v1(1)]

v2=[x2f-x2i;y2f-y2i];
v2=v2/norm(v2);

md2=[x2i+x2f;y2i+y2f]/2;

% plot(md2(1,:),md2(2,:),'*k')

v2i=[-v2(2); 1*v2(1)]

% plot([md1(1,:) md1(1,:)+v1i(1,:)],[md1(2,:) md1(2,:)+v1i(2,:)],'b')
% plot([md2(1,:) md2(1,:)+v2i(1,:)],[md2(2,:) md2(2,:)+v2i(2,:)],'b')

md1x=md1(1,1)
md1y=md1(2,1)

md2x=md2(1,1)
md2y=md2(2,1)

v1ix=v1i(1,1)
v1iy=v1i(2,1)

v2ix=v2i(1,1)
v2iy=v2i(2,1)

k1=-(md1x*v2iy - md1y*v2ix - md2x*v2iy + md2y*v2ix)/(v1ix*v2iy - v1iy*v2ix)
k2=-(md1x*v1iy - md1y*v1ix - md2x*v1iy + md2y*v1ix)/(v1ix*v2iy - v1iy*v2ix)


plot([md1x md1x+v1ix*k1],[md1y md1y+v1iy*k1],'b')
plot([md2x md2x+v2ix*k2],[md2y md2y+v2iy*k2],'b')

cc=[md1x+v1ix*k1; md1y+v1iy*k1]

plot(cc(1),cc(2),'r*')

r1=sqrt( (cc(1)-x1i)^2 + (cc(2)-y1i)^2 )
da1=abs(atan2(cc(2)-y1i,cc(1)-x1i)-atan2(cc(2)-y1f,cc(1)-x1f))
s=r1*da1
vel1=s/dt

r2=sqrt( (cc(1)-x2i)^2 + (cc(2)-y2i)^2 )
da2=abs(atan2(cc(2)-y2i,cc(1)-x2i)-atan2(cc(2)-y2f,cc(1)-x2f))
s=r2*da2
vel2=s/dt

phi1=asin(dtheta*l/(vel1*dt))
phi2=asin(dtheta*l/(vel2*dt))
theta=atan2(y2f-y2i,x2f-x2i)

phiReal=0.349065850398866
thetaReal=0.261799387799149

x1p=cc(1)+r1*cos(atan2(y1f-cc(2),x1f-cc(1))+da1)
y1p=cc(2)+r1*sin(atan2(y1f-cc(2),x1f-cc(1))+da1)

plot(x1p,y1p,'*k')

x2p=cc(1)+r2*cos(atan2(y2f-cc(2),x2f-cc(1))+da2)
y2p=cc(2)+r2*sin(atan2(y2f-cc(2),x2f-cc(1))+da2)

plot(x2p,y2p,'*k')

dthetaP=dtheta; %Next dtheta



% plot(dx,dy,'*r')
% plot(arrowMod(1,:),arrowMod(2,:),'r')

%%

clear all;close all;clc

syms md1x md1y k1 k2 md2x md2y v1ix v1iy v2ix v2iy

f1=md2x+k2*v2ix-md1x-k1*v1ix
f2=md2y+k2*v2iy-md1y-k1*v1iy

s=solve(f1,f2,k1,k2)

%% 

clear all;close all;clc

syms ccx ccy y1 y2 x1 x2 dt t1 t2 r

% f1=atan((ccy-y2)/(ccx-x2))-atan((ccy-y1)/(ccx-x1)) - dt
% f2=ccx+r*cos(t1)-x1
% f3=ccx+r*cos(t2)-x2
% f4=ccy+r*sin(t1)-y1
% f5=ccy+r*sin(t2)-y2

% s=solve(f1,f2,f3,f4,f5,ccx,ccy,t1,t2)

p1=[2;2];
p2=[2.3;4];

fig=figure('position',[1000 50 900 900]);
set(gca, 'LooseInset', get(gca,'TightInset'))
hold on;grid on

axis([-3 8 -3 8])

plot(p1(1,:),p1(2,:),'*')
plot(p2(1,:),p2(2,:),'*r')

alpha=30*pi/180;
[cc, r]=centerFromPointsAndAlpha(p1,p2,alpha);

ccx=cc(1);
ccy=cc(2);

plot(ccx,ccy,'*r')

plot(ccx+r*cos(linspace(0,2*pi,100)),ccy+r*sin(linspace(0,2*pi,100)))

% syms v1x v1y v2x v2y x1 x2 y1 y2 k1 k2
% 
% f1=x2+v2x*k2-x1-v1x*k1
% f2=y2+v2y*k2-y1-v1y*k1
% 
% s=solve(f1,f2,k1,k2)