text:
/* ode-initval2/msadams.c
 *
 * Copyright (C) 2009, 2010 Tuomo Keskitalo
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

/* A variable-coefficient linear multistep Adams method in Nordsieck
   form. This stepper uses explicit Adams-Bashforth (predictor) and
   implicit Adams-Moulton (corrector) methods in P(EC)^m functional
   iteration mode. Method order varies dynamically between 1 and 12.

   References:

   Byrne, G. D., and Hindmarsh, A. C., A Polyalgorithm for the
   Numerical Solution of Ordinary Differential Equations,
   ACM Trans. Math. Software, 1 (1975), pp. 71-96.

   Brown, P. N., Byrne, G. D., and Hindmarsh, A. C., VODE: A
   Variable-coefficient ODE Solver, SIAM J. Sci. Stat. Comput. 10,
   (1989), pp. 1038-1051.

   Hindmarsh, A. C., Brown, P. N., Grant, K. E., Lee, S. L., Serban,
   R., Shumaker, D. E., and Woodward, C. S., SUNDIALS: Suite of
   Nonlinear and Differential/Algebraic Equation Solvers, ACM
   Trans. Math. Software 31 (2005), pp. 363-396.

   Note: The algorithms have been adapted for GSL ode-initval2
   framework.
*/

#include <config.h>
#include <stdlib.h>
#include <string.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_odeiv2.h>
#include <gsl/gsl_blas.h>

#include "odeiv_util.h"

/* Maximum order of Adams methods */
#define MSADAMS_MAX_ORD 12

typedef struct
{
  /* Nordsieck history matrix. Includes concatenated
     Nordsieck vectors [y_n, h*y_n', (h^2/2!)*y_n'', ...,
     (h^ord/ord!)*d^(ord)(y_n)]. Nordsieck vector number i is located
     at z[i*dim] (i=0..ord).
   */
  double *z;

  double *zbackup;              /* backup of Nordsieck matrix */
  double *ytmp;                 /* work area */
  double *ytmp2;                /* work area */
  double *pc;                   /* product term coefficients */
  double *l;                    /* polynomial coefficients */
  double *hprev;                /* previous step sizes */
  double *hprevbackup;          /* backup of hprev */
  double *errlev;               /* desired error level of y */
  gsl_vector *abscor;           /* absolute y values for correction */
  gsl_vector *relcor;           /* relative y values for correction */
  gsl_vector *svec;             /* saved abscor & work area */
  gsl_vector *tempvec;          /* work area */
  const gsl_odeiv2_driver *driver;      /* pointer to gsl_odeiv2_driver object */

  long int ni;                  /* stepper call counter */
  size_t ord;                   /* current order of method */
  size_t ordprev;               /* order of previous call */
  size_t ordprevbackup;         /* backup of ordprev */
  double tprev;                 /* t point of previous call */
  size_t ordwait;               /* counter for order change */
  size_t ordwaitbackup;         /* backup of ordwait */
  size_t failord;               /* order of convergence failure */
  double failt;                 /* t point of convergence failure */
  double ordm1coeff;            /* saved order-1 coefficiet */
  double ordp1coeffprev;        /* saved order+1 coefficient */
  size_t failcount;             /* counter for rejected steps */
}
msadams_state_t;

/* Introduce msadams_reset for use in msadams_alloc and _apply */

static int msadams_reset (void *, size_t);

static void *
msadams_alloc (size_t dim)
{
  msadams_state_t *state =
    (msadams_state_t *) malloc (sizeof (msadams_state_t));

  if (state == 0)
    {
      GSL_ERROR_NULL ("failed to allocate space for msadams_state",
                      GSL_ENOMEM);
    }

  state->z =
    (double *) malloc ((MSADAMS_MAX_ORD + 1) * dim * sizeof (double));

  if (state->z == 0)
    {
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for z", GSL_ENOMEM);
    }

  state->zbackup =
    (double *) malloc ((MSADAMS_MAX_ORD + 1) * dim * sizeof (double));

  if (state->zbackup == 0)
    {
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for zbackup", GSL_ENOMEM);
    }

  state->ytmp = (double *) malloc (dim * sizeof (double));

  if (state->ytmp == 0)
    {
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for ytmp", GSL_ENOMEM);
    }

  state->ytmp2 = (double *) malloc (dim * sizeof (double));

  if (state->ytmp2 == 0)
    {
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for ytmp2", GSL_ENOMEM);
    }

  state->pc = (double *) malloc ((MSADAMS_MAX_ORD + 1) * sizeof (double));

  if (state->pc == 0)
    {
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for pc", GSL_ENOMEM);
    }

  state->l = (double *) malloc ((MSADAMS_MAX_ORD + 1) * sizeof (double));

  if (state->l == 0)
    {
      free (state->pc);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for l", GSL_ENOMEM);
    }

  state->hprev = (double *) malloc (MSADAMS_MAX_ORD * sizeof (double));

  if (state->hprev == 0)
    {
      free (state->l);
      free (state->pc);
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for hprev", GSL_ENOMEM);
    }

  state->hprevbackup = (double *) malloc (MSADAMS_MAX_ORD * sizeof (double));

  if (state->hprevbackup == 0)
    {
      free (state->hprev);
      free (state->l);
      free (state->pc);
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for hprevbackup", GSL_ENOMEM);
    }

  state->errlev = (double *) malloc (dim * sizeof (double));

  if (state->errlev == 0)
    {
      free (state->hprevbackup);
      free (state->hprev);
      free (state->l);
      free (state->pc);
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for errlev", GSL_ENOMEM);
    }

  state->abscor = gsl_vector_alloc (dim);

  if (state->abscor == 0)
    {
      free (state->errlev);
      free (state->hprevbackup);
      free (state->hprev);
      free (state->l);
      free (state->pc);
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for abscor", GSL_ENOMEM);
    }

  state->relcor = gsl_vector_alloc (dim);

  if (state->relcor == 0)
    {
      gsl_vector_free (state->abscor);
      free (state->errlev);
      free (state->hprevbackup);
      free (state->hprev);
      free (state->l);
      free (state->pc);
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for relcor", GSL_ENOMEM);
    }

  state->svec = gsl_vector_alloc (dim);

  if (state->svec == 0)
    {
      gsl_vector_free (state->relcor);
      gsl_vector_free (state->abscor);
      free (state->errlev);
      free (state->hprevbackup);
      free (state->hprev);
      free (state->l);
      free (state->pc);
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for svec", GSL_ENOMEM);
    }

  state->tempvec = gsl_vector_alloc (dim);

  if (state->tempvec == 0)
    {
      gsl_vector_free (state->svec);
      gsl_vector_free (state->relcor);
      gsl_vector_free (state->abscor);
      free (state->errlev);
      free (state->hprevbackup);
      free (state->hprev);
      free (state->l);
      free (state->pc);
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for tempvec", GSL_ENOMEM);
    }

  msadams_reset ((void *) state, dim);

  state->driver = NULL;

  return state;
}

static int
msadams_failurehandler (void *vstate, const size_t dim, const double t)
{
  /* Internal failure handler routine for msadams. Adjusted strategy
     for GSL: Decrease order if this is the second time a failure
     has occurred at this order and point.
   */

  msadams_state_t *state = (msadams_state_t *) vstate;

  const size_t ord = state->ord;

  if (ord > 1 && (ord - state->ordprev == 0) &&
      ord == state->failord && t == state->failt)
    {
      state->ord--;
    }

  /* Save information about failure */

  state->failord = ord;
  state->failt = t;
  state->ni++;

  /* Force reinitialization if failure took place at lowest
     order
   */

  if (ord == 1)
    {
      msadams_reset (vstate, dim);
    }

  return GSL_SUCCESS;
}

static int
msadams_calccoeffs (const size_t ord, const size_t ordwait,
                    const double h, const double hprev[],
                    double pc[], double l[],
                    double *errcoeff, double *ordm1coeff,
                    double *ordp1coeff, double *ordp2coeff)
{

  /* Calculates coefficients (l) of polynomial Lambda, error and
     auxiliary order change evaluation coefficients.
   */

  if (ord == 1)
    {
      l[0] = 1.0;
      l[1] = 1.0;
      *errcoeff = 0.5;
      *ordp1coeff = 1.0;
      *ordp2coeff = 12.0;
    }
  else
    {
      size_t i, j;
      double hsum = h;
      double st1 = 0.0;         /* sum term coefficients */
      double st2 = 0.0;

      /* Calculate coefficients (pc) of product terms */

      DBL_ZERO_MEMSET (pc, MSADAMS_MAX_ORD + 1);

      pc[0] = 1.0;

      for (i = 1; i < ord; i++)
        {
          /* Calculate auxiliary coefficient used in evaluation of
             change of order
           */

          if (i == ord - 1 && ordwait == 1)
            {
              int s = 1;

              *ordm1coeff = 0.0;

              for (j = 0; j < ord - 1; j++)
                {
                  *ordm1coeff += s * pc[j] / (j + 2);
                  s = -s;
                }

              *ordm1coeff = pc[ord - 2] / (ord * (*ordm1coeff));
            }

          for (j = i; j > 0; j--)
            {
              pc[j] += pc[j - 1] * h / hsum;
            }

          hsum += hprev[i - 1];
        }

      /* Calculate sum term 1 for error estimation */

      {
        int s = 1;

        for (i = 0; i < ord; i++)
          {
            st1 += s * pc[i] / (i + 1.0);
            s = -s;
          }
      }

      /* Calculate sum term 2 for error estimation */

      {
        int s = 1;

        for (i = 0; i < ord; i++)
          {
            st2 += s * pc[i] / (i + 2.0);
            s = -s;
          }
      }

      /* Calculate the actual polynomial coefficients (l) */

      DBL_ZERO_MEMSET (l, MSADAMS_MAX_ORD + 1);

      l[0] = 1.0;

      for (i = 1; i < ord + 1; i++)
        {
          l[i] = pc[i - 1] / (i * st1);
        }

#ifdef DEBUG
      {
        size_t di;

        printf ("-- calccoeffs l: ");
        for (di = 0; di < ord + 1; di++)
          {
            printf ("%.5e ", l[di]);
          }
        printf ("\n");

        printf ("-- calccoeffs pc: ");
        for (di = 0; di < ord; di++)
          {
            printf ("%.5e ", pc[di]);
          }
        printf ("\n");

        printf ("-- calccoeffs st1=%.5e, st2=%.5e\n", st1, st2);
      }
#endif

      /* Calculate error coefficient */

      *errcoeff = (h / hsum) * (st2 / st1);

      /* Calculate auxiliary coefficients used in evaluation of change
         of order
       */

      if (ordwait < 2)
        {
          int s = 1;

          *ordp1coeff = hsum / (h * l[ord]);

          *ordp2coeff = 0.0;

          for (i = ord; i > 0; i--)
            {
              pc[i] += pc[i - 1] * (h / hsum);
            }

          for (i = 0; i < ord + 1; i++)
            {
              *ordp2coeff += s * pc[i] / (i + 2);
              s = -s;
            }

          *ordp2coeff = (ord + 1) * st1 / (*ordp2coeff);
        }
    }

#ifdef DEBUG
  printf ("-- calccoeffs ordm1coeff=%.5e ", *ordm1coeff);
  printf ("ordp1coeff=%.5e ", *ordp1coeff);
  printf ("ordp2coeff=%.5e ", *ordp2coeff);
  printf ("errcoeff=%.5e\n", *errcoeff);
#endif

  return GSL_SUCCESS;
}

static int
msadams_corrector (void *vstate, const gsl_odeiv2_system * sys,
                   const double t, const double h, const size_t dim,
                   const double z[], const double errlev[],
                   const double l[], const double errcoeff,
                   gsl_vector * abscor, gsl_vector * relcor,
                   double ytmp[], double ytmp2[])
{
  /* Calculates the correction step (abscor). Non-linear equation
     system is solved by functional iteration.
   */

  size_t mi, i;
  const size_t max_iter = 3;    /* Maximum number of iterations */
  double convrate = 1.0;        /* convergence rate */
  double stepnorm = 0.0;        /* norm of correction step */
  double stepnormprev = 0.0;

  /* Evaluate at predicted values */

  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + h, z, ytmp);

    if (s == GSL_EBADFUNC)
      {
        return s;
      }

    if (s != GSL_SUCCESS)
      {
        msadams_failurehandler (vstate, dim, t);

#ifdef DEBUG
        printf ("-- FAIL at user function evaluation\n");
#endif
        return s;
      }
  }

  /* Calculate correction step (abscor) */

  gsl_vector_set_zero (abscor);

  for (mi = 0; mi < max_iter; mi++)
    {
      const double safety = 0.3;
      const double safety2 = 0.1;

      /* Calculate new y values to ytmp2 */

      for (i = 0; i < dim; i++)
        {
          ytmp[i] *= h;
          ytmp[i] -= z[1 * dim + i];
          ytmp[i] /= l[1];
          ytmp2[i] = z[0 * dim + i] + ytmp[i];
        }

#ifdef DEBUG
      {
        size_t di;
        printf ("-- dstep: ");
        for (di = 0; di < dim; di++)
          {
            printf ("%.5e ", ytmp[di]);
          }
        printf ("\n");
      }
#endif
      /* Convergence test. Norms used are root-mean-square norms. */

      for (i = 0; i < dim; i++)
        {
          gsl_vector_set (relcor, i,
                          (ytmp[i] - gsl_vector_get (abscor, i)) / errlev[i]);
          gsl_vector_set (abscor, i, ytmp[i]);
        }

      stepnorm = gsl_blas_dnrm2 (relcor) / sqrt (dim);

      if (mi > 0)
        {
          convrate = GSL_MAX_DBL (safety * convrate, stepnorm / stepnormprev);
        }
      else
        {
          convrate = 1.0;
        }

      {
        const double convtest =
          GSL_MIN_DBL (convrate, 1.0) * stepnorm * errcoeff / safety2;

#ifdef DEBUG
        printf
          ("-- func iter loop %d, errcoeff=%.5e, stepnorm =%.5e, convrate = %.5e, convtest = %.5e\n",
           (int) mi, errcoeff, stepnorm, convrate, convtest);
#endif
        if (convtest <= 1.0)
          {
            break;
          }
      }

      /* Check for divergence during iteration */
      {
        const double div_const = 2.0;

        if (mi > 1 && stepnorm > div_const * stepnormprev)
          {
            msadams_failurehandler (vstate, dim, t);

#ifdef DEBUG
            printf ("-- FAIL, diverging functional iteration\n");
#endif
            return GSL_FAILURE;
          }
      }

      /* Evaluate at new y */

      {
        int s = GSL_ODEIV_FN_EVAL (sys, t + h, ytmp2, ytmp);

        if (s == GSL_EBADFUNC)
          {
            return s;
          }

        if (s != GSL_SUCCESS)
          {
            msadams_failurehandler (vstate, dim, t);

#ifdef DEBUG
            printf ("-- FAIL at user function evaluation\n");
#endif
            return s;
          }
      }

      stepnormprev = stepnorm;
    }

