root/applications/dual/SIDP/transformace(3)/sidp_transformace.m @ 1105

function sidp_transformace(sidp_parameters, rsss_parameters,system,apriori)
%transformace
mode=0;
N=sidp_parameters.horizont-2;
poc=zeros(N,2);
kon=zeros(N,2);

eps=10^-2;
system.dim=2;

if abs(apriori.y0)-2*apriori.y0_range<0
    apriori.eta0=(abs(apriori.y0)+2*apriori.y0_range)/2/system.sigma;
    apriori.eta0_range=apriori.eta0;
else
    apriori.eta0=abs(apriori.y0)/system.sigma;
    apriori.eta0_range=2*apriori.y0_range/system.sigma;
end
etar=system.yr/system.sigma;

apriori.beta0=abs(apriori.b0)/sqrt(apriori.P0);
apriori.beta0_range=2*apriori.b0_range/sqrt(apriori.P0-apriori.P0_range);    
if apriori.beta0-apriori.beta0_range<eps
    apriori.beta0=(abs(apriori.b0)+2*apriori.b0_range)/2/sqrt(apriori.P0-apriori.P0_range);
    apriori.beta0_range=apriori.beta0-eps;
end

%pomocne promenne a konstanty
step=[1; sidp_parameters.n_grid];

[H ny]=init_hyperstate(N,sidp_parameters.n_grid,apriori,mode);  %vytvori H a mi, nastavi H0 a mi*

for i=1:sidp_parameters.n_pass
    
    for j=1:sidp_parameters.n_iter
        [i j]
        search_region=sidp_parameters.gama^(j-1)*sidp_parameters.lambda^(i-1)*sidp_parameters.search_region_init;
        [H ny]=update_hyperstate(H, ny);   %vygeneruje trajektorie, v zasazenem regionu rovnomerne rozmisti body a prekopiruje nalezena mi*
      %>1
      
      %los=mc_study2(system,apriori,1000);
      %porovnej(los)
      
        poc(:,:)=H(:,1,:);
        kon(:,:)=H(:,end,:);      
        scale=(kon-poc)/(sidp_parameters.n_grid-1);
      
        for k=N:-1:1    %pozor na meze
            level=N-k+1;
            k
            
            for l=1:size(H,2)  
                candidates=generate_candidates(ny(k,l), search_region, sidp_parameters.num_of_candidates,sidp_parameters.generate_candidates_mode);
              
                %compare candidates
                realization1=zeros(size(candidates,1),rsss_parameters.n0);
                
                for m=1:sidp_parameters.num_of_candidates
                    best_control=-candidates(m);
                    for n=1:rsss_parameters.n0
                        eta=H(k,l,1);
                        beta=H(k,l,2);
                        %generate realization
                        for o=1:level
                            s=randn;
                            pom=sqrt(1+best_control^2);
                            eta=abs(eta+beta*best_control+pom*s);
                            beta=abs(pom*beta+best_control*s);
                            
                            realization1(m,n)=realization1(m,n)+(eta-etar(k+o))^2;
                            
                            if o<level
                                %find in H
                                stav=round(([eta beta]-poc(k+o,:))./scale(k+o,:)+1); 
                                index=min(max(stav,1),sidp_parameters.n_grid)-[0 1];
                                index=index*step;
                                
                                best_control=-ny(k+o,index);
                                %index==find_in_hyperstate5(state, H(k+o,:,:))
                            %    plot(H(k+o,:,1),H(k+o,:,2), 'r.' ); hold on; plot(eta,beta, 'b+' ); plot(H(k+o,index,1),H(k+o,index,2), 'g+' ); hold off
                            end
                        end
                        best_control=-(eta-etar(end))*beta/(1+beta^2);
                        s=randn;
                        pom=sqrt(1+best_control^2);
                        eta=abs(eta+beta*best_control+pom*s);
                        
                        realization1(m,n)=realization1(m,n)+(eta-etar(end))^2;
                    end
                end
                    mean_values=mean(realization1,2);
                    [min_val min_index]=min(mean_values);
                    best_control=candidates(min_index);
                    ny(k,l)=best_control;
            end
        end
        
        vypis(:,:)=H(:,:,1);
        save 'eta.txt' vypis -ASCII;
        vypis(:,:)=H(:,:,2);
        save 'Beta.txt' vypis -ASCII;
        save 'ny.txt' ny -ASCII; 
        
       %  los=mc_study2(system,apriori,1000);
       %  porovnej(los(1,:))
    end 
end

end