%20170105
%
%  interp1 has an issue -- returns Nan.  Added a hack, but not sure its ok
%
%  Interesting:  if symmetric Rberg has "lobes", it can converge on any of them
%  and some have wrong b and psiberg --- but are those unique?  observable?
%
%  Strange numerical things when w=0 (or nan?), if don't resample,
%  eventually, all particles "disappear" and get nana.  Setting w=0.0001 
%  wpart=w^n-->0, and problem still occurs.
%  Maybe:  all particles become equally unbelievable --> garbage results
%  Conclusion#1:  resampling is important
%
%  Testing--> all particle weights-->0  --> 1/sum(weight)--nan
%  Q:  if a particle weight ever goes to zero, can it recover before a
%  resampling?
%  !!!   to avoid divide by zero, added wpart(j)=max(wpart(j),0.000001);  IS THIS OK?



if(~exist('position'))
  position = get(0,'screensize');
  npposition(1:2) = position(1:2) + round([position(3)*.02 position(4)*.04]);
  npposition(3:4) = round([position(3)*.95 position(4)*.90]);
  position = npposition;
end
fontsize = 20;


V=1.5;  % m
Ro=100;  % m

dt=1;
t=-dt;
psilocation=0;
psiauv=psilocation+pi/2;
Nauv=Ro*cos(psilocation);
Eauv=Ro*sin(psilocation);
Nauvbc=Nauv;
Eauvbc=Eauv;

npts=500;
tout=zeros(1,npts);
Nauvout=zeros(1,npts);
Eauvout=zeros(1,npts);
Nauvoutbc=zeros(1,npts);
Eauvoutbc=zeros(1,npts);
rangeout=zeros(1,npts);
rbergout=zeros(1,npts);
rauvout=zeros(1,npts);
psibergout=zeros(1,npts);
psiauvlocout=zeros(1,npts);
Vxout=zeros(1,npts);
Vyout=zeros(1,npts);
xout=zeros(1,npts);
yout=zeros(1,npts);
psipartout=zeros(1,npts);
iresample=zeros(1,npts);
xhatPFout=zeros(1,npts);
sigmaPFout=zeros(1,npts);
thetatruthout=zeros(1,npts);
thetahatout=zeros(1,npts);
resampleout=zeros(1,npts);

nparticles=100
nstates=4;
thetapartout=zeros(npts,nparticles);
wpartout=zeros(npts,nparticles);
xhatPFout=zeros(npts,nstates);



psib=0;
psidotb=-(1.5/100)*.1;  %about 10percent of AUV velocity
%define berg
thetaberg=-pi:.01:pi;
nberg=max(size(thetaberg));
nlobes=3;
Rberg=75*ones(size(thetaberg))+10*sin(nlobes*thetaberg);


%IC for AUV
Nauv=Ro;
Eauv=0;



for i=1:npts
    t=t+dt;
    tout(i)=t;
    
    %Berg Motion
    psib=minuspi2pi(psib+psidotb*dt);
    psibergout(i)=psib;
 
    %Vehicle fly a circular orbit
    R=Ro;  %constant radius;
    psiauv=minuspi2pi(pi/2+atan2(Eauv,Nauv));
    
    Nvel=V*cos(psiauv);
    Evel=V*sin(psiauv);

    
    % make a range measurment  (radial distance auv to berg)
    psiauvloc=minuspi2pi(atan2(Eauv,Nauv));
    psiauvlocout(i)=psiauvloc;
    psiwrtb=minuspi2pi(angle1minus2(psiauvloc,psib));
    rberg=interp1(thetaberg,Rberg,psiwrtb,'nearest');   % a bit of weirdness at zero crossing
    if isnan(rberg),
        psiwrtb
        thetagap=pi;
        rberg=75*ones(size(thetagap))+10*sin(nlobes*thetagap); 
    end
    rauv=sqrt(Nauv^2+Eauv^2);
    range=rauv-rberg;
    rangeout(i)=range;
    rbergout(i)=rberg;
    rauvout(i)=rauv;
        
    % DVL reading
    % velocity of AUV = Nvel, Evel
    % velocity of berg = velocity of point on radial line under AUV
    Vberg=rberg*psidotb;
    Ebergvel=Vberg*cos(psiauvloc);   %psiauvloc goe through point on berg's surface
    Nbergvel=-Vberg*sin(psiauvloc);
    dVN=Nvel-Nbergvel;
    dVE=Evel-Ebergvel;
    Vx=dVN*cos(psiauv)+dVE*sin(psiauv);
    Vy=-dVN*sin(psiauv)+dVE*cos(psiauv);
    Vxout(i)=Vx;
    Vyout(i)=Vy;
    
    
    if i==1   % initalize the PF
        %  [x y psiberg psidotberg(bias)]  x and y are in berg frame (NED)
        % set up init cloud
        xparticles=zeros(nparticles,4); % North East bias
        rangeofparts=180; %+/1 range in degrees
        thetap=(rangeofparts*pi/180)*2*(rand(1,nparticles)-0.5*ones(1,nparticles)); 
        for j=1:nparticles
            xic=Ro*cos(thetap(j));   %  xic,yic assumed to be at radius Ro, if not true, need more points in initcloud
            xparticles(j,1)=xic;
            yic=Ro*sin(thetap(j));
            xparticles(j,2)=yic;
            xparticles(j,3)=minuspi2pi(psiauv-atan2(yic,xic)-pi/2);  % if auv is at (xic,yic) with psiauv AND flying perp to bergs radius, what would bergs psi be?  (need bigger cloud if conditions not true)
            xparticles(j,4)=0;  % b=psidot of berg
        end
        wpart=ones(1,nparticles)/nparticles;  % initialize weights
        sigmaw1=.1; % xdot DVL
        sigmaw2=.1; % ydot DVl
        sigmaw3=2*pi/180; % psiberg_ic
        sigmaw4=5*abs(psidotb); % bias on psidot
        sigmav=1;  % range     
    end
 
    %  have measurements of Vx, Vy, psiauv, range  (Vx and Vy are auv frame DVL measurments) 
    for j=1:nparticles
        % process update
        xpart=xparticles(j,:)';

        w1=sigmaw1*randn;
        w2=sigmaw2*randn;
        w3=sigmaw3*randn;
        w4=sigmaw4*randn;
            psirel=minuspi2pi(psiauv-xpart(3));
            xdot=cos(psirel)*Vx - sin(psirel)*Vy +w1;
            ydot=sin(psirel)*Vx + cos(psirel)*Vy +w2;
            psibergdot= xpart(4) + w3;  %psiid
            bdot=0 + w4;

            xpart(1)=xpart(1)+xdot*dt;  % x
            xpart(2)=xpart(2)+ydot*dt;  % y
            xpart(3)=minuspi2pi(xpart(3)+psibergdot*dt);  % psiberg
            xpart(4)=xpart(4)+bdot*dt;  %  bias
            
            %xout(i)=xpart(1);
            %yout(i)=xpart(2);

        psi_est=minuspi2pi(atan2(xpart(2),xpart(1)));  %ray going through auv's assumed location
        %measurement update  compare with "range"
        rberg_part=interp1(thetaberg,Rberg,psi_est,'nearest');   % a bit of weirdness at zero crossing
            if isnan(rberg_part),
            psi_est
            thetagap=pi;
            rberg_part=75*ones(size(thetagap))+10*sin(1*thetagap);; 
            end
        rauv_part=sqrt(xpart(1)^2+xpart(2)^2);
        range_part=(rauv_part-rberg_part);
        w=(1/sqrt(2*pi*sigmav^2))*exp(-.5*(range_part-range)^2/sigmav^2);
            %if or(isnan(w),w==0)   % not sure this helps
            %    w=0.00001 
            %end
        wpart(j)=wpart(j)*w;
	wpart(j)=max(wpart(j),0.00001);   %  avoids divide by zero  Q: IS THIS OK?
        xparticles(j,:)=xpart';
        
        %%%for plotting
        thetapartout(i,j)=minuspi2pi(atan2(xpart(2),xpart(1)));
         
    end
    wpart=wpart/sum(wpart);
    
        %%% for plotting
        wpartout(i,:)=wpart;
        thetatruthout(i)=minuspi2pi(psiauvloc-psib);
        
        figure(1)
        %plot((180/pi)*(thetapartout(i,:)-thetatruthout(i)),wpartout(i,:),'*')
        plot((180/pi)*minuspi2pi((thetapartout(i,:)-thetatruthout(i))),wpartout(i,:),'*')
        axis([-180 180 0 5/nparticles])
	set(gcf,'position',position)
	set(gca,'fontsize',fontsize);
	set(gcf,'defaultaxesfontsize',fontsize,'defaulttextfontsize',fontsize)
        pause(0.05)
    
    
    % calculate xhat and "sigma"
       for k=1:nstates
       xhatPFout(i,k)=sum(xparticles(:,k).*wpart');
       thetahatout(i)=atan2(xhatPFout(i,2),xhatPFout(i,1));
       end

    Neff(i)=1/sum(wpart.*wpart);
    
    if Neff(i)<.75*nparticles
        iresample(i)=1;
        xparticles=resampLV(xparticles,wpart);
        wpart=ones(1,nparticles)/nparticles;
    end
    

 
    
%  update AUV position
    Nauv=Nauv+Nvel*dt;
    Eauv=Eauv+Evel*dt;
    Nauvout(i)=Nauv;
    Eauvout(i)=Eauv;

end % i=1:npts

xhatPFout


figure(2)
plot(tout,(180/pi)*thetatruthout,tout,(180/pi)*thetahatout,'*')
title('Circumferential direction of AUV in Berg Coords (best est)   b=truth')
ylabel('degrees')
set(gcf,'position',position)
set(gca,'fontsize',fontsize);
set(gcf,'defaultaxesfontsize',fontsize,'defaulttextfontsize',fontsize)

figure(3)
plot(tout,(180/pi)*psibergout,tout,(180/pi)*xhatPFout(:,3))
title('Rotation of Berg  b=truth')
ylabel('degrees')
set(gcf,'position',position)
set(gca,'fontsize',fontsize);
set(gcf,'defaultaxesfontsize',fontsize,'defaulttextfontsize',fontsize)

figure(4)
plot(tout,(180/pi)*psidotb*ones(size(tout)),tout,(180/pi)*xhatPFout(:,4),'*')
title('Roation Rate of Berg b=truth')
ylabel('degrees/sec')
set(gcf,'position',position)
set(gca,'fontsize',fontsize);
set(gcf,'defaultaxesfontsize',fontsize,'defaulttextfontsize',fontsize)


figure(5)
plot(Eauvout,Nauvout,'.')
set(gcf,'position',position)
set(gca,'fontsize',fontsize);
set(gcf,'defaultaxesfontsize',fontsize,'defaulttextfontsize',fontsize)

    
    