#ifdef DEBUG
  printf ("-- functional iteration exit at mi=%d\n", (int) mi);
#endif

  /* Handle convergence failure */

  if (mi == max_iter)
    {
      msadams_failurehandler (vstate, dim, t);

#ifdef DEBUG
      printf ("-- FAIL, max_iter reached\n");
#endif
      return GSL_FAILURE;
    }

  return GSL_SUCCESS;
}

static int
msadams_eval_order (gsl_vector * abscor, gsl_vector * tempvec,
                    gsl_vector * svec, const double errcoeff,
                    const size_t dim, const double errlev[],
                    const double ordm1coeff, const double ordp1coeff,
                    const double ordp1coeffprev, const double ordp2coeff,
                    const double hprev[],
                    const double h, const double z[],
                    size_t * ord, size_t * ordwait)
{
  /* Evaluates and executes change in method order (current, current-1
     or current+1). Order which maximizes the step length is selected.
   */

  size_t i;

  /* step size estimates at current order, order-1 and order+1 */
  double ordest = 0.0;
  double ordm1est = 0.0;
  double ordp1est = 0.0;

  const double safety = 1e-6;
  const double bias = 6.0;
  const double bias2 = 10.0;

  /* Relative step length estimate for current order */

  ordest = 1.0 / (pow (bias * gsl_blas_dnrm2 (abscor) / sqrt (dim)
                       * errcoeff, 1.0 / (*ord + 1)) + safety);

  /* Relative step length estimate for order ord - 1 */

  if (*ord > 1)
    {
      for (i = 0; i < dim; i++)
        {
          gsl_vector_set (tempvec, i, z[*ord * dim + i] / errlev[i]);
        }

      ordm1est = 1.0 / (pow (bias * gsl_blas_dnrm2 (tempvec) / sqrt (dim)
                             / ordm1coeff, 1.0 / (*ord)) + safety);
    }
  else
    {
      ordm1est = 0.0;
    }

  /* Relative step length estimate for order ord + 1 */

  if (*ord < MSADAMS_MAX_ORD)
    {
      const double c = -ordp1coeff / ordp1coeffprev *
        pow (h / hprev[1], *ord + 1);

      for (i = 0; i < dim; i++)
        {
          gsl_vector_set (svec, i, gsl_vector_get (svec, i) * c +
                          gsl_vector_get (abscor, i));
        }

      ordp1est = 1.0 / (pow (bias2 * gsl_blas_dnrm2 (svec) / sqrt (dim)
                             / ordp2coeff, 1.0 / (*ord + 2)) + safety);
    }
  else
    {
      ordp1est = 0.0;
    }

#ifdef DEBUG
  printf
    ("-- eval_order ord=%d, ordest=%.5e, ordm1est=%.5e, ordp1est=%.5e\n",
     (int) *ord, ordest, ordm1est, ordp1est);
#endif

  /* Choose order that maximises step size and increases step
     size markedly compared to current step
   */

  {
    const double min_incr = 1.5;

    if (ordm1est > ordest && ordm1est > ordp1est && ordm1est > min_incr)
      {
        *ord -= 1;
#ifdef DEBUG
        printf ("-- eval_order order DECREASED to %d\n", (int) *ord);
#endif
      }

    else if (ordp1est > ordest && ordp1est > ordm1est && ordp1est > min_incr)
      {
        *ord += 1;
#ifdef DEBUG
        printf ("-- eval_order order INCREASED to %d\n", (int) *ord);
#endif
      }
  }

  *ordwait = *ord + 2;

  return GSL_SUCCESS;
}

static int
msadams_apply (void *vstate, size_t dim, double t, double h,
               double y[], double yerr[],
               const double dydt_in[], double dydt_out[],
               const gsl_odeiv2_system * sys)
{
  /* Conducts a step by Adams linear multistep methods. */

  msadams_state_t *state = (msadams_state_t *) vstate;

  double *const z = state->z;
  double *const zbackup = state->zbackup;
  double *const ytmp = state->ytmp;
  double *const ytmp2 = state->ytmp2;
  double *const pc = state->pc;
  double *const l = state->l;
  double *const hprev = state->hprev;
  double *const hprevbackup = state->hprevbackup;
  double *const errlev = state->errlev;
  gsl_vector *const abscor = state->abscor;
  gsl_vector *const relcor = state->relcor;
  gsl_vector *const svec = state->svec;
  gsl_vector *const tempvec = state->tempvec;

  size_t ord = state->ord;
  double ordm1coeff = 0.0;
  double ordp1coeff = 0.0;
  double ordp2coeff = 0.0;
  double errcoeff = 0.0;        /* error coefficient */

  int deltaord;

#ifdef DEBUG
  {
    size_t di;

    printf ("msadams_apply: t=%.5e, ord=%d, h=%.5e, y:", t, (int) ord, h);

    for (di = 0; di < dim; di++)
      {
        printf ("%.5e ", y[di]);
      }
    printf ("\n");
  }
#endif

  /* Check if t is the same as on previous stepper call (or last
     failed call). This means that calculation of previous step failed
     or the step was rejected, and therefore previous state will be
     restored or the method will be reset.
   */

  if (state->ni > 0 && (t == state->tprev || t == state->failt))
    {
      if (state->ni == 1)
        {
          /* No step has been accepted yet, reset method */

          msadams_reset (vstate, dim);
#ifdef DEBUG
          printf ("-- first step was REJECTED, msadams_reset called\n");
#endif
        }
      else
        {
          /* A succesful step has been saved, restore previous state. */

          /* If previous step suggests order increase, but the step was
             rejected, then do not increase order.
           */

          if (ord > state->ordprev)
            {
              state->ord = state->ordprev;
              ord = state->ord;
            }

          /* Restore previous state */

          DBL_MEMCPY (z, zbackup, (MSADAMS_MAX_ORD + 1) * dim);
          DBL_MEMCPY (hprev, hprevbackup, MSADAMS_MAX_ORD);
          state->ordprev = state->ordprevbackup;
          state->ordwait = state->ordwaitbackup;

#ifdef DEBUG
          printf ("-- previous step was REJECTED, state restored\n");
#endif
        }

      /* If step is repeatedly rejected, then reset method */

      state->failcount++;

      {
        const size_t max_failcount = 3;

        if (state->failcount > max_failcount && state->ni > 1)
          {
            msadams_reset (vstate, dim);
            ord = state->ord;

#ifdef DEBUG
            printf ("-- max_failcount reached, msadams_reset called\n");
#endif
          }
      }
    }
  else
    {
      /* The previous step was accepted. Backup current state. */

      DBL_MEMCPY (zbackup, z, (MSADAMS_MAX_ORD + 1) * dim);
      DBL_MEMCPY (hprevbackup, hprev, MSADAMS_MAX_ORD);
      state->ordprevbackup = state->ordprev;
      state->ordwaitbackup = state->ordwait;

      state->failcount = 0;

#ifdef DEBUG
      if (state->ni > 0)
        {
          printf ("-- previous step was ACCEPTED, state saved\n");
        }
#endif
    }

#ifdef DEBUG
  printf ("-- ord=%d, ni=%ld, ordwait=%d\n", (int) ord, state->ni,
          (int) state->ordwait);
  printf ("-- ordprev: %d\n", (int) state->ordprev);
#endif

  /* Get desired error levels via gsl_odeiv2_control object through driver
     object, which is a requirement for this stepper.
   */

  if (state->driver == NULL)
    {
      return GSL_EFAULT;
    }
  else
    {
      size_t i;

      for (i = 0; i < dim; i++)
        {
          if (dydt_in != NULL)
            {
              gsl_odeiv2_control_errlevel (state->driver->c, y[i],
                                           dydt_in[i], h, i, &errlev[i]);
            }
          else
            {
              gsl_odeiv2_control_errlevel (state->driver->c, y[i],
                                           0.0, h, i, &errlev[i]);
            }
        }
    }

#ifdef DEBUG
  {
    size_t di;
    printf ("-- errlev: ");
    for (di = 0; di < dim; di++)
      {
        printf ("%.5e ", errlev[di]);
      }
    printf ("\n");
  }
#endif

  /* On first call initialize Nordsieck matrix */

  if (state->ni == 0)
    {
      size_t i;

      DBL_ZERO_MEMSET (z, (MSADAMS_MAX_ORD + 1) * dim);

      if (dydt_in != NULL)
        {
          DBL_MEMCPY (ytmp, dydt_in, dim);
        }
      else
        {
          int s = GSL_ODEIV_FN_EVAL (sys, t, y, ytmp);

          if (s != GSL_SUCCESS)
            {
              return s;
            }
        }

      DBL_MEMCPY (&z[0 * dim], y, dim);
      DBL_MEMCPY (&z[1 * dim], ytmp, dim);

      for (i = 0; i < dim; i++)
        {
          z[1 * dim + i] *= h;
        }
    }

  /* Sanity check  */

  deltaord = ord - state->ordprev;

  if (deltaord > 1 || deltaord < -1)
    {
      printf ("-- order change %d\n", deltaord);
      GSL_ERROR_NULL ("msadams_apply too large order change", GSL_ESANITY);
    }

  /* Modify Nordsieck matrix if order or step length has been changed */

  /* If order increased by 1, initialize new Nordsieck vector */

  if (deltaord == 1)
    {
      DBL_ZERO_MEMSET (&z[ord * dim], dim);

#ifdef DEBUG
      printf ("-- order increase detected, Nordsieck modified\n");
#endif
    }

  /* If order decreased by 1, adjust Nordsieck matrix */

  if (deltaord == -1)
    {
      double hsum = 0.0;
      size_t i, j;

      /* Calculate coefficients used in adjustment to l */

      DBL_ZERO_MEMSET (l, MSADAMS_MAX_ORD + 1);

      l[1] = 1.0;

      for (i = 1; i < ord; i++)
        {
          hsum += hprev[i - 1];

          for (j = i + 1; j > 0; j--)
            {
              l[j] *= hsum / hprev[0];
              l[j] += l[j - 1];
            }
        }

      for (i = 1; i < ord; i++)
        {
          l[i + 1] = (ord + 1) * l[i] / (i + 1);
        }

      /* Scale Nordsieck matrix */

      for (i = 2; i < ord + 1; i++)
        for (j = 0; j < dim; j++)
          {
            z[i * dim + j] += -l[i] * z[(ord + 1) * dim + j];
          }

#ifdef DEBUG
      printf ("-- order decrease detected, Nordsieck modified\n");
#endif
    }

  /* Scale Nordsieck vectors if step size has been changed */

  if (state->ni > 0 && h != hprev[0])
    {
      size_t i, j;
      const double hrel = h / hprev[0];
      double coeff = hrel;

      for (i = 1; i < ord + 1; i++)
        {
          for (j = 0; j < dim; j++)
            {
              z[i * dim + j] *= coeff;
            }

          coeff *= hrel;
        }

#ifdef DEBUG
      printf ("-- h != hprev, Nordsieck modified\n");
#endif
    }

  /* Calculate polynomial coefficients (l), error coefficient and
     auxiliary coefficients
   */

  msadams_calccoeffs (ord, state->ordwait, h, hprev, pc, l, &errcoeff,
                      &ordm1coeff, &ordp1coeff, &ordp2coeff);

  /* Carry out the prediction step */

  {
    size_t i, j, k;

    for (i = 1; i < ord + 1; i++)
      for (j = ord; j > i - 1; j--)
        for (k = 0; k < dim; k++)
          {
            z[(j - 1) * dim + k] += z[j * dim + k];
          }

#ifdef DEBUG
    {
      size_t di;
      printf ("-- predicted y: ");
      for (di = 0; di < dim; di++)
        {
          printf ("%.5e ", z[di]);
        }
      printf ("\n");
    }
#endif
  }

  /* Calculate correction step to abscor */
  {
    int s;
    s = msadams_corrector (vstate, sys, t, h, dim, z, errlev, l, errcoeff,
                           abscor, relcor, ytmp, ytmp2);
    if (s != GSL_SUCCESS)
      {
        return s;
      }
  }

  {
    /* Add accepted final correction step to Nordsieck matrix */

    size_t i, j;

    for (i = 0; i < ord + 1; i++)
      for (j = 0; j < dim; j++)
        {
          z[i * dim + j] += l[i] * gsl_vector_get (abscor, j);
        }

#ifdef DEBUG
    {
      size_t di;
      printf ("-- corrected y: ");
      for (di = 0; di < dim; di++)
        {
          printf ("%.5e ", z[di]);
        }
      printf ("\n");
    }
#endif

    /* Derivatives at output */

    if (dydt_out != NULL)
      {
        int s = GSL_ODEIV_FN_EVAL (sys, t + h, z, dydt_out);

        if (s == GSL_EBADFUNC)
          {
            return s;
          }

        if (s != GSL_SUCCESS)
          {
            msadams_failurehandler (vstate, dim, t);

#ifdef DEBUG
            printf ("-- FAIL at user function evaluation\n");
#endif
            return s;
          }
      }

    /* Calculate error estimate */

    for (i = 0; i < dim; i++)
      {
        yerr[i] = fabs (gsl_vector_get (abscor, i)) * errcoeff;
      }

#ifdef DEBUG
    {
      size_t di;
      printf ("-- yerr: ");
      for (di = 0; di < dim; di++)
        {
          printf ("%.5e ", yerr[di]);
        }
      printf ("\n");
    }
#endif

    /* Save y values */

    for (i = 0; i < dim; i++)
      {
        y[i] = z[0 * dim + i];
      }
  }

  /* Scale abscor with errlev for later use in norm calculations */
  {
    size_t i;

    for (i = 0; i < dim; i++)
      {
        gsl_vector_set (abscor, i, gsl_vector_get (abscor, i) / errlev[i]);
      }
  }

  /* Save items needed for evaluation of order increase on next
     call, if needed
   */

  if (state->ordwait == 1 && ord < MSADAMS_MAX_ORD)
    {
      size_t i;

      state->ordp1coeffprev = ordp1coeff;
      state->ordm1coeff = ordm1coeff;

      for (i = 0; i < dim; i++)
        {
          gsl_vector_set (svec, i, gsl_vector_get (abscor, i));
        }
    }

  /* Consider and execute order change for next step */

  if (state->ordwait == 0)
    {
      msadams_eval_order (abscor, tempvec, svec, errcoeff, dim, errlev,
                          state->ordm1coeff, ordp1coeff,
                          state->ordp1coeffprev, ordp2coeff,
                          hprev, h, z, &(state->ord), &(state->ordwait));
    }

  /* Save information about current step in state and update counters */
  {
    size_t i;

    state->ordprev = ord;

    for (i = MSADAMS_MAX_ORD - 1; i > 0; i--)
      {
        hprev[i] = hprev[i - 1];
      }
    hprev[0] = h;

#ifdef DEBUG
    {
      size_t di;
      printf ("-- hprev: ");
      for (di = 0; di < MSADAMS_MAX_ORD; di++)
        {
          printf ("%.5e ", hprev[di]);
        }
      printf ("\n");
    }
#endif

    state->tprev = t;
    state->ordwait--;
    state->ni++;
  }

  return GSL_SUCCESS;
}

