/* ==================================================================================
   Program to run square-root ensemble DA with Lorenz 63 model

   Language: C++

   setup gnu
   g++ lorenz_ensrkf.cpp -o lorenz_ensrkf.out
   ./lorenz_ensrkf.out S 0.01 6   3.4 -2.3 12.6   1.0 1.0 1.0   0 4.0 4.0 4.0   1 1 1   1.0 1.0 1.0   250 500 3   1 1 ID 123456

   Ross N Bannister
   National Centre for Earth Observation
   University of Reading
   r.n.bannister@reading.ac.uk

   Stefano Migliorini
   National Centre for Earth Observation
   University of Reading
   s.migliorini@reading.ac.uk

   Uses a 4th-order Runge-Kutta solver

   Modification history
   --------------------
   16/10/12 Start translation from Stefano Migliorini's IDL code (2006).  RNB

   =============================================================================== */

#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <math.h>


/* -------------------------------------------------------------------------------
   Variables that are visible from everywhere in program
   ------------------------------------------------------------------------------- */
// Max values of parameters
const int max_window      = 1001;         // Max total steps in time window (assim + fc)
const int max_nens        = 25;           // Max num ensemble members
const int max_nmeas_times = 100;          // Max ob times
const int RandTest        = 1000;         // No. of random numbers in test (opt R)

int       seed            = 7363746;      // Random number seed
int       iset            = 0;            // For random number generator
double    gset            = 0.0;          // For random number generator
int       intro_diags     = 0;            // Hard-wire for polite program messages



/* -------------------------------------------------------------------------------
   data types
   ------------------------------------------------------------------------------- */

struct user_choices
{ char   run_mode;                        // R = Test random numbers
                                          // E = Test eigenvalues and eigenvectors
                                          // F = Forecast only (single)
                                          // S = Ensemble square-root filter
  double dt;                              // Model time step
  int    nens;                            // Number of ensemble members
  double truth_ic[3];                     // Initial conditions for the truth
  double bg_std[3];                       // Initial background error st-dev
  int    moderr_flag;                     // Model error on/off
  double moderr_std[3];                   // Model error st-dev
  int    observe_xyz[3];                  // Observe x, y, z flags
  double measerr_std[3];                  // Observation error st-dev
  int    n_assim_window;                  // Assimilation window time steps
  int    n_fore_window;                   // Pure forecast window time steps
  int    n_meas_times_approx;             // Number of observation times
  int    html_style_output;               // To set html-style output
  int    diagnostics;                     // To set output of diagnostics
  char   *id;                             // Unique id
  int    seed;                            // Seed for random number generator
  int    test_mat_size;                   // Size of test matrix
};

/* -------------------------------------------------------------------------------
   Function declarations
   ------------------------------------------------------------------------------- */

void Array_2d_double_create
            ( double ***Array,
              int    ylen,
              int    xlen );

void Array_2d_double_destroy
            ( double ***Array,
              int    ylen );

int Program_Arguments
            ( int    argument_count,      // Number of arguments
              char   **argument_list,     // arguments
              struct user_choices
                     *user_parameters );  // User parameters to be read from args

void TestEigen
            ( int    N );                 // Matrix size

double fx   ( double x,
              double y,
              double z,
              double sigma );

double fy   ( double x,
              double y,
              double z,
              double r );

double fz   ( double x,
              double y,
              double z,
              double b );

void lorenz ( int    na,                  // Start time step
              int    nb,                  // End time step
              double dtt,                 // Time step
              double ens[3][::max_window],// Ensemble members
              int    moderr_flag,         // Set to include model error
              double moderr_std[3] );     // Model error standard deviation

double randomno_tophat
            ();

double randomno_normal
            ();

void OutputTimeSeq 
            ( int    na,                  // Start time step
              int    nb,                  // End time step
              double t[],                 // Times
              int    flag_state,          // Output state switch,
              double state[3][::max_window],
                                          // State
              int    flag_truth,          // Output state switch,
              double truth[3][::max_window],
                                          // Truth
              int    flag_std,            // Output standard dev switch,
              double std[3][::max_window],// Standard deviation
              char   path[],              // Output path
              char   filename[],          // Fixed part of filename
              char   pid[] );             // Process ID

void Mean   ( int    nens,                // No ensemble members
              double ens[3][::max_nens][::max_window],
                                          // Ensemble members
              int    step,                // Time step of interest
              double Mean[3] );           // Mean

void StdDev ( int    nens,                // No ensemble members
              double ens[3][::max_nens][::max_window],
                                          // Ensemble members
              int    step,                // Time step of interest
              double Mean[3],             // Mean (input)
              double StdDev[3] );         // Standard deviation


void M1M2   ( int    M1a,                 // Dimension a of matrix 1
              int    M1b,                 // Dimension b of matrix 1
              double **M1,                // Matrix 1
              int    M2b,                 // Dimension b of matrix 2
              double **M2,                // Matrix 2
              double **O );               // Output matrix

void M1TM2  ( int    M1a,                 // Dimension a of matrix 1
              int    M1b,                 // Dimension b of matrix 1
              double **M1,                // Matrix 1
              int    M2b,                 // Dimension b of matrix 2
              double **M2,                // Matrix 2
              double **O );               // Output matrix

void M1M2T  ( int    M1a,                 // Dimension a of matrix 1
              int    M1b,                 // Dimension b of matrix 1
              double **M1,                // Matrix 1
              int    M2a,                 // Dimension a of matrix 2
              double **M2,                // Matrix 2
              double **O );               // Output matrix

void lorenz_enkf
            ( double dt,                  // Model time step
              int    nens,                // Number of ensemble members
              double truth_ic[3],         // Initial conditions for the truth
              double bg_std[3],           // Initial background error st-dev
              int    moderr_flag,         // Model error on/off
              double moderr_std[3],       // Model error st-dev
              int    observe_xyz[3],      // Observe x, y, z flags
              double measerr_std[3],      // Observation error st-dev
              int    n_assim_window,      // Assimilation window time steps
              int    n_fore_window,       // Pure forecast window time steps
              int    n_meas_times_approx, // Number of observation times
              int    html_style_output,   // To set html-style output
              int    diags,               // To output diagnostics
              char   *id );               // Unique id (for path)

void Jacobi ( int    nels,
              double **A,
              double **B,
              double **T,
              double lim );

void message_i
            ( char   *text,
              int    value,
              int    html_flag,
              FILE   *unit );

void message_i_array
            ( char   *text,
              int    *value,
              int    len,
              int    html_flag,
              FILE   *unit );

void message_d
            ( char   *text,
              double value,
              int    html_flag,
              FILE   *unit );

void message_s
            ( char   *text,
              char   *value,
              int    html_flag,
              FILE   *unit );

void message_d_array
            ( char   *text,
              double *value,
              int    len,
              int    html_flag,
              FILE   *unit );




/* -------------------------------------------------------------------------------
   Main part of the program
   ------------------------------------------------------------------------------- */
int main    ( int   argument_count,
              char  **argument_list )
{ // Declare variables
  struct user_choices  user_parameters;
  int                  success;
  int                  i, j;


  if (intro_diags)
  { printf ("====================================================================\n");
    printf ("Lorenz forecast / data assimilation code\n");
    printf ("====================================================================\n");
    printf ("Stefano Migliorini (s.migliorini@reading.ac.uk)\n");
    printf ("Ross Bannister (r.n.bannister@reading.ac.uk)\n");
    printf ("c++ version (Oct 2012, RNB)\n");
    printf ("====================================================================\n");
  }

  success = Program_Arguments (argument_count, argument_list, &user_parameters);


  if (success)
  { ::seed = user_parameters.seed;

    switch (user_parameters.run_mode)
    { case 'R':
        // Test the random number generator
        // -------------------------------------
        double rnos[::RandTest];
        double m, v;
        FILE*  unit;


        // Generate the numbers and find the mean
        m    = 0.0;
        unit = fopen ("RandomNos.dat", "w");
        for (i=0; i<(::RandTest); i++)
        { rnos[i] = randomno_normal();
          m      += rnos[i];
          fprintf (unit, "%i %f\n", 0, rnos[i]);
        }
        fclose (unit);
        m /= double(::RandTest);

        // Find the variance
        v = 0.0;
        for (i=0; i<(::RandTest); i++)
        { v += (rnos[i] - m) * (rnos[i] - m);
        }
        v /= double(::RandTest - 1);
        printf ("Mean     = %f\nVariance = %f\n", m, v);
        break;
      case 'E':
        // Test the eigenvalues and eigenvectors
        // -------------------------------------
        TestEigen (user_parameters.test_mat_size);
        break;

      case 'F':
        // Run a forecast only
        // -------------------------------------
        double state[3][::max_window];
        double t[::max_window];

        // Set-up the times
        for (j=0; j<user_parameters.n_fore_window; j++)
        { t[j] = j * user_parameters.dt;
        }
        // Set the initial conditions
        for (i=0; i<3; i++)
        { state[i][0] = user_parameters.truth_ic[i];
        }
        // Run the model
        lorenz ( 0,
                 user_parameters.n_fore_window,
                 user_parameters.dt,
                 state,
                 user_parameters.moderr_flag,
                 user_parameters.moderr_std );
		 
        // Output the trajectory
        OutputTimeSeq
               ( 0,
                 user_parameters.n_fore_window,
                 t,
                 1, // Output state
                 state,
                 0, // No truth (but need something to pass)
                 state,
                 0, // No standard deviation (but need something to pass)
                 state,
                 ".",
                 "ModelTrajectory",
                 "" );

        break;

      case 'S':
        // Ensemble square-root Kalman filter
        // -------------------------------------
        lorenz_enkf
            ( user_parameters.dt,
              user_parameters.nens,
              user_parameters.truth_ic,
              user_parameters.bg_std,
              user_parameters.moderr_flag,
              user_parameters.moderr_std,
              user_parameters.observe_xyz,
              user_parameters.measerr_std,
              user_parameters.n_assim_window,
              user_parameters.n_fore_window,
              user_parameters.n_meas_times_approx,
              user_parameters.html_style_output,
              user_parameters.diagnostics,
              user_parameters.id );
    }
  }

  else

  { printf ("Program failed due to input error\n");
  }


}


