clc;
clear all;
close all;
warning off all;

int_x = 0.01;
x=[0:int_x:15]';
syms z;

cost_s = 0.5*z^(2); %funzione costo (letterale)
value_s_1 =20*log(z*5+1); %funzione valore (letterale)
value_s_2 =30*log(z*5+1); %funzione valore (letterale)
value_s_3 =25*log(z*5+1); %funzione valore (letterale)
value_s_4 =15*log(z*5+1); %funzione valore (letterale)

cost=subs(cost_s,x); %funzione costo (valori)
value_1=subs(value_s_1,x);%funzione valore (valori)
value_2=subs(value_s_2,x);%funzione valore (valori)
value_3=subs(value_s_3,x);%funzione valore (valori)
value_4=subs(value_s_4,x);%funzione valore (valori)

figure(1);
plot(x,cost,'k',x,value_1,'m',x,value_2,'c',x,value_3,'b',x,value_4,'k');
title('Funzione costo (rosso), funzione valore (blu)');

%% Calcolo delle derivate delle funzioni costo e value
% c/v_punto_s = differenziali

c_punto_s=diff(cost_s,z);
v_punto_s_1=diff(value_s_1,z);
v_punto_s_2=diff(value_s_2,z);
v_punto_s_3=diff(value_s_3,z);
v_punto_s_4=diff(value_s_4,z);

c_punto=subs(c_punto_s,x); % lambda_w
v_punto_1=subs(v_punto_s_1,x); % lambda_r
v_punto_2=subs(v_punto_s_2,x); % lambda_r
v_punto_3=subs(v_punto_s_3,x); % lambda_r
v_punto_4=subs(v_punto_s_4,x); % lambda_r

figure(2);
plot(x,c_punto,'k',x,v_punto_1,'m',x,v_punto_2,'c',x,v_punto_3,'b',x,v_punto_4,'k');
title('Derivate costo e valore');
axis([0 10 0 25]);

%% Calcola le inverse della c punto e v punto
c_punto_inv_s =finverse(c_punto_s,z);
v_punto_inv_s_1 =finverse(v_punto_s_1,z);
v_punto_inv_s_2 =finverse(v_punto_s_2,z);
v_punto_inv_s_3 =finverse(v_punto_s_3,z);
v_punto_inv_s_4 =finverse(v_punto_s_4,z);

int_lambda = 0.001; %intervallo dei valori dei prezzi
lambda=[0:int_lambda:300]';

% calcola inverse 
c_punto_inv =subs(c_punto_inv_s,lambda);
v_punto_inv_1 =subs(v_punto_inv_s_1,lambda);
v_punto_inv_2 =subs(v_punto_inv_s_2,lambda);
v_punto_inv_3 =subs(v_punto_inv_s_3,lambda);
v_punto_inv_4 =subs(v_punto_inv_s_4,lambda);

figure(3);
plot(lambda,c_punto_inv);
title('$\dot{c}^{-1}$', 'Interpreter', 'latex');

%%
load file_dati;
x0=4.4; %quantita iniziale di energia
durata=24; %numero di passi
xt=x0;

prezzi_retail = -1*ones(durata,1); %vettore contenente i prezzi al dettaglio
prezzi_w = -1*ones(durata,1); %vettore contenente i prezzi all'ingrosso
energia = -1*ones(durata,1); %vettore delle quantita di energia
%gamma = 0.5;

% somme delle differenze dei prezzi al dettaglio e all'ingrosso
integrale = zeros(durata,1); %vettore contenente i "guadagni" dell'ISO
temp = 0;

lambda_r = subs(v_punto_s_1, xt);
lambda_w = subs(c_punto_s, xt);

x_temp = [quantita; xt];
y_temp = [prezzi_r; lambda_r];

k = [0:pi/24:pi]';
k = cos(k).^10;
Q = diag(k); % matrice dei pesi 25 x 25
error
%%
for v=1:1
for i=1:6
    prezzi_retail(i,1)=lambda_r;
    prezzi_w(i,1)=lambda_w;
    energia(i,1)=xt;
    
    %S=[1./x_temp ones(25,1)];
    S=[1./x_temp]; % 25 x 1
    Y = y_temp; % 25 x 1
    
%     [Q,R] = QR(A,0) produces the "economy size" decomposition.
%     If m>n, only the first n columns of Q and the first n rows of R are
%     computed. If m<=n, this is the same as [Q,R] = QR(A).

%     Example: The least squares approximate solution to A*x = b can be found
%     with the Q-less QR decomposition and one step of iterative refinement:
%  
%           if issparse(A), R = qr(A); else R = triu(qr(A)); end
%           x = R\(R'\(A'*b));
%           r = b - A*x;
%           e = R\(R'\(A'*r));
%           x = x + e;
    
    A = S'*Q*S;
    b = S'*Q*Y;
    if issparse(A), R = qr(A); 
    else R = triu(qr(A)); end
    Theta = R\(R'\(A'*b));
    r = b - A*Theta;
    e = R\(R'\(A'*r));
    Theta = Theta + e;
    % Theta = (S'*Q*S)^(-1)*S'*Q*Y;
            
    %xi=linspace(0,12,1/int_x+1);
    a = Theta;
    %B= Theta(2,1);
    %yi= A./x+B;
    yi= a./x;
    
    figure(12); %guardo come evolve
    plot(x,c_punto,'b',x,yi,'g',x_temp,y_temp,'o',x,v_punto_1,'r');
    axis([-2 12 -2 10]);
    
    %lambda_ott=round(lambda_ott/int_lambda+1);    
    [xt,lambda_ott]= polyxpoly(x,yi,x,c_punto);

    xt = subs(v_punto_inv_s_1, lambda_ott);
    lambda_r = lambda_ott;
    lambda_w = subs(c_punto_s, xt);

    x_temp = [x_temp(2:25); xt];
    y_temp = [y_temp(2:25); lambda_r];
    
    integrale(i,1) = (prezzi_retail(i,1)-prezzi_w(i,1))*xt + temp;
    temp = integrale(i,1);
    i
end
for i=7:12
    prezzi_retail(i,1)=lambda_r;
    prezzi_w(i,1)=lambda_w;
    energia(i,1)=xt;
    
    %S=[1./x_temp ones(25,1)];
    S=[1./x_temp];
    Y = y_temp;
    
    A = S'*Q*S;
    b = S'*Q*Y;
    if issparse(A), R = qr(A); 
    else R = triu(qr(A)); end
    Theta = R\(R'\(A'*b));
    r = b - A*Theta;
    e = R\(R'\(A'*r));
    Theta = Theta + e;
    % Theta = (S'*Q*S)^(-1)*S'*Q*Y;
            
    %xi=linspace(0,12,1/int_x+1);
    a = Theta;
    %B= Theta(2,1);
    %yi= A./x+B;
    yi= a./x;
    
    figure(12); %guardo come evolve
    plot(x,c_punto,'b',x,yi,'g',x_temp,y_temp,'o',x,v_punto_2,'r');
    axis([-2 12 -2 10]);
    
    %lambda_ott=round(lambda_ott/int_lambda+1);    
    [xt,lambda_ott]= polyxpoly(x,yi,x,c_punto);

    xt = subs(v_punto_inv_s_2, lambda_ott);
    lambda_r = lambda_ott;
    lambda_w = subs(c_punto_s, xt);

    x_temp = [x_temp(2:25); xt];
    y_temp = [y_temp(2:25); lambda_r];
    
    integrale(i,1) = (prezzi_retail(i,1)-prezzi_w(i,1))*xt + temp;
    temp = integrale(i,1);
    i
end
for i=13:18
    prezzi_retail(i,1)=lambda_r;
    prezzi_w(i,1)=lambda_w;
    energia(i,1)=xt;
    
    %S=[1./x_temp ones(25,1)];
    S=[1./x_temp];
    Y = y_temp;
    
    A = S'*Q*S;
    b = S'*Q*Y;
    if issparse(A), R = qr(A); 
    else R = triu(qr(A)); end
    Theta = R\(R'\(A'*b));
    r = b - A*Theta;
    e = R\(R'\(A'*r));
    Theta = Theta + e;
    % Theta = (S'*Q*S)^(-1)*S'*Q*Y;
            
    %xi=linspace(0,12,1/int_x+1);
    a = Theta;
    %B= Theta(2,1);
    %yi= A./x+B;
    yi= a./x;
    
    figure(12); %guardo come evolve
    plot(x,c_punto,'b',x,yi,'g',x_temp,y_temp,'o',x,v_punto_3,'r');
    axis([-2 12 -2 10]);
    
    %lambda_ott=round(lambda_ott/int_lambda+1);    
    [xt,lambda_ott]= polyxpoly(x,yi,x,c_punto);

    xt = subs(v_punto_inv_s_3, lambda_ott);
    lambda_r = lambda_ott;
    lambda_w = subs(c_punto_s, xt);

    x_temp = [x_temp(2:25); xt];
    y_temp = [y_temp(2:25); lambda_r];
    
    integrale(i,1) = (prezzi_retail(i,1)-prezzi_w(i,1))*xt + temp;
    temp = integrale(i,1);
    i
end
for i=19:24
    prezzi_retail(i,1)=lambda_r;
    prezzi_w(i,1)=lambda_w;
    energia(i,1)=xt;
    
    %S=[1./x_temp ones(25,1)];
    S=[1./x_temp];
    Y = y_temp;
    
    A = S'*Q*S;
    b = S'*Q*Y;
    if issparse(A), R = qr(A); 
    else R = triu(qr(A)); end
    Theta = R\(R'\(A'*b));
    r = b - A*Theta;
    e = R\(R'\(A'*r));
    Theta = Theta + e;
    % Theta = (S'*Q*S)^(-1)*S'*Q*Y;
            
    %xi=linspace(0,12,1/int_x+1);
    a = Theta;
    %B= Theta(2,1);
    %yi= A./x+B;
    yi= a./x;
    
    figure(12); %guardo come evolve
    plot(x,c_punto,'b',x,yi,'g',x_temp,y_temp,'o',x,v_punto_4,'r');
    axis([-2 12 -2 10]);
    
    %lambda_ott=round(lambda_ott/int_lambda+1);    
    [xt,lambda_ott]= polyxpoly(x,yi,x,c_punto);

    xt = subs(v_punto_inv_s_4, lambda_ott);
    lambda_r = lambda_ott;
    lambda_w = subs(c_punto_s, xt);

    x_temp = [x_temp(2:25); xt];
    y_temp = [y_temp(2:25); lambda_r];
    
    integrale(i,1) = (prezzi_retail(i,1)-prezzi_w(i,1))*xt + temp;
    temp = integrale(i,1);
    i
end

end

%%
t=[1:durata]';

figure(4);
plot(t,prezzi_retail);
title('$\lambda_r$', 'Interpreter', 'latex');

figure(5);
plot(t,prezzi_w);
title('$\lambda_w$', 'Interpreter', 'latex');

figure(6);
plot(t,prezzi_retail-prezzi_w);
title('Differenza dei prezzi al dettaglio e all''ingrosso');

figure(7);
plot(t,integrale);
title('Somme delle differenze di prezzo');

integrale(durata,1)

figure(8);
plot(x,c_punto, 'k');
hold on;
plot(x,v_punto_1,':m');
hold on;
plot(x,v_punto_2,':c');
hold on;
plot(x,v_punto_3,':b');
hold on;
plot(x,v_punto_4,':k');
for t=1:durata-1
    plot([energia(t) energia(t)],[prezzi_retail(t) prezzi_retail(t+1)],'r')
    plot([energia(t) energia(t+1)],[prezzi_retail(t+1) prezzi_retail(t+1)],'r')
    plot([energia(t) energia(t)], [prezzi_retail(t+1) subs(c_punto_s,energia(t))],'k')
end    
title('$\dot{c},\dot{v}$ e andamento delle quantita', 'Interpreter', 'latex');
axis([0 15 0 10]);



