source: altifloat/src/floater.h @ 142

#define UPDATE_BCK

//constantes
YREAL rdtflo=2*3600;
//YREAL rdtflo=1;
YREAL ur_dx=0;
YREAL ur_dy=0;
YREAL vr_dx=0;
YREAL vr_dy=0;


//Mode Forward ou assim;
typedef enum {ASSIM,FORW} assim_t;

assim_t tassim = ASSIM;

//Tableaux

//Variable d'état
YREAL umod[nlon][nlat][jptfl];
YREAL vmod[nlon][nlat][jptfl];

//Background
YREAL ubck[nlon][nlat][jptfl];
YREAL vbck[nlon][nlat][jptfl];
int mask[nlon][nlat];

//Trajectoires des flotteurs (en points de grille)
YREAL piobs[jptfl][jpnfl];
YREAL pjobs[jptfl][jpnfl];
YREAL piret[jptfl][jpnfl];
YREAL pjret[jptfl][jpnfl];
short pmask[jptfl][jpnfl];

//FILTER COEFFICIENTS
#ifdef K_FILTER 
//YREAL c1;
//YREAL c2,c3,c4,c5;
//YREAL dtfil=26042000;
YREAL dtfil=2.604166666666667e+07;

#endif

//Options du run incr士ental
int nb_extiter=4;
short inc_save=0;
char dirsave[50]="../obs_float/";
char savename[50]="optim25float-filtre15-inc";
extern double Y3ddf1;
//Declaration pour multirun
void erase_lobs();
short is_activ(int jpfl, int it);
void read_obs(int argv, char *argc[]);
extern int Yobs_insert_data (char *nmmod, int sortie, int iaxe, int jaxe, int kaxe,int pdt, YREAL val);
void run_inc(int argv, char *argc[]);

void appli_start(int argc, char *argv[]){
#ifdef K_FILTER
  //c1=0.5;
  //c2=c3=c4=c5
#endif

  //Init des tableaux
  for (int i=0;i<nlon;i++)
    for (int j=0;j<nlat;j++) {
        mask[i][j]=1;   
      for (int k=0;k<jptfl;k++) {
        umod[i][j][k]=0;
        vmod[i][j][k]=0;
        ubck[i][j][k]=0;
        vbck[i][j][k]=0;
      }
}
 for (int i=0;i<jptfl;i++)
    for (int j=0;j<jpnfl;j++) {
      piobs[i][j]=0;
      pjobs[i][j]=0;
      piret[i][j]=0;
      pjret[i][j]=0; 
      pmask[i][j]=0;
      }

char L1[50]="read_obs";
char L2[50]="../obs_float/obs_float_test.dat";
char *L[2];
 L[0]=L1;
 L[1]=L2;
 read_obs(2,L);
 //Test de isactiv
 //printf("isactiv(0,0):%d\n",is_activ(0,0));
 //printf("isactiv(1,0):%d\n",is_activ(1,0));
 //printf("isactiv(0,3):%d\n",is_activ(0,3));
 //printf("isactiv(1,3):%d\n",is_activ(1,3));
 //printf("isactiv(0,6):%d\n",is_activ(0,6));
 //printf("isactiv(1,6):%d\n",is_activ(1,6));

  
 // erase_lobs();
}
void before_it(int nit){}
void cost_function(int pdt){}
void adjust_target(){}
void after_it(int nit){
        
        //printf("u_d(9,16)=%f %f %f %f\n",YS1_u_d(9,16),YS1_u_d(10,16),YS1_u_d(10,17),YS1_u_d(9,17));
        }
void forward_before(int ctrp){
}
void forward_after(int ctrp){

}

void backward_before(int ctrp){
}

void backward_after(int ctrp){
        }

short select_io(int indic, char *nmod, int sortie, int iaxe,
                int jaxe, int kaxe, int pdt, YREAL *val){
        //if(indic==YIO_LOADSTATE && !strcmp(nmod,"u")) {
        //printf("s=%d t=%d Yi=%d Yj=%d val=%f\n",sortie,pdt,iaxe,jaxe,*val);   
        //}
        
        return(1);
}