static int
msadams_set_driver (void *vstate, const gsl_odeiv2_driver * d)
{
  msadams_state_t *state = (msadams_state_t *) vstate;

  state->driver = d;

  return GSL_SUCCESS;
}

static int
msadams_reset (void *vstate, size_t dim)
{
  msadams_state_t *state = (msadams_state_t *) vstate;

  state->ni = 0;
  state->ord = 1;
  state->ordprev = 1;
  state->ordprevbackup = 1;
  state->ordwait = 2;
  state->ordwaitbackup = 2;
  state->failord = 0;
  state->failt = GSL_NAN;
  state->failcount = 0;

  DBL_ZERO_MEMSET (state->hprev, MSADAMS_MAX_ORD);
  DBL_ZERO_MEMSET (state->z, (MSADAMS_MAX_ORD + 1) * dim);

#ifdef DEBUG
  printf ("-- msadams_reset called\n");
#endif

  return GSL_SUCCESS;
}

static unsigned int
msadams_order (void *vstate)
{
  msadams_state_t *state = (msadams_state_t *) vstate;

  return state->ord;
}

static void
msadams_free (void *vstate)
{
  msadams_state_t *state = (msadams_state_t *) vstate;

  gsl_vector_free (state->tempvec);
  gsl_vector_free (state->svec);
  gsl_vector_free (state->relcor);
  gsl_vector_free (state->abscor);
  free (state->errlev);
  free (state->hprevbackup);
  free (state->hprev);
  free (state->l);
  free (state->pc);
  free (state->ytmp2);
  free (state->ytmp);
  free (state->zbackup);
  free (state->z);
  free (state);
}

static const gsl_odeiv2_step_type msadams_type = {
  "msadams",                    /* name */
  1,                            /* can use dydt_in? */
  1,                            /* gives exact dydt_out? */
  &msadams_alloc,
  &msadams_apply,
  &msadams_set_driver,
  &msadams_reset,
  &msadams_order,
  &msadams_free
};

const gsl_odeiv2_step_type *gsl_odeiv2_step_msadams = &msadams_type;

max_issues_repo_name: Brian-ning/HMNE

max_forks_repo_name: Brian-ning/HMNE


text:
#ifndef QRTRANSPOSE_H
#define QRTRANSPOSE_H

#include <gsl/gsl_matrix.h>
#include "QR.h"

/**
 * Compute and return the QR decomposition of a given matrix' transpose. If
 * `M` is passed to the constructor, the `getQ` function will return the Q
 * matrix in the decomposition M^T = QR et similis for `getR`.
 *
 * Convenient for computation of QR-backed nullspace (see NullspaceQR)
 */
class TransposeQR{
 public:
  TransposeQR(gsl_matrix* M);

  void updateFromMatrix();

  gsl_matrix* getMatrix() const;

  /** Return the Q-matrix from the decomposition M^T = QR */
  gsl_matrix* getQ() const;

  /** Return the R-matrix from the decomposition M^T = QR */
  gsl_matrix* getR() const;

 private:
  QR * const m_qr;
  gsl_matrix * const m_origMatrix;
};

#endif

ext: h


text:
#include <stdio.h>
#include <math.h>
#include <gsl/gsl_sf_bessel.h>
#include "global.h"
#include "utility.h"
#include "constants.h"
#include <gsl/gsl_cblas.h>
#include <time.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <unistd.h>
#include <omp.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include "steppmethods.h"

#define NUMBER_SAMPLES 1
#define NUMBER_VOLUMES 1
#define SHIFT_LATE 200
#define BINS 100
#define PDF_NR 12

// regular main but with multiple sampling runs over multiple volume fractions phi.