/* -------------------------------------------------------------------------------
   Function definitions
   ------------------------------------------------------------------------------- */

  
// ========== Reserve space for a 2-D array ======================================
void Array_2d_double_create
            ( double ***Array,
              int    ylen,
              int    xlen )
{ // Declare local variables
  int    j;

  *Array = new double *[ylen];
  for (j=0; j<ylen; j++)
  { (*Array)[j] = new double[xlen];
  }
}

// ========== Free space from a 2-D array ========================================
void Array_2d_double_destroy
            ( double ***Array,
              int    ylen )
{ // Declare local variables
  int    j;
  for (j=0; j<ylen; j++)
  { delete (*Array)[j];
  }
  delete (*Array);
}


// ========== Extract program arguments ==========================================
int Program_Arguments
            ( int    argument_count,      // Number of arguments
              char   **argument_list,     // arguments
              struct user_choices
                     *user_parameters )   // User parameters to be read from args

{ // Declare local variables
  int    success, count, valid_mode;
  // Return 1 from subroutine if OK, 0 otherwise
  success    = 1;
  count      = 1;
  valid_mode = 0;


  if (argument_count > count)
  { (*user_parameters).run_mode = argument_list[count][0];
    // Force into upper case
    if ((*user_parameters).run_mode > 96) {(*user_parameters).run_mode -= 32;}
    if (::intro_diags) { printf ("Program run mode: %c\n", (*user_parameters).run_mode); }
  }
  else
  { success = 0;
    printf ("Please include the program run mode: R, E, F, S\n");
    printf ("  R = Test random number generator\n");
    printf ("  E = Test eigenvalues and eigenvectors\n");
    printf ("  F = Forecast only run (single)\n");
    printf ("  S = Ensemble square-root filter run\n");
  }


  // Extract arguments for chosen run mode

  // R = Test random number generator
  // --------------------------------
  if ((*user_parameters).run_mode == 'R')
  { printf ("Test random number generator\n");
    printf ("----------------------------\n");
    valid_mode = 1;

    count += 1;
    if (argument_count > count)
    { (*user_parameters).seed = atoi(argument_list[count]);
      printf ("Random number seed: %u\n", (*user_parameters).seed);
    }
    else
    { success = 0;
      printf ("Expecting random number seed (integer)\n");
    }
  }

  // E = Test eigenvalues and eigenvectors
  // -------------------------------------
  if ((*user_parameters).run_mode == 'E')
  { printf ("Test eienvalues and eigenvectors\n");
    printf ("--------------------------------\n");
    valid_mode = 1;

    count += 1;
    if (argument_count > count)
    { (*user_parameters).seed = atoi(argument_list[count]);
      printf ("Random number seed: %u\n", (*user_parameters).seed);
    }
    else
    { success = 0;
      printf ("Expecting random number seed (integer)\n");
    }

    count += 1;
    if (argument_count > count)
    { (*user_parameters).test_mat_size = atoi(argument_list[count]);
      printf ("Size of test matrix: %u\n", (*user_parameters).test_mat_size);
    }
    else
    { success = 0;
      printf ("Expecting size of test matrix (integer)\n");
    }
  }



  // F = Forecast only
  // --------------------------------
  if ((*user_parameters).run_mode == 'F')
  { printf ("Forecast only mode\n");
    printf ("----------------------------\n");
    valid_mode = 1;

    count += 1;
    if (argument_count > count)
    { (*user_parameters).dt = atof(argument_list[count]);
      printf ("Timestep: %f\n", (*user_parameters).dt);
    }
    else
    { success = 0;
      printf ("Expecting time step\n");
    }

    count += 1;
    if (argument_count > count)
    { (*user_parameters).truth_ic[0] = atof(argument_list[count]);
      printf ("x initial condition: %f\n", (*user_parameters).truth_ic[0]);
    }
    else
    { success = 0;
      printf ("Expecting x initial condition\n");
    }

    count += 1;
    if (argument_count > count)
    { (*user_parameters).truth_ic[1] = atof(argument_list[count]);
      printf ("y initial condition: %f\n", (*user_parameters).truth_ic[1]);
    }
    else
    { success = 0;
      printf ("Expecting y initial condition\n");
    }

    count += 1;
    if (argument_count > count)
    { (*user_parameters).truth_ic[2] = atof(argument_list[count]);
      printf ("z initial condition: %f\n", (*user_parameters).truth_ic[2]);
    }
    else
    { success = 0;
      printf ("Expecting z initial condition\n");
    }

    count += 1;
    if (argument_count > count)
    { (*user_parameters).moderr_flag = atoi(argument_list[count]);
      printf ("Model error flag: %u\n", (*user_parameters).moderr_flag);
    }
    else
    { success = 0;
      printf ("Expecting model error flag (0,1)\n");
    }

    count += 1;
    if (argument_count > count)
    { (*user_parameters).moderr_std[0] = atof(argument_list[count]);
      printf ("Model error x stand-dev: %f\n", (*user_parameters).moderr_std[0]);
    }
    else
    { success = 0;
      printf ("Expecting model error x stand-dev\n");
    }

    count += 1;
    if (argument_count > count)
    { (*user_parameters).moderr_std[1] = atof(argument_list[count]);
      printf ("Model error y stand-dev: %f\n", (*user_parameters).moderr_std[1]);
    }
    else
    { success = 0;
      printf ("Expecting model error y stand-dev\n");
    }

    count += 1;
    if (argument_count > count)
    { (*user_parameters).moderr_std[2] = atof(argument_list[count]);
      printf ("Model error z stand-dev: %f\n", (*user_parameters).moderr_std[2]);
    }
    else
    { success = 0;
      printf ("Expecting model error z stand-dev\n");
    }

    count += 1;
    if (argument_count > count)
    { (*user_parameters).n_fore_window = atoi(argument_list[count]);
      printf ("Length of forecast window (timesteps): %u\n",
              (*user_parameters).n_fore_window);
    }
    else
    { success = 0;
      printf ("Expecting length of forecast window (timesteps)\n");
    }

    count += 1;
    if (argument_count > count)
    { (*user_parameters).html_style_output = atoi(argument_list[count]);
      printf ("Html style output: %u\n", (*user_parameters).html_style_output);
    }
    else
    { success = 0;
      printf ("Expecting html style output flag (0,1)\n");
    }

    count += 1;
    if (argument_count > count)
    { (*user_parameters).diagnostics = atoi(argument_list[count]);
      printf ("Diagnostics: %u\n", (*user_parameters).diagnostics);
    }
    else
    { success = 0;
      printf ("Diagnostics flag (0,1)\n");
    }

    count += 1;
    if (argument_count > count)
    { (*user_parameters).id = argument_list[count];
      printf ("Unique id: %s\n", (*user_parameters).id);
    }
    else
    { success = 0;
      printf ("Expecting unique id\n");
    }

    count += 1;
    if (argument_count > count)
    { (*user_parameters).seed = atoi(argument_list[count]);
      printf ("Random number seed: %u\n", (*user_parameters).seed);
    }
    else
    { success = 0;
      printf ("Expecting random number seed (integer)\n");
    }


    // Parameter checks
    if ((*user_parameters).n_fore_window > ::max_window)
    { printf ("Forecast window length must not exceed %u\n", ::max_window-1);
      success = 0;
    }

  }



  // S = Ensemble square-root filter
  // --------------------------------
  if ((*user_parameters).run_mode == 'S')
  { if (::intro_diags) { printf ("Ensemble sqrt filter mode\n");
                         printf ("----------------------------\n"); }
    valid_mode = 1;

    count += 1;
    if (argument_count > count)
    { (*user_parameters).dt = atof(argument_list[count]);
      if (::intro_diags) { printf ("Timestep: %f\n", (*user_parameters).dt); }
    }
    else
    { success = 0;
      printf ("Expecting time step\n");
    }

    count += 1;
    if (argument_count > count)
    { (*user_parameters).nens = atoi(argument_list[count]);
      if (::intro_diags) { printf ("Number of ensemble members: %i\n", (*user_parameters).nens); }
    }
    else
    { success = 0;
      printf ("Expecting number of ensemble members\n");
    }

    count += 1;
    if (argument_count > count)
    { (*user_parameters).truth_ic[0] = atof(argument_list[count]);
      if (::intro_diags) { printf ("x truth initial condition: %f\n", (*user_parameters).truth_ic[0]); }
    }
    else
    { success = 0;
      printf ("Expecting x truth initial condition\n");
    }

    count += 1;
    if (argument_count > count)
    { (*user_parameters).truth_ic[1] = atof(argument_list[count]);
      if (::intro_diags) { printf ("y truth initial condition: %f\n", (*user_parameters).truth_ic[1]); }
    }
    else
    { success = 0;
      printf ("Expecting y truth initial condition\n");
    }

    count += 1;
    if (argument_count > count)
    { (*user_parameters).truth_ic[2] = atof(argument_list[count]);
      if (::intro_diags) { printf ("z truth initial condition: %f\n", (*user_parameters).truth_ic[2]); }
    }
    else
    { success = 0;
      printf ("Expecting z truth initial condition\n");
    }

    count += 1;
    if (argument_count > count)
    { (*user_parameters).bg_std[0] = atof(argument_list[count]);
      if (::intro_diags) { printf ("Background error x stand-dev: %f\n", (*user_parameters).bg_std[0]); }
    }
    else
    { success = 0;
      printf ("Expecting background error x stand-dev\n");
    }

    count += 1;
    if (argument_count > count)
    { (*user_parameters).bg_std[1] = atof(argument_list[count]);
      if (::intro_diags) { printf ("Background error y stand-dev: %f\n", (*user_parameters).bg_std[1]); }
    }
    else
    { success = 0;
      printf ("Expecting background error y stand-dev\n");
    }

    count += 1;
    if (argument_count > count)
    { (*user_parameters).bg_std[2] = atof(argument_list[count]);
      if (::intro_diags) { printf ("Background error z stand-dev: %f\n", (*user_parameters).bg_std[2]); }
    }
    else
    { success = 0;
      printf ("Expecting background error z stand-dev\n");
    }

    count += 1;
    if (argument_count > count)
    { (*user_parameters).moderr_flag = atoi(argument_list[count]);
      if (::intro_diags) { printf ("Model error flag: %u\n", (*user_parameters).moderr_flag); }
    }
    else
    { success = 0;
      printf ("Expecting model error flag (0,1)\n");
    }

    count += 1;
    if (argument_count > count)
    { (*user_parameters).moderr_std[0] = atof(argument_list[count]);
      if (::intro_diags) { printf ("Model error x stand-dev: %f\n", (*user_parameters).moderr_std[0]); }
    }
    else
    { success = 0;
      printf ("Expecting model error x stand-dev\n");
    }

    count += 1;
    if (argument_count > count)
    { (*user_parameters).moderr_std[1] = atof(argument_list[count]);
      if (::intro_diags) { printf ("Model error y stand-dev: %f\n", (*user_parameters).moderr_std[1]); }
    }
    else
    { success = 0;
      printf ("Expecting model error y stand-dev\n");
    }

    count += 1;
    if (argument_count > count)
    { (*user_parameters).moderr_std[2] = atof(argument_list[count]);
      if (::intro_diags) { printf ("Model error z stand-dev: %f\n", (*user_parameters).moderr_std[2]); }
    }
    else
    { success = 0;
      printf ("Expecting model error z stand-dev\n");
    }

    count += 1;
    if (argument_count > count)
    { (*user_parameters).observe_xyz[0] = atoi(argument_list[count]);
      if (::intro_diags) { printf ("Observe x variable flag: %i\n", (*user_parameters).observe_xyz[0]); }
    }
    else
    { success = 0;
      printf ("Expecting observe x variable flag\n");
    }

    count += 1;
    if (argument_count > count)
    { (*user_parameters).observe_xyz[1] = atoi(argument_list[count]);
      if (::intro_diags) { printf ("Observe y variable flag: %i\n", (*user_parameters).observe_xyz[1]); }
    }
    else
    { success = 0;
      printf ("Expecting observe y variable flag\n");
    }

    count += 1;
    if (argument_count > count)
    { (*user_parameters).observe_xyz[2] = atoi(argument_list[count]);
      if (::intro_diags) { printf ("Observe z variable flag: %i\n", (*user_parameters).observe_xyz[2]); }
    }
    else
    { success = 0;
      printf ("Expecting observe z variable flag\n");
    }

    count += 1;
    if (argument_count > count)
    { (*user_parameters).measerr_std[0] = atof(argument_list[count]);
      if (::intro_diags) { printf ("Observation error x stand-dev: %f\n",
                           (*user_parameters).measerr_std[0]); }
    }
    else
    { success = 0;
      printf ("Expecting observation error x stand-dev\n");
    }

    count += 1;
    if (argument_count > count)
    { (*user_parameters).measerr_std[1] = atof(argument_list[count]);
      if (::intro_diags) { printf ("Observation error y stand-dev: %f\n",
                           (*user_parameters).measerr_std[1]); }
    }
    else
    { success = 0;
      printf ("Expecting observation error y stand-dev\n");
    }

    count += 1;
    if (argument_count > count)
    { (*user_parameters).measerr_std[2] = atof(argument_list[count]);
      if (::intro_diags) { printf ("Observation error z stand-dev: %f\n",
                           (*user_parameters).measerr_std[2]); }
    }
    else
    { success = 0;
      printf ("Expecting observation error z stand-dev\n");
    }

    count += 1;
    if (argument_count > count)
    { (*user_parameters).n_assim_window = atoi(argument_list[count]);
      if (::intro_diags) { printf ("Length of assimilation window (timesteps): %u\n",
                           (*user_parameters).n_assim_window); }
    }
    else
    { success = 0;
      printf ("Expecting length of assimilation window (timesteps)\n");
    }

    count += 1;
    if (argument_count > count)
    { (*user_parameters).n_fore_window = atoi(argument_list[count]);
      if (::intro_diags) { printf ("Length of forecast window (timesteps): %u\n",
                           (*user_parameters).n_fore_window); }
    }
    else
    { success = 0;
      printf ("Expecting length of forecast window (timesteps)\n");
    }

    count += 1;
    if (argument_count > count)
    { (*user_parameters).n_meas_times_approx = atoi(argument_list[count]);
      if (::intro_diags) { printf ("Rough number of observation times: %u\n",
                           (*user_parameters).n_meas_times_approx); }
    }
    else
    { success = 0;
      printf ("Expecting rough number of observation times\n");
    }

    count += 1;
    if (argument_count > count)
    { (*user_parameters).html_style_output = atoi(argument_list[count]);
      if (::intro_diags) { printf ("Html style output: %u\n",
                           (*user_parameters).html_style_output); }
    }
    else
    { success = 0;
      printf ("Expecting html style output flag (0,1)\n");
    }

    count += 1;
    if (argument_count > count)
    { (*user_parameters).diagnostics = atoi(argument_list[count]);
      if (::intro_diags) { printf ("Diagnostics: %u\n", (*user_parameters).diagnostics); }
    }
    else
    { success = 0;
      printf ("Diagnostics flag (0,1)\n");
    }

    count += 1;
    if (argument_count > count)
    { (*user_parameters).id = argument_list[count];
      if (::intro_diags) { printf ("Unique id: %s\n", (*user_parameters).id); }
    }
    else
    { success = 0;
      printf ("Expecting unique id\n");
    }

    count += 1;
    if (argument_count > count)
    { (*user_parameters).seed = atoi(argument_list[count]);
      if (::intro_diags) { printf ("Random number seed: %u\n", (*user_parameters).seed); }
    }
    else
    { success = 0;
      printf ("Expecting random number seed (integer)\n");
    }


    // Parameter checks
    if ((*user_parameters).nens > ::max_nens)
    { printf ("Number of ensemble members must not exceed %u\n", ::max_nens);
      success = 0;
    }
    if ((*user_parameters).nens < 2)
    { printf ("Number of ensemble members must not be less than 2\n");
      success = 0;
    }

    if ( ((*user_parameters).n_assim_window + (*user_parameters).n_fore_window)
         > (::max_window-1))
    { printf ("Assim + fcast window length must not exceed %u\n", ::max_window-1);
      success = 0;
    }
    if ( (*user_parameters).n_meas_times_approx > ::max_nmeas_times)
    { printf ("Number of observation times must not exceed %u\n", ::max_nmeas_times);
      success = 0;
    }

  }

  return success;
}



// ========== Test eigenvalues and eigenvectors ==================================
void TestEigen
            ( int    N )                  // Matrix size
{ // Declare variables
  double               **A, **EVal, **EVec, **VecA, **VecB;
  double               sum;
  int                  i, j;

  // Set-up matrices
  Array_2d_double_create (&A, N, N);
  Array_2d_double_create (&EVec, N, N);
  Array_2d_double_create (&EVal, N, N);
  Array_2d_double_create (&VecA, N, 1);
  Array_2d_double_create (&VecB, N, 1);
  printf ("Created arrays\n");


  // Fill with numbers
  for (i=0; i<N; i++)
  { for (j=i; j<N; j++)
    { A[i][j] = randomno_normal();
      if (i != j)
      { A[j][i] = A[i][j];
      }
    }
  }

  // Print the matrix
  printf ("This is the matrix to be tested\n");
  for (i=0; i<N; i++)
  { for (j=0; j<N; j++)
    { printf ("%f  ", A[i][j]);
    }
    printf ("\n");
  }

  // Find eigenvalues and eigenvectors
  Jacobi (N, A, EVal, EVec, 0.0001);

  // Check that the eigenvectors and eigenvalues are correct
  for (i=0; i<N; i++)
  { printf ("Checking eigenvector %u\n", i+1);
    printf ("E-vec: ");
    for (j=0; j<N; j++)
    { printf("%f  ", EVec[j][i]);
    }
    printf ("\nE-val: %f\n", EVal[i][i]);
    // Copy this eigenvector into VecA
    for (j=0; j<N; j++)
    { VecA[j][0] = EVec[j][i];
    }
    // Act with the matrix on this eignvector
    M1M2 (N, N, A, 1, VecA, VecB);
    // Remove eigenvalue time eigenvector
    for (j=0; j<N; j++)
    { VecB[j][0] -= EVal[i][i] * VecA[j][0];
    }
    // Calculate norm of this vector
    sum = 0.0;
    for (j=0; j<N; j++)
    { sum += VecB[j][0] * VecB[j][0];
    }
    printf ("|A * e-vec - lambda * e-vec|2 = %f\n", sqrt(sum));
  }

  // Remove arrays
  Array_2d_double_destroy (&A, N);
  Array_2d_double_destroy (&EVec, N);
  Array_2d_double_destroy (&EVal, N);
  Array_2d_double_destroy (&VecA, N);
  Array_2d_double_destroy (&VecB, N);

}