void clear_Yst_nodo(struct Yst_nodo *nobs, int lev, int max) {
  /* lev est le niveau courant (racine=0)
max est le niveau des feuilles soit
4 si dimod=1
5 si dimod=2
6 si dimod=3
dimod est défini au niveau 2 avec dimod=YTabMod[imod].dim


  */
  if (lev==max) {
    //on nettoie une feuille (ni frere ni fils)
    free(nobs);
    nobs = NULL ;
  }
  
  else {

    if (lev==2) {
      //on peut déterminer le niveau max
      int dimod=YTabMod[nobs->iind].dim;
      max=dimod+3;
    }

    if (nobs->fils!=NULL && nobs->frere!=NULL) {
      clear_Yst_nodo(nobs->frere,lev+1,max);
      nobs->frere=NULL;
    }    
    if (nobs->fils!=NULL) {
      clear_Yst_nodo(nobs->fils,lev+1,max);
      nobs->fils=NULL;
    }
    free(nobs);
    nobs=NULL;
  } // else (pas feuille)
  

}

//in case we want to see how it s loading the data we do this above
void erase_lobs() {
  //Efface toute l'arboresence des obs (y compris les ébauches)
  int itraj;
  struct Yst_nodo *YRobs = NULL;
  for (itraj=0;itraj<YNBTRAJ;itraj++) {
    //Effacement des obs liées au modul imod
    if (YTabMod[itraj].is_target || !YTabMod[itraj].is_cout) {
      YRobs=YTabTraj[itraj].YRobs;
      clear_Yst_nodo(YRobs,0,10);
      YTabTraj[itraj].YRobs=NULL;
  }

  } //for itraj
  for (int wi = 0; wi < YNBTRAJ; ++wi)
    {
      YTabTraj[wi].YRobs = (struct Yst_nodo*) malloc (sizeof (struct Yst_nodo)); /* init du Root des arborescences */
      YTabTraj[wi].YRobs->frere = YTabTraj[wi].YRobs->fils = NULL;      /* d'observations pour chaque trajectoire */
    }

}

void read_mask(int argv, char *argc[]) {
char fname[50];
if (argv==1) {
        sprintf(fname,"../obs_float/mask.dat");
}
if (argv==2) {
        sprintf(fname,argc[1]);
}
if (argv>2)
 {
        fprintf(stderr,"(read_mask) Error %d : bad number of arguments\n",argv);
        return;
}
FILE *fid;
fid=fopen(fname,"r");
if (fid==NULL) {
   fprintf(stderr,"(read_mask) Error : unable to open %s\n",fname);
   return;
 }
 int ilon,ilat,status;
float val;
for (ilat=0;ilat<nlat;ilat++)
        for (ilon=0;ilon<nlon;ilon++) {
        status=fscanf(fid,"%f",&val);
        mask[ilon][ilat]=(int)val;
if (status!=1) {
        fprintf(stderr,"(read_mask) unable to read mask value (lon=%d,lat=%d)\n",ilon,ilat);
        fclose(fid);    
        return;
}
}
fclose(fid);

}

void read_bck (int argv, char *argc[]) {
/* Read background in a ascii file
   format
   Column 1 (%d) : time step (from 1 to jptfl)
   Column 2 (%d) : lon (Yi) (from 1 to nlon)
   Column 3 (%d) : lat (Yj) (from 1 to nlat)
   Column 4 (%f) : u value
   Column 5 (%f) : v value
*/

if (argv!=2) {
  fprintf(stderr,"(read_bck) Error : read_bck with %d (1 nedded)\n",argv-1);
  fprintf(stderr,"No background read\n");
  return;
 }
 FILE *fid;
 fid=fopen(argc[1],"r");
 if (fid==NULL) {
   fprintf(stderr,"(read_bck) Error : unable to open %s\n",argc[1]);
   return;
 }
 
 int it,ilon,ilat,count;
 count=0;
 YREAL uval,vval;
 int status=5;
 while (status==5) {
   status=fscanf(fid,"%d %d %d %lf %lf",&it,&ilon,&ilat,&uval,&vval);
   if (status!=5) break;
   if (ilon>nlon || ilon<=0 || ilat>nlat || ilat<=0 || it<=0 || it>jptfl) {
     fprintf(stderr,"(read_bck) Error : bad coordinate values in %s (ilon=%d, ilat=%d, it=%d)\n",argc[1],ilon,ilat,it);
     fclose(fid);
     return;
   }
   ubck[ilon-1][ilat-1][it-1]=uval;
   vbck[ilon-1][ilat-1][it-1]=vval;
   count++;
 }
 if (count%(nlon*nlat)!=0) 
   fprintf(stderr,"(read_bck) Warning : only %d data was loaded(nlon*nlat=%d)\n",count,nlon*nlat);
 fclose(fid);
 fprintf(stdout,"(read_bck) %d was loaded as background\n",count);
 
}
 