int
main (void){
	FILE *source, *target;
  	source = fopen("global.h", "r"); // get source global.h at program start so as to not copy edits during run

  	if( source == NULL )
  	{
  		printf("cannot open global.h\n");
  		exit(EXIT_FAILURE);
  	}
  	SETNUMTHREADS
  	int i,j,l,k,m ,n;
	Constants();									// draw coupling constants and other derived constants
	double *t = calloc(LENGTH_T, sizeof(double));
	time_t tme = time(NULL);
	char ordnerLABEL[300];
	char ueberordnerLABEL[300];
	char ordnerLABEL_original[300];
	strftime(ordnerLABEL, sizeof ordnerLABEL, "%A %c", localtime(&tme));
  	double squares_all_calcs[LENGTH_T][NUMBER_SAMPLES];			// for printing all squares in shared table
  	i = 0;
  	printf("\n%s\n", ordnerLABEL);
  	deleteSpaces(ordnerLABEL,ordnerLABEL);
  	strcpy (ordnerLABEL_original, ordnerLABEL);    	// keep original, change ordnerLABEL per run
  	double alpha_global = ALPHA;
  	double temp_global = TEMP;						// macros from global make trouble when used as print arguments, hence copy to memory
  	sprintf(ueberordnerLABEL,"N=%d_SIM=%d_T=%1.1E_ALPH=%1.1f", OSSZI, SIMULATIONEN, temp_global, alpha_global);
  	char tempLABEL[300];
  	strcpy (tempLABEL, ordnerLABEL);
  	strcat(tempLABEL, LABELPREFIX);
  	strcat(tempLABEL, ueberordnerLABEL);
  	strcpy (ueberordnerLABEL, tempLABEL);
  	struct stat st2 = {0};

  	double prob_density[BINS][PDF_NR];
  	for (i = 0; i < BINS ; i++)
  	{
  		for (j = 0; j < PDF_NR; j++)
  		{
  			prob_density[i][j] = 0.0;
  		}
  	}
  	int check_times[PDF_NR];
  	for (j = 0; j < PDF_NR; j++)
  	{
  		check_times[j] = (int) ( 10.0 * (j+1)) ;
  	}
  	check_times[PDF_NR-1] = LENGTH_T - 1;
  	for (j = 0; j < PDF_NR; j++)
  	{
  		printf("check times nr %d is %d \n", j, check_times[j]);
  	}
  	printf("\nprint results into ");
  	printf(ueberordnerLABEL);

  	if (stat(ueberordnerLABEL, &st2) == -1)
	{												//Teste ob Ordner existiert, erstelle Ordner
		mkdir(ueberordnerLABEL, 0700);
	}
	if (chdir(ueberordnerLABEL))						// change into directory of simulation
	{
		printf("Error changing directory");
		return 1;
	}

  	// reserviere speicher fuer eine Vollständige trajektorie ------------------
	m = LENGTH_T; n = ORDER;
	double **z = (double **) malloc(m * sizeof(double *));
	z[0] = (double *) malloc(m* n * sizeof(double));
	for (i=1; i<m; i++) z[i] = z[0] + n *i;

		double *squares = calloc(LENGTH_T, sizeof(double));
	double *short_correlation_x = calloc(LENGTH_T, sizeof(double));	//short term (1 timestep) correlations
	double *short_correlation_y = calloc(LENGTH_T, sizeof(double));
	double *total_momentum_x = calloc(LENGTH_T, sizeof(double));
	double *total_momentum_y = calloc(LENGTH_T, sizeof(double));
	double *long_correlation_x = calloc(LENGTH_T, sizeof(double)); //longterm correlation from first timestep
	double *long_correlation_y = calloc(LENGTH_T, sizeof(double));
	double *squares_increase = calloc(LENGTH_T, sizeof(double));	// Increase a from assumed form x^2 0 = a t
	double real_volumes[10];

	double *bathp = calloc(LENGTH_T, sizeof(double));
	double *bathq = calloc(LENGTH_T, sizeof(double));

	double *px_correlation = calloc(LENGTH_T, sizeof(double));	// save correlation of impulse_x to px(t=0)
	double *py_correlation = calloc(LENGTH_T, sizeof(double));
	double *px_correlation_late =  calloc(LENGTH_T, sizeof(double));	// corelation after time SHIFT_LATE
	double *px_correlation_late2 = calloc(LENGTH_T, sizeof(double));	// corelation after time SHIFT_LATE

	int n_zplots = SIMULATIONEN;
	if (SIMULATIONEN > MAX_NR_PLOTS ) n_zplots = MAX_NR_PLOTS;    // speichere maximal 30*DIM trajectorien
  	// reserviere speicher fuer samplepfade massives Teilchen zum plotten ------------------
	m = LENGTH_T; n = DIM*n_zplots;
	double **zplots = (double **) malloc(m * sizeof(double *));
	zplots[0] = (double *) malloc(m* n * sizeof(double));
	for (i=1; i<m; i++) zplots[i] = zplots[0] + n *i;

	// reserviere speicher fuer samplepfade 1. bad  Teilchen zum plotten ------------------
		m = LENGTH_T; n = DIM*n_zplots;
	double **qplots = (double **) malloc(m * sizeof(double *));
	qplots[0] = (double *) malloc(m* n * sizeof(double));
	for (i=1; i<m; i++) qplots[i] = qplots[0] + n *i;

	// reserviere speicher fuer samplepfade letztes bad  Teilchen zum plotten ------------------
		m = LENGTH_T; n = DIM*n_zplots;
	double **qlplots = (double **) malloc(m * sizeof(double *));
	qlplots[0] = (double *) malloc(m* n * sizeof(double));
	for (i=1; i<m; i++) qlplots[i] = qlplots[0] + n *i;

	// reserviere speicher fuer samplepfade zusätzlicher Kac_Zwanzig Kraft Term ------------------
		m = LENGTH_T; n = DIM*n_zplots;
	double **Zwanzig_Force = (double **) malloc(m * sizeof(double *));
	Zwanzig_Force[0] = (double *) malloc(m* n * sizeof(double));
	for (i=1; i<m; i++) Zwanzig_Force[i] = Zwanzig_Force[0] + n *i;

	// reserviere speicher fuer probability Density ------------------
		m = 80; n = 80;
	double **P_density = (double **) malloc(m * sizeof(double *));
	P_density[0] = (double *) malloc(m* n * sizeof(double));
	for (i=1; i<m; i++) P_density[i] = P_density[0] + n *i;
  	// reserviere speicher fuer Gitterkoordinaten ------------------

		double DENSITY_POINTS = 0.0;

	for(m=0; m<80; m++)
	{
		for(n=0; n<80; n++)
		{
			P_density[m][n] = 0.0;
		}
	}

	double *EKIN = calloc(LENGTH_T, sizeof(double));
	double *EBAD = calloc(LENGTH_T, sizeof(double));
	double *ETOT = calloc(LENGTH_T, sizeof(double));
	double *PTOT = calloc(LENGTH_T, sizeof(double));
	double *PTOTY = calloc(LENGTH_T, sizeof(double));
	double *LTOT = calloc(LENGTH_T, sizeof(double));

	// GSL random number Setup for Taus Generator
	const gsl_rng_type * T;
	gsl_rng * r;
	gsl_rng_env_setup();
	T = gsl_rng_ranlxd2;
	r = gsl_rng_alloc (T);
	gsl_rng_set(r, time(NULL)); // Seed with time
	// RNG setup End //
	// GSL random number Setup for Taus Generator for global rand
	const gsl_rng_type * T2;
	T2 = gsl_rng_ranlxd2;
	RAND_GLOBAL = gsl_rng_alloc (T2);
	gsl_rng_set(RAND_GLOBAL, time(NULL)); // Seed with time
	// RNG setup End //

	clock_t start, end;

	printf("evenly spread t \n");	//set up time vec
  	for (i = 0; i < 10; i++) // first ten values in small steps
  	{
  		t[i] =  i * TIME_STEPS/10.0;
  	}
  	for (i = 10; i < LENGTH_T; i++)
  	{
  		t[i] = (i-9) * TIME_STEPS;
  	}
  	double squarespace = 0.0;

  	DIFF_COEFF = KBOLTZ * TEMP/GAMMA * sin(M_PI * ALPHA ) / (M_PI * ALPHA);
  	if(ALPHA > 0.95)
  	{
  		DIFF_COEFF = KBOLTZ * TEMP/GAMMA;
  	}
  	LATTICE_SPACING =  sqrt(2*DIM*DIFF_COEFF * pow( TIME_ARRIVAL, ALPHA) );
  	printf("LATTICE_SPACING = %3.3E\n", LATTICE_SPACING);
  	double latlength = LATTICE_SPACING;

		//------------------------------EIGENTLICHE SIMULATION ------------------------------------------------------------------
  	int sim_count = 0;
  	start = clock();
  	time_first_contact = 0.0;
  	int error_count = 0;
		//
  	for (i = 0; i < SIMULATIONEN; i++)
  	{

		vec_zero_i(lattice_position,DIM);		// set stating cell to zero !
		TARGET_CONTACT = 0;
		FLAG_DUMP_VALUE = 0;					// set to 1 if error occures
		// -------------------------BAD
		Bath_Setup(y, LABEL, r);				// draw intial conditions
		//---------------------Bad aufgesetzt
		VVerlet_parallel(ORDER, y, z, t, Poti_Handle);
		end = clock();
		//------------------------------auswertung Integrationsergebnisse
		if ( !(FLAG_DUMP_VALUE))			// only count result without err
		{
		  	double *kahan_c = calloc(LENGTH_T, sizeof(double));                //
		  	double *kahan_t = calloc(LENGTH_T, sizeof(double));
		  	double *kahan_y = calloc(LENGTH_T, sizeof(double));
		  	#pragma omp parallel for
		  	for(l = 0; l < LENGTH_T; l++)
		  	{
		  		if (l>10)
		  		{
		  			long_correlation_x[l] += z[1][0] *z[l][0]/((double) SIMULATIONEN);
		  			long_correlation_y[l] += z[1][1] *z[l][1]/((double) SIMULATIONEN);
		  		}
		  		kahan_y[l] = z[10][DIM] * z[l][DIM] / ((double) SIMULATIONEN) / (mass * KBOLTZ * TEMP)\
		  					- kahan_c[l];
		  		kahan_t[l] = px_correlation[l] + kahan_y[l];
		  		kahan_c[l] = (kahan_y[l] - px_correlation[l]) - kahan_y[l];
		  		px_correlation[l] = kahan_t[l];
		  		py_correlation[l] += z[10][DIM + 1] * z[l][DIM + 1] / ((double) SIMULATIONEN) / (mass * KBOLTZ * TEMP);
		  		px_correlation_late[l] += z[SHIFT_LATE][DIM] * z[l][DIM] / ((double) SIMULATIONEN) / (mass * KBOLTZ * TEMP);
		  		px_correlation_late2[l] += z[SHIFT_LATE * 2][DIM] * z[l][DIM] / ((double) SIMULATIONEN) / (mass * KBOLTZ * TEMP);
		  		for(j=0; j< DIM; j++)
		  		{
		  			if (i < n_zplots)
		  			{
		  				zplots[l][DIM * i + j] = z[l][j];
		  				qplots[l][DIM * i + j] = z[l][2*DIM + j];
		  				double Force_TEMP = 0.0;
		  				for(int os = 0; os < OSSZI; os++)
		  					{
		  						Force_TEMP += (pow(coupling[os],2.0)/pow(ommega[os],2.0) - coupling[os]) * z[l][(2 + 2 * os) * DIM + j];
		  					}
		  					Zwanzig_Force[l][DIM * i + j]=Force_TEMP;
		  					int i_last = OSSZI -1;
		  					qlplots[l][DIM * i + j] = z[l][2*OSSZI*DIM + j];
		  			}
		  			squares[l] = squares[l] + pow(z[l][j],2.0)/((double) SIMULATIONEN);
		  			EKIN[l] = EKIN[l] + pow(z[l][DIM+j],2.0)/(mass * SIMULATIONEN * KBOLTZ *TEMP * DIM);
		  			for(k=0;k<OSSZI;k++)
		  			{
		  				bathq[l] = bathq[l] + pow(z[l][(2 + 2*k) *DIM +j],2.0)/( (double) (OSSZI*SIMULATIONEN)\
		  						*KBOLTZ*TEMP) * pow(ommega[k],2.0)/2.0;// mittlere potentielle E pro Badteilchen
		  																//reskaliert durch kT
		  				bathp[l] = bathp[l] + pow(z[l][(3 + 2*k) *DIM +j],2.0)/( (double) (OSSZI*SIMULATIONEN)\
		  						*KBOLTZ*TEMP) /2.0/massq[k];						// mittlere kinetische E pro Badteilchen
		  																//reskaliert durch kT
		  				EBAD[l] = EBAD[l] + pow(z[l][(3 + 2*k) *DIM +j], 2.0)/2.0\
		  						+ 0.5  * pow(ommega[k],2.0) \
		  						* pow(z[l][(2 + 2*k) *DIM +j] - coupling[k]/(pow(ommega[k],2.0) ) * z[l][j] , 2.0) /((double) SIMULATIONEN);
		  			}
		  		}
			  	for(k=0;k<OSSZI;k++)	// add p and angular momentum L for bath
			  	{
			  		PTOT[l] 	+= z[l][(3 + 2*k) *DIM + 0];
			  		PTOTY[l]	+= z[l][(3 + 2*k) *DIM + 1];
			  		LTOT[l] += 	(z[l][(2 + 2*k) *DIM + 0] * z[l][(3 + 2*k) *DIM + 1]\
			  					- z[l][(2 + 2*k) *DIM + 1] * z[l][(3 + 2*k) *DIM + 0]) /((double) SIMULATIONEN);
			  	}
			  	PTOT[l] += z[l][2];
			  	PTOT[l] += z[l][3];
			  	LTOT[l] += (z[l][0] * z[l][3] - z[l][1] * z[l][2]) / ((double) SIMULATIONEN);
			}
			if(DIM == 1)
			{
			  	// setup pdf checks
			  	double bins = BINS * 1.0;
			  	double dx = LATTICE_SPACING/bins;
			  	double sims = SIMULATIONEN;
			  	for (j = 0; j < PDF_NR; j++)
			  	{
			  		int t_check = check_times[j];
			  		for (int i_bin = 0; i_bin < BINS; i_bin++)
			  		{
			  			double lower = -LATTICE_SPACING / 2.0 + i_bin * dx;
			  			double upper =  -LATTICE_SPACING / 2.0 + (i_bin + 1) * dx;
			  			if 	( 	( z[t_check][0] <= upper) &&
			  					( z[t_check][0] > lower)
			  				)
			  			{
			  				prob_density[i_bin][j] += 1.0/sims;
			  			}
			  		}
			  	}
			}
			sim_count += 1;
			//printf("%d  und t %4.2f \n", i ,((double) (end - start)));
			printf("\r%d von %d mit t/count = %4.2f s und average t_rest =%4.2f h ", sim_count , SIMULATIONEN, (double) (end - start) /  sim_count/ THREADS / CLOCKS_PER_SEC ,\
			  		((double) (end - start) / sim_count/ THREADS/ CLOCKS_PER_SEC * (SIMULATIONEN - sim_count)/3600));
			printf(" %d hits on Lat  and avrgtcntct = %4.2f",TARGET_CONTACT, time_first_contact/(i+1) );
			fflush(stdout);
			for(l = 0; l < LENGTH_T; l++)
			{
			  	for (int oss_i=0; oss_i < OSSZI; oss_i++)
			  	{
			  		total_momentum_x[l] += z[l][(3 + 2*oss_i) *DIM + 0];
			  		total_momentum_y[l] += z[l][(3 + 2*oss_i) *DIM + 1];
			  	}
			  	total_momentum_x[l] += z[l][(0 + 2*0) *DIM + 0];
			  	total_momentum_y[l] += z[l][(0 + 2*0) *DIM + 1];
			}
			// -------------- end evaluation if----------------------
			free(kahan_y); free(kahan_t); free(kahan_c);
		}else
		{
			error_count++;
			printf("\n err occured at calc %d, calculation dumped, %d totatl errors\n",  i,error_count );
			i -= 1;		// do one more calculation
		}

	}
	tme = time(NULL);
	char end_label[90];
	strftime( end_label, sizeof end_label, "%A %c", localtime(&tme));
	printf("\n%s\n", end_label);
	for(l = 0; l < LENGTH_T; l++)
	{
		ETOT[l] = EKIN[l] + EBAD[l];
			  	PTOT[l] = sqrt(pow(PTOT[l]/((double) SIMULATIONEN),2.0) + pow(PTOTY[l]/((double) SIMULATIONEN),2.0));
	}
	for(l = 1; l < LENGTH_T; l++)
	{
		squares_increase[l] = (squares[l]- squares[l-1])/(t[l] - t[l-1]);
	}
			//------------------------------ENDE SIMULATION, speichere daten ------------------------------------------------------------------
	for(m=0; m<80; m++)
	{
		for(n=0; n<80; n++)
		{
			P_density[m][n] *= 1.0/DENSITY_POINTS;
		}
	}
	FILE *fp;

	struct stat st = {0};

	if (stat(ordnerLABEL, &st) == -1){		//Teste ob Ordner existiert, erstelle Ordner
		mkdir(ordnerLABEL, 0700);
	}

	fp = fopen ("shellscript.sh", "w");		//create Shellscript to start Gnuplot from c main()
	fprintf(fp, "cd %s\n", ordnerLABEL);
	fprintf(fp, "gnuplot gnuplot.txt\n");
	fprintf(fp, "cd ..");
	fclose (fp);

	 // copy global.h 	/

   	// copy to same name into directory, clould be anything different

   	if (chdir(ordnerLABEL))			// change into directory of simulation
   	{
   		printf("Error changing directory");
   		return 1;
   	}

   	mkdir("plots", 0700);
   	mkdir("trajec", 0700);

   	target = fopen("global.h", "w");

   	if( target == NULL )
   	{
   		fclose(source);
   		printf("Press any key to exit...\n");
   		exit(EXIT_FAILURE);
   	}
   	char ch;
   	while( ( ch = fgetc(source) ) != EOF )
   	{
   		fputc(ch, target);
   	}

   	printf("File copied successfully.\n");

   	fclose(target);

   	fp = fopen ("latlength.dat", "w");
   	fprintf(fp, "%lf  \n", latlength);
   	fclose (fp);

   	fp = fopen ("squares_rohdaten.dat", "w");
   	for(l = 0; l < LENGTH_T; l++){
   		fprintf(fp, "%1.3E  %1.3E\n", t[l], squares[l]);
   	}
   	fclose (fp);

   	fp = fopen ("ekin.dat", "w");
   	for(l = 1; l < LENGTH_T; l++){
   		fprintf(fp, "%lf  %lf\n", t[l]/TIME_ARRIVAL, EKIN[l]);
   	}
   	fclose (fp);

   	fp = fopen ("ekinbath.dat", "w");
   	for(l = 1; l < LENGTH_T; l++){
   		fprintf(fp, "%lf  %lf\n", t[l]/TIME_ARRIVAL, bathp[l]);
   	}
   	fclose (fp);

   	fp = fopen ("ommega.dat", "w");
   	for(l = 0; l < OSSZI; l++){
   		fprintf(fp, "%lf\n", ommega[l]);
   	}
   	fclose (fp);

   	fp = fopen ("PTOT.dat", "w");
   	for(l = 0; l < LENGTH_T; l++){
   		fprintf(fp, "%lf  %lf\n", t[l]/TIME_ARRIVAL, PTOT[l]);
   	}
   	fclose (fp);

   	fp = fopen ("P_X.dat", "w");
   	for(l = 0; l < LENGTH_T; l++){
   		fprintf(fp, "%lf  %1.3e\n", t[l]/TIME_ARRIVAL, total_momentum_x[l]);
   	}
   	fclose (fp);

   	fp = fopen ("P_Y.dat", "w");
   	for(l = 0; l < LENGTH_T; l++){
   		fprintf(fp, "%lf  %1.3e\n", t[l]/TIME_ARRIVAL, total_momentum_y[l]);
   	}
   	fclose (fp);

   	fp = fopen ("xshortcorellation.dat", "w");
   	for(l = 1; l < LENGTH_T; l++){
   		fprintf(fp, "%lf  %lf\n", t[l]/TIME_ARRIVAL, short_correlation_x[l]/latlength/latlength);
   	}
   	fclose (fp);

   	fp = fopen ("yshortcorellation.dat", "w");
   	for(l = 1; l < LENGTH_T; l++){
   		fprintf(fp, "%lf  %lf\n", t[l]/TIME_ARRIVAL, short_correlation_y[l]/latlength/latlength);
   	}
   	fclose (fp);

   	fp = fopen ("xlongcorellation.dat", "w");
   	for(l = 1; l < LENGTH_T; l++){
   		fprintf(fp, "%lf  %lf\n", t[l]/TIME_ARRIVAL, long_correlation_x[l]/latlength/latlength);
   	}
   	fclose (fp);

   	fp = fopen ("ylongcorellation.dat", "w");
   	for(l = 1; l < LENGTH_T; l++){
   		fprintf(fp, "%lf  %lf\n", t[l]/TIME_ARRIVAL, long_correlation_y[l]/latlength/latlength);
   	}
   	fclose (fp);

   	fp = fopen ("PX_corr.dat", "w");
   	for(l = 1; l < LENGTH_T; l++){
   		fprintf(fp, "%1.3E  %1.3E\n", t[l]/TIME_ARRIVAL, px_correlation[l]);
   	}
   	fclose (fp);

   	fp = fopen ("PY_corr.dat", "w");
   	for(l = 1; l < LENGTH_T; l++){
   		fprintf(fp, "%1.3E  %1.3E\n", t[l]/TIME_ARRIVAL, py_correlation[l]);
   	}
   	fclose (fp);

   	fp = fopen ("PX_corr_late.dat", "w");
   	for(l = 1; l < LENGTH_T; l++){
   		fprintf(fp, "%1.3E  %1.3E\n", t[l]/TIME_ARRIVAL, px_correlation_late[l]);
   	}
   	fclose (fp);

   	fp = fopen ("PX_corr_late2.dat", "w");
   	for(l = 1; l < LENGTH_T; l++){
   		fprintf(fp, "%1.3E  %1.3E\n", t[l]/TIME_ARRIVAL, px_correlation_late2[l]);
   	}
   	fclose (fp);

   	fp = fopen ("PX_corr_abs.dat", "w");
   	for(l = 1; l < LENGTH_T; l++){
   		fprintf(fp, "%1.3E  %1.3E\n", t[l]/TIME_ARRIVAL, fabs(px_correlation[l]));
   	}
   	fclose (fp);

   	fp = fopen ("PX_corr_late_abs.dat", "w");
   	for(l = 1; l < LENGTH_T; l++){
   		fprintf(fp, "%1.3E  %1.3E\n", t[l]/TIME_ARRIVAL, fabs(px_correlation_late[l]));
   	}
   	fclose (fp);

   	fp = fopen ("PX_corr_late2_abs.dat", "w");
   	for(l = 1; l < LENGTH_T; l++){
   		fprintf(fp, "%1.3E  %1.3E\n", t[l]/TIME_ARRIVAL, fabs(px_correlation_late2[l]));
   	}
   	fclose (fp);

   	if (DIM == 1)
   	{
   		fp = fopen ("PDF_1D.dat", "w");
   		for (int pos = 0; pos < BINS; pos++)
   		{
   			fprintf(fp, "%1.3E ", -LATTICE_SPACING/2.0 + pos*LATTICE_SPACING/((double) BINS));
   			for(j = 0; j < PDF_NR; j++)
   			{
   				fprintf(fp, "%1.3E ", prob_density[pos][j]);
   			}
   			fprintf(fp,"\n");
   		}
   		fclose (fp);
   	}

	// finde größtes s^2/t^alpha zum endzeitpunkt
   	int t_start = 10;
   	double temp_ende = 0.0;
   	for(int i_alpha = 1; i_alpha < 12; i_alpha ++){
   		double alpha_temp = 0.45 + 0.05 * i_alpha;
   		l = LENGTH_T-1;
   		if (temp_ende < (squares[l] / (pow(t[l], alpha_temp))) )
   		{
   			temp_ende = squares[l] / (pow(t[l], alpha_temp));
   		}
   	}
   	if (DIM>1)
   	{

   		char zplotsLABEL[30];
   		for (i = 0; i < n_zplots; i++)
   		{
   			sprintf(zplotsLABEL, "trajec/zplots%d.dat",i);
   			fp = fopen (zplotsLABEL, "w");
   			for(l = 0; l < LENGTH_T; l++){
   				for (j = 0; j < DIM ; j++){
   					fprintf(fp, "%lf  ", ((zplots[l][j +i*DIM])/latlength) );
   				}
   				fprintf(fp, "\n");
   			}
   			fclose (fp);
   		}
   		for (i = 0; i < n_zplots; i++)
   		{
   			sprintf(zplotsLABEL, "trajec/qplots%d.dat",i);
   			fp = fopen (zplotsLABEL, "w");
   			for(l = 0; l < LENGTH_T; l++){
   				for (j = 0; j < DIM ; j++){
   					fprintf(fp, "%lf  ", ((qplots[l][j +i*DIM])) );
   				}
   				fprintf(fp, "\n");
   			}
   			fclose (fp);
   		}
   		for (i = 0; i < n_zplots; i++)
   		{
   			sprintf(zplotsLABEL, "trajec/Zwanzig_Force%d.dat",i);
   			fp = fopen (zplotsLABEL, "w");
   			for(l = 0; l < LENGTH_T; l++){
   				for (j = 0; j < DIM ; j++){
   					fprintf(fp, "%lf  ", ((Zwanzig_Force[l][j +i*DIM])) );
   				}
   				fprintf(fp, "\n");
   			}
   			fclose (fp);
   		}
   		for (i = 0; i < n_zplots; i++)
   		{
   			sprintf(zplotsLABEL, "trajec/qlplots%d.dat",i);
   			fp = fopen (zplotsLABEL, "w");
   			for(l = 0; l < LENGTH_T; l++){
   				for (j = 0; j < DIM ; j++){
   					fprintf(fp, "%lf  ", ((qlplots[l][j +i*DIM])) );
   				}
   				fprintf(fp, "\n");
   			}
   			fclose (fp);
   		}
   	}
   	if (DIM==1)
   	{

   		char zplotsLABEL[30];
   		for (i = 0; i < n_zplots; i++)
   		{
   			sprintf(zplotsLABEL, "trajec/zplots%d.dat",i);
   			fp = fopen (zplotsLABEL, "w");
   			for(l = 0; l < LENGTH_T; l++){
   				fprintf(fp, " %lf  %lf \n" ,t[l]/TIME_ARRIVAL, zplots[l][i*DIM]/latlength);
   			}
   			fclose (fp);
   		}
   	}

   	fp = fopen ("ETOT.dat", "w");
   	for(l = 0; l < LENGTH_T; l++){
   		fprintf(fp, "%lf  %lf\n", t[l], ETOT[l]);
   	}
   	fclose (fp);

   	fp = fopen ("DENSITY.dat", "w");
   	for(m=0; m<80; m++)
   	{
   		for(n=0; n<80; n++)
   		{
   			fprintf(fp, "%1.2e ", P_density[m][n]);
   		}
   		fprintf(fp, "\n");
   	}
   	fclose (fp);

	gsl_rng_free (r); gsl_rng_free (RAND_GLOBAL);
	free(zplots[0]); free(zplots);
	free(qplots[0]); free(qplots);
	free(Zwanzig_Force[0]); free(Zwanzig_Force);
	free(qlplots[0]); free(qlplots);
	free (P_density[0]); free (P_density);
	free(t);
	free(bathp); free(bathq);
	free(squares_increase);
	free(short_correlation_x);
	free(total_momentum_x);
	free(total_momentum_y);
	free(short_correlation_y);
	free(long_correlation_x);
	free(px_correlation); free(py_correlation);
	free(px_correlation_late);
	free(px_correlation_late2);
	free(long_correlation_y);
	free(ETOT); free(EKIN); free(EBAD);
	free(PTOT); free(PTOTY);free(LTOT);
	free(squares);
	free(z[0]); free(z);
}