// ========== dx/dt function =====================================================
double fx (double x,
           double y,
           double z,
           double sigma )
{ double result;
  result = -1.0*sigma*x + sigma*y;
  return result;
}

// ========== dy/dt function =====================================================
double fy (double x,
           double y,
           double z,
           double r )
{ double result;
  result = -1.0*x*z + r*x - y;
  return result;
}

// ========== dz/dt function =====================================================
double fz (double x,
           double y,
           double z,
           double b )
{ double result;
  result = x*y - b*z;
  return result;
}

// ========== Run Lorenz 63 ======================================================
void lorenz ( int    na,                  // Start time step
              int    nb,                  // End time step
              double dtt,                 // Time step
              double ens[3][::max_window],// Ensemble members
              int    moderr_flag,         // Set to include model error
              double moderr_std[3] )      // Model error standard deviation

{ double dt, b, r, sigma, x, y, z, sdt;
  double f1, g1, h1, f2, g2, h2, f3, g3, h3, f4, g4, h4;
  int    nsteps, j, jj;

  // For integration purposes, the increment should not be much greater than 0.01
  // if dt is greater than 0.01 then dt/0.01 time steps are calculated before each output
  if (dtt > 0.01)
  { nsteps = int(dtt / 0.01);
    printf ("%u intermediate steps are calculated\n", nsteps);
    dt = 0.01;
  }
  else
  { nsteps = 1;
    dt     = dtt;
  }

  sdt = sqrt(dt);
   
  if (nb < ::max_window)
  { // Parameters
    b     = 8.0 / 3.0;  // geometric factor
    r     = 28.0;       // Rayleigh number
    sigma = 10.0;       // Prandl number

    x     = ens[0][na];
    y     = ens[1][na];
    z     = ens[2][na];


    for (j=na+1; j<=nb; j++)
    { for (jj=1; jj<=nsteps; jj++)
      { //4th order Runge-Kutta
        f1 = fx(x, y, z, sigma);
        g1 = fy(x, y, z, r);
        h1 = fz(x, y, z, b);
        f2 = fx(x+dt*f1/2.0, y+dt*g1/2.0, z+dt*h1/2.0, sigma);
        g2 = fy(x+dt*f1/2.0, y+dt*g1/2.0, z+dt*h1/2.0, r);
        h2 = fz(x+dt*f1/2.0, y+dt*g1/2.0, z+dt*h1/2.0, b);
        f3 = fx(x+dt*f2/2.0, y+dt*g2/2.0, z+dt*h2/2.0, sigma);
        g3 = fy(x+dt*f2/2.0, y+dt*g2/2.0, z+dt*h2/2.0, r);
        h3 = fz(x+dt*f2/2.0, y+dt*g2/2.0, z+dt*h2/2.0, b);
        f4 = fx(x+dt*f3, y+dt*g3, z+dt*h3, sigma);
        g4 = fy(x+dt*f3, y+dt*g3, z+dt*h3, r);
        h4 = fz(x+dt*f3, y+dt*g3, z+dt*h3, b);
        x  = x + dt * (f1+2.0*f2+2.0*f3+f4) / 6.0;
        y  = y + dt * (g1+2.0*g2+2.0*g3+g4) / 6.0;
        z  = z + dt * (h1+2.0*h2+2.0*h3+h4) / 6.0;
      }

      if (!moderr_flag)
      { // No model error
        ens[0][j] = x;
        ens[1][j] = y;
        ens[2][j] = z;
      }
      else
      { // Add model error
        ens[0][j] = x + sdt * moderr_std[0] * randomno_normal();
        ens[1][j] = y + sdt * moderr_std[1] * randomno_normal();
        ens[2][j] = z + sdt * moderr_std[2] * randomno_normal();
      }
    }
  }
}


// ========== Generate a random number between -1 and 1 ==========================
   double randomno_tophat ()
// This routine uses (and modifies) global variable ::seed
{ // Declare local variables
  int    k;
  int    a    = 16807;
  int    m    = 2147483647;
  int    q    = 127773;
  int    r    = 2836;
  int    mask = 123459876;
  double am, result;

  am       = 1.0 / m;
  ::seed   = ::seed ^ mask;
  k        = ::seed / q;
  ::seed   = a * (::seed - k*q) - r*k;
  if (::seed < 0)
  { ::seed += m;
  }
  result   = 2.0 * am * ::seed - 1.0;
  ::seed = ::seed ^ mask;
  return result;
}


// ========== Generate a normal random number with mean zero and variance unity ==
   double randomno_normal ()
// This routine uses (and modifies) global variables ::iset and ::gset
{ // Declare local variables
  double fac, rsq, v1, v2, result;

  if (::iset == 0)
  { do
    { v1  = randomno_tophat();
      v2  = randomno_tophat();
      rsq = v1*v1 + v2*v2;
    }
    while ((rsq >= 1) || (rsq == 0));
    fac    = sqrt(-2.0 * log(rsq) / rsq);
    ::gset = v1 * fac;
    result = v2 * fac;
    ::iset = 1;
  }
  else
  { result = ::gset;
    ::iset = 0;
  }

  return result;
}


// ========== Output time sequence ===============================================
void OutputTimeSeq 
            ( int    na,                  // Start time step
              int    nb,                  // End time step
              double t[],                 // Times
              int    flag_state,          // Output state switch,
              double state[3][::max_window],
                                          // State
              int    flag_truth,          // Output state switch,
              double truth[3][::max_window],
                                          // Truth
              int    flag_std,            // Output standard dev switch,
              double std[3][::max_window],// Standard deviation
              char   path[],              // Output path
              char   filename[],          // Fixed part of filename
              char   pid[] )              // Process ID

{ FILE* unit = NULL;
  int   i, j;
  char  wholepath[100];
  char  slash[] = "/";
  char  dat[] = ".dat";


  // Construct filename: path + "/" + filename + pid + ".dat"
  strcpy (wholepath, path);
  strcat (wholepath, slash);
  strcat (wholepath, filename);
  strcat (wholepath, pid);
  strcat (wholepath, dat);
  // printf ("<br><br> Output data file : %s#<br>\n", wholepath);

  unit = fopen (wholepath, "w");
  fprintf (unit, "# Time,   state (x, y, z),   error (x, y, z),   std dev (x, y, z)\n");
  for (j=na; j<nb; j++)
  { fprintf (unit, "%f", t[j]);
    // Output state
    if (flag_state)
    { for (i=0; i<3; i++)
      { fprintf (unit, " %f", state[i][j]);
      }
    }
    else
    { fprintf (unit, " %f %f %f", 0.0, 0.0, 0.0);
    }
    fprintf (unit, "   ");

    // Output error (only if truth is present)
    if (flag_truth)
    { for (i=0; i<3; i++)
      { fprintf (unit, " %f", state[i][j] - truth[i][j]);
      }
    }
    else
    { fprintf (unit, " %f %f %f", 0.0, 0.0, 0.0);
    }
    fprintf (unit, "   ");

    // Output standard deviation (only if present)
    if (flag_std)
    { for (i=0; i<3; i++)
      { fprintf (unit, " %f", std[i][j]);
      }
    }
    else
    { fprintf (unit, " %f %f %f", 0.0, 0.0, 0.0);
    }
    fprintf (unit, "\n");
  }
  fclose (unit);
}



// ========== Find the mean of an enemble ========================================
void Mean   ( int    nens,                // Number of ensemble members
              double ens[3][::max_nens][::max_window],
                                          // Ensemble members
              int    step,                // Time step of interest
              double Mean[3] )            // Mean

{ int    i, k;
  double sum;

  for (i=0; i<3; i++)
  { // Calculate the mean
    sum = 0.0;
    for (k=0; k<nens; k++)
    { sum += ens[i][k][step];
    }
    Mean[i] = sum / double(nens);
  }
}


// ========== Find the mean and standard deviation of an enemble =================
void StdDev ( int    nens,                // Number of ensemble members
              double ens[3][::max_nens][::max_window],
                                          // Ensemble members
              int    step,                // Time step of interest
              double Mean[3],             // Mean
              double StdDev[3] )          // Standard deviation
{ int    i, k;
  double sum, value;

  for (i=0; i<3; i++)
  { // Calculate the standard deviation
    sum = 0.0;
    for (k=0; k<nens; k++)
    { value = ens[i][k][step] - Mean[i];
      sum  += value * value;
    }
    StdDev[i] = sqrt(sum/double(nens-1));
  }
}


// ========== Matrix multiplication O = M1 M2 ====================================
void M1M2   ( int    M1a,                 // Dimension a of matrix 1
              int    M1b,                 // Dimension b of matrix 1
              double **M1,                // Matrix 1
              int    M2b,                 // Dimension b of matrix 2
              double **M2,                // Matrix 2
              double **O )                // Output matrix

{ int    i, j, k;
  double sum, value;

  for (i=0; i<M1a; i++)
  { for (j=0; j<M2b; j++)
    { sum = 0.0;
      for (k=0; k<M1b; k++)
      { sum += M1[i][k] * M2[k][j];
      }
      O[i][j] = sum;
    }
  }
}


// ========== Matrix multiplication O = M1^T M2 ==================================
void M1TM2  ( int    M1a,                 // Dimension a of matrix 1
              int    M1b,                 // Dimension b of matrix 1
              double **M1,                // Matrix 1
              int    M2b,                 // Dimension b of matrix 2
              double **M2,                // Matrix 2
              double **O )                // Output matrix

{ int    i, j, k;
  double sum, value;

  for (i=0; i<M1b; i++)
  { for (j=0; j<M2b; j++)
    { sum = 0.0;
      for (k=0; k<M1a; k++)
      { sum += M1[k][i] * M2[k][j];
      }
      O[i][j] = sum;
    }
  }
}


// ========== Matrix multiplication O = M1 M2^T ==================================
void M1M2T  ( int    M1a,                 // Dimension a of matrix 1
              int    M1b,                 // Dimension b of matrix 1
              double **M1,                // Matrix 1
              int    M2a,                 // Dimension a of matrix 2
              double **M2,                // Matrix 2
              double **O )                // Output matrix

{ int    i, j, k;
  double sum, value;

  for (i=0; i<M1a; i++)
  { for (j=0; j<M2a; j++)
    { sum = 0.0;
      for (k=0; k<M1b; k++)
      { sum += M1[i][k] * M2[j][k];
      }
      O[i][j] = sum;
    }
  }
}




// ========== Lorenz ENKF (square-root) ==========================================
void lorenz_enkf
            ( double dt,                  // Model time step
              int    nens,                // Number of ensemble members
              double truth_ic[3],         // Initial conditions for the truth
              double bg_std[3],           // Initial background error st-dev
              int    moderr_flag,         // Model error on/off
              double moderr_std[3],       // Model error st-dev
              int    observe_xyz[3],      // Observe x, y, z flags
              double measerr_std[3],      // Observation error st-dev
              int    n_assim_window,      // Assimilation window time steps
              int    n_fore_window,       // Pure forecast window time steps
              int    n_meas_times_approx, // Number of observation times
              int    html_style_output,   // To set html-style output
              int    diags,               // To output diagnostics
              char   id[] )               // Unique id (for path)

{ // Declare variables
  int    nmeas1t;                     // No observed components at obs times
  int    n_meas_times;                // Adjusted number of measurement times
  int    na, nb;                      // The start and end steps for a particular stage
  double truth[3][::max_window];      // Truth trajectory
  double cenfc[3][::max_window];      // Central forecast
  double bgfc[3][::max_window];       // Forecast starting from the background state
  double OneMember[3][::max_window];  // To pass to/from lorenz model
  double StdDevs[3][::max_window];    // Standard deviations
  int    n_total_window;              // Total number of steps
  double t[::max_window];             // Time values of time steps
  double A[3][::max_nens][::max_window];
                                      // Ensemble members
  double mean[3], stddev[3];          // Mean and standard deviation
  double **H;                         // Observation operator
  double **R;                         // Observation err covariance matrix
  int    j, jj, i, ii, k, kk;         // Loop indices
  int    deltaobs;                    // Time steps between observations
  int    obstep[::max_nmeas_times];   // Array of observation time indices
  double **yy;                        // Array of all measurements
  double **ym;                        // Array of measurements at one time
  double **d;                         // Innovation vector
  double meanAf[3];                   // Mean forecast at particular time
  double meanAa[3];                   // Mean analysis at particular time
  double **Afp;                       // Forecast perturbations
  double **Aap;                       // Analysis perturbations
  double **S;                         // Forecast perturbations in obs space
  double **C;                         // SS^T
  double **Z, **Lambda;               // Eigenvectors and eigenvalues (of C)
  double mineval, sum, sqrtevali, ele, nensm1;
  double **vec1, **vec2, **vec3, **X, **XTX, **V, **SigmaTSigma;
  double **analinc;
  double **ScaledVT, **Trans;
  double temp[3];
  FILE   *html_unit = NULL;
  FILE   *obfile = NULL;
  char  whole_html_path[100];
  char  whole_obs_path[3][100];

  // ===== 1. Intitial set-up of the system =====

  // Set-up the verbose output file
  strcpy (whole_html_path, "/tmp/Verbose");
  strcat (whole_html_path, id);
  strcat (whole_html_path, ".html");
  html_unit = fopen (whole_html_path, "w");


  n_total_window = n_assim_window + n_fore_window;
  nensm1         = double(nens-1);

  // 1.1 Set-up the time values
  for (j=0; j<n_total_window; j++)
  { t[j] = j * dt;
  }

  // 1.2 Set the 'true' initial conditions
  for (i=0; i<3; i++)
  { truth[i][0] = truth_ic[i];
  }

  // 1.3 Calculate the truth trajectory
  if (diags) { printf ("Generating the truth trajectory\n"); }
  // The truth trajectory is always done with no model noise
  lorenz (0, n_total_window, dt, truth, 0, moderr_std);
  OutputTimeSeq (0, n_total_window, t, 1, truth, 0, truth, 0, truth,
                 "/tmp", "Truth", id);

  // 1.4 Find out exactly how many observation times
  // Time steps between observations
  deltaobs     = int(double(n_assim_window) / double(n_meas_times_approx-1));
  n_meas_times = int(double(n_assim_window) / double(deltaobs)) + 1;
  if (diags)
  { printf ("Timesteps between obs %u\n", deltaobs);
    printf ("Number of observation times %u\n", n_meas_times);
  }

  // 1.5 Find the number of observed elements at each observation time and report
  if (diags) { printf ("Observe flags for each element %u %u %u\n",
               observe_xyz[0], observe_xyz[1], observe_xyz[2]); }
  nmeas1t = 0;
  for (i=0; i<3; i++)
  { if (observe_xyz[i])
    { nmeas1t += 1;
    }
  }
  if (nmeas1t == 0)
  { printf ("Fatal error: at least one component of the state must be observed!\n");
    exit(1);
  }
  else
  { if (html_style_output)
    { fprintf (html_unit, "<table cols=2>\n");
      fprintf (html_unit, "<tr><td valign=top>\n");
      fprintf (html_unit, "<table cols=2 border=1>\n");
    }
    message_s ("<b>Summary of input specifications</b>", "", html_style_output, html_unit);
    message_i  ("Number of ensemble members", nens, html_style_output, html_unit);
    message_d_array ("Initial conditions for the truth", truth_ic, 3, html_style_output,
                     html_unit);
    message_d_array ("Initial b/g error standard devs", bg_std, 3, html_style_output,
                     html_unit);
    message_i  ("Model error 0/1", moderr_flag, html_style_output, html_unit);
    if (moderr_flag)
    { message_d_array ("Model error standard devs", moderr_std, 3, html_style_output,
                       html_unit);
    }
    message_i_array ("x, y, z measurement flags", observe_xyz, 3, html_style_output,
                     html_unit);
    message_d_array ("Meas error standard devs", measerr_std,  nmeas1t, html_style_output,
                     html_unit);
    message_i  ("Assimilation window time steps", n_assim_window, html_style_output,
                html_unit);
    message_i  ("Forecast window time steps", n_fore_window, html_style_output, html_unit);
    message_i  ("Number of measurement times", n_meas_times, html_style_output, html_unit);
    message_s ("<b>Figure key</b>", "", html_style_output, html_unit);
    message_s ("<font color=0000FF>Blue</font>", "<font color=0000FF>Truth trajectory</font>", html_style_output, html_unit);
    message_s ("<font color=FF0000>Red</font>", "<font color=FF0000>Analysis/forecast trajectory</font>", html_style_output, html_unit);
    message_s ("<font color=808080>Grey</font>", "<font color=808080>Analysis/forecast 1 std err bars</font>", html_style_output, html_unit);
    message_s ("<font color=00FF00>Green</font>", "<font color=00FF00>Observations and 1 std err bars</font>", html_style_output, html_unit);
    if (html_style_output)
    { fprintf (html_unit, "</table>\n");
      fprintf (html_unit, "</td>\n");
    }
  }
  if (diags) { printf ("Number of elements observed %u\n", nmeas1t); }


  // 1.6 Allocate variable-size data arrays
  // H is nmeas1t x 3 array
  Array_2d_double_create (&H, nmeas1t, 3);
  // R is nmeas1t x nmeas1t array
  Array_2d_double_create (&R, nmeas1t, nmeas1t);
  // Afp is 3 x nens array
  Array_2d_double_create (&Afp, 3, nens);
  // Aap is 3 x nens array
  Array_2d_double_create (&Aap, 3, nens);
  // S is nmeas1t x nens array
  Array_2d_double_create (&S, nmeas1t, nens);
  // C is nmeas1t x nmeas1t array
  Array_2d_double_create (&C, nmeas1t, nmeas1t);
  // Lambda is nmeas1t x nmeas1t array
  Array_2d_double_create (&Lambda, nmeas1t, nmeas1t);
  // Z is nmeas1t x nmeas1t array
  Array_2d_double_create (&Z, nmeas1t, nmeas1t);
  // yy is nmeas1t x n_meas_times array
  Array_2d_double_create (&yy, nmeas1t, n_meas_times);
  // ym is nmeas1t x 1 array
  Array_2d_double_create (&ym, nmeas1t, 1);
  // d is nmeas1t x 1 array
  Array_2d_double_create (&d, nmeas1t, 1);
  // vec1 is nmeas1t x 1 array
  Array_2d_double_create (&vec1, nmeas1t, 1);
  // vec2 is nmeas1t x 1 array
  Array_2d_double_create (&vec2, nmeas1t, 1);
  // vec3 is nens x 1 array
  Array_2d_double_create (&vec3, nens, 1);
  // analinc is 3 x 1 array
  Array_2d_double_create (&analinc, 3, 1);
  // X is nmeas1t x nens array
  Array_2d_double_create (&X, nmeas1t, nens);
  // XTX is nens x nens array
  Array_2d_double_create (&XTX, nens, nens);
  // V is nens x nens array
  Array_2d_double_create (&V, nens, nens);
  // SigmaTSigma is nens x nens array
  Array_2d_double_create (&SigmaTSigma, nens, nens);
  // ScaledVT is nens x nens array
  Array_2d_double_create (&ScaledVT, nens, nens);
  // Trans is nens x nens array
  Array_2d_double_create (&Trans, nens, nens);


  // ===== 2. Set-up the initial central forecast and ensemble =====

  // 2.1 Generate initial conditions for the central forecast (and initial background)
  for (i=0; i<3; i++)
  { cenfc[i][0] = truth[i][0] +  bg_std[i] * randomno_normal();
    bgfc[i][0]  = cenfc[i][0];   // For background trajectory
  }
  if (diags) { printf ("Central forecast at time 0 %f %f %f\n",
               cenfc[0][0], cenfc[1][0], cenfc[2][0]); }
  if (diags)
  { printf ("Here are the central forecast components (a)\n");
    printf ("%f  %f  %f\n", cenfc[0][0],  cenfc[1][0], cenfc[2][0]);
  }

  // 2.2 Calculate the background trajectory
  if (diags) { printf ("Generating the background trajectory\n"); }
  lorenz (0, n_total_window, dt, bgfc, moderr_flag, moderr_std);
  OutputTimeSeq (0, n_total_window, t, 1, bgfc, 1, truth, 0, truth,
                 "/tmp", "Back", id);

  if (diags)
  { printf ("Here are the central forecast components (b)\n");
    printf ("%f  %f  %f\n", cenfc[0][0],  cenfc[1][0], cenfc[2][0]);
  }


  // 2.3 Set the initial ensemble members
  // Insert random numbers between 0 and 1
  for (k=0; k<nens; k++)
  { for (i=0; i<3; i++)
    { A[i][k][0] = randomno_normal();
    }
  }

  if (diags)
  { printf ("Here are the central forecast components (c)\n");
    printf ("%f  %f  %f\n", cenfc[0][0],  cenfc[1][0], cenfc[2][0]);
  }

  if (diags)
  { printf ("Here are the random numbers\n");
    for (k=0; k<nens; k++)
    { printf ("Member %u : %f  %f  %f\n", k, A[0][k][0], A[1][k][0], A[2][k][0] );
    }
  }

  // Find the mean and standard deviation
  Mean (nens, A, 0, mean);
  StdDev (nens, A, 0, mean, stddev);
  // Transform so that ensemble has mean=0 and standard deviation=1 exactly
  for (k=0; k<nens; k++)
  { for (i=0; i<3; i++)
    { A[i][k][0] = (A[i][k][0] - mean[i]) / stddev[i];
    }
  }

  if (diags)
  { printf ("Here are the normalized random numbers\n");
    for (k=0; k<nens; k++)
    { printf ("Member %u : %f  %f  %f\n", k, A[0][k][0], A[1][k][0], A[2][k][0] );
    }
  }

  if (diags)
  { printf ("Here are the background standard deviations\n");
    printf ("%f  %f  %f\n", bg_std[0],  bg_std[1], bg_std[2]);
  }
  if (diags)
  { printf ("Here are the central forecast components (d)\n");
    printf ("%f  %f  %f\n", cenfc[0][0],  cenfc[1][0], cenfc[2][0]);
  }


  // Transform so that ensemble has mean=central forecast and std-dev bg_std
  for (k=0; k<nens; k++)
  { for (i=0; i<3; i++)
    { A[i][k][0] = cenfc[i][0] + bg_std[i] * A[i][k][0];
    }
  }
  if (diags)
  { printf ("Here are the ensemble members at t=0\n");
    for (k=0; k<nens; k++)
    { printf ("Member %u : %f  %f  %f\n", k, A[0][k][0], A[1][k][0], A[2][k][0] );
    }
  }


  // ===== 3. Set-up the observations from the reference (true) trajectory =====

  // 3.1 Create array of obs times
  for (j=0; j<n_meas_times; j++)
  { obstep[j] = deltaobs * j;     // index of obs time
  }

  // 3.2 Create synthetic observations
  for (j=0; j<n_meas_times; j++)
  { ii = -1;
    for (i=0; i<3; i++)
    { if (observe_xyz[i])
      { ii++;
        yy[ii][j] = truth[i][obstep[j]] + measerr_std[i] * randomno_normal();
      }
    }
  }

  // Output the observation details
  { if (html_style_output)
    { fprintf (html_unit, "<td valign=top>\n");
      fprintf (html_unit, "<table cols=2 border=1>\n");
    }
    message_s ("<b>Observation specifications</b>", "", html_style_output, html_unit);
    for (j=0; j<n_meas_times; j++)
    { message_i ("<b>Batch</b>", j+1, html_style_output, html_unit);
      message_i ("Timestep", obstep[j], html_style_output, html_unit);
      message_d ("Time", obstep[j]*dt, html_style_output, html_unit);
      for (i=0; i<nmeas1t; i++) { temp[i] = yy[i][j]; }
      message_d_array ("Obs values", temp, nmeas1t, html_style_output, html_unit);
      for (i=0; i<3; i++) { temp[i] = truth[i][obstep[j]]; }
      message_d_array ("True values", temp, 3, html_style_output, html_unit);
    }
    if (html_style_output)
    { fprintf (html_unit, "</table>\n");
      fprintf (html_unit, "</td></tr>\n");
      fprintf (html_unit, "</table>\n");
    }
  }


  // ===== 4. Set-up the R and H matrices (same for all times) =====
  for (i=0; i<nmeas1t; i++)
  { for (ii=0; ii<3; ii++)
    { H[i][ii] = 0.0;
    }
    for (ii=0; ii<nmeas1t; ii++)
    { R[i][ii] = 0.0;
    }
  }

  ii = -1;
  for (i=0; i<3; i++)
  { if (observe_xyz[i])
    { ii++;
      R[ii][ii] = measerr_std[i] * measerr_std[i];
      H[ii][i]  = 1.0;
    }
  }

  if (diags)
  { printf ("  ----------------------------------\n");
    printf ("  Here is the R matrix for this time\n  ");
    for (i=0; i<nmeas1t; i++)
    { for (ii=0; ii<nmeas1t; ii++)
      { printf ("%f  ", R[i][ii]);
      }
      printf ("\n  ");
    }
    printf ("  Here is the H matrix for this time\n  ");
    for (i=0; i<nmeas1t; i++)
    { for (ii=0; ii<3; ii++)
      { printf ("%f  ", H[i][ii]);
      }
      printf ("\n  ");
    }
    printf ("  ----------------------------------\n");
  }


  // =============================================================================
  // ===== 5. Run through obs sequentially (EnKF) ================================
  // =============================================================================
  for (j=0; j<n_meas_times; j++)
  { // 5.1 Determine the forecast period to get to this time
    if (diags) { printf ("  Observation batch %u\n", j); }
    if (j == 0)
    { na = 0;           // start time
    }
    else
    { na = obstep[j-1];   // start time
    }
    nb = obstep[j];     // end time
    if (diags)
    { printf ("This forecast period, start time = %u, end time = %u\n", na, nb);
      printf ("  Assimilating observation %u of %u\n", j+1, n_meas_times);
    }

    // 5.2 Perform ensemble forecasts between na and nb (with model error if selected)
    if (nb > na)
    { for (k=0; k<nens; k++)
      { for (i=0; i<3; i++) { OneMember[i][na] = A[i][k][na]; }
        lorenz (na, nb, dt, OneMember, moderr_flag, moderr_std);
        for (jj=na+1; jj<=nb; jj++)
        { for (i=0; i<3; i++) { A[i][k][jj] = OneMember[i][jj]; }
        }
      }
    }


    // 5.3 Find the mean and standard deviations
    for (jj=na; jj<=nb; jj++)
    { Mean (nens, A, jj, meanAf);
      StdDev (nens, A, jj, meanAf, temp);
      for (i=0; i<3; i++)
      { cenfc[i][jj]   = meanAf[i];
        StdDevs[i][jj] = temp[i];
      }
    }
    if (diags) { printf ("  Found the mean trajectory\n"); }

    // 5.4 Calculate the forecast perturbations at time nb
    for (k=0; k<nens; k++)
    { for (i=0; i<3; i++)
      { Afp[i][k] = A[i][k][nb] - cenfc[i][nb];
      }
    }
    if (diags) { printf ("  Found the forecast perturbations at this time\n"); }

    // 5.5 Calculate S = H Afp (the ensemble pertrubations in observation space)
    M1M2 (nmeas1t, 3, H, nens, Afp, S);
    if (diags) { printf ("  5.5 ok\n"); }

    // 5.6 Calculate C = SS^T  This is called ZLZT in the idl code
    M1M2T (nmeas1t, nens, S, nmeas1t, S, C);
    if (diags) { printf ("  5.6 ok\n"); }

    // 5.7 Calculate C = C + (N-1)R
    for (i=0; i<nmeas1t; i++)
    { for (ii=0; ii<nmeas1t; ii++)
      { C[i][ii] += nensm1 * R[i][ii];
      }
    }
    if (diags) { printf ("  5.7 ok\n"); }

    // 5.8 Calculate the eigenvectors and eigenvalues of C
    Jacobi (nmeas1t, C, Lambda, Z, 0.0001);
    if (diags) { printf ("  5.8 ok\n"); }
    if (diags)
    { printf ("  Here is the C matrix ansd its eigenvectors and eigenvalues\n  ");
      for (i=0; i<nmeas1t; i++)
      { for (ii=0; ii<nmeas1t; ii++)
        { printf ("%f  ", C[i][ii]);
        }
        printf ("    ");
        for (ii=0; ii<nmeas1t; ii++)
        { printf ("%f  ", Z[i][ii]);
        }
        printf ("    ");
        for (ii=0; ii<nmeas1t; ii++)
        { printf ("%f  ", Lambda[i][ii]);
        }
        printf ("\n  ");
      }
    }

    // Check for positive-definiteness
    mineval = Lambda[0][0];
    for (i=1; i<nmeas1t; i++)
    { if (Lambda[i][i] < mineval)
      { mineval = Lambda[i][i];
      }
    }
    if (mineval < 0.0)
    { printf ("Fatal error - matrix is non positive definite\n");
      exit (1);
    }

    // 5.9 Compute the innovation vector
    for (i=0; i<nmeas1t; i++)
    { sum = 0.0;
      for (ii=0; ii<3; ii++)
      { sum += H[i][ii] * meanAf[ii];
      }
      ym[i][0] = sum;
      d[i][0]  = yy[i][j] - sum;
    }
    if (diags)
    { printf ("  5.9 ok\n");
      printf ("  Time step %u\n", j);
      printf ("  State, Obs, model obs and innovations at this time\n  ");
      printf ("State: %f %f %f  | Obs: ", meanAf[0], meanAf[1], meanAf[2]);
      for (i=0; i<nmeas1t; i++)
      { printf ("%f ", yy[i][j]);
      }
      printf (" | ModObs: ");
      for (i=0; i<nmeas1t; i++)
      { printf ("%f ", ym[i][0]);
      }
      printf (" | Innov: ");
      for (i=0; i<nmeas1t; i++)
      { printf ("%f ", d[i][0]);
      }
      printf ("\n");
    }

    // 5.10 Calculate mat2 = C^-1 d: 3 stages from eigen-decomposition
    // vec1 = Z^T d
    M1TM2 (nmeas1t, nmeas1t, Z, 1, d, vec1);
    //  vec1 = Lambda^-1 vec1
    for (i=0; i<nmeas1t; i++)
    { vec1[i][0] /= Lambda[i][i];
    }
    // vec2 = Z vec1
    M1M2 (nmeas1t, nmeas1t, Z, 1, vec1, vec2);
    if (diags) { printf ("  5.10 ok\n"); }

    // 5.11 Calculate vec3 = S^T vec2
    M1TM2 (nmeas1t, nens, S, 1, vec2, vec3);
    if (diags) { printf ("  5.11 ok\n"); }

    // 5.12 Calculate analinc = Afp vec3
    M1M2 (3, nens, Afp, 1, vec3, analinc);
    if (diags) { printf ("  5.12 ok\n"); }

    // 5.13 Add to mean forecast to get mean analysis
    for (i=0; i<3; i++)
    { meanAa[i] = meanAf[i] + analinc[i][0];
    }
    if (diags) { printf ("  5.13 ok\n"); }

    // 5.14 Calculate X: 3 stages
    // Calculate X = Z^T S (not yet final X)
    M1TM2 (nmeas1t, nmeas1t, Z, nens, S, X);
    // Calculate X = Lambda^-1 mat1
    for (i=0; i<nmeas1t; i++)
    { sqrtevali = sqrt(Lambda[i][i]);
      for (k=0; k<nens; k++)
      { X[i][k] /= sqrtevali;
      }
    }
    if (diags) { printf ("  5.14 ok\n"); }

    // 5.15 Calculate singular values and vectors of X^T X
    // Calculate XTX = X^T X
    M1TM2 (nmeas1t, nens, X, nens, X, XTX);
    // Eigenvectors of this matrix are V and eigenvalues are SigmaTSigma
    // These are the right singular vectors and square of singular values of X
    Jacobi (nens, XTX, SigmaTSigma, V, 0.0001);
    if (diags) { printf ("  5.15 ok\n"); }
    if (diags)
    { printf ("  Here is the XTX matrix ansd its eigenvectors and eigenvalues\n  ");
      for (k=0; k<nens; k++)
      { for (kk=0; kk<nens; kk++)
        { printf ("%f  ", XTX[k][kk]);
        }
        printf ("    ");
        for (kk=0; kk<nmeas1t; kk++)
        { printf ("%f  ", V[k][kk]);
        }
        printf ("    ");
        for (kk=0; kk<nmeas1t; kk++)
        { printf ("%f  ", SigmaTSigma[k][kk]);
        }
        printf ("\n  ");
      }
    }


    // 5.16 Calculate the new analysis perturbation matrix: 3 stages
    // Calculate ScaledVT = sqrt(I-SigmaTSigma) V^T
    for (k=0; k<nens; k++)
    { ele = sqrt(1.0 - SigmaTSigma[k][k]);
      if (ele < 0.0)
      { printf ("Fatal error - negative square-root\n");
      exit (1);
      }
      for (kk=0; kk<nens; kk++)
      { ScaledVT[k][kk] = ele * V[kk][k];
      }
    }
    // Calculate Trans = V ScaledVT
    M1M2 (nens, nens, V, nens, ScaledVT, Trans);
    // Final step to calculate the analyis perturbation matrix
    // Aap = Afp Trans
    M1M2 (3, nens, Afp, nens, Trans, Aap);
    if (diags) { printf ("  5.16 ok\n"); }

    // 5.17 Pass the result back into the main structures
    // Analyis goes into the central forecast
    for (i=0; i<3; i++)
    { cenfc[i][nb] = meanAa[i];
    }
    if (diags) { printf ("  5.17 ok\n"); }

    // 5.18 Perturbed analyses
    for (k=0; k<nens; k++)
    { for (i=0; i<3; i++)
      { A[i][k][nb] = meanAa[i] + Aap[i][k];
      }
    }
    if (diags) { printf ("  5.18 ok - end of this obs period\n"); }

  }

  // End of assimilation period


  // ===== 6. Do a final forecast with no data assimilation =====
  na = nb;
  nb = n_total_window;
  // The ensemble members
  for (k=0; k<nens; k++)
  { for (i=0; i<3; i++) { OneMember[i][na] = A[i][k][na]; }
    lorenz (na, nb, dt, OneMember, moderr_flag, moderr_std);
    for (jj=na+1; jj<=nb; jj++)
    { for (i=0; i<3; i++) { A[i][k][jj] = OneMember[i][jj]; }
    }
  }
  if (diags) { printf ("6 ok\n"); }

  // ===== 7. Find the mean trajectory and standard deviations =====
  for (j=na; j<=nb; j++)
  { Mean (nens, A, j, mean);
    StdDev (nens, A, j, mean, temp);
    for (i=0; i<3; i++)
    { cenfc[i][j]   = mean[i];
      StdDevs[i][j] = temp[i];
    }
  }
  if (diags) { printf ("7 ok\n"); }


  // Output the central forecast
  OutputTimeSeq (0, n_total_window, t, 1, cenfc, 1, truth, 1, StdDevs,
                 "/tmp", "Centralfc", id);

  // Output the obs: x, y, z
  strcpy (whole_obs_path[0], "/tmp/Obsx");
  strcpy (whole_obs_path[1], "/tmp/Obsy");
  strcpy (whole_obs_path[2], "/tmp/Obsz");
  ii = -1;
  for (i=0; i<3; i++)
  { // Output obs for element i
    strcat (whole_obs_path[i], id);
    strcat (whole_obs_path[i], ".dat");
    // printf ("Ob filename for %i is:%s#<br>\n", i, whole_obs_path[i]);
    obfile = fopen (whole_obs_path[i], "w");
    fprintf (obfile, "# Observations for %c\n", i+120);
    fprintf (obfile, "# time, ob stddev\n", i+120);
    if (observe_xyz[i])
    { ii++;
      for (j=0; j<n_meas_times; j++)
      { fprintf (obfile, "%f %f %f\n", obstep[j]*dt, yy[ii][j], measerr_std[i]);
      }
    }
    fclose (obfile);
  }

  fclose (html_unit);


  // ===== 8. Deallocate variable-size data arrays =====
  Array_2d_double_destroy (&H, nmeas1t);
  Array_2d_double_destroy (&R, nmeas1t);
  Array_2d_double_destroy (&Afp, 3);
  Array_2d_double_destroy (&Aap, 3);
  Array_2d_double_destroy (&S, nmeas1t);
  Array_2d_double_destroy (&C, nmeas1t);
  Array_2d_double_destroy (&Lambda, nmeas1t);
  Array_2d_double_destroy (&Z, nmeas1t);
  Array_2d_double_destroy (&yy, nmeas1t);
  Array_2d_double_destroy (&ym, nmeas1t);
  Array_2d_double_destroy (&d, nmeas1t);
  Array_2d_double_destroy (&vec1, nmeas1t);
  Array_2d_double_destroy (&vec2, nmeas1t);
  Array_2d_double_destroy (&vec3, nens);
  Array_2d_double_destroy (&analinc, 3);
  Array_2d_double_destroy (&X, nmeas1t);
  Array_2d_double_destroy (&XTX, nens);
  Array_2d_double_destroy (&V, nens);
  Array_2d_double_destroy (&SigmaTSigma, nens);
  Array_2d_double_destroy (&ScaledVT, nens);
  Array_2d_double_destroy (&Trans, nens);

}



// ========== Jacobi matrix diagonalisation ======================================
void Jacobi ( int    nels,             // Dimension of matrix
              double **A,	       // Input matrix in original representation
              double **B,	       // Output matrix in diagonal representation
              double **T,	       // Output matrix of eigenvectors (columns)
              double lim )	       // Convergence criteria, e.g. lim=0.01

{ // To diagonalize a square symmetric matrix by a sequence of Jacobi rotations
  // Ross Bannister.  Fortran January 2001, C++ April 2004
  // Code tested and shown to work

  // Declare local variables
  int    i, j, p, q, sweeps;
  double off, diag, theta, t1, t2, tt;
  double cc, ss, sc, Tip, Tiq, Bpp, Bqq, Bpq, dis, s, c, norm;

  // The matrix B is initially equal to A
  for (j=0; j<nels; j++)
  { for (i=0; i<nels; i++)
    { B[i][j] = A[i][j];
    }
  }

  // The matrix T is initially the identity matrix
  for (j=0; j<nels; j++)
  { for (i=0; i<nels; i++)
    { if (i == j)
      { T[i][j] = 1.0;
      }
        else
      { T[i][j] = 0.0;
      }
    }
  }

  // Calculate the sum of the square of off-diagonal elements of B (B is symmetric)
  off = 0.0;
  for (j=1; j<nels; j++)
  { for (i=0; i<j; i++)
    { off += B[i][j] * B[i][j];
    }
  }
  off *= 2.0;

  // Calculate the sum of the square of diagonal elements of B
  diag = 0.0;
  for (i=0; i<nels; i++)
  { diag += B[i][i] * B[i][i];
  }

  // The number of sweeps completed
  sweeps = 0;

  do
  { // Loop round each upper-right element (a 'sweep')
    for (p=0; p<(nels-1); p++)
    { for (q=(p+1); q<nels; q++)
      { if (fabs(B[p][q]) > 0.0)
        {

          // In order to find the new representation of the matrix, work out some
          // details regarding the rotation, U to eliminate element q,p of B (p<q)
          theta = ( B[q][q] - B[p][p] ) / ( 2.0 * B[q][p] );
          dis = sqrt ( theta * theta + 1.0 );
          // Two possible values of t
          t1 = -theta + dis;
          t2 = -theta - dis;
          // Choose the smaller value
          if (fabs(t1) < fabs(t2))
          { tt=t1;
          }
            else
          { tt=t2;
          }
          // Find the values of s and c
          c = 1.0 / sqrt ( tt * tt + 1.0 );
          s = tt * c;

          cc = c * c;
          ss = s * s;
          sc = s * c;

          // Modify the operator U^T B U (piecewise)
          // Update only upper-right part of matrix for now
          for (i=0; i<p; i++)
          { B[i][p] = c * B[p][i] - s * B[q][i];  // Column p
          }
          for (i=0; i<q; i++)
          { if (i != p)
            { B[i][q] = c * B[q][i] + s * B[p][i];// Column q
            }
          }
          for (j=(p+1); j<nels; j++)
          { if (j != q)
            { B[p][j] = c * B[j][p] - s * B[q][j];// Row p
            }
          }
          for (j=(q+1); j<nels; j++)
          { B[q][j] = c * B[j][q] + s * B[j][p];  // Row q
          }

          // Individual elements
          Bpp = B[p][p];
          Bqq = B[q][q];
          Bpq = B[p][q];
          B[p][p] = cc * Bpp + ss * Bqq - 2.0 * sc * Bpq;
          B[q][q] = ss * Bpp + cc * Bqq + 2.0 * sc * Bpq;
          B[p][q] = 0.0;
          
          // Modified value of sum of square of off-diagonal elements
          off -= 2.0 * Bpq * Bpq;

          // Modified value of sum of square of diagonal elements
          diag += B[p][p]*B[p][p] +
                  B[q][q] * B[q][q] - Bpp * Bpp - Bqq * Bqq;

          // Update lower-left of matrix
          for (i=0; i<nels; i++)
          { for (j=0; j<i; j++)
            { B[i][j] = B[j][i];
            }
          }

          // Modify the transformation T(new)=U T(old)
          // Only rows p and q are affected
          for (i=0; i<nels; i++)
          { Tip     = T[i][p];
            Tiq     = T[i][q];
            T[i][p] = c * Tip - s * Tiq;
            T[i][q] = c * Tiq + s * Tip;
          }
        }
      }
    }
    sweeps += 1;
  }
  while ( ((off/diag) > lim) || (sweeps < 3) );


  // Normalize the eigenvectors
  for (j=0; j<nels; j++)
  { norm = 0.0;
    for (i=0; i<nels; i++)
    { norm += T[i][j] * T[i][j];
    }
    norm = sqrt(norm);
    for (i=0; i<nels; i++)
    { T[i][j] /= norm;
    }
  }
}


// ========== Output a message and 1 integer to standard output ==================
void message_i
            ( char   *text,
              int    value,
              int    html_flag,
              FILE   *unit )
{ if (html_flag)
  { fprintf (unit, "<tr> <td>%s</td> <td>%u</td></tr>\n", text, value);
  }
  else
  { fprintf (unit, "%s : %u\n", text, value);
  }
}


// ========== Output a message and array of integers to standard output ==========
void message_i_array
            ( char   *text,
              int    *value,
              int    len,
              int    html_flag,
              FILE   *unit )
{ // Declare local variables
  int    i;
  if (html_flag)
  { fprintf (unit, "<tr><td>%s</td><td>", text);
    for (i=0; i<len; i++)
    { fprintf (unit, "%u ", value[i]);
    }
    fprintf (unit, "</td></tr>\n");
  }
  else
  { fprintf (unit, "%s  ", text);
    for (i=0; i<len; i++)
    { fprintf (unit, "%u ", value[i]);
    }
    fprintf (unit, "\n");
  }
}


// ========== Output a message and 1 double to standard output ===================
void message_d
            ( char   *text,
              double value,
              int    html_flag,
              FILE   *unit )
{ if (html_flag)
  { fprintf (unit, "<tr> <td>%s</td> <td>%f</td></tr>\n", text, value);
  }
  else
  { fprintf (unit, "%s : %f\n", text, value);
  }
}


// ========== Output a message and 1 string to standard output ===================
void message_s
            ( char   *text,
              char   *value,
              int    html_flag,
              FILE   *unit )
{ if (html_flag)
  { fprintf (unit, "<tr> <td>%s</td> <td>%s</td></tr>\n", text, value);
  }
  else
  { fprintf (unit, "%s : %s\n", text, value);
  }
}


// ========== Output a message and array of doubles to standard output ===========
void message_d_array
            ( char   *text,
              double *value,
              int    len,
              int    html_flag,
              FILE   *unit )
{ // Declare local variables
  int    i;
  if (html_flag)
  { fprintf (unit, "<tr><td>%s</td><td>", text);
    for (i=0; i<len; i++)
    { fprintf (unit, "%f ", value[i]);
    }
    fprintf (unit, "</td></tr>\n");
  }
  else
  { fprintf (unit, "%s  ", text);
    for (i=0; i<len; i++)
    { fprintf (unit, "%f ", value[i]);
    }
    fprintf (unit, "\n");
  }
}