

clear all
close all
randn('state',sum(100*clock))

niter=300;
pRKHS=40; % length of eigenfunctions
p=30;%input('# of predictor i.r. samples = ');%lunghezza risposte impulsive predittore (100)
p2=300;%numero dati da togliere per andare a regime
n=470;%input('# of measurements = ');%numero dati effettivi per la stima (50,300)
mordn=30;%input('model order = ');if isempty(mord);mord=10;end%ordine ARMAX (10,20)
off=1;%minimo ordine di modello
inp=input('1-WN 2-LP: ');if isempty(inp);inp=1;end
lowpar=0.8;%parametro per l'ingresso 2
al=200;%allunga con zeri le i.r.
nn=n+p+p2;
nt=500;%dimensione test set
frac=1/3% fraction of data used for validation/selection
ntval=floor(frac*(n+p));%dimensione validation set
red=n;
SV=5;%20
m=15;%15numero ingressi incogniti
mPEM=min(15,ceil((9/10)*(n+p)/(m+2)))%20;%max ordine PEM
Multi=[1:20];
nM=size(Multi,2);
ou=1;
oee=1;
BL=[];%i blocchi per la marginale, se [] disattivato

maxp_GLARS = 30 % maximum order for GLARS;

%m=4, n=170 o 270 o 370, p=30
%m=9, n=270, p=30
%m=14, n=370, p=30
%m=19, n=380, p=20, off=10, mordn=25


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%% VARIABLES TO BE STORED %%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
errnp0a=[];
errnp1a=[];
errnp0b=[];
errnp1b=[];
errnp0c=[];
errnp1c=[];
errnp0d=[];
errnp1d=[];
errnp0e=[];
errnp1e=[];
errnp0f=[];
errnp1f=[];
errnp0g=[];
errnp1g=[];
errgl0=[];
errgl1=[];
errbic0=[];
errbic1=[];
errbic0b=[];
errbic1b=[];
erraic0=[];
erraic1=[];
errOr0=[];
errOr1=[];


for jj=1:niter;jj
    clear numG numH den b U
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%%%%%GENERA MODELLO, TRAINING E TEST SET%%%%%%%%%%%%%%%%%
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %T=1: modello normale
    %T=2: output error, no regressione
    %T=3: sparsita' casuale
    %T=4; come T=3 ma non crea mai un output error, ok per l'oracolo
    T=4;;
    [den,numG,numH,mord,Ind(jj,:),V]=genmod2(m,mordn,off,SV,T);
    Ind%disp(sqrt([V1 V2 V3 V4 V5]))
    [gtrue,htrue,a,b,y,U,Pr,ytest_tot,ytest,Utest,Ttest]=genri(den,numG,numH,inp,ou,oee,p,p2,nn,al,nt,lowpar,ntval);
    ya=y(1:(end-ntval+pRKHS),:);
    Ua=U(1:(end-ntval+pRKHS),:);
    yval=y((end-ntval+1):end,:);
    Uval=U((end-ntval+1):end,:);
    
    yaLARS=y(1:(end-ntval),:);
    UaLARS=U(1:(end-ntval),:);
    yvalLARS=y((end-ntval+1):end,:);
    UvalLARS=U((end-ntval+1):end,:);
    
    VarTest=sum((ytest-mean(ytest)).^2);
    
%     for i=1:m;
%         MOD(i,:,jj)=[numG(i,:) zeros(1,30-mord)];
%     end;
%     MOD(m+1,:,jj)=[den zeros(1,29-mord)];
%     MOD(m+2,:,jj)=[numH zeros(1,29-mord)];
%     DAT(:,:,jj)=[y U];%close all;plot(gtrue(1:100,1),'ko--');pause;close
%     all;plot(htrue(1:100,1),'ko--');pause
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%%%%%%%%%%%%%%%%%%%MULTI-STEP OPTIMAL%%%%%%%%%%%%%%%%%%%%%%
    RMSEbest=norm(Ttest*Pr-ytest)/sqrt(nt)
    VarTest=sum((ytest-mean(ytest)).^2);
    M = idpoly(1,gtrue(1:130,:)',htrue(1:130)',1,ones(m,1),1,1);
    for gi=1:nM;
        pred=predict(M,[ytest_tot Utest],Multi(gi));pred=iddata(pred);pred=pred.y;
        RoptM(jj,gi)=norm(pred((p2+1):(p2+nt))-ytest)/sqrt(nt);
        Ropt2M(jj,gi)=1-sum([pred((p2+1):(p2+nt))-ytest].^2)/VarTest;%norm([pred((p2+1):(p2+nt))-ytest]./ytest,1)/nt;
    end
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%%%%%% STIMA NONPARAMETRICA %%%%%%%%%%%(e) una var poi estesa ad m, (f) m var con starting point da c %%%%%%%%%%%%%%%%%%%%%%%%%
    %%%%%%%%%%%%%%%%(b) e (c) come (e) (f) ma senza poli%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    lab='yy';%'yy' se c'e' parte autoregressiva e ingresso
    mv=1;%mv = 1 se si stima una sola varianza, se no si stimano m varianze
    mb=1;%mb = 1 se si stima un solo cambiamento di scala, se no si stimano m cambiamenti di scala
    
%     mv=1;LP=0;LP2=0;%una sola varianza ora
%     cn=0;ipn=[];
%     [eta,Mnpa,Da,Sa,objvala,A,ipera,MP,nv]=SSpline(y,U,p,lab,mv,mb,ipn,[],LP,LP2,cn,red,BL);
%     [errnp0a,errnp1a,Errnpa(jj),RnpMa(jj,:),Rnp2Ma(jj,:),Da,Sa]=CompErr(Mnpa,gtrue,htrue,Ind(jj,:),errnp0a,errnp1a,ytest_tot,ytest,Utest,p2,nt,VarTest,Multi);
%     %ipera
%     objvala
%     
%     mv=1;LP=0;LP2=1;%una sola varianza ora, LP=1 sarebbe assurdo in questo caso, con LP2=1 restituite alla fine m varianze
%     cn=0;ipn=[];
%     [eta,Mnpb,Db,Sb,objvalb,A,iperb,MP,nv]=SSpline(y,U,p,lab,mv,mb,ipn,[],LP,LP2,cn,red,BL);
%     [errnp0b,errnp1b,Errnpb(jj),RnpMb(jj,:),Rnp2Mb(jj,:),Db,Sb]=CompErr(Mnpb,gtrue,htrue,Ind(jj,:),errnp0b,errnp1b,ytest_tot,ytest,Utest,p2,nt,VarTest,Multi);
%     %ipera
%     objvalb
%         
%     mv=0;LP=1;LP2=0;%piu' varianze con pto iniziale quello di prima
%     cn=0;ipn=[];for i=1:size(iperb,2);if iperb(i)==-inf;iperb(i)=-40;end;end
%     [eta,Mnpc,Dc,Sc,objvalc,A,iperc,MP,nv]=SSpline(y,U,p,lab,mv,mb,ipn,[],LP,LP2,cn,red,BL,iperb);
%     [errnp0c,errnp1c,Errnpc(jj),RnpMc(jj,:),Rnp2Mc(jj,:),Dc,Sc]=CompErr(Mnpc,gtrue,htrue,Ind(jj,:),errnp0c,errnp1c,ytest_tot,ytest,Utest,p2,nt,VarTest,Multi);
%     %iperb
%     objvalc
%     
%     mv=1;LP=0;LP2=0;%una sola varianza ora e 2 poli
%     cn=2;ipn=-0.5*ones(1,cn);
%     [eta,Mnpd,Dd,Sd,objvald,A,iperd,MP,nv]=SSpline(y,U,p,lab,mv,mb,ipn,[],LP,LP2,cn,red,BL);
%     [errnp0d,errnp1d,Errnpd(jj),RnpMd(jj,:),Rnp2Md(jj,:),Dd,Sd]=CompErr(Mnpd,gtrue,htrue,Ind(jj,:),errnp0d,errnp1d,ytest_tot,ytest,Utest,p2,nt,VarTest,Multi);
%     %iperc
%     objvald
    
    mv=1;LP=0;LP2=1;%una sola varianza ora, LP=1 sarebbe assurdo in questo caso, con LP2=1 restituite alla fine m varianze
    cn=2;ipn=-0.5*ones(1,cn);
    [Mnpe,ipere]=SSpline(y,U,p,lab,mv,mb,cn,red,LP,LP2);
    %SSpline(y,U,p,lab,mv,mb,cn,red,LP,LP2,iperstart)
    [errnp0e,errnp1e,Errnpe(jj),RnpMe(jj,:),Rnp2Me(jj,:),De,Se]=CompErr(Mnpe,gtrue,htrue,Ind(jj,:),errnp0e,errnp1e,ytest_tot,ytest,Utest,p2,nt,VarTest,Multi);
    %iperc
    %objvale
       
    mv=0;LP=1;LP2=0;%piu' varianze con pto iniziale quello di prima
    cn=2;ipn=-0.5*ones(1,cn);for i=1:size(ipere,2);if ipere(i)==-inf;ipere(i)=-40;end;end
    [Mnpf,iperf]=SSpline(y,U,p,lab,mv,mb,cn,red,LP,LP2,ipere);
    [errnp0f,errnp1f,Errnpf(jj),RnpMf(jj,:),Rnp2Mf(jj,:),Df,Sf]=CompErr(Mnpf,gtrue,htrue,Ind(jj,:),errnp0f,errnp1f,ytest_tot,ytest,Utest,p2,nt,VarTest,Multi);
    %iperd
    %objvalf
    %HIP(jj,:)=ipera;

    clear AIK
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%%%%%%%%%%% STIMA NONPARAMETRICA 2%%%%%%%%%%%%%%%%%%%%%%%
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    [Mnpg,beta,sigma,NT(jj),Mnpc,beta2,sigma2,NT2(jj)] = RKHS_Pred_LARS_Validation_all_k_v2(ya,Ua,1,30,yval,Uval,pRKHS);
    [errnp0g,errnp1g,Errnpg(jj),RnpMg(jj,:),Rnp2Mg(jj,:),Dg,Sg]=CompErr(Mnpg,gtrue,htrue,Ind(jj,:),errnp0g,errnp1g,ytest_tot,ytest,Utest,p2,nt,VarTest,Multi);
    %[errnp0c,errnp1c,Errnpc(jj),RnpMc(jj,:),Rnp2Mc(jj,:),Dc,Sc]=CompErr(Mnpc,gtrue,htrue,Ind(jj,:),errnp0c,errnp1c,ytest_tot,ytest,Utest,p2,nt,VarTest,Multi);
     %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%%%%%%%%%%%%%%%%%%%% STIMA GLARS VALIDATION %%%%%%%%%%%%%%%%%%%%%%%%
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    dat_val=iddata(yval,Uval);
    t=0;
    for pl=1:1:maxp_GLARS;
        t=t+1;
        ll(t,:)=pl;
        [Mgl,Mgls] = Dyn_GLARS_all_k(yaLARS,[],UaLARS,pl,1,0,yvalLARS,[],UvalLARS);
        predd=predict(Mgl,dat_val);predd=iddata(predd);predd=predd.y;
        RM1(t)=sum((predd-yval).^2);
        predd=predict(Mgls,dat_val);predd=iddata(predd);predd=predd.y;
        RM2(t)=sum((predd-yval).^2);
    end
    [q,w1]=min(RM1);ll(w1,:)
    [Mgl,Mgls] = Dyn_GLARS_all_k(yaLARS,[],UaLARS,ll(w1,:),1,0,yvalLARS,[],UvalLARS);
    [errgl0,errgl1,Errgl(jj),RglM(jj,:),Rgl2M(jj,:),Da,Sa]=CompErr(Mgl,gtrue,htrue,Ind(jj,:),errgl0,errgl1,ytest_tot,ytest,Utest,p2,nt,VarTest,Multi);
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%%%%%%%%%%%%STIMA PEM%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    clear L1 L2 ll1 Or0 Or1 Or2 RROrM RROr2M DOr SOr Orr0 Orr1 Orr2
    dat=iddata(y,U);
    dat_test=iddata(ytest_tot,Utest);
    t=0;
    for i1=1:mPEM;
            i2=i1*ones(1,m);
            i3=i1;
            t=t+1;
            Mpem=pem(dat,[i1 i2 i3 zeros(1,m+1) ones(1,m)],'LimitError',0);
            predd=predict(Mpem,dat);predd=iddata(predd);predd=predd.y;
            RM=sum((predd-y).^2);npar=max(size(Mpem.ParameterVector));ndat=Mpem.EstimationInfo.DataLength;
            L1(t)=ndat*log(RM/ndat)+log(ndat)*npar;%BIC
            L2(t)=ndat*log(RM/ndat)+[ndat*(ndat+npar)]/[ndat-npar-2];%AICC
            ll1(t,:)=[i1 i2 i3];

            MpemOR=pem(dat,[i1 i2.*Ind(jj,(2:end)) i3 zeros(1,m+1) ones(1,m)],'LimitError',0);
            predd=predict(MpemOR,dat);predd=iddata(predd);predd=predd.y;
            RM=sum((predd-y).^2);npar=max(size(MpemOR.ParameterVector));ndat=MpemOR.EstimationInfo.DataLength;
            L1b(t)=ndat*log(RM/ndat)+log(ndat)*npar;%BIC+1/2Or
            [Or0,Or1,Or2,RROrM(i1,:),RROr2M(i1,:),DOr,SOr]=CompErr(MpemOR,gtrue,htrue,Ind(jj,:),[],[],ytest_tot,ytest,Utest,p2,nt,VarTest,Multi);
            Orr0(i1)=mean(Or0);Orr1(i1)=mean(Or1);Orr2(i1)=mean(Or2);
    end
    [q,w1]=min(L1);ll1(w1,:)
    Mpem=pem(dat,[ll1(w1,:) zeros(1,m+1) ones(1,m)],'LimitError',0);
    [errbic0,errbic1,Errbic(jj),RbicM(jj,:),Rbic2M(jj,:),Dbic,Sbic]=CompErr(Mpem,gtrue,htrue,Ind(jj,:),errbic0,errbic1,ytest_tot,ytest,Utest,p2,nt,VarTest,Multi);
    [q,w1]=min(L1b);i1=ll1(w1,1);i2=i1;i3=i1;ll1(w1,:)
    Mpem=pem(dat,[i1 i2.*Ind(jj,(2:end)) i3 zeros(1,m+1) ones(1,m)],'LimitError',0);
    [errbic0b,errbic1b,Errbicb(jj),RbicMb(jj,:),Rbic2Mb(jj,:),Dbic,Sbic]=CompErr(Mpem,gtrue,htrue,Ind(jj,:),errbic0b,errbic1b,ytest_tot,ytest,Utest,p2,nt,VarTest,Multi);
    [q,w2]=min(L2);ll1(w2,:)
    Mpem=pem(dat,[ll1(w2,:) zeros(1,m+1) ones(1,m)],'LimitError',0);
    [erraic0,erraic1,Erraic(jj),RaicM(jj,:),Raic2M(jj,:),Daic,Saic]=CompErr(Mpem,gtrue,htrue,Ind(jj,:),erraic0,erraic1,ytest_tot,ytest,Utest,p2,nt,VarTest,Multi);
    
    ErrOr(jj)=min(Orr2);
    ROrM(jj,:)=min(RROrM);
    ROr2M(jj,:)=max(RROr2M);
    [q,w3]=min(Orr1);ll1(w3,:)%l'oracolo sceglie l'ordine che da' la media piu' bassa sugli errori relativi
    i1=ll1(w3,1);i2=i1*ones(1,m);i3=i1
%     if Ind(jj,1)==1;
%         MpemOR=pem(dat,[i1 i2.*Ind(jj,(2:end)) i3 zeros(1,m+1) ones(1,m)],'LimitError',0);
%     else
%         MpemOR= oe(dat,[i1*Ind(jj,(2:end))  i1*Ind(jj,(2:end))  ones(1,m) ]);
%     end
    MpemOR=pem(dat,[ll1(w3,1)  ll1(w3,2).*Ind(jj,(2:end))  ll1(w3,3)  zeros(1,m+1) ones(1,m)],'LimitError',0);
    [errOr0,errOr1,gian1,gian2,gian3,DOr,SOr]=CompErr(MpemOR,gtrue,htrue,Ind(jj,:),errOr0,errOr1,ytest_tot,ytest,Utest,p2,nt,VarTest,Multi);
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%%%%%%%%%%%%%%%%%RESULTS%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    close all
    figure(1)
    hold on
    %subplot(2,1,2);
    %plot(mean(Rbic2M),'b*');hold on;
    plot(mean(Rbic2Mb),'b*');hold on;
    plot(mean(Rnp2Mf),'ko');hold on
    plot(mean(Rnp2Me),'k+');
    %plot(mean(Rnp2Mg),'bo');
    %plot(mean(Rnp2Mb),'go');
    plot(mean(ROr2M),'r.');
    plot(mean(Rgl2M),'g*');

%     subplot(2,1,2);
%     plot(mean(RbicM),'b*');hold on
%     plot(mean(RnpMa),'ko');
%     %plot(mean(RnpMb),'go');
%     plot(mean(ROrM),'r.');
%     %plot(mean(RoptM),'r.');
%     pause(1)
    
    disp('np')
%     [mean(errnp0a) mean(errnp1a) mean(Errnpa)  mean(mean(Rnp2Ma))]%norma di quelle che dovrebbero essere a zero, errore relativo sulle norme delle altre, 
%                                            %errore relativo sulle norme
%                                            %generale (a differenza degli altri, un solo indice per corsa)
%     [mean(errnp0b) mean(errnp1b) mean(Errnpb)  mean(mean(Rnp2Mb))]
%     
%     [mean(errnp0c) mean(errnp1c) mean(Errnpc)  mean(mean(Rnp2Mc))]
%     
%     [mean(errnp0d) mean(errnp1d) mean(Errnpd)  mean(mean(Rnp2Md))]
        
    [mean(errnp0e) mean(errnp1e) mean(Errnpe)  mean(mean(Rnp2Me))]  
           
    [mean(errnp0f) mean(errnp1f) mean(Errnpf)  mean(mean(Rnp2Mf))]
               
    [mean(errnp0g) mean(errnp1g) mean(Errnpg)  mean(mean(Rnp2Mg))] 
    
    disp('GLARS')
    [mean(errgl0) mean(errgl1) mean(Errgl)  mean(mean(Rgl2M))  ]
    
    disp('PEM+BIC')
    [mean(errbic0) mean(errbic1) mean(Errbic)  mean(mean(Rbic2M))]
    
    disp('PEM+BIC')
    [mean(errbic0b) mean(errbic1b) mean(Errbicb)  mean(mean(Rbic2Mb))]
    
    disp('PEM+AICC')
    [mean(erraic0) mean(erraic1) mean(Erraic)  mean(mean(Raic2M))]
    
    disp('PEM+OR')
    [mean(errOr0) mean(errOr1) mean(ErrOr)  mean(mean(ROr2M))]
    
    %boxplot([errnp0' errbic0'])%sparsita'
    %bode(M,'r',Mnp,'k',Mpem,'b')

end