ext: c


text:
///
/// @file
///
/// @author Mirko Myllykoski (mirkom@cs.umu.se), Umeå University
///
/// @internal LICENSE
///
/// Copyright (c) 2019-2020, Umeå Universitet
///
/// Redistribution and use in source and binary forms, with or without
/// modification, are permitted provided that the following conditions are met:
///
/// 1. Redistributions of source code must retain the above copyright notice,
///    this list of conditions and the following disclaimer.
///
/// 2. Redistributions in binary form must reproduce the above copyright notice,
///    this list of conditions and the following disclaimer in the documentation
///    and/or other materials provided with the distribution.
///
/// 3. Neither the name of the copyright holder nor the names of its
///    contributors may be used to endorse or promote products derived from this
///    software without specific prior written permission.
///
/// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
/// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
/// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
/// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
/// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
/// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
/// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
/// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
/// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
/// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
/// POSSIBILITY OF SUCH DAMAGE.
///

#include <starneig_config.h>
#include <starneig/configuration.h>
#include "common.h"
#include "../common/common.h"
#include "../common/node_internal.h"
#include <starneig/gep_sm.h>
#include <cblas.h>
#include <stdlib.h>

__attribute__ ((visibility ("default")))
starneig_error_t starneig_GEP_SM_HessenbergTriangular(
    int n,
    double A[], int ldA,
    double B[], int ldB,
    double Q[], int ldQ,
    double Z[], int ldZ)
{
    extern void dgeqrf_(int const *, int const *, double *, int const *,
        double *, double *, int const *, int *);

    extern void dormqr_(char const *, char const *, int const *, int const *,
        int const *, double const *, int const *, double const *, double *,
        int const *, double*, const int *, int *);

    extern void dgghd3_(char const *, char const *, int const *, int const *,
        int const *, double *, int const *, double *, int const *, double *,
        int const *, double *, int const *, double *, int const *, int *);

    if (n < 1)          return -1;
    if (A == NULL)      return -2;
    if (ldA < n)        return -3;
    if (B == NULL)      return -4;
    if (ldB < n)        return -5;
    if (Q == NULL)      return -6;
    if (ldQ < n)        return -7;
    if (Z == NULL)      return -8;
    if (ldZ < n)        return -9;

    if (!starneig_node_initialized())
        return STARNEIG_NOT_INITIALIZED;

    starneig_wrappers_prepare();
    starneig_node_set_blas_mode(STARNEIG_BLAS_MODE_PARALLEL);

    double *tau = NULL, *work = NULL;
    int info, ilo = 1, ihi = n;

    //
    // allocate workspace
    //

    int lwork = 0;

    {
        int _lwork = -1;
        double dlwork;

        dgeqrf_(&n, &n, B, &ldB, tau, &dlwork, &_lwork, &info);
        if (info != 0)
            goto cleanup;

        lwork = MAX(lwork, dlwork);
    }

    {
        int _lwork = -1;
        double dlwork;

        dormqr_("L", "T", &n, &n, &n,
            B, &ldB, tau, A, &ldA, &dlwork, &_lwork, &info);
        if (info != 0)
            goto cleanup;

        lwork = MAX(lwork, dlwork);
    }

    {
        int _lwork = -1;
        double dlwork;

        dormqr_("R", "N", &n, &n, &n,
            B, &ldB, tau, Q, &ldQ, &dlwork, &_lwork, &info);
        if (info != 0)
            goto cleanup;

        lwork = MAX(lwork, dlwork);
    }

    {
        int _lwork = -1;
        double dlwork;

        dgghd3_("V", "V", &n, &ilo, &ihi,
            A, &ldA, B, &ldB, Q, &ldQ, Z, &ldZ, &dlwork, &_lwork, &info);
        if (info != 0)
            goto cleanup;

        lwork = MAX(lwork, dlwork);
    }

    tau = malloc(n*sizeof(double));
    work = malloc(lwork*sizeof(double));

    //
    // reduce
    //

    // form B = ~Q * R
    dgeqrf_(&n, &n, B, &ldB, tau, work, &lwork, &info);
    if (info != 0)
        goto cleanup;

    // A <- ~Q^T * A
    dormqr_("L", "T", &n, &n, &n, B, &ldB, tau, A, &ldA, work, &lwork, &info);
    if (info != 0)
        goto cleanup;

    // Q <- Q * ~Q
    dormqr_("R", "N", &n, &n, &n, B, &ldB, tau, Q, &ldQ, work, &lwork, &info);
    if (info != 0)
        goto cleanup;

    // clean B (B <- R)
    for (int i = 0; i < n; i++)
        for (int j = i+1; j < n; j++)
            B[(size_t)i*ldB+j] = 0.0;

    // reduce (A,B) to Hessenberg-triangular form
    dgghd3_("V", "V", &n, &ilo, &ihi,
        A, &ldA, B, &ldB, Q, &ldQ, Z, &ldZ, work, &lwork, &info);
    if (info != 0)
        goto cleanup;

cleanup:

    starneig_node_set_blas_mode(STARNEIG_BLAS_MODE_ORIGINAL);
    starneig_wrappers_finish();

    free(tau);
    free(work);

    return info;
}

max_stars_repo_name: NLAFET/StarNEig

max_issues_repo_name: NLAFET/StarNEig

max_forks_repo_name: NLAFET/StarNEig


text:
/**
 * author: Jochen K"upper
 * created: Jan 2002
 * file: pygsl/src/statisticsmodule.c
 * $Id: doublemodule.c,v 1.9 2004/03/24 08:40:45 schnizer Exp $
 *
 * optional usage of Numeric module, available at http://numpy.sourceforge.net
 * "
 */

#include <pygsl/error_helpers.h>
#include <pygsl/block_helpers.h>
#include <Python.h>
#include <gsl/gsl_statistics.h>

/* include real functions for default data-types (double in C) */

#define STATMOD_WEIGHTED
#define STATMOD_APPEND_PY_TYPE(X) X ## Float
#define STATMOD_APPEND_PYC_TYPE(X) X ## DOUBLE
#define STATMOD_FUNC_EXT(X, Y) X ## Y
#define STATMOD_PY_AS_C PyFloat_AsDouble
#define STATMOD_C_TYPE double
#define PyGSL_STATISTICS_IMPORT_API
#include "functions.c"

PyGSL_STATISTICS_INIT(double, "double")

/*
 * Local Variables:
 * mode: c
 * c-file-style: "Stroustrup"
 * End:
 */

max_stars_repo_name: juhnowski/FishingRod

max_issues_repo_name: juhnowski/FishingRod

max_forks_repo_name: juhnowski/FishingRod


text:
/*
 * CARP/Kaczmarz sweep on diagonally banded matrix. The essential loop is:
 *
 * for each row i
 *     x = x + w*(b(i) - A(i,:)*x)*A(i,:)'
 * end
 *
 * The matrix is given in band storage format, where each row (stored
 * contiguously in memory) of the array R stores a diagonal of the matrix with
 * offset idx(j), such that
 *
 * 	   A(i,i+idx(j)) = R(i,j)
 *
 * use (from MATLAB):
 * 		y = sweepR_mex(R,idx,x,b,w,dir)
 *
 * 		R			- matrix of diagonals of matrix A
 *		idx			- offsets of diagonals
 *		x			- initial guess
 * 		b 			- right hand side (source)
 *		w 			- relaxation parameter (0 <= w <= 2)
 *		dir 		- if dir > 0, go through matrix rows in ascending order.
 * 			  		  if dir < 0, go through matrix rows in descending order.
 * 		n_threads	- OPTIONAL argument to control the number of execution
 * 		threads solving CARP blocks in parallel. The number of threads can also
 * 		be defined via an environment variable (OMP_NUM_THREADS), but this
 * 		optional argument takes precedence. The default number of threads is
 * 		one. Take care if using more than one MATLAB worker per node: each
 * 		MATLAB worker will use OMP_NUM_THREADS, so if there are four workers on
 * 		a node, there will be 4 x OMP_NUM_THREADS parallel CARP sweeps.
 *
 * Author: Art Petrenko, Tristan van Leeuwen
 *         Seismic Laboratory for Imaging and Modeling
 *         Department of Earth, Ocean, and Atmosperic Sciences
 *         The University of British Columbia
 *
 * Date: July, 2014

 * You may use this code only under the conditions and terms of the
 * license contained in the file LICENSE provided with this source
 * code. If you do not agree to these terms you may not use this
 * software.
*/

#include <stdlib.h> /* for getenv */
#include <stddef.h> /* for size_t type */
#include <string.h> /* for memcpy */
#include <pthread.h> /* for threading */

/* The following section allows this file to compile on Mac OS X 10.8.5. Pass
 * the flag -DARCH_MACI64 to the compiler to activate it. */
#ifdef ARCH_MACI64
#include "pthread_barrier.h"
#include <mach/error.h>
typedef wchar_t char16_t;
#else /* not ARCH_MACI64 */
#include <error.h>
#endif /* ARCH_MACI64 */

#include <math.h>
#include <mex.h>
#include <matrix.h>
#include <blas.h>

struct copy_init_guess_data_t
{
	double *copy_src_real, *copy_dst_real;
	double *copy_src_imag, *copy_dst_imag;
	long n_to_copy;
};

struct sweep_data_t
{
	long start_row, end_row, ncol, ny, nx, haloWidth, main_diagonal_offset;
	double *Rr, *Ri, *yr, *yi, *br, *bi;
	long *idx;
	double w;
	int dir;
};

struct average_data_t
{
	double *copy_src_real, *copy_dst_real;
	double *copy_src_imag, *copy_dst_imag;
	double *halo_1_real, *halo_2_real;
	double *halo_1_imag, *halo_2_imag;
	double *halo_dst_real, *halo_dst_imag;
	long n_to_copy, n_in_halo;
};

struct thread_data_t
{
	struct copy_init_guess_data_t copy_init_guess_data;
	struct sweep_data_t sweep_data;
	struct average_data_t average_data;
	pthread_barrier_t *barrier;
};

void *do_sweep(void *thread_args_void)
{
    struct sweep_data_t *thread_args;
	/* Variables contained in thread_args_void struct */
	long start_row, end_row, haloWidth, ncol, ny, nx;
	/* Rr and Ri are pointers to short fat ncol-by-N matrices */
	double *Rr, *Ri, *yr, *yi, *br, *bi;
	long *idx;
	double w;
	int dir;
	/* Temporary storage variables */
	double cr = 0, ci = 0;
	long offset, main_diagonal_offset;

	/* Assign local pointers to data locations in shared memory */
	thread_args = (struct sweep_data_t *) thread_args_void;
	start_row 	= thread_args->start_row;
	end_row 	= thread_args->end_row;
	ncol 		= thread_args->ncol;
	ny 			= thread_args->ny;
	nx			= thread_args->nx;
	haloWidth	= thread_args->haloWidth;
	main_diagonal_offset = thread_args->main_diagonal_offset;
	Rr 			= thread_args->Rr;
	Ri 			= thread_args->Ri;
	idx 		= thread_args->idx;
	yr 			= thread_args->yr;
	yi 			= thread_args->yi;
	br 			= thread_args->br;
	bi 			= thread_args->bi;
	w 			= thread_args->w;
	dir 		= thread_args->dir;

	offset = (start_row == 0 ? 0 : haloWidth - main_diagonal_offset);

	/* Kaczmarz sweep on one row block */
	for(long i = (dir > 0 ? start_row : end_row-1);
		dir > 0 ? i<end_row : i>=start_row;
		dir > 0 ? i++ : i--)
	{

		if (0 <= i + main_diagonal_offset && i + main_diagonal_offset < nx){
			cr = br[i + main_diagonal_offset];
			ci = bi[i + main_diagonal_offset];
		}
		else{
		  //error(1,0,"Discovery of whether the iterate vector is haloed failed.");
		}
		/* First loop over non-zero row elements calculates inner product
		 * of matrix row and CARP iterate */
		for(long j=0, k; j<ncol;j++){
			/* i + idx[j] is the column index for the full Helmholtz matrix.
			 * k is the index into the vector representing the CARP iterate
			 * of the given block. */
			k = i + idx[j] - start_row + offset;
			if(0<=k && k<ny){
				cr -= Rr[i*ncol + j]*yr[k] - Ri[i*ncol + j]*yi[k];
				ci -= Rr[i*ncol + j]*yi[k] + Ri[i*ncol + j]*yr[k];
			}
		}
		/* Second loop over non-zero row elements updates CARP iterate */
		cr *= w;
		ci *= w;
		for(long j=0, k; j<ncol;j++){
			k = i + idx[j] - start_row + offset;
			if(0<=k && k<ny){
				yr[k] +=   cr*Rr[i*ncol + j] + ci*Ri[i*ncol + j];
				yi[k] +=  -cr*Ri[i*ncol + j] + ci*Rr[i*ncol + j];
			}
		}
	}

	return NULL;
}

void *average_halos(void *thread_args_void)
{
	struct average_data_t *thread_args;
	double *copy_src_real, *copy_dst_real;
	double *copy_src_imag, *copy_dst_imag;
	double *halo_1_real, *halo_2_real;
	double *halo_1_imag, *halo_2_imag;
	double *halo_dst_real, *halo_dst_imag;
	size_t n_to_copy, n_in_halo;

	/* Assign local pointer to data locations in shared memory */
	thread_args 	= (struct average_data_t *) thread_args_void;
	copy_src_real	= thread_args->copy_src_real;
	copy_dst_real	= thread_args->copy_dst_real;
	copy_src_imag	= thread_args->copy_src_imag;
	copy_dst_imag	= thread_args->copy_dst_imag;
	n_to_copy		= thread_args->n_to_copy;
	halo_1_real     = thread_args->halo_1_real;
	halo_2_real		= thread_args->halo_2_real;
	halo_dst_real	= thread_args->halo_dst_real;
	halo_1_imag		= thread_args->halo_1_imag;
	halo_2_imag		= thread_args->halo_2_imag;
	halo_dst_imag   = thread_args->halo_dst_imag;
	n_in_halo		= thread_args->n_in_halo;

	/* Copy the non-halo parts of the domain block directly to output array */
	memcpy((void *)copy_dst_real, (void *)copy_src_real, sizeof(double)*n_to_copy);
	memcpy((void *)copy_dst_imag, (void *)copy_src_imag, sizeof(double)*n_to_copy);

	/* Average the halo parts of the domain block and copy to output array.
	 * NOTE: this assumes domain blocks overlap only with their nearest
	 * neighbour segments. */
	for (long i = 0; i < n_in_halo; i++){
		halo_dst_real[i] = (halo_1_real[i] + halo_2_real[i]) / 2;
		halo_dst_imag[i] = (halo_1_imag[i] + halo_2_imag[i]) / 2;
	}

	return NULL;
}

void *sweep_and_average(void *thread_data_void)
{
	struct thread_data_t *thread_data = (struct thread_data_t *) thread_data_void;

	/* Copy the initial guess into the working arrays */
	memcpy((void *)thread_data->copy_init_guess_data.copy_dst_real, (void *)thread_data->copy_init_guess_data.copy_src_real, sizeof(double)*thread_data->copy_init_guess_data.n_to_copy);
	memcpy((void *)thread_data->copy_init_guess_data.copy_dst_imag, (void *)thread_data->copy_init_guess_data.copy_src_imag, sizeof(double)*thread_data->copy_init_guess_data.n_to_copy);
	do_sweep((void *) &(thread_data->sweep_data));

	pthread_barrier_wait(thread_data->barrier);

	average_halos((void *) &(thread_data->average_data));

	return NULL;
}

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
	/* structs to hold all arguments to each thread in one variable */
	struct sweep_data_t **thread_args_sweep = NULL;
	struct average_data_t **thread_args_average = NULL;
	struct thread_data_t **thread_data = NULL;
	struct copy_init_guess_data_t **copy_init_guess_data = NULL;
	pthread_barrier_t barrier_after_sweep_before_halo_average;

	mwSize ncol, nx;
	ptrdiff_t ncolBlas, idxIncBlas = 1, maxIdxLoc = 0;
	mwSize haloWidth;
	mwSize n_threads = 1;
	char *n_threads_str = NULL;
	double *Rr,*Ri,*idxd = NULL,*xr,*xi,*br,*bi,*yr,*yi;
	long *idx = NULL;
    double w = 0;
	int dir = 1;
	mwSize N=1, numGridPointsPerBlock, main_diagonal_offset;

	/* Flags that are set if memory is allocated within the MEX file */
	int Ri_alloc=0, xi_alloc=0, bi_alloc=0;

	/* a return code flag and segment demarcation arrays */
	mwSize i_thread;
	mwSize *seg_bounds_hi, *seg_bounds_mid, *seg_bounds_row, *seg_bounds_lo;

	/* Threading variables */
	pthread_t *threadIDs = NULL;
	pthread_attr_t attr;

	/* Arrays to hold (overlapping) segments of yr and yi, a pair for each
	 * thread */
	double **yr_seg = NULL, **yi_seg = NULL;

	/* Allow worker threads to join back to main thread once they are done */
	pthread_attr_init(&attr);
	pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);

	/* Read input arguments; initialize complex part to zero if input is real. */
	N = mxGetN(prhs[0]);
    ncol = mxGetM(prhs[0]);
	ncolBlas = (ptrdiff_t)ncol;
	Rr  = mxGetPr(prhs[0]);
    if(mxIsComplex(prhs[0])){
        Ri  = mxGetPi(prhs[0]);
    }
    else{
        Ri = mxCalloc(N*ncol,sizeof(double));
		Ri_alloc = 1;
    }
	idxd = mxGetPr(prhs[1]);
    nx  = mxGetM(prhs[2]);
	xr  = mxGetPr(prhs[2]);
    if(mxIsComplex(prhs[2])){
        xi  = mxGetPi(prhs[2]);
    }
    else{
        xi = mxCalloc(nx,sizeof(double));
		xi_alloc = 1;
    }
	br  = mxGetPr(prhs[3]);
    if(mxIsComplex(prhs[3])){
        bi  = mxGetPi(prhs[3]);
    }
    else{
        bi = mxCalloc(nx,sizeof(double));
		bi_alloc = 1;
    }
	if (mxGetM(prhs[3]) != nx){
	    mexPrintf('%d %d \n',mxGetM(prhs[3]),nx);
		mexErrMsgIdAndTxt("SLIM_release_apps:tools:algorithms:ThreeDFreqModeling:sweepR_mex:NumElements",
				"The number of elements in the iterate and right hand side vectors must be equal.");
	}
	w   = mxGetScalar(prhs[4]);
	dir = lrint(mxGetScalar(prhs[5]));

	/* The default value for the number of threads can be overridden by an
	 * environment variable. */
	n_threads_str = getenv("OMP_NUM_THREADS");
	if (n_threads_str == NULL){
		n_threads = 1;
	}
	else{
		n_threads = strtol(n_threads_str, NULL, 10);
		if(n_threads < 1){
			n_threads = 1;
		}
	}
	/* The environment variable can in turn be overridden by an optional
	 * argument to the mexFunction. */
	if (nrhs >= 7){
		if(1 <= lrint(mxGetScalar(prhs[6]))){
			n_threads = lrint(mxGetScalar(prhs[6]));
		}
	}

	/* Allocate the final output vector */
	plhs[0]  = mxCreateDoubleMatrix(nx, 1, mxCOMPLEX);
	yr       = mxGetPr(plhs[0]);
    yi       = mxGetPi(plhs[0]);

	/* Check to make sure memory was allocated correctly */
	if (Rr==NULL || Ri==NULL || idxd==NULL || xr==NULL || xi==NULL || br==NULL || bi==NULL ||
	    yr==NULL || yi==NULL){
		mexErrMsgIdAndTxt("SLIM_release_apps:tools:algorithms:ThreeDFreqModeling:sweepR_mex:OutOfMemory",
				"Could not allocate memory for main computational variables.");
	}

	if ((idx = (long *) mxCalloc(ncol, sizeof(long))) == NULL){
		mexErrMsgIdAndTxt("SLIM_release_apps:tools:algorithms:ThreeDFreqModeling:sweepR_mex:OutOfMemory",
				"Could not allocate memory for main computational variables.");
	}
	for (mwSize i=0; i < ncol; i++){
		idx[i] = lrint(idxd[i]);
	}

	/* Compute (half) halo width. Remember that BLAS routines like idamax
	 * return FORTRAN-style indices. */
	maxIdxLoc = idamax_(&ncolBlas, idxd, &idxIncBlas) - 1;
	haloWidth = (mwSize)labs(idx[maxIdxLoc]);

	/* Partition the iterate vector into blocks. Note that the below
	 * partitioning scheme is slighlty different from that in pCARPCG.m in this
	 * directory. The partitioning scheme of pCARPCG corresponds to
	 * distributing a three dimensional array with dimensions given by n
	 * according to Matlab's codistributor1d.defaultPartition(n(3)), and then
	 * vectorizing it. The partition scheme of the present file instead uses
	 * Matlab's codistributor1d.defaultPartion(prod(n)). In other words,
	 * pCARPCG divides the iterate into blocks along the slow dimension,
	 * whereas sweepR_mex.c does not take dimensionality into account, only the
	 * total number of gridpoints. This is done to avoid needing an extra input
	 * parameter with the the system dimensions. The seg_bounds_hi, _lo and
	 * _mid arrays contain indices into non-haloed vectors, while the
	 * seg_bounds_row array contains indices to the rows of the system matrix.
	 *
 	 * yr_seg[i_thread-1]        overlap      yr_seg[i_thread]
	 * ------------------------|-----|-----|-------------------------------
	 *         .----------------^     |     ^-------------------.
	 *    seg_bounds_lo[i_thread], seg_bounds_mid[i_thread], seg_bounds_hi[i_thread]
	 */
	numGridPointsPerBlock = N / n_threads;
	seg_bounds_hi  = (mwSize *)mxCalloc(n_threads+1,sizeof(mwSize));
	seg_bounds_mid = (mwSize *)mxCalloc(n_threads+1,sizeof(mwSize));
	seg_bounds_lo  = (mwSize *)mxCalloc(n_threads+1,sizeof(mwSize));
	seg_bounds_row = (mwSize *)mxCalloc(n_threads+1,sizeof(mwSize));
	if (N == nx){
		main_diagonal_offset = 0;
	}
	else{
		/* The vector is haloed. We are only able to correctly process matrices
		 * with a non-zero main diagonal and symmetric off-main diagonal offsets. */
		if (ncol % 2 != 1){
			mexErrMsgIdAndTxt("SLIM_release_apps:tools:algorithms:ThreeDFreqModeling:sweepR_mex:EvenNumberOfDiags",
					"Input iterate vector appears to be haloed but there is an even number of non-zero diagonals in the system matrix.");
		}
		main_diagonal_offset = idx[ncol/2];
		for (mwSize i = 1; i <= ncol/2; i++){
			if (idx[ncol/2 + i] - main_diagonal_offset != -(idx[ncol/2 - i] - main_diagonal_offset)){
				mexErrMsgIdAndTxt("SLIM_release_apps:tools:algorithms:ThreeDFreqModeling:sweepR_mex:DiagsNotSymmetric",
						"Input iterate vector appears to be haloed but the pattern of non-zero diagonals in the system matrix is not symmetric.");
			}
		}
	}
	for (i_thread=0; i_thread<n_threads; i_thread++){
		/* First domain block */
		if (i_thread==0){
			seg_bounds_hi[i_thread]  = 0;
			seg_bounds_mid[i_thread] = 0;
			seg_bounds_row[i_thread] = 0;
			seg_bounds_lo[i_thread]  = 0;
		}
		/* Other domain blocks */
		else {
			if (i_thread <= N % n_threads) {
				seg_bounds_mid[i_thread] = (numGridPointsPerBlock+1)*i_thread + main_diagonal_offset;
				seg_bounds_row[i_thread] = (numGridPointsPerBlock+1)*i_thread;
				seg_bounds_hi[i_thread]  = (numGridPointsPerBlock+1)*i_thread + haloWidth + main_diagonal_offset;
				seg_bounds_lo[i_thread]  = (numGridPointsPerBlock+1)*i_thread - haloWidth + main_diagonal_offset;
			}
			else{
				seg_bounds_mid[i_thread] = (N % n_threads) + numGridPointsPerBlock*i_thread + main_diagonal_offset;
				seg_bounds_row[i_thread] = (N % n_threads) + numGridPointsPerBlock*i_thread;
				seg_bounds_hi[i_thread]  = (N % n_threads) + numGridPointsPerBlock*i_thread + haloWidth + main_diagonal_offset;
				seg_bounds_lo[i_thread]  = (N % n_threads) + numGridPointsPerBlock*i_thread - haloWidth + main_diagonal_offset;
			}
			/* Check that halos do not overlap each other */
			if (seg_bounds_lo[i_thread] < seg_bounds_hi[i_thread-1]){
				mexErrMsgIdAndTxt("SLIM_release_apps:tools:algorithms:ThreeDFreqModeling:sweepR_mex:TooManyThreads",
						"Too many threads; non-adjacent domain blocks share nearest-neighbour grid points.");
			}
		}
	}
	seg_bounds_lo[n_threads]  = nx;
	seg_bounds_hi[n_threads]  = nx;
	seg_bounds_mid[n_threads] = nx;
	seg_bounds_row[n_threads] = N;

	/* Allocate pointers to segments, to thread arguments and thread IDs */
	thread_args_sweep = (struct sweep_data_t **)mxCalloc(n_threads, sizeof(struct sweep_data_t *));
	if (n_threads > 1) {
		/* Set up a barrier for synchronization of all threads save the master that
		 * executes mexFunction. Note that strictly speaking, only threads working
		 * on domain blocks that share halos need to synchronize with each other.
		 * */
		if (pthread_barrier_init(&barrier_after_sweep_before_halo_average, NULL, n_threads)){
			mexErrMsgIdAndTxt("SLIM_release_apps:tools:algorithms:ThreeDFreqModeling:sweepR_mex:pthreads",
					"Could not initialize pthread barrier.");
		}

		thread_data = (struct thread_data_t **)mxCalloc(n_threads, sizeof(struct thread_data_t *));
		thread_args_average = (struct average_data_t **)mxCalloc(n_threads, sizeof(struct average_data_t *));
		copy_init_guess_data = (struct copy_init_guess_data_t **)mxCalloc(n_threads, sizeof(struct copy_init_guess_data_t *));
		yr_seg = (double **)mxCalloc(n_threads,sizeof(double *));
		yi_seg = (double **)mxCalloc(n_threads,sizeof(double *));
		threadIDs = (pthread_t *)mxCalloc(n_threads,sizeof(pthread_t));
	}
	for (i_thread=0; i_thread<n_threads; i_thread++){
		/* Allocate the segments for each thread */
		if (n_threads > 1) {
			yr_seg[i_thread] = (double *)mxCalloc((seg_bounds_hi[i_thread+1]-seg_bounds_lo[i_thread]),sizeof(double));
			yi_seg[i_thread] = (double *)mxCalloc((seg_bounds_hi[i_thread+1]-seg_bounds_lo[i_thread]),sizeof(double));
			/* Check that segments were allocated correctly */
			if (yr_seg[i_thread]==NULL || yi_seg[i_thread]==NULL){
				mexErrMsgIdAndTxt("SLIM_release_apps:tools:algorithms:ThreeDFreqModeling:sweepR_mex:pthreadsOutOfMemory",
						"Could not allocate memory for thread computational variables.");
			}
			copy_init_guess_data[i_thread] = (struct copy_init_guess_data_t *)mxCalloc(1,sizeof(struct copy_init_guess_data_t));
			copy_init_guess_data[i_thread]->copy_src_real = &xr[seg_bounds_lo[i_thread]];
			copy_init_guess_data[i_thread]->copy_src_imag = &xi[seg_bounds_lo[i_thread]];
			copy_init_guess_data[i_thread]->copy_dst_real = yr_seg[i_thread];
			copy_init_guess_data[i_thread]->copy_dst_imag = yi_seg[i_thread];
			copy_init_guess_data[i_thread]->n_to_copy = (size_t) (seg_bounds_hi[i_thread+1] - seg_bounds_lo[i_thread]);
		}

		/* Set thread arguments */
		thread_args_sweep[i_thread] = (struct sweep_data_t *)mxCalloc(1,sizeof(struct sweep_data_t));
		thread_args_sweep[i_thread]->start_row 	= seg_bounds_row[i_thread];
		thread_args_sweep[i_thread]->end_row   	= seg_bounds_row[i_thread+1];
		thread_args_sweep[i_thread]->ncol 	 	= ncol;
		thread_args_sweep[i_thread]->ny 	 	= seg_bounds_hi[i_thread+1]-seg_bounds_lo[i_thread];
		thread_args_sweep[i_thread]->nx			= nx;
		thread_args_sweep[i_thread]->haloWidth 	= haloWidth*(i_thread ? 1 : 0);
		thread_args_sweep[i_thread]->main_diagonal_offset = main_diagonal_offset;
		thread_args_sweep[i_thread]->Rr  		= Rr;
		thread_args_sweep[i_thread]->Ri  		= Ri;
		thread_args_sweep[i_thread]->idx 		= idx;
		if (n_threads > 1){
			thread_args_sweep[i_thread]->yr 	= yr_seg[i_thread];
			thread_args_sweep[i_thread]->yi 	= yi_seg[i_thread];
		}
		else{
			thread_args_sweep[i_thread]->yr	 	= yr;
			thread_args_sweep[i_thread]->yi	 	= yi;
		}
		thread_args_sweep[i_thread]->br 		= br;
		thread_args_sweep[i_thread]->bi 		= bi;
		thread_args_sweep[i_thread]->w 		 	= w;
		thread_args_sweep[i_thread]->dir 		= dir;

		if (n_threads > 1){
			/* Set the arguments for the averaging threads. Note that each
			 * thread is responsible for averaging the low end of its address
			 * range. The middle of its address range is copied, while the high
			 * end of its address range is left for the next thread. */
			thread_args_average[i_thread] = (struct average_data_t *)mxCalloc(1,sizeof(struct average_data_t));
			thread_args_average[i_thread]->copy_src_real = &(yr_seg[i_thread][seg_bounds_hi[i_thread] - seg_bounds_lo[i_thread]]);
			thread_args_average[i_thread]->copy_dst_real = &(yr[seg_bounds_hi[i_thread]]);
			thread_args_average[i_thread]->copy_src_imag = &(yi_seg[i_thread][seg_bounds_hi[i_thread] - seg_bounds_lo[i_thread]]);
			thread_args_average[i_thread]->copy_dst_imag = &(yi[seg_bounds_hi[i_thread]]);
			thread_args_average[i_thread]->n_to_copy 	 = (size_t) (seg_bounds_lo[i_thread+1] - seg_bounds_hi[i_thread]);
			thread_args_average[i_thread]->halo_1_real   = &(yr_seg[i_thread-1 >= 0 ? i_thread-1 : 0][seg_bounds_lo[i_thread] - seg_bounds_lo[i_thread-1 >= 0 ? i_thread-1 : 0]]);
			thread_args_average[i_thread]->halo_2_real	 = yr_seg[i_thread];
			thread_args_average[i_thread]->halo_dst_real = &(yr[seg_bounds_lo[i_thread]]);
			thread_args_average[i_thread]->halo_1_imag   = &(yi_seg[i_thread-1 >= 0 ? i_thread-1 : 0][seg_bounds_lo[i_thread] - seg_bounds_lo[i_thread-1 >= 0 ? i_thread-1 : 0]]);
			thread_args_average[i_thread]->halo_2_imag	 = yi_seg[i_thread];
			thread_args_average[i_thread]->halo_dst_imag = &(yi[seg_bounds_lo[i_thread]]);
			thread_args_average[i_thread]->n_in_halo 	 = (size_t) (seg_bounds_hi[i_thread] - seg_bounds_lo[i_thread]);

			thread_data[i_thread] = (struct thread_data_t *)mxCalloc(1,sizeof(struct thread_data_t));
			thread_data[i_thread]->copy_init_guess_data = *copy_init_guess_data[i_thread];
			thread_data[i_thread]->sweep_data = *thread_args_sweep[i_thread];
			thread_data[i_thread]->average_data = *thread_args_average[i_thread];
			thread_data[i_thread]->barrier = &barrier_after_sweep_before_halo_average;
		}
	}

	if (n_threads == 1) {
		/* Set the initial guess directly in the output array too */
		memcpy((void *)yr, (void *)xr, sizeof(double)*nx);
		memcpy((void *)yi, (void *)xi, sizeof(double)*nx);
		do_sweep((void *)thread_args_sweep[0]);
	}
	else{
		/* Sweep and average, in separate worker threads */
		for (i_thread=0; i_thread<n_threads; i_thread++){
			if (pthread_create(&threadIDs[i_thread], &attr, sweep_and_average, (void *)thread_data[i_thread])){
				mexErrMsgIdAndTxt("SLIM_release_apps:tools:algorithms:ThreeDFreqModeling:sweepR_mex:pthreadCreate",
						"pthread_create returned non-zero.");
			}
		}

		/* Wait for threads to finish */
		for (i_thread=0; i_thread<n_threads; i_thread++){
			if (pthread_join(threadIDs[i_thread], NULL)){
				mexErrMsgIdAndTxt("SLIM_release_apps:tools:algorithms:ThreeDFreqModeling:sweepR_mex:pthreadCreate",
						"pthread_join returned non-zero.");
			}
		}
	}

	/* Free memory if it was allocated within the MEX file. */
	if (Ri_alloc){
		mxFree(Ri);
	}
	if (xi_alloc){
		mxFree(xi);
	}
	if (bi_alloc){
		mxFree(bi);
	}
	mxFree(seg_bounds_hi);
	mxFree(seg_bounds_mid);
	mxFree(seg_bounds_row);
	mxFree(seg_bounds_lo);
	if (n_threads > 1) {
		mxFree(threadIDs);
	}
	for (i_thread=0; i_thread<n_threads; i_thread++){
		mxFree(thread_args_sweep[i_thread]);
		if (n_threads > 1){
			mxFree(thread_args_average[i_thread]);
			mxFree(thread_data[i_thread]);
			mxFree(copy_init_guess_data[i_thread]);
			mxFree(yr_seg[i_thread]);
			mxFree(yi_seg[i_thread]);
		}
	}
	if (n_threads > 1){
		pthread_barrier_destroy(&barrier_after_sweep_before_halo_average);
		mxFree(thread_args_average);
		mxFree(thread_data);
		mxFree(copy_init_guess_data);
		mxFree(yr_seg);
		mxFree(yi_seg);
	}
	mxFree(idx);
	mxFree(thread_args_sweep);
	pthread_attr_destroy(&attr);

	/* Don't think I need pthread_exit() here, because pthread_join is called above */
	return;
}

max_stars_repo_name: liaman/SLIM-release-apps-public


text:
#pragma once
#include "global.h"

#ifdef USE_MKL
#include <mkl.h>
inline void* _malloc(size_t size) {return mkl_malloc(size,64);}
inline void _free(void* ptr) {return mkl_free(ptr);}
#else
#include <cblas.h>
inline void* _malloc(size_t size) {return malloc(size);}
inline void _free(void* ptr) {return free(ptr);}
#endif

void init_glorot(size_t dim_x, size_t dim_y, vec_t &weight, unsigned seed);

// symmetric sparse matrix transpose
void symmetric_csr_transpose(int N, int nnz, int* A_idx_ptr, int* A_nnz_idx,
                             float* A_nonzeros, float* &B_nonzeros);

// get accuracy for single-class problems
float masked_accuracy_single(int begin, int end, int count, int num_classes,
                             mask_t* masks, float* preds, label_t* ground_truth);

// get accuracy for multi-class problems
float masked_accuracy_multi(int begin, int end, int count, int num_classes,
                            mask_t* masks, float* preds, label_t* ground_truth);

void matmul_naive(const size_t x, const size_t y, const size_t z,
                  const float* A, const float* B, float* C);
void matmul(const size_t x, const size_t y, const size_t z,
            const float_t* A, const float_t* B, float* C,
            bool transA = false, bool transB = false, bool accum = false);

void l2norm(int n, int dim, const float* in, float* out);
void d_l2norm(int n, int dim, const float* feat_in, const float* grad_in, float* grad_out);
void bias_mv(int n, int len, float* x, float* b);
void reduce_sum(int n, int len, float* x, float* a);
void reduce_sum(int n, int len, float* x, vec_t &a);

// single-precision dense matrix multiply
void sgemm_cpu(const CBLAS_TRANSPOSE TransA, const CBLAS_TRANSPOSE TransB,
               const int M, const int N, const int K, const float alpha,
               const float* A, const float* B, const float beta, float* C);

void csr2csc(int nrows, int ncols, int nnz,
             const float* values, const int* rowptr, const int* colidx,
             float* valuesT, int* rowptrT, int* colidxT);

// single-precision sparse matrix dense matrix multiply, C = A * B, A is sparse
void csrmm_cpu(const int M, const int N, const int K, const int nnz,
               const float alpha, float* A_nonzeros, int* A_idx_ptr,
               int* A_nonzero_idx, const float* B, const float beta, float* C, bool transA = false);

void spmm(size_t x, size_t y, size_t z, size_t nnz,
          float* A_nonzeros, int* A_idx_ptr, int* A_nnz_idx,
          const float* B, float* C, float* temp = NULL,
          bool transA = false, bool transB = false, bool accum = false);

// matrix-vector multiply
void mvmul(const CBLAS_TRANSPOSE TransA, const int M, const int N,
           const float alpha, const float* A, const float* x, const float beta, float* y);

//! add 2 arrays for n elements
void vadd_cpu(int n, const float* a, const float* b, float* out);
void scaled_vadd_cpu(int n, const float a, const float* x, const float* y, float* z);

// atomic y[i] += x[i]
void atomic_vreduce_cpu(size_t n, const float* x, float* y);

//! multiply n elements of vector by scalar
void scal(size_t n, const float alpha, float* x);
void scale(int n, const float alpha, const float* x, float* y);
void mul_scalar(size_t n, const float alpha, const float* x, float* y);

//! do dot product of 2 vectors
float dot(int n, const float* x, const float* y);

// concatenation of two vectors into one
void concat(size_t n, const float* x, const float* y, float* z);

// SAXPY stands for Single-precision A*X Plus Y"
void axpy(size_t n, const float a, float* x, float* y);

// Returns the index of the maximum value
int argmax(int num, const float* arr);

//! clear n elements of a vector
void clear_cpu(int n, float* in);

//! copy vector from in -> out; first len elements
void copy_cpu(size_t len, const float* in, float* out);

// dropout functions randomly remove weights
void dropout_cpu(size_t n, size_t m, float scale, float dropout_rate,
                 const float* in, mask_t* mask, float* out);

// dropout derivative: use existing dropouts in masks instead of generating them;
void d_dropout_cpu(size_t n, size_t m, float scale, const float* in,
                   const mask_t* mask, float* out);

void leaky_relu(float epsilon, float in, float &out);
void d_leaky_relu(float epsilon, float in, float data, float &out);
void relu_cpu(size_t n, const float* in, float* out);
void d_relu_cpu(size_t n, const float* in, const float* data, float* out);
void leaky_relu_cpu(size_t n, float epsilon, const float* in, float* out);
void d_leaky_relu_cpu(size_t n, float epsilon, const float* in, const float* data, float* out);
void softmax(size_t n, const float* in, float* out);
void d_softmax(int n, const float* p, const float* dp, float* dy);
void sigmoid(size_t n, const float* in, float* out);
void d_sigmoid(size_t n, const float*, const float* p, float* dy, const float* dp);
float cross_entropy(size_t n, const float* y, const float* p);
void d_cross_entropy(size_t n, const float* y, const float* p, float* d);
float sigmoid_cross_entropy(size_t n, const label_t* y, const float* p);

// use sigmoid instead of softmax for multi-class datasets, e.g. ppi, yelp and amazon
//inline float sigmoid_func(float x) { return 0.5 * tanh(0.5 * x) + 0.5; }
inline float sigmoid_func(float x) { return 1. / (1. + expf(-x)); }

// GPU operators
bool isnan_gpu(int n, const float_t* array); // does array contain any 'nan' element
void init_const_gpu(int n, float_t value, float_t* array);
void copy_gpu(int len, const float_t* in, float_t* out);
void vadd_gpu(const int n, const float_t* a, const float_t* b, float_t* out); // vector add
void axpy_gpu(const int n, const float_t a, const float_t* x, float_t* y);    // axpy
void relu_gpu(const int n, const float_t* in, float_t* out); // ReLU
void d_relu_gpu(const int n, const float_t* in_diff, const float_t* data, float_t* out_diff); // ReLU derivative
void leaky_relu_gpu(const int n, const float_t epsilon, const float_t* in, float_t* out); // Leaky ReLU
void d_leaky_relu_gpu(const int n, const float_t epsilon,
                      const float_t* in_diff, const float_t* data,
                      float_t* out_diff); // Leaky ReLU derivative
void dropout_gpu(int n, float scale, float drop_rate, const float* in, mask_t* masks, float* out); // dropout
void d_dropout_gpu(int n, float scale, const float* in, const mask_t* masks, float* out); // dropout derivative
void sgemm_gpu(const CBLAS_TRANSPOSE TransA, const CBLAS_TRANSPOSE TransB,
               const int M, const int N, const int K, const float alpha,
               const float* A, const float* B, const float beta, float* C);
void csrmm_gpu(const int M, const int N, const int K, const int nnz,
               const float alpha, const float* A_nonzeros, const int* A_idx_ptr,
               const int* A_nonzero_idx, const float* B, const float beta,
               float* trans_C, float* C);
void softmax_cross_entropy_gpu(int len, int begin, int end,
                               const float_t* in_data, const mask_t* masks,
                               const label_t* labels, float_t* loss,
                               float_t* out_data);
void d_softmax_cross_entropy_gpu(int len, int bengin, int end,
                                 const mask_t* masks, const label_t* labels,
                                 const float_t* out_data, float_t* diff);
void sigmoid_cross_entropy_gpu(int len, int begin, int end,
                               const float_t* in_data, const mask_t* masks,
                               const label_t* labels, float_t* loss,
                               float_t* out_data);
void d_sigmoid_cross_entropy_gpu(int len, int bengin, int end,
                                 const mask_t* masks, const label_t* labels,
                                 const float_t* out_data, float_t* diff);
void scal_gpu(const int n, const float alpha, float* X);
void add_scalar_gpu(const int n, const float_t alpha, float_t* Y);
void rng_uniform_gpu(size_t n, const float_t a, const float_t b, float_t* r);
void l2_norm_gpu(size_t x, size_t y, const float_t* in, float_t* out);
void d_l2_norm_gpu(size_t x, size_t y, const float_t* in_data, float_t* in_diff, float_t* out_diff);
acc_t l2_norm_gpu(int n, const float_t* in);
acc_t masked_avg_loss_gpu(int begin, int end, int count, mask_t* masks, float_t* loss);

bool is_allocated_device(float_t* data);
void copy_masks_device(int n, mask_t* h_masks, mask_t*& d_masks);
void float_malloc_device(int n, float_t*& ptr);
void float_free_device(float_t*& ptr);
void copy_float_device(int n, float* h_ptr, float* d_ptr);
void copy_float_host(int n, const float* d_ptr, float* h_ptr);
void uint_malloc_device(int n, uint32_t*& ptr);
void uint_free_device(uint32_t*& ptr);
void copy_uint_device(int n, uint32_t* h_ptr, uint32_t* d_ptr);
void uint8_malloc_device(int n, uint8_t*& ptr);
void uint8_free_device(uint8_t*& ptr);
void copy_uint8_device(int n, uint8_t* h_ptr, uint8_t* d_ptr);
void gpu_rng_uniform(size_t n, float* r);

max_stars_repo_name: chenxuhao/GraphAIBench

max_issues_repo_name: chenxuhao/GraphAIBench

max_forks_repo_name: chenxuhao/GraphAIBench


text:
/******************************************************************************
* Copyright 2018 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the License);
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an AS IS BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#pragma once

#include <cblas.h>

#include "modules/perception/camera/common/camera_frame.h"

namespace apollo {
namespace perception {
namespace camera {

class BaseSimilar {
 public:
  virtual bool Calc(CameraFrame *frame1,
                    CameraFrame *frame2,
                    base::Blob<float> *sim) = 0;
};

class CosineSimilar : public BaseSimilar {
 public:
  CosineSimilar() = default;

  bool Calc(CameraFrame *frame1,
            CameraFrame *frame2,
            base::Blob<float> *sim) override;
};

class GPUSimilar : public BaseSimilar {
 public:
  bool Calc(CameraFrame *frame1,
            CameraFrame *frame2,
            base::Blob<float> *sim) override;
};
}  // namespace camera
}  // namespace perception
}  // namespace apollo

max_stars_repo_name: ghdawn/apollo