void read_obs(int argv, char *argc[]) {
  /* Read observation in a ascii file
     format 
     Column 1 (%d) : dimension (1 : lon(Yi), 2 lat(Yj))
     Column 2 (%d) : time step (from 1 to jptfl)
     Column 3 (%d) : idfloat
     Column 4 (%f) : grid point of the floater
  */


  if (argv!=2) {
    printf("(read_obs) Error : read_obs with %d (1 nedded)\n",argv-1);
    printf("No obs read\n");
    return;
  }
  FILE *fid;
  int count=0;
  fid=fopen(argc[1],"r");
  if (fid==NULL) {
    printf("(read_obs) Error : unable to open %s\n",argc[1]);
    return;
  }
  int i,it,id;
  YREAL pos;
  int status=4;
  while(status==4) {
    status=fscanf(fid,"%d %d %d %lf",&i,&it,&id,&pos);
    if (status!=4) break;
    pjobs[it-1][id-1]=pos;
    status=fscanf(fid,"%d %d %d %lf",&i,&it,&id,&pos);
    if (status!=4 && i!=2) {
      printf("(read_obs) Error : unable to read obs in %s\n",argc[1]);
      return;
    }
    count++;
    piobs[it-1][id-1]=pos;
    pmask[it-1][id-1]=1;
  }
  fclose(fid);
  fprintf(stdout,"(read_obs) %d data were loaded as observations\n",count);
}

void update_uv(){
  int j,k,t;
  for (t=0;t<jtlag+1;t++)
    for (j=0;j<nlon;j++)
      for (k=0;k<nlat;k++) {
        YS1_u(j,k,t)+=YS1_u_d(j,k);
        YS1_v(j,k,t)+=YS1_v_d(j,k);
        //printf("u=%f delu=%f \n",YS1_u(j,k),YS1_u_d(j,k));    

}
}

short is_activ(int jpfl, int it) {
  /* return 1 if float jpfl is active after it (stricly) */

  //If forward mode, dont test the flaot ater it
  if (tassim==FORW)
    return 1;
  
  int jptend=it+jtlag+1;  
  it++;
  
  while(it<jptend && pmask[it][jpfl]==0)
    it++;
  return(it<jptend && pmask[it][jpfl]==1);
}

void load_eb(int iti, int itn) {
  /*Load background from ubck and vbck from time iti to itn*/
  int j,k,it;
  for (it=iti;it<itn;it++)
    for (j=0;j<nlon;j++)
      for (k=0;k<nlat;k++) {
        YS1_u(j,k,it-iti)=ubck[j][k][it];
        YS1_v(j,k,it-iti)=vbck[j][k][it];
#ifdef K_FILTER
        YS1_uc_d(j,k)=0;
        YS1_vc_d(j,k)=0;
 // Yobs_insert_data (char *nmmod, int sortie, int iaxe, int jaxe, int kaxe,int pdt, YREAL val)

          Yobs_insert_data("uc_d",0,j,k,0,-1,0);
          Yobs_insert_data("vc_d",0,j,k,0,-1,0);
#else
          Yobs_insert_data("u_d",0,j,k,0,-1,0);
          Yobs_insert_data("v_d",0,j,k,0,-1,0);

#endif

         

        
      }
}

void save_uv(int iti, int itn) {
  /* save u,v fields in umod,vmod 
   and save piret,pjret*/
  int j,k,it;
  int Yifloat=0;
  for (it=iti;it<itn;it++) {
    Yifloat=0;
    for (j=0;j<nlon;j++)
      for (k=0;k<nlat;k++) {
        umod[j][k][it]=YS1_u(j,k,it-iti);
        vmod[j][k][it]=YS1_v(j,k,it-iti);
#ifdef UPDATE_BCK
        ubck[j][k][it]=YS1_u(j,k,it-iti);
        vbck[j][k][it]=YS1_v(j,k,it-iti);
#endif
      }
    for (j=0;j<jpnfl;j++) 
      if (pmask[iti][j]==1 && is_activ(j,iti)>0 && it>iti) {
        pjret[it][j]=YS1_r_float(Yifloat,it-iti);
        piret[it][j]=YS2_r_float(Yifloat,it-iti);
        Yifloat++;
      }
  }
}

