% choose indata file and discretization
fil='path1.txt';
scal=1;

disp(' *** Dynamic programming for PHEV *** ');
tic; % timing

% read data
fid=fopen(fil);
bb=textscan(fid,'%d',1); b=bb{1};
n=textscan(fid,'%d',1); nlinks=n{1};
links = textscan(fid,'%f %f');
fclose(fid);
ltypes0 = links{1};
ldist0 = links{2};
disp(sprintf('Path with %d links read from file %s.',nlinks,fil));

ldzero=sum(ldist0==0);
if (ldzero>0)
    disp(sprintf('Omitting %d links with length zero.',ldzero));
    isnotzero=(find(ldist0~=0));
    ldist=ldist0(isnotzero);
    ltypes=ltypes0(isnotzero);
    nlinks=length(ldist);
else
    ldist=ldist0;
    ltypes=ltypes0;
end

disp(sprintf('Initial battery level %d.',b));

licost=dlmread('linkcost.txt');
libatt=dlmread('linkbatt.txt');

% setup data: c and a
c=zeros(nlinks,4);
a=zeros(nlinks,4);
for j=1:nlinks
    for k=1:4
        c(j,k)=round(ldist(j)*licost(ltypes(j),k)/scal);
        a(j,k)=round(ldist(j)*libatt(ltypes(j),k)/scal);
    end
end
b=round(b/scal);

% remove all zero parts
jix=find(sum(((a~=0)|(c~=0)),2)~=0);
a=a(jix,:);
c=c(jix,:);
ldist=ldist(jix);
[n,m]=size(a);
nlinks=n;
disp(sprintf('%d links remains.',n));

% initiate zero solution
xopt = zeros(1,n);
z=0;
s=b;

% check if battery is enough for whole trip
dosolve=1;
cbig=sum(sum(c));
abig=sum(a(:,1));
abigscal=scal*abig;
disp(sprintf('Driving electric all the way would need %d.',abigscal));
if (abig<=b)
    disp(sprintf('Large battery. You can drive electric all the way!'));
    xopt = ones(1,n);
    z=sum(c(:,1));
    s=b-abig;
    dosolve=0; % no need to solve
end

% stupid driver solution, all electric until battery empty
bel=0;
j0=0;
xstupid = zeros(1,n);
cstupid=0;
for j=1:nlinks
    bel=bel+a(j,1);
    if (bel<=b)
        xstupid(j)=1;
        cstupid=cstupid+c(j,1);
    else
        xstupid(j)=4;
        cstupid=cstupid+c(j,4);
    end
end
cstupid=scal*cstupid;

if (dosolve)
    disp(' !!!!!!!!! Here you should find the optimal solution!!!!!!!');
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % insert your code here!

    
    
    % no need to change below
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end

z=scal*z;

disp('*************************');
disp(sprintf('Path with %d links read from file %s.',nlinks,fil));
disp(sprintf('Initial battery level %d.',b));
disp(sprintf('Driving electric all the way would need %d.',abigscal));
%disp(sprintf('Stupid solution:'));
%disp(sprintf('%d ',xstupid));
disp(sprintf('Optimal solution:'));
disp(sprintf('%d ',xopt));

% statistics
use1=(xopt==1);use2=(xopt==2);use3=(xopt==3);use4=(xopt==4);
ant1=sum(use1);ant2=sum(use2);ant3=sum(use3);ant4=sum(use4);
len1=sum(use1.*ldist');len2=sum(use2.*ldist');len3=sum(use3.*ldist');len4=sum(use4.*ldist');

disp(sprintf('Number of segments per type: %d, %d, %d, %d',ant1,ant2,ant3,ant4));
disp(sprintf('Total length per type: %.1f, %.1f, %.1f, %.1f',len1,len2,len3,len4));
disp(sprintf('Cost for stupid driver: %d',cstupid));
disp(sprintf('Optimal cost %d, final battery level: %d',z,s));

toc; % end timing
