%function [aoldxvi,aoldyvi,xnodevi,voldvi,time,count,Valuesvi] = valitfaccelerada(deltax,ntheta,mmfile)
deltax=0.025;
ntheta=128;
mmfile=0;

x=1;
xnode=-x:deltax:x;
h=1*deltax;
dim=length(xnode);
[X,Y]=ndgrid(xnode);

Vold=ones(dim,dim);

%wt=round(0.1/deltax);
%Vold(round(dim/2)-wt:round(dim/2)+wt,round(dim/2)-wt:round(dim/2)+wt)=0;

rad=0.1;
%Vold(X.^2+Y.^2<=rad.^2)=0;error

Vold((X-1).^2+Y.^2<=rad.^2)=0;


Tg=Vold;


aoldxvi=zeros(dim,dim);
aoldyvi=zeros(dim,dim);


beta=exp(-h);


%error

diff1=1;
count=0;

for kk=1:ntheta
    Acontx(kk)=cos(2*pi*(kk-1)/ntheta);
    Aconty(kk)=sin(2*pi*(kk-1)/ntheta);
end

%Vx=@(x,y) .5+0.*x;
%Vy=@(x,y) .5+0.*x;

Vx=0.4+0.3*(-0.5)+cos(2*pi*X);
Vy=0.3*(-0.5)+sin(2*pi*X);


Valuesvi=zeros(dim,dim,dim);

tol=1/5*deltax.^2;
if (mmfile==1)
tol=10*deltax^2;
end

Varr=zeros(dim,dim,ntheta);
Vmax=1;
tic
while(diff1>tol)
    for k=1:ntheta
        arrx=X+h*(Vx+Vmax*Acontx(k));
        arry=Y+h*(Vy+Vmax*Aconty(k));
        arrinterp=interpn(X,Y,Vold,arrx,arry,'linear',1);
        %arrinterp=interpn(X,Y,Vold,arrx,arry,'cubic',1);
        %arrinterp(arrx.^2+arry.^2<=rad.^2)=0;
        Varr(:,:,k)=arrinterp;
    end
    [valor,dondex]=min(Varr,[],3);
    Vnew=Tg.*(beta*valor+1-beta);
    count=count+1;
    diff1=max(max(abs(Vnew-Vold)))
    Valuesvi(:,:,count)=Vold;
    Vold=Vnew;
end
time=toc;

xnodevi=xnode;



    for i=1:ntheta
        aoldxvi(dondex==i)=Acontx(i);
        aoldyvi(dondex==i)=Aconty(i);
    end


aoldxvi(Tg==0)=0;
aoldyvi(Tg==0)=0;
voldvi=Vold;
Valuesvi(:,:,[count+1:dim])=[];

%end
figure
surf(X,Y,Vold,'LineStyle','none')
camlight
lighting gouraud
title('Value Function')
% figure
% contourf(X,Y,Vold)
% figure
% quiver(X,Y,aoldxvi,aoldyvi)
% axis([-1 1 -1 1])

figure(1)
h1 = quiver(X,Y,aoldxvi,aoldyvi)
set(h1,'AutoScale','on', 'AutoScaleFactor', 1.5)
axis([-1 1 -1 1])
title('Controls')
% figure
% quiver(X,Y,Vx,Vy)
% figure
% quiver(X,Y,Vx+aoldxvi,Vy+aoldyvi)