void save_output_rfloat (int argc, char *argv[]) {
  FILE *fid;
  fid=fopen(argv[1],"w");
  if (fid==NULL) {
    printf("\nfailed to open %s",argv[1]);
    exit(3);
  }
  int j,it;
  for (j=0;j<jpnfl;j++)
    for (it=0;it<jptfl;it++)
      
      fprintf(fid,"%d %d %f %f\n",it,j,piret[it][j],pjret[it][j]);
        
                                             
  fclose(fid);

}

void save_output_uv (int argc, char *argv[]) {
  FILE *fid;
  fid=fopen(argv[1],"w");
  if (fid==NULL) {
    printf("\nfailed to open %s",argv[1]);
    exit(3);
  }
  int j,k,it;
    for (it=0;it<jptfl;it++)
      for (j=0;j<nlon;j++)
        for (k=0;k<nlat;k++) {
          fprintf(fid,"%d %d %d %f %f\n",it,j,k,umod[j][k][it],vmod[j][k][it]);
        }
                                             
  fclose(fid);

}

void save_output_uinter (int argc, char *argv[]) {
  FILE *fid;
  fid=fopen(argv[1],"w");
  if (fid==NULL) {
    printf("\nfailed to open %s",argv[1]);
    exit(3);
  }
  int j,it;
  for (j=0;j<jpnfl;j++)
    for (it=0;it<jptfl;it++)
      
      fprintf(fid,"%d %d %f %f\n",it,j,piret[it][j],pjret[it][j]);
        
                                             
  fclose(fid);

}

void load_init(int it) {
  /*Load floater position at time it as init.
  Only floats that have another position after it
  Warning : nbre de flotteurs maxiums : nfloat */
  int jfl;
  int Yifloat=0;
int nfobs=0;
int jptend=it+1+jtlag;
  for (jfl=0;jfl<jpnfl;jfl++) {
    nfobs=0;
    if (pmask[it][jfl]==1 && is_activ(jfl,it)>0) {
      //Chargemente de l'init
      YS1_r_float(Yifloat,0)=pjobs[it][jfl];
      YS2_r_float(Yifloat,0)=piobs[it][jfl];
      //Chargement des obs
      for (int itfl=it+1;itfl<jptend;itfl++) {
        if (pmask[itfl][jfl]==1) {
          // Yobs_insert_data (char *nmmod, int sortie, int iaxe, int jaxe, int kaxe,int pdt, YREAL val)
          Yobs_insert_data("r_cout_d",0,Yifloat,0,0,itfl-it,pjobs[itfl][jfl]);
          Yobs_insert_data("r_cout_d",1,Yifloat,0,0,itfl-it,piobs[itfl][jfl]);
        nfobs++;
        } //if pmask==1
        } //for itfl
     
      Yifloat++; 
      printf("--Float nr %d : %d obs loaded\n",Yifloat,nfobs); 
    } //if pmask==1

    } //for jfl
  
  
  }

void myforward() {
  int niti=0; //Temps init
  int nitn=jptfl-1; //Temps fin
  char sactiv[20]="activ";
  char sMD[20]="M";
  char sonly[20]="only";
  char *liste[3];
  liste[0]=sactiv;
  liste[1]=sMD;
  liste[2]=sonly;
  
  //Mode forward
  tassim=FORW;

  //Effacement des obs potentielles
  erase_lobs();
  
  // Chargement de toutes les init et obs dans les bon pas de temps
  load_init(niti);
  
  //Réglage de l'ébauche
   load_eb(niti,nitn);
 
   //Forward
   strcpy(liste[1],"M");
   Yactraj(3, liste);
   Yset_modeltime(0);
   before_it(1);

   Yforward(-1, 0);

   //Save outputs
   save_uv(niti,nitn);

  //On se remet au mode ASSIM par défaut
  tassim=ASSIM;
}

void multirun() {
  int niti=0; //Temps init
  int nitn=jtlag;//Temps fin
    while(nitn<jptfl) {
      
      //Effacement de toutes les obs précédentes
      erase_lobs();

      // Chargement de toutes les init et obs dans les bon pas de temps
      load_init(niti);

      // Réglage de l'ébauche
      load_eb(niti,nitn);

      // Run_inc
      run_inc(0,NULL);

      // Sauvegarde du résultat dans les tableaux
      save_uv(niti,nitn);


      niti=nitn;
      nitn=niti+jtlag;
    } //while nitn<jptfl
}

void erase_udvd() {
  int j,k;
  for (j=0;j<nlon;j++)
        for (k=0;k<nlat;k++) {
#ifdef K_FILTER
          YS1_uc_d(j,k)=0;
          YS1_vc_d(j,k)=0;
#else     
          YS1_u_d(j,k)=0;
          YS1_v_d(j,k)=0;
#endif
}
}
//void load_aviso(int argc, char *argv[]){
        
//      }

void run_inc(int argv, char *argc[]) {
//Run the incremental optimisation
//1 forward of the complete model
//2 initialize du et dv to zero
//3 minimize the incremental cost function
//4 update u and v

/*
Options
nb_extiter : number of extern loop (default=6)
inc_save : save option
        0  : no save
        1 (default): save after each extern loop r_float,u and v
dirsave : directory in case inc_save>0 (default="../obs_float/")
savename : basename of save fils (default "optim")
*/
char filesave[100];
int i;

char sactiv[20]="activ";
char sMD[20]="M";
char sonly[20]="only";
char *liste[3];
liste[0]=sactiv;
liste[1]=sMD;
liste[2]=sonly;
/*Check M1QN3 config*/
 if (YNbItRun<=0)
                {        printf("runm(2): number of run iteration not seted; use set_nbiter please\n");
                         //return(0);
                }
        if (Y3ddf1<=0)
                {       printf("runm(2): expected positive fcost decrease missed; use setm_ddf1\n");
                         //return(0);
                }
        if (YioInsertObsCtr<0)
                   printf("runm(2): warning : oh oh, run with no obs !!! \n");
                
        YTypeAdjust = ADJUST_M1QN3; //d'office avec M1QN3
        
//Savestate config
 YioModulot = OFF; //On sauvegarde tous les pas de temps
    YioWrite = ON;
    YioRead= OFF;
   // Yio_savestate(cdes[1], cdes[3], 0, cdes[7]);
    YioState=0 ;//Save all states
    YioBin=OFF; //ascii output
    YioAscii=ON; //ascii output
    YioAxes=ON; //save axe numbers
printf("Run with %d ext_iter\n",nb_extiter);
for (i=0;i<nb_extiter;i++) {
printf("Ext loop %d/%d\n",i+1,nb_extiter);
     strcpy(liste[1],"M");
         Yactraj(3, liste);
         Yset_modeltime(0);
         before_it(1);
         //printf("---forward(i=%d)---\n",i);
         Yforward(-1, 0);
         
         if (inc_save==1 && i>0) {
          //save rfloat
          YioTime=ON;
          sprintf(filesave,"%sr_float_%s_%d.dat",dirsave,savename,i);
          Yio_savestate("r_float","i",0,filesave);
         }
             strcpy(liste[1],"D");
         Yactraj(3,liste);
         Yset_modeltime(0);
         
        
        Y3run ('0');
#ifdef K_FILTER
        //compute values for u_d and v_d
        strcpy(liste[1],"Tfil");
        Yactraj(3,liste);
        Yset_modeltime(0);
        before_it(1);
        Yforward(-1, 0);
        strcpy(liste[1],"T_euler_d");
        Yactraj(3,liste);
        Yset_modeltime(0);
        before_it(1);
        Yforward(-1, 0);
#endif


    update_uv();
    erase_udvd();

    if (inc_save==1) {
   
    
    //save u
    YioTime=ON;
    sprintf(filesave,"%su%s%d.dat",dirsave,savename,i+1);
    Yio_savestate("u","ij",0,filesave);
    
    //save v
    sprintf(filesave,"%sv%s%d.dat",dirsave,savename,i+1);
    Yio_savestate("v","ij",0,filesave);
    
    }
    }
    /*End of the optimization, we run another time the model to compute the final value of r_float
    */
     strcpy(liste[1],"M");
    Yactraj(3, liste);
         Yset_modeltime(0);
         before_it(1);
         Yforward(-1, 0);
 if (inc_save==1) {
          //save rfloat
          YioTime=ON;
          sprintf(filesave,"%sr_float_%s_%d.dat",dirsave,savename,nb_extiter);
          Yio_savestate("r_float","i",0,filesave);
         }
}