profil_cena = [0.17, 0.17, 0.17, 0.17, 0.17, 0.2999, 0.2999, 0.2999, 0.2999, 0.2999, 0.2999, 0.2999, 0.2999, 0.17, 0.17,  0.2999, 0.2999, 0.2999, 0.2999, 0.2999, 0.2999, 0.2999, 0.17, 0.17];
profil_poboru = [40, 30, 30, 40, 60, 80, 140, 180, 160, 110, 90, 80, 70, 80, 80, 110, 120, 110, 110, 90, 80, 70, 60, 50];

Hs = 24;
Hp = 24;
w_u = 1;
w_q = 100;
v0 = 100;
eps_v0 = 50;
v_min_max = [50 500];
u_min_max = [0 100 100; -100 100 100];


da = opcda('192.168.1.11', 'Kepware.KEPServerEX.V5');
connect(da);
grp = addgroup(da, 'Fanuc')
itm1 = additem(grp, {'Channel1.Fanuc.pompa'});
itm2 = additem(grp, {'Channel1.Fanuc.kat'});
itm3 = additem(grp, {'Channel1.Fanuc.flaga3'});

i=1;
while(1)
    
    profil_poboru_new = 0;
    profil_cena_new = 0;
    
    flaga = read(itm3);
    
    pause(1);
  
   if flaga.Value == 1
    
    
    
    for j = 1:Hs
            profil_poboru_new(j) = profil_poboru(j+i-1); 
            profil_cena_new(j) = profil_cena(j+i-1);
    end
    
    
 [U tr_zb koszt Q] = MPC_SW(Hs, Hp, w_u, w_q, v0, eps_v0, v_min_max, u_min_max, profil_cena_new, profil_poboru_new);
 
    for k = 1:Hs
      U(k,2) = 1.1436789*atand(0.05*U(k,2)) + 90;
    end
    
   profil_poboru_new
    profil_cena_new
    Hs
    Hp
    U
    i
 



     Hp = Hp - 1;
     Hs = Hs - 1;
    write(itm1, int16(U(1,1)*320))
    write(itm2, int16(U(1,2)*32000/180))
    
    

    
     i = i + 1;
     if i == 24 
         i = 1;
         Hp = 24;
         Hs = 24;
     end
     
   end
    write(itm3, 0);
end