/* ode-initval/test_odeiv.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman
 *
 * 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 2 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., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

/* Author:  G. Jungman
 */
#include <config.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <gsl/gsl_test.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_ieee_utils.h>
#include "gsl_odeiv.h"

int rhs_linear (double t, const double y[], double f[], void *params);
int jac_linear (double t, const double y[], double *dfdy, double dfdt[],
                void *params);
int rhs_sin (double t, const double y[], double f[], void *params);
int jac_sin (double t, const double y[], double *dfdy, double dfdt[],
             void *params);
int rhs_exp (double t, const double y[], double f[], void *params);
int jac_exp (double t, const double y[], double *dfdy, double dfdt[],
             void *params);
int rhs_stiff (double t, const double y[], double f[], void *params);
int jac_stiff (double t, const double y[], double *dfdy, double dfdt[],
               void *params);
void test_stepper_linear (const gsl_odeiv_step_type * T, double h,
                         double base_prec);
void test_stepper_sin (const gsl_odeiv_step_type * T, double h, double base_prec);
void test_stepper_exp (const gsl_odeiv_step_type * T, double h, double base_prec);
void test_stepper_stiff (const gsl_odeiv_step_type * T, double h, double base_prec);
void test_evolve_system_flat (gsl_odeiv_step * step,
                              const gsl_odeiv_system * sys,
                              double t0, double t1, double hstart,
                              double y[], double yfin[],
                              double err_target, const char *desc);

void test_evolve_system (const gsl_odeiv_step_type * T,
                         const gsl_odeiv_system * sys,
                         double t0, double t1, double hstart,
                         double y[], double yfin[],
                         double err_target, const char *desc);

void test_evolve_exp (const gsl_odeiv_step_type * T, double h, double err);
void test_evolve_sin (const gsl_odeiv_step_type * T, double h, double err);
void test_evolve_stiff1 (const gsl_odeiv_step_type * T, double h, double err);
void test_evolve_stiff5 (const gsl_odeiv_step_type * T, double h, double err);

/* RHS for a + b t */

int
rhs_linear (double t, const double y[], double f[], void *params)
{
  f[0] = 0.0;
  f[1] = y[0];
  return GSL_SUCCESS;
}

int
jac_linear (double t, const double y[], double *dfdy, double dfdt[],
	    void *params)
{
  gsl_matrix dfdy_mat;
  dfdy_mat.size1 = 2;
  dfdy_mat.size2 = 2;
  dfdy_mat.tda = 2;
  dfdy_mat.data = dfdy;
  dfdy_mat.block = 0;
  gsl_matrix_set (&dfdy_mat, 0, 0, 0.0);
  gsl_matrix_set (&dfdy_mat, 0, 1, 0.0);
  gsl_matrix_set (&dfdy_mat, 1, 0, 1.0);
  gsl_matrix_set (&dfdy_mat, 1, 1, 0.0);
  dfdt[0] = 0.0;
  dfdt[1] = 0.0;
  return GSL_SUCCESS;
}

gsl_odeiv_system rhs_func_lin = {
  rhs_linear,
  jac_linear,
  2,
  0
};

/* RHS for sin(t),cos(t) */

int
rhs_sin (double t, const double y[], double f[], void *params)
{
  f[0] = -y[1];
  f[1] = y[0];
  return GSL_SUCCESS;
}

int
jac_sin (double t, const double y[], double *dfdy, double dfdt[],
	 void *params)
{
  gsl_matrix dfdy_mat;
  dfdy_mat.data = dfdy;
  dfdy_mat.size1 = 2;
  dfdy_mat.size2 = 2;
  dfdy_mat.tda = 2;
  dfdy_mat.block = 0;
  gsl_matrix_set (&dfdy_mat, 0, 0, 0.0);
  gsl_matrix_set (&dfdy_mat, 0, 1, -1.0);
  gsl_matrix_set (&dfdy_mat, 1, 0, 1.0);
  gsl_matrix_set (&dfdy_mat, 1, 1, 0.0);
  dfdt[0] = 0.0;
  dfdt[1] = 0.0;
  return GSL_SUCCESS;
}

gsl_odeiv_system rhs_func_sin = {
  rhs_sin,
  jac_sin,
  2,
  0
};

/* RHS for a exp(t)+ b exp(-t) */

int
rhs_exp (double t, const double y[], double f[], void *params)
{
  f[0] = y[1];
  f[1] = y[0];
  return GSL_SUCCESS;
}

int
jac_exp (double t, const double y[], double *dfdy, double dfdt[],
	 void *params)
{
  gsl_matrix dfdy_mat;
  dfdy_mat.data = dfdy;
  dfdy_mat.size1 = 2;
  dfdy_mat.size2 = 2;
  dfdy_mat.tda = 2;
  dfdy_mat.block = 0;
  gsl_matrix_set (&dfdy_mat, 0, 0, 0.0);
  gsl_matrix_set (&dfdy_mat, 0, 1, 1.0);
  gsl_matrix_set (&dfdy_mat, 1, 0, 1.0);
  gsl_matrix_set (&dfdy_mat, 1, 1, 0.0);
  dfdt[0] = 0.0;
  dfdt[1] = 0.0;
  return GSL_SUCCESS;
}

gsl_odeiv_system rhs_func_exp = {
  rhs_exp,
  jac_exp,
  2,
  0
};

/* RHS for stiff example */

int
rhs_stiff (double t, const double y[], double f[], void *params)
{
  f[0] = 998.0 * y[0] + 1998.0 * y[1];
  f[1] = -999.0 * y[0] - 1999.0 * y[1];
  return GSL_SUCCESS;
}

int
jac_stiff (double t, const double y[], double *dfdy, double dfdt[],
	   void *params)
{
  gsl_matrix dfdy_mat;
  dfdy_mat.data = dfdy;
  dfdy_mat.size1 = 2;
  dfdy_mat.size2 = 2;
  dfdy_mat.tda = 2;
  dfdy_mat.block = 0;
  gsl_matrix_set (&dfdy_mat, 0, 0, 998.0);
  gsl_matrix_set (&dfdy_mat, 0, 1, 1998.0);
  gsl_matrix_set (&dfdy_mat, 1, 0, -999.0);
  gsl_matrix_set (&dfdy_mat, 1, 1, -1999.0);
  dfdt[0] = 0.0;
  dfdt[1] = 0.0;
  return GSL_SUCCESS;
}

gsl_odeiv_system rhs_func_stiff = {
  rhs_stiff,
  jac_stiff,
  2,
  0
};

void
test_stepper_linear (const gsl_odeiv_step_type * T, double h,
		     double base_prec)
{
  int s = 0;
  double y[2];
  double yerr[2];
  double t;
  double del;
  double delmax = 0.0;
  int count = 0;

  gsl_odeiv_step *stepper = gsl_odeiv_step_alloc (T, 2);

  y[0] = 1.0;
  y[1] = 0.0;

  for (t = 0.0; t < 4.0; t += h)
    {
      gsl_odeiv_step_apply (stepper, t, h, y, yerr, 0, 0, &rhs_func_lin);
      del = fabs ((y[1] - (t + h)) / y[1]);
      delmax = GSL_MAX_DBL (del, delmax);
      if (del > (count + 1.0) * base_prec)
	{
	  printf ("  LINEAR(%20.17g)  %20.17g  %20.17g  %8.4g\n", t + h, y[1],
		  t + h, del);
	  s++;
	}
      count++;
    }

  gsl_test (s, "%s, linear [0,4], max relative error = %g",
	    gsl_odeiv_step_name (stepper), delmax);

  gsl_odeiv_step_free (stepper);
}

void
test_stepper_sin (const gsl_odeiv_step_type * T, double h, double base_prec)
{
  int s = 0;
  double y[2];
  double yerr[2];
  double t;
  double del;
  double delmax = 0.0;
  int count = 0;

  gsl_odeiv_step *stepper = gsl_odeiv_step_alloc (T, 2);

  y[0] = 1.0;
  y[1] = 0.0;

  for (t = 0.0; t < M_PI; t += h)
    {
      int stat;
      double sin_th = sin (t + h);
      gsl_odeiv_step_apply (stepper, t, h, y, yerr, 0, 0, &rhs_func_sin);
      del = fabs ((y[1] - sin_th) / sin_th);
      delmax = GSL_MAX_DBL (del, delmax);
      {
	if (t < 0.5 * M_PI)
	  {
	    stat = (del > (count + 1.0) * base_prec);
	  }
	else if (t < 0.7 * M_PI)
	  {
	    stat = (del > 1.0e+04 * base_prec);
	  }
	else if (t < 0.9 * M_PI)
	  {
	    stat = (del > 1.0e+06 * base_prec);
	  }
	else
	  {
	    stat = (del > 1.0e+09 * base_prec);
	  }
	if (stat != 0)
	  {
	    printf ("  SIN(%22.18g)  %22.18g  %22.18g  %10.6g\n", t + h, y[1],
		    sin_th, del);
	  }
	s += stat;
      }
      count++;
    }
  if (delmax > 1.0e+09 * base_prec)
    {
      s++;
      printf ("  SIN(0 .. M_PI)  delmax = %g\n", delmax);
    }

  gsl_test (s, "%s, sine [0,pi], max relative error = %g",
	    gsl_odeiv_step_name (stepper), delmax);

  delmax = 0.0;
  for (; t < 100.5 * M_PI; t += h)
    {
      gsl_odeiv_step_apply (stepper, t, h, y, yerr, 0, 0, &rhs_func_sin);
      del = fabs (y[1] - sin (t));
      delmax = GSL_MAX_DBL (del, delmax);
      count++;
    }

  if (del > count * 2.0 * base_prec)
    {
      s++;
      printf ("  SIN(%22.18g)  %22.18g  %22.18g  %10.6g\n", t + h, y[1],
	      sin (t), del);
    }

  gsl_test (s, "%s, sine [pi,100.5*pi], max absolute error = %g",
	    gsl_odeiv_step_name (stepper), delmax);

  gsl_odeiv_step_free (stepper);
}

void
test_stepper_exp (const gsl_odeiv_step_type * T, double h, double base_prec)
{
  int s = 0;
  double y[2];
  double yerr[2];
  double t;
  double del, delmax = 0.0;
  int count = 0;

  gsl_odeiv_step *stepper = gsl_odeiv_step_alloc (T, 2);

  y[0] = 1.0;
  y[1] = 1.0;

  for (t = 0.0; t < 20.0; t += h)
    {
      double ex = exp (t + h);
      gsl_odeiv_step_apply (stepper, t, h, y, yerr, 0, 0, &rhs_func_exp);
      del = fabs ((y[1] - ex) / y[1]);
      delmax = GSL_MAX_DBL (del, delmax);
      if (del > (count + 1.0) * 2.0 * base_prec)
	{
	  printf ("  EXP(%20.17g)  %20.17g  %20.17g  %8.4g\n", t + h, y[1],
		  ex, del);
	  s++;
	}
      count++;
    }

  gsl_test (s, "%s, exponential [0,20], max relative error = %g",
	    gsl_odeiv_step_name (stepper), delmax);

  gsl_odeiv_step_free (stepper);
}

void
test_stepper_stiff (const gsl_odeiv_step_type * T, double h, double base_prec)
{
  int s = 0;
  double y[2];
  double yerr[2];
  double t;
  double del, delmax = 0.0;
  int count = 0;

  gsl_odeiv_step *stepper = gsl_odeiv_step_alloc (T, 2);

  y[0] = 1.0;
  y[1] = 0.0;

  for (t = 0.0; t < 20.0; t += h)
    {
      gsl_odeiv_step_apply (stepper, t, h, y, yerr, NULL, NULL,
			    &rhs_func_stiff);

      if (t > 0.04)
	{
	  double arg = t + h;
	  double e1 = exp (-arg);
	  double e2 = exp (-1000.0 * arg);
	  double u = 2.0 * e1 - e2;
	  /* double v = -e1 + e2; */
	  del = fabs ((y[0] - u) / y[0]);
	  delmax = GSL_MAX_DBL (del, delmax);

	  if (del > (count + 1.0) * 100.0 * base_prec)
	    {
	      printf ("  STIFF(%20.17g)  %20.17g  %20.17g  %8.4g\n", arg,
		      y[0], u, del);
	      s++;
	    }
	}
      count++;
    }

  gsl_test (s, "%s, stiff [0,20], max relative error = %g",
	    gsl_odeiv_step_name (stepper), delmax);

  gsl_odeiv_step_free (stepper);
}

void
test_evolve_system_flat (gsl_odeiv_step * step,
			 const gsl_odeiv_system * sys,
			 double t0, double t1, double hstart,
			 double y[], double yfin[],
			 double err_target, const char *desc)
{
  int s = 0;
  double frac;

  double t = t0;
  double h = hstart;

  gsl_odeiv_evolve *e = gsl_odeiv_evolve_alloc (sys->dimension);

  while (t < t1)
    {
      gsl_odeiv_evolve_apply (e, NULL, step, sys, &t, t1, &h, y);
    }

  frac = fabs ((y[1] - yfin[1]) / yfin[1]) + fabs ((y[0] - yfin[0]) / yfin[0]);

  if (frac > 2.0 * e->count * err_target)
    {
      printf ("FLAT t = %.5e  y0 = %g y1= %g\n", t, y[0], y[1]);
      s++;
    }

  gsl_test (s, "%s, %s, evolve, no control, max relative error = %g",
	    gsl_odeiv_step_name (step), desc, frac);

  gsl_odeiv_evolve_free (e);
}

void
test_evolve_system (const gsl_odeiv_step_type * T,
		    const gsl_odeiv_system * sys,
		    double t0, double t1, double hstart,
		    double y[], double yfin[],
		    double err_target, const char *desc)
{
  int s = 0;
  double frac;

  double t = t0;
  double h = hstart;

  gsl_odeiv_step * step = gsl_odeiv_step_alloc (T, sys->dimension);

  gsl_odeiv_control *c =
    gsl_odeiv_control_standard_new (0.0, err_target, 1.0, 1.0);
  gsl_odeiv_evolve *e = gsl_odeiv_evolve_alloc (sys->dimension);

  while (t < t1)
    {
      gsl_odeiv_evolve_apply (e, c, step, sys, &t, t1, &h, y);
      /* printf ("SYS  t = %.18e h = %g y0 = %g y1= %g\n", t, h, y[0], y[1]); */
    }

  frac = fabs ((y[1] - yfin[1]) / yfin[1]) + fabs ((y[0] - yfin[0]) / yfin[0]);

  if (frac > 2.0 * e->count * err_target)
    {
      printf ("SYS  t = %.5e h = %g y0 = %g y1= %g\n", t, h, y[0], y[1]);
      s++;
    }

  gsl_test (s, "%s, %s, evolve, standard control, relative error = %g",
	    gsl_odeiv_step_name (step), desc, frac);

  gsl_odeiv_evolve_free (e);
  gsl_odeiv_control_free (c);
  gsl_odeiv_step_free (step);
}

void
test_evolve_exp (const gsl_odeiv_step_type * T, double h, double err)
{
  double y[2];
  double yfin[2];

  y[0] = 1.0;
  y[1] = 1.0;
  yfin[0] = exp (10.0);
  yfin[1] = yfin[0];
  test_evolve_system (T, &rhs_func_exp, 0.0, 10.0, h, y, yfin, err, "exp [0,10]");
}

void
test_evolve_sin (const gsl_odeiv_step_type * T, double h, double err)
{
  double y[2];
  double yfin[2];

  y[0] = 1.0;
  y[1] = 0.0;
  yfin[0] = cos (2.0);
  yfin[1] = sin (2.0);
  test_evolve_system (T, &rhs_func_sin, 0.0, 2.0, h, y, yfin, err, "sine [0,2]");
}

void
test_evolve_stiff1 (const gsl_odeiv_step_type * T, double h, double err)
{
  double y[2];
  double yfin[2];

  y[0] = 1.0;
  y[1] = 0.0;
  {
    double arg = 1.0;
    double e1 = exp (-arg);
    double e2 = exp (-1000.0 * arg);
    yfin[0] = 2.0 * e1 - e2;
    yfin[1] = -e1 + e2;
  }
  test_evolve_system (T, &rhs_func_stiff, 0.0, 1.0, h, y, yfin, err,
                      "stiff [0,1]");
}

void
test_evolve_stiff5 (const gsl_odeiv_step_type * T, double h, double err)
{
  double y[2];
  double yfin[2];

  y[0] = 1.0;
  y[1] = 0.0;
  {
    double arg = 5.0;
    double e1 = exp (-arg);
    double e2 = exp (-1000.0 * arg);
    yfin[0] = 2.0 * e1 - e2;
    yfin[1] = -e1 + e2;
  }
  test_evolve_system (T, &rhs_func_stiff, 0.0, 5.0, h, y, yfin, err,
                      "stiff [0,5]");
}

int
main (void)
{
  int i;

  struct ptype
  {
    const gsl_odeiv_step_type *type;
    double h;
  }
  p[20];

  p[0].type = gsl_odeiv_step_rk2;
  p[0].h = 1.0e-03;
  p[1].type = gsl_odeiv_step_rk2imp;
  p[1].h = 1.0e-03;
  p[2].type = gsl_odeiv_step_rk4;
  p[2].h = 1.0e-03;
  p[3].type = gsl_odeiv_step_rk4imp;
  p[3].h = 1.0e-03;
  p[4].type = gsl_odeiv_step_rkf45;
  p[4].h = 1.0e-03;
  p[5].type = gsl_odeiv_step_rk8pd;
  p[5].h = 1.0e-03;
  p[6].type = gsl_odeiv_step_rkck;
  p[6].h = 1.0e-03;
  p[7].type = gsl_odeiv_step_bsimp;
  p[7].h = 0.1;
  p[8].type = gsl_odeiv_step_gear1;
  p[8].h = 1.0e-03;
  p[9].type = gsl_odeiv_step_gear2;
  p[9].h = 1.0e-03;
  p[10].type = 0;

  gsl_ieee_env_setup ();

  for (i = 0; p[i].type != 0; i++)
    {
      test_stepper_linear (p[i].type, p[i].h, GSL_SQRT_DBL_EPSILON);
      test_stepper_sin (p[i].type, p[i].h / 10.0, GSL_SQRT_DBL_EPSILON);
      test_stepper_exp (p[i].type, p[i].h / 10.0, GSL_SQRT_DBL_EPSILON);
      test_stepper_stiff (p[i].type, p[i].h / 10.0, GSL_SQRT_DBL_EPSILON);
    }

  for (i = 0; p[i].type != 0; i++)
    {
      test_evolve_exp (p[i].type, p[i].h, GSL_SQRT_DBL_EPSILON);
      test_evolve_sin (p[i].type, p[i].h, GSL_SQRT_DBL_EPSILON);
      test_evolve_stiff1 (p[i].type, p[i].h, 1e-5);
      test_evolve_stiff5 (p[i].type, p[i].h, 1e-5);
    }

  exit (gsl_test_summary ());
}


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <float.h>
#include <string.h>
#include <stdbool.h>
#include <png.h>
#include "beachball.h"
#include "parmt_config.h"
#ifdef PARMT_USE_INTEL
#include <mkl_lapacke.h>
#include <mkl_cblas.h>
#else
#include <lapacke.h>
#include <cblas.h>
#endif
//#include "pixmap.h"
//#include "pixmap_png.h"
//#include "pixmap_jpg.h"

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
#ifndef M_PI_2
#define M_PI_2 1.57079632679489661923
#endif
#ifndef MAX
#define MAX(x,y) (((x) > (y)) ? (x) : (y))
#endif
#ifndef MIN
#define MIN(x,y) (((x) < (y)) ? (x) : (y))
#endif
#define PIXMAP_COLORS 3

static void vecstrd(const double str, const double dip, double r[3]);
static void strdv(const double r[3], double *str, double *dip);
static void crossp(const double *__restrict__ u,
                   const double *__restrict__ v,
                   double *__restrict__ n);
static void strdvec(double r[3], double *str, double *dip);
static void rotmat(const int naxis, const double theta,
                   double *__restrict__ r,
                   double *__restrict__ rt);
static void isItNear(const double *__restrict__ pt,
                     double *howClose,
                     double *__restrict__ ptold);
static double dot3(const double *__restrict__ x,
                   const double *__restrict__ y);
static void gemv3(const double *__restrict__ A, const double *__restrict__ x,
                  double *__restrict__ y);
static double transform(const double a, const double b,
                        const double c, const double d,
                        const double x);

/*
int beachball_setMomentTensor(const double m11, const double m22,
                              const double m33, const double m12,
                              const double m13, const double m23,
                              const enum cmopad_basis_enum basis,
                              struct beachballPlot_struct *beachball)
{
    struct cmopad_struct cmt;
    double mt[3][3], m9[9], eigs[3];
    int ierr, info;
    bool lexplosion, limplosion;
    memset(&cmt, 0, sizeof(struct cmopad_struct));
    // the edge case is for purely isotropic sources
    lexplosion = false;
    limplosion = false;
    m9[0] = m11;
    m9[1] = m12;
    m9[2] = m13;
    m9[3] = m12;
    m9[4] = m22;
    m9[5] = m23;
    m9[6] = m13;
    m9[7] = m23;
    m9[8] = m33;
    info = LAPACKE_dsyev(LAPACK_COL_MAJOR, 'N', 'L', 3, m9, 3, eigs);
    if (info != 0)
    {
        printf("Failed classifying eigenvalues\n");
    }
    if ((eigs[0] > 0.0 && eigs[1] > 0.0 && eigs[2] > 0.0))
    {
        //printf("Source is pure explosion\n");
        lexplosion = true;
    }
    if ((eigs[0] < 0.0 && eigs[1] < 0.0 && eigs[2] < 0.0))
    {
        printf("Source is pure implosion\n");
        limplosion = true;
    }
    // copy the input moment tensor
    mt[0][0] = m11;
    mt[1][1] = m22;
    mt[2][2] = m33;
    mt[0][1] = mt[1][0] = m12;
    mt[0][2] = mt[2][0] = m13;
    mt[1][2] = mt[2][1] = m23;
    ierr = cmopad_basis_transformMatrixM33(mt, basis, NED);
    if (ierr != 0)
    {
        printf("Error transforming matrix\n");
        return -1;
    }
    // decompose the moment tensor
    ierr = cmopad_standardDecomposition(mt, &cmt);
    if (ierr != 0)
    {
        printf("Failed initializing moment tensor from strike, dip, rake\n");
        return -1;
    }
    // get the principal tension, null, and plunge axes
    ierr = cmopad_MT2PrincipalAxisSystem(0, &cmt);
    if (ierr != 0)
    {
        printf("Error computing principal axes\n");
        return -1;
    }
    if (lexplosion || limplosion)
    {
        cmt.p_axis[2] = cmt.eigenvalues[0];
        cmt.null_axis[2] = cmt.eigenvalues[1];
        cmt.t_axis[2] = cmt.eigenvalues[2];
    }
    else
    {
        // Convert pressure, null, and tension principal axes to
        // azimuth, plunge, and length
        ierr += cmopad_Eigenvector2PrincipalAxis(NED, cmt.eig_pnt[0],
                                                 cmt.p_axis,    cmt.p_axis);
        ierr += cmopad_Eigenvector2PrincipalAxis(NED, cmt.eig_pnt[1],
                                                 cmt.null_axis, cmt.null_axis);
        ierr += cmopad_Eigenvector2PrincipalAxis(NED, cmt.eig_pnt[2],
                                                 cmt.t_axis,    cmt.t_axis);
        if (ierr != 0)
        {
            printf("Error converting eigenvector to principal axis!\n");
            return -1;
        }
    }
    memcpy(&beachball->cmt, &cmt, sizeof(struct cmopad_struct));
    beachball->lhaveMT = true;
    return 0;
}
*/

int beachball_writePNG(const char *fileName,
                       size_t height, size_t width, unsigned char *bytes)
{
    FILE *file = NULL;
    png_structp png = NULL;
    png_infop info = NULL;
    png_bytepp rows = NULL;
    int h;

    file = fopen(fileName, "wb");
    if (file == NULL){printf("failed to open file\n");}
    png = png_create_write_struct(PNG_LIBPNG_VER_STRING, NULL, NULL, NULL);
    if (png == NULL){printf("error making png struct\n");}
    info = png_create_info_struct(png);
    if (info == NULL){printf("error making png info struct\n");}
    png_init_io(png, file);
    png_set_IHDR(png, info, width, height, 8,
                 PNG_COLOR_TYPE_RGB, PNG_INTERLACE_NONE,
                 PNG_COMPRESSION_TYPE_BASE, PNG_FILTER_TYPE_BASE);
    png_write_info(png, info);
    rows = calloc(height, sizeof(png_bytep));
    for (h=0; h<(int) height; h++)
    {
        rows[h] = bytes + (PIXMAP_COLORS*h*(int) width);
    }
    png_write_rows(png, rows, height);
    png_write_end(png, NULL); //info);
    png_destroy_write_struct(&png, &info);
    if (rows != NULL){free(rows);}
    fclose(file);
    return 0;
}

int beachball_draw(struct beachballPlot_struct beachball)
{
    const char *fcnm = "beachball_draw\0";
    struct beachBall_struct beachImage;
    unsigned char *image;
    double baz, bdp, bev, paz, pdp, pev, taz, tdp, tev, x, xnorm, y;
    int i, i1, i2, indx, ix, iy, jy, k;
    size_t npixel;
    taz = beachball.tAxis[0];
    tdp = beachball.tAxis[1];
    tev = beachball.tAxis[2];
    baz = beachball.nAxis[0];
    bdp = beachball.nAxis[1];
    bev = beachball.nAxis[2];
    paz = beachball.pAxis[0];
    pdp = beachball.pAxis[1];
    pev = beachball.pAxis[2];
    // normalize the moment tensor for my sanity
    xnorm = MAX(fabs(tev), MAX(fabs(bev), fabs(pev)));
    if (xnorm == 0.0)
    {
        printf("%s: Error normalization is zero %f %f %f\n", fcnm, tev, bev, pev);
        return -1;
    }
    // compute everything describing the beachball
    memset(&beachImage, 0, sizeof(struct beachBall_struct));
    cliffsNodes_beachBallP(beachball.xc, beachball.yc, beachball.rad,
                           taz, tdp, tev,
                           baz, bdp, bev,
                           paz, pdp, pev,
                           beachball.nNodalLine,
                           beachball.nFocalSphere,
                           beachball.nxPixel, beachball.nyPixel,
                           true, true, true,
                           &beachImage);
    // draw the image
    npixel = (size_t) (beachball.nxPixel*beachball.nyPixel);
    image = (unsigned char *)
            calloc(PIXMAP_COLORS*npixel, sizeof(unsigned char));
    // Set a white background
    for (indx=0; indx<3*beachball.nxPixel*beachball.nyPixel; indx++)
    {
        image[indx] = 255;
    }
    // Draw the polarity
    if (beachball.lwantPolarity)
    {
        for (i=0; i<beachImage.npolarity; i++)
        {
            x = transform(beachball.xc - beachball.rad,
                          beachball.xc + beachball.rad,
                          0, (double) (beachball.nxPixel-1),
                          beachImage.xPolarity[i]);
            y = transform(beachball.yc - beachball.rad,
                          beachball.yc + beachball.rad,
                          0, (double) (beachball.nyPixel-1),
                          beachImage.yPolarity[i]);
            ix = (int) (x + 0.5);
            iy = (int) (y + 0.5);
            ix = MAX(0, MIN(ix, beachball.nxPixel - 1));
            iy = MAX(0, MIN(iy, beachball.nyPixel - 1));
            jy = beachball.nyPixel - 1 - iy;
            if (beachImage.polarity[i] ==-1)
            {
                // white
                image[3*(jy*beachball.nxPixel+ix)+0] = 255;
                image[3*(jy*beachball.nxPixel+ix)+1] = 255;
                image[3*(jy*beachball.nxPixel+ix)+2] = 255;
            }
            else
            {
                // red
                image[3*(jy*beachball.nxPixel+ix)+0] = 255;
                image[3*(jy*beachball.nxPixel+ix)+1] = 0;
                image[3*(jy*beachball.nxPixel+ix)+2] = 0;
            }
        }
    }
    // Add the nodal lines
    if (beachball.lwantNodalLines)
    {
        // draw first nodal line
        for (k=0; k<beachImage.nNodalLineSegs1; k++)
        {
            i1 = beachImage.nodalLine1Ptr[k];
            i2 = beachImage.nodalLine1Ptr[k+1];
            for (i=i1; i<i2; i++)
            {
                // start point in vector (line)
                x = transform(beachball.xc - beachball.rad,
                              beachball.xc + beachball.rad,
                              0, (double) (beachball.nxPixel-1),
                              beachImage.xNodalLine1[2*i]);
                y = transform(beachball.yc - beachball.rad,
                              beachball.yc + beachball.rad,
                              0, (double) (beachball.nyPixel-1),
                              beachImage.yNodalLine1[2*i]);
                ix = (int) (x + 0.5);
                iy = (int) (y + 0.5);
                ix = MAX(0, MIN(ix, beachball.nxPixel - 1));
                iy = MAX(0, MIN(iy, beachball.nyPixel - 1));
                jy = beachball.nyPixel - 1 - iy;
                // black
                image[3*(jy*beachball.nxPixel+ix)+0] = 0;
                image[3*(jy*beachball.nxPixel+ix)+1] = 0;
                image[3*(jy*beachball.nxPixel+ix)+2] = 0;
                // end point in vector (line)
                x = transform(beachball.xc - beachball.rad,
                              beachball.xc + beachball.rad,
                              0, (double) (beachball.nxPixel-1),
                              beachImage.xNodalLine1[2*i+1]);
                y = transform(beachball.yc - beachball.rad,
                              beachball.yc + beachball.rad,
                              0, (double) (beachball.nyPixel-1),
                              beachImage.yNodalLine1[2*i+1]);
                ix = (int) (x + 0.5);
                iy = (int) (y + 0.5);
                ix = MAX(0, MIN(ix, beachball.nxPixel - 1));
                iy = MAX(0, MIN(iy, beachball.nyPixel - 1));
                jy = beachball.nyPixel - 1 - iy;
                // black
                image[3*(jy*beachball.nxPixel+ix)+0] = 0;
                image[3*(jy*beachball.nxPixel+ix)+1] = 0;
                image[3*(jy*beachball.nxPixel+ix)+2] = 0;
            }
        }
        // draw second nodal line
        for (k=0; k<beachImage.nNodalLineSegs2; k++)
        {
            i1 = beachImage.nodalLine2Ptr[k];
            i2 = beachImage.nodalLine2Ptr[k+1];
            for (i=i1; i<i2; i++)
            {
                // start point in vector (line)
                x = transform(beachball.xc - beachball.rad,
                              beachball.xc + beachball.rad,
                              0, (double) (beachball.nxPixel-1),
                              beachImage.xNodalLine2[2*i]);
                y = transform(beachball.yc - beachball.rad,
                              beachball.yc + beachball.rad,
                              0, (double) (beachball.nyPixel-1),
                              beachImage.yNodalLine2[2*i]);
                ix = (int) (x + 0.5);
                iy = (int) (y + 0.5);
                ix = MAX(0, MIN(ix, beachball.nxPixel - 1));
                iy = MAX(0, MIN(iy, beachball.nyPixel - 1));
                jy = beachball.nyPixel - 1 - iy;
                // black
                image[3*(jy*beachball.nxPixel+ix)+0] = 0;
                image[3*(jy*beachball.nxPixel+ix)+1] = 0;
                image[3*(jy*beachball.nxPixel+ix)+2] = 0;
                // end point in vector (line)
                x = transform(beachball.xc - beachball.rad,
                              beachball.xc + beachball.rad,
                              0, (double) (beachball.nxPixel-1),
                              beachImage.xNodalLine2[2*i+1]);
                y = transform(beachball.yc - beachball.rad,
                              beachball.yc + beachball.rad,
                              0, (double) (beachball.nyPixel-1),
                              beachImage.yNodalLine2[2*i+1]);
                ix = (int) (x + 0.5);
                iy = (int) (y + 0.5);
                ix = MAX(0, MIN(ix, beachball.nxPixel - 1));
                iy = MAX(0, MIN(iy, beachball.nyPixel - 1));
                jy = beachball.nyPixel - 1 - iy;
                // black
                image[3*(jy*beachball.nxPixel+ix)+0] = 0;
                image[3*(jy*beachball.nxPixel+ix)+1] = 0;
                image[3*(jy*beachball.nxPixel+ix)+2] = 0;
            }
        }
    }
    // Draw an outline
    for (i=0; i<beachImage.nFocalSphere; i++)
    {
        x = transform(beachball.xc - beachball.rad,
                      beachball.xc + beachball.rad,
                      0, (double) (beachball.nxPixel-1),
                      beachImage.xFocalSphere[i]);
        y = transform(beachball.yc - beachball.rad,
                      beachball.yc + beachball.rad,
                      0, (double) (beachball.nyPixel-1),
                      beachImage.yFocalSphere[i]);
        ix = (int) (x + 0.5);
        iy = (int) (y + 0.5);
        ix = MAX(0, MIN(ix, beachball.nxPixel - 1));
        iy = MAX(0, MIN(iy, beachball.nyPixel - 1));
        jy = beachball.nyPixel - 1 - iy;
        // black
        image[3*(jy*beachball.nxPixel+ix)+0] = 0;
        image[3*(jy*beachball.nxPixel+ix)+1] = 0;
        image[3*(jy*beachball.nxPixel+ix)+2] = 0;
    }
    // Write it
    beachball_writePNG("temp2.png",
                       (size_t) beachball.nxPixel, (size_t) beachball.nyPixel,
                       image);
    free(image);
    cliffsNodes_free(&beachImage);
    return 0;
}

int beachball_plotDefaults(struct beachballPlot_struct *beachball)
{
    memset(beachball, 0, sizeof(struct beachballPlot_struct));
    beachball->rad = 1.0;
    beachball->xc = 1.5;
    beachball->yc = 1.5;
    beachball->nxPixel = 251;
    beachball->nyPixel = 251;
    beachball->nNodalLine = 3*beachball->nxPixel;
    beachball->nFocalSphere = 4*beachball->nxPixel;
    beachball->lwantPolarity = true;
    beachball->lwantNodalLines = true;
    beachball->lhaveMT = false;
    return 0;
}

static double transform(const double a, const double b,
                        const double c, const double d,
                        const double x)
{
    double det, c1, c2, xi;
    det = 1.0/(b - a);
    c1 = det*(b*c - a*d);
    c2 = det*(-c + d);
    xi = c1 + x*c2;
    return xi;
}
//============================================================================//
int cliffsNodes_plotStations(const double xc, const double yc, const double rad,
                             const int nstat,
                             const double *__restrict__ az,
                             const double *__restrict__ toa,
                             double *__restrict__ xp,
                             double *__restrict__ yp);

/*!
 * @brief Computes the station locations for the given azimuth
 *        and take-off angle on a focal sphere with radius rad
 *        centered at (xc, yc).  This is based on Cliff Frohlich's
 *        stnPlt but now processes multiple stations and does no plotting.
 *
 * @param[in] xc     x center of focal sphere
 * @param[in] yc     y center of focal sphere
 * @param[in] rad    radius of sphere
 * @param[in] nstat  number of stations
 * @param[in] az     source station azimuths (degrees) [nstat]
 * @param[in] toa    take off angle (degrees) from source to stations [nstat]
 *
 * @param[out] xp    station's x location on focal sphere [nstat]
 * @param[out] yp    station's y location on focal sphere [nstat]
 *
 * @author Ben Baker
 *
 * @copyright BSD
 *
 */
int cliffsNodes_plotStations(const double xc, const double yc, const double rad,
                             const int nstat,
                             const double *__restrict__ az,
                             const double *__restrict__ toa,
                             double *__restrict__ xp,
                             double *__restrict__ yp)
{
    const char *fcnm = "cliffsNodes_plotStations\0";
    double azrad, rp, sign, toaN;
    int i;
    const double sqrt2 = 1.4142135623730951;
    const double degrad = M_PI/180.0;
    //------------------------------------------------------------------------//
    //
    // error handling
    if (nstat < 1 || az == NULL || toa == NULL || xp == NULL || yp == NULL)
    {
        if (nstat < 1){printf("%s: No stations\n", fcnm);}
        if (az == NULL){printf("%s: az is NULL\n", fcnm);}
        if (toa == NULL){printf("%s: toa is NULL\n", fcnm);}
        if (xp == NULL){printf("%s: xp is NULL\n", fcnm);}
        if (yp == NULL){printf("%s: yp is NULL\n", fcnm);}
        return -1;
    }
    // compute point on focal shere for each station
    for (i=0; i<nstat; i++)
    {
        // ray normally goes down
        sign = 1.0;
        toaN = toa[i];
        // ray is going up
        if (toaN > 90.)
        {
            toaN = 180. - toa[i];
            sign =-1.;
        }
        rp = sign*rad*sqrt2*sin(0.5*degrad*toaN);
        azrad = az[i]*degrad;
        xp[i] = xc + rp*sin(azrad);
        yp[i] = yc + rp*cos(azrad);
    }
    return 0;
}
//============================================================================//
/*!
 * @brief Given the azimuth and dip of the tension and pressure eigenvectors
 *        in geographic coordinates along with normalized eigenvalues
 *        this computes the moment tensort in geographic coordinates as
 *        well as the rotation and transpose rotation matrices which rotate
 *        from a system where t=1-axis and p=2-axis.  This is based on
 *        Cliff Frohlich's mtensor.
 *
 * @param[in] taz    tension axis azimuth (degrees)
 * @param[in] tdp    tension axis dip (degrees)
 * @param[in] paz    pressure axis azimuth (degrees)
 * @param[in] pdp    pressure axis dip
 * @param[in] tev1   normalized eigenvalue for tension eigenvector
 * @param[in] bev1   normalized eigenvalue for null eigenvector
 * @param[in] pev1   normalized eigenvalue for pressure eigenvector
 *
 * @param[out] mt    [3 x 3] moment tensor (column major)
 * @param[out] rm    [3 x 3] rotation matrix as described above (column major)
 * @param[out] rmt   [3 x 3] transpose rotation matrix described above
 *                   (column major)
 * @author Ben Baker
 *
 * @copyright BSD
 *
 */
void cliffsNodes_mtensor(const double taz, const double tdp,
                         const double paz, const double pdp,
                         const double tev1, const double bev1,
                         const double pev1,
                         double *__restrict__ mt,
                         double *__restrict__ rm,
                         double *__restrict__ rmt,
                         double *ampMax)
{
    double mpt[9], sm1[9], smt1[9], sm2[9], smt2[9],  sm3[9],
           smt3[9], work[9], p[3], pnew[3], pxt[3], t[3], pang;
    const double degrad = M_PI/180.0;
    // set up moment tensor in t-p system
    memset(mpt, 0, 9*sizeof(double));
    mpt[0] = tev1;
    mpt[4] = pev1;
    mpt[8] = bev1;
    // condition p-axis so that it really is perpendicular to t-axis
    vecstrd(taz, tdp, t);
    vecstrd(paz, pdp, p);
    crossp(p, t, pxt);
    crossp(t, pxt, p);
    // Step 1: rotate 1-axis about 3-axis so that 1-axis lies along t azimuth
    rotmat(3, taz-90., sm1, smt1);
    // Step 2: rotate 1-axis about 2-axis so that 1-axis liesalong t
    rotmat(2, tdp, sm2, smt2);
    cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, 3, 3, 3,
                1.0, sm2, 3, sm1, 3, 0.0, sm3, 3);
    cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, 3, 3, 3,
                1.0, smt1, 3, smt2, 3, 0.0, smt3, 3);
    // in this system, p should lie in 2-3 plane
    cblas_dgemv(CblasColMajor, CblasNoTrans, 3, 3,
                1.0, sm3, 3, p, 1, 0.0, pnew, 1);
    // Step 3: rotate about 1 axis so that 2 axis lies along p
    pang = atan2(pnew[2], pnew[1])/degrad; //(3.14159/180.)
    rotmat(1, -pang, sm1, smt1);
    cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, 3, 3, 3,
                1.0, sm1, 3, sm3, 3, 0.0, rm, 3);
    cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, 3, 3, 3,
                1.0, smt3, 3, smt1, 3, 0.0, rmt, 3);
    // Finally: find moment tensor in space coordinates.
    // This is because Mpt=RM*Mspace*RMT, thus Mspace=RMT*Mpt*RM
    cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, 3, 3, 3,
                1.0, rmt, 3, mpt, 3, 0.0, work, 3);
    cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, 3, 3, 3,
                1.0, work, 3, rm, 3, 0.0, mt, 3);
    // ampMax is the absolute maximum amplitude
    *ampMax = MAX(fabs(pev1), MAX(fabs(tev1),  fabs(bev1)));
    return;
}
//============================================================================//
/*!
 * @brief Computes the lower-hemisphere projection `beachball' diagram for a
 *        focal mechanism.  This is based on Cliff Frohlich's bballP and
 *        has been translated to C and does no plotting.
 *
 * @param[in] xc               plot coordinates (e.g. pixels) of center of
 *                             beachball
 * @param[in] yc               plot coordinates (e.g. pixels) of center of
 *                             beachball
 * @param[in] rad              beachball radius (e.g. pixels)
 * @param[in] taz1             azimuth (degrees) of tension eigenvector
 * @param[in] tdp1             dip (degrees) of tension eigenvector
 * @param[in] tevI             eigenvalue corresponding to tension eigenvector
 *                             (e.g. Newton meters)
 * @param[in] baz1             azimuth (degrees) of null eigenvector
 * @param[in] bdp1             dip (degrees) of null eigenvector
 * @param[in] bevI             eigenvalue corresponding to null eigenvector
 *                             (e.g. Newton meters)
 * @param[in] paz1             azimuth (degrees) of pressure eigenvector
 * @param[in] pdp1             dip (degrees) of pressure eigenvector
 * @param[in] pevI             eigenvalue corresponding to pressure eigenvector
 *                             (e.g. Newton meters)
 * @param[in] nNodalLine       number of points to draw in nodal line.
 * @param[in] nFocalSphere     number of points to draw in focal sphere.
 * @param[in] nxPixel          number of pixels in x when drawing polarity
 * @param[in] nyPixel          number of pixels in y when drawing polairty
 * @param[in] lwantNodalLines  if true then compute the nodal lines.
 * @param[in] lwantPTBAxes     if true then compute the PTB axis points.
 * @param[in] lwantPolarity    if true then compute the polarity.
 *
 * @param[out] beachBall       container for the pressure, tension and null axes,
 *                             the nodal lines, and the polarity
 *
 * @author Ben Baker
 *
 * @copyright BSD
 *
 * @bug this routine fails if rad is not = 1.
 *
 */
void cliffsNodes_beachBallP(const double xc, const double yc, const double rad,
                            const double taz1, const double tdp1,
                            const double tevI,
                            const double baz1, const double bdp1,
                            const double bevI,
                            const double paz1, const double pdp1,
                            const double pevI,
                            const int nNodalLine,
                            const int nFocalSphere,
                            const int nxPixel, const int nyPixel,
                            const bool lwantNodalLines,
                            const bool lwantPTBAxes,
                            const bool lwantPolarity,
                            struct beachBall_struct *beachBall)
{
    double *xw1, *xw2, *yw1, *yw2;
    double rm[9] __attribute__ ((aligned(64)));
    double rmt[9] __attribute__ ((aligned(64)));
    double tm[9] __attribute__ ((aligned(64)));
    double pt[3] __attribute__ ((aligned(64)));
    double ptnew[3] __attribute__ ((aligned(64)));
    double ax1[3] __attribute__ ((aligned(64)));
    double ax2[3] __attribute__ ((aligned(64)));
    double ax3[3] __attribute__ ((aligned(64)));
    double ptLine1[3] __attribute__ ((aligned(64)));
    double ptLine2[3] __attribute__ ((aligned(64)));
    double ptLine1Old[3] __attribute__ ((aligned(64)));
    double ptLine2Old[3] __attribute__ ((aligned(64)));
    double ampMax, az, baz, bdp, bev, cosPsi, cosTheta,
           dip, dp, dpsi, dx, dy, evI, fclvd, fIso, howClose, rad2,
           rb, rp, rpt, rpt2, rt, paz, pdp, pev, plusMin, psi,
           sinsq, sinPsi, sinTheta, str,
           taz, tdp, tev, theta, x, x0, xpt, y, y0, ypt;
    int *pn1, *pn2, ix, ixy, iy, k, nlines, nnl1, nnl2, npts, nseg,
        nx, ny, penDown, psave1, psave2;
    size_t nwork;
    const double sqrt2 = 1.4142135623730951;
    const double sqrt2i = 1.0/sqrt2;
    const double degrad = M_PI/180.0;
    const double degradi = 180.0/M_PI;
    const double third = 1.0/3.0;
    const double tol = 0.95;
    // null out result
    memset(beachBall, 0, sizeof(struct beachBall_struct));
    // Avoid overwriting memory
    taz = taz1;
    tdp = tdp1;
    baz = baz1;
    bdp = bdp1;
    paz = paz1;
    pdp = pdp1;
    // Step 1: set up axes:
    //  - D axis ('dominant' 1-axis - with largest absolute eigenvalue)
    //  - M axis ('minor'    3-axis)
    //  - B axis (           2-axis)
    evI = (tevI + bevI + pevI)*third;
    tev = tevI - evI;
    bev = bevI - evI;
    pev = pevI - evI;
    vecstrd(paz, pdp, ax1);
    vecstrd(taz, tdp, ax3);
    fclvd = fabs(bev/pev);
    fIso = evI/pev;
    if (fabs(tev) > fabs(pev))
    {
        vecstrd(taz, tdp, ax1);
        vecstrd(paz, pdp, ax3);
        fclvd = fabs(bev/tev);
        fIso = evI/tev;
    }
    //if (fabs(evI) > .03)
    //{
    //    write(6,901) evI
    //}
    crossp(ax1, ax3, ax2);
    strdvec(ax2, &baz, &bdp);
    cliffsNodes_mtensor(taz, tdp, paz, pdp, tevI, bevI, pevI,
                        tm, rm, rmt, &ampMax);
    // Step 2: Now: plot upper half nodal line:
    // point on nodal line is represented by vector pt;
    //  pt is defined by two angles
    //  - psi is azimuth with respect to 3-axis (M-axis)
    //    in 2-3 plane (in B-M plane)
    //  - theta is angle with respect to 1 axis.
    // It can be shown (i. e., cliff did it once) that:
    //   sin(theta)**2=(2+2*fIso)/(3+(1-2*f)*cos(2*psi))
    // Additionally, a constraint corresponding to Cliff's Rule 1
    // for existance of P nodal planes is applied
    //psi = 0.;
    //dpsi = 2.;
    //npts  = nNodalLine1; //1 + (int)(360.0/dpsi + 0.5); //nint((360./dpsi))
    //dpsi = dpsi*degrad;
    psave1 =-1;
    psave2 =-1;
    memset(ptLine1Old, 0, 3*sizeof(double));
    memset(ptLine2Old, 0, 3*sizeof(double));
    if (lwantNodalLines && nNodalLine > 1 &&
        !(tevI > 0.0 && bevI > 0.0 && pevI > 0.0) &&
        !(tevI < 0.0 && bevI < 0.0 && pevI < 0.0))
    {
        npts = nNodalLine;
        psi = 0.0;
        dpsi = 360.0/(double) (nNodalLine - 1);
        dpsi = dpsi*degrad;
        xw1 = (double *) calloc((size_t) npts, sizeof(double));
        yw1 = (double *) calloc((size_t) npts, sizeof(double));
        pn1 = (int *) calloc((size_t) npts+1, sizeof(int));
        xw2 = (double *) calloc((size_t) npts, sizeof(double));
        yw2 = (double *) calloc((size_t) npts, sizeof(double));
        pn2 = (int *) calloc((size_t) npts+1, sizeof(int));
        nnl1 = 0;
        nnl2 = 0;
        for (k=0; k<npts; k++)
        {
            psi = k*dpsi;
            theta = 0.0;
            sinsq = (2. + 2.*fIso)/(3. + (1. - 2.*fclvd)*cos(2.*psi));
            if (sinsq > 0 && sinsq <= 1.0)
            {
                theta = asin(sqrt(sinsq));
                cosTheta = cos(theta);
                sinTheta = sin(theta);
                cosPsi = cos(psi);
                sinPsi = sin(psi);
                // upper half nodal line
                ptLine1[0] = cosTheta*ax1[0]
                           + sinTheta*(sinPsi*ax2[0] + cosPsi*ax3[0]);
                ptLine1[1] = cosTheta*ax1[1]
                           + sinTheta*(sinPsi*ax2[1] + cosPsi*ax3[1]);
                ptLine1[2] = cosTheta*ax1[2]
                           + sinTheta*(sinPsi*ax2[2] + cosPsi*ax3[2]);
                // lower half nodal line
                ptLine2[0] =-cosTheta*ax1[0]
                           - sinTheta*(sinPsi*ax2[0] + cosPsi*ax3[0]);
                ptLine2[1] =-cosTheta*ax1[1]
                           - sinTheta*(sinPsi*ax2[1] + cosPsi*ax3[1]);
                ptLine2[2] =-cosTheta*ax1[2]
                           - sinTheta*(sinPsi*ax2[2] + cosPsi*ax3[2]);
                isItNear(ptLine1, &howClose, ptLine1Old);
                // upper half nodal line
                penDown =-1;
                if (ptLine1[2] < 0.0){penDown = 1;}
                if (howClose < tol){penDown =-1;}
                strdv(ptLine1, &str, &dip);
                rpt = rad*sqrt2*sin(0.5*degrad*(90.-dip));
                xpt = xc + rpt*sin(str*degrad);
                ypt = yc + rpt*cos(str*degrad);
                xw1[k] = xpt;
                yw1[k] = ypt;
                if (k == 0){pn1[k] = 3;}
                if (penDown == 1 && psave1 == 1)
                {
                    pn1[k] = 2;
                    //printf("%f %f\n", xpt, ypt);
                }
                if (penDown ==-1){pn1[k] = 3;}
                nnl1 = nnl1 + 1;
                psave1 = penDown;
                // lower half nodal line
                penDown =-1;
                if (ptLine2[2] < 0.0){penDown = 1;}
                if (howClose < tol){penDown =-1;}
                strdv(ptLine2, &str, &dip);
                rpt = rad*sqrt2*sin(0.5*degrad*(90.-dip));
                xpt = xc + rpt*sin(str*degrad);
                ypt = yc + rpt*cos(str*degrad);
                xw2[k] = xpt;
                yw2[k] = ypt;
                if (k == 0){pn2[k] = 3;} // pen down
                if (penDown == 1 && psave2 == 1)
                {
                    pn2[k] = 2;
                    //xptsLine2[k] = xpt;
                    //yptsLine2[k] = ypt;
                    //penLine2[k] = penDwn;
                }
                if (penDown ==-1){pn2[k] = 3;}
                nnl2 = nnl2 + 1;
                psave2 = penDown;
            }
        } // loop on points
        nwork = (size_t) (MAX(2, 2*nnl1));
        beachBall->xNodalLine1 = (double *) calloc(nwork, sizeof(double));
        beachBall->yNodalLine1 = (double *) calloc(nwork, sizeof(double));
        nwork = (size_t) (nnl1 + 1);
        beachBall->nodalLine1Ptr = (int *) calloc(nwork, sizeof(int));
        nwork = (size_t) (MAX(2, 2*nnl2));
        beachBall->xNodalLine2 = (double *) calloc(nwork, sizeof(double));
        beachBall->yNodalLine2 = (double *) calloc(nwork, sizeof(double));
        nwork = (size_t) (nnl2 + 1);
        beachBall->nodalLine2Ptr = (int *) calloc(nwork, sizeof(int));
        // make sure the lines end
        pn1[nnl1] = 3;
        pn2[nnl2] = 3;
        // compute the nodal line 1 line segments
        nlines = 0;
        nseg = 0;
        for (k=0; k<nnl1; k++)
        {
            // line begins
            if (pn1[k] == 3 && pn1[k+1] == 2)
            {
                beachBall->xNodalLine1[2*nlines  ] = xw1[k];
                beachBall->xNodalLine1[2*nlines+1] = xw1[k+1];
                beachBall->yNodalLine1[2*nlines  ] = yw1[k];
                beachBall->yNodalLine1[2*nlines+1] = yw1[k+1];
                nlines = nlines + 1;
            }
            // line continues
            if (pn1[k] == 2 && pn1[k+1] == 2)
            {
                beachBall->xNodalLine1[2*nlines  ] = xw1[k];
                beachBall->xNodalLine1[2*nlines+1] = xw1[k+1];
                beachBall->yNodalLine1[2*nlines  ] = yw1[k];
                beachBall->yNodalLine1[2*nlines+1] = yw1[k+1];
                nlines = nlines + 1;
            }
            // line ends
            if (pn1[k] == 2 && pn1[k+1] == 3)
            {
/*
                beachBall->xNodalLine1[2*nlines  ] = xw1[k];
                beachBall->xNodalLine1[2*nlines+1] = xw1[k+1];
                beachBall->yNodalLine1[2*nlines  ] = yw1[k];
                beachBall->yNodalLine1[2*nlines+1] = yw1[k+1];
                nlines = nlines + 1;
*/
                nseg = nseg + 1;
                beachBall->nodalLine1Ptr[nseg] = nlines;
            }
        }
        beachBall->nNodalLineSegs1 = nseg;
        // compute the nodal line 2 line segments
        nlines = 0;
        nseg = 0;
        for (k=0; k<nnl2; k++)
        {
            // line begins
            if (pn2[k] == 3 && pn2[k+1] == 2)
            {
                beachBall->xNodalLine2[2*nlines  ] = xw2[k];
                beachBall->xNodalLine2[2*nlines+1] = xw2[k+1];
                beachBall->yNodalLine2[2*nlines  ] = yw2[k];
                beachBall->yNodalLine2[2*nlines+1] = yw2[k+1];
                nlines = nlines + 1;
            }
            // line continues
            if (pn2[k] == 2 && pn2[k+1] == 2)
            {
                beachBall->xNodalLine2[2*nlines  ] = xw2[k];
                beachBall->xNodalLine2[2*nlines+1] = xw2[k+1];
                beachBall->yNodalLine2[2*nlines  ] = yw2[k];
                beachBall->yNodalLine2[2*nlines+1] = yw2[k+1];
                nlines = nlines + 1;
            }
            // line ends
            if (pn2[k] == 2 && pn2[k+1] == 3)
            {
                beachBall->xNodalLine2[2*nlines  ] = xw2[k];
                beachBall->xNodalLine2[2*nlines+1] = xw2[k+1];
                beachBall->yNodalLine2[2*nlines  ] = yw2[k];
                beachBall->yNodalLine2[2*nlines+1] = yw2[k+1];
                nlines = nlines + 1;
                nseg = nseg + 1;
                beachBall->nodalLine2Ptr[nseg] = nlines;
            }
        }
        beachBall->nNodalLineSegs2 = nseg;
//printf("%d %d\n", nlines, nseg);
        free(pn1);
        free(pn2);
        free(xw1);
        free(xw2);
        free(yw1);
        free(yw2);
    }
    // Step 4: Plot the, P, T, and B axes
    if (lwantPTBAxes)
    {
        rp = rad*sqrt2*sin(0.5*degrad*(90.-pdp));
        rt = rad*sqrt2*sin(0.5*degrad*(90.-tdp));
        rb = rad*sqrt2*sin(0.5*degrad*(90.-bdp));
        beachBall->xp = xc + rp*sin(paz*degrad);
        beachBall->yp = yc + rp*cos(paz*degrad);
        beachBall->xt = xc + rt*sin(taz*degrad);
        beachBall->yt = yc + rt*cos(taz*degrad);
        beachBall->xb = xc + rb*sin(baz*degrad);
        beachBall->yb = yc + rb*cos(baz*degrad);
    }
    // Step 5: Plot the focal sphere
    if (nFocalSphere > 1)
    {
        beachBall->nFocalSphere = nFocalSphere;
        beachBall->xFocalSphere = (double *)
                                  calloc((size_t) nFocalSphere, sizeof(double));
        beachBall->yFocalSphere = (double *)
                                  calloc((size_t) nFocalSphere, sizeof(double));
        dpsi = 2.0*M_PI/(double) (nFocalSphere - 1);
#ifdef _OPENMP
        #pragma omp simd
#endif
        for (k=0; k<nFocalSphere; k++)
        {
            psi = (double) k*dpsi;
            beachBall->xFocalSphere[k] = xc + rad*sin(psi);
            beachBall->yFocalSphere[k] = yc + rad*cos(psi);
        }
    }
    // Step 6: Plot the + and -
    if (lwantPolarity && nxPixel > 0 && nyPixel > 0)
    {
        // begin in lower left corner
        nx = nxPixel;
        ny = nyPixel;
        dx = 2.0*rad/(double) (nx - 1);
        dy = 2.0*rad/(double) (ny - 1);
        x0 = xc - rad;
        y0 = yc - rad;
        rad2 = rad*rad; // saves a sqrt computation when comparing distances
        xw1 = (double *)calloc((size_t) (nx*ny), sizeof(double));
        yw1 = (double *)calloc((size_t) (nx*ny), sizeof(double));
        pn1 = (int *)calloc((size_t) (nx*ny), sizeof(int));
        nwork = 0;
        // loop on pixel grid
        for (iy=0; iy<ny; iy++)
        {
            for (ix=0; ix<nx; ix++)
            {
                x = x0 + (double) ix*dx;
                y = y0 + (double) iy*dy;
                k = iy*nx + ix;
                pn1[k] = 0;
                xw1[k] = x;
                yw1[k] = y;
                // require (x,y) be in the focal sphere
                rpt2 = pow(x - xc, 2) + pow(y - yc, 2);
                if (rpt2 < rad2)
                {
                    nwork = nwork + 1;
                    rpt = sqrt(rpt2);
                    // compute the azimuth
                    theta = atan2(y - yc, x - xc);  // measure from x axis
                    az = (M_PI_2 - theta)*degradi;  // az is measured from north
                    if (az < 0.0){az = az + 360.0;} // convention is [0,360]
                    // compute the dip
                    dp = (M_PI_2 - 2.0*asin(rpt*sqrt2i))*degradi;
                    vecstrd(az, dp, pt);
                    //cblas_dgemv(CblasColMajor, CblasNoTrans, 3, 3,
                    //            1.0, tm, 3, pt, 1, 0.0, ptnew, 1);
                    gemv3(tm, pt, ptnew);
                    plusMin = dot3(pt, ptnew);
                    pn1[k] =-1;
                    if (plusMin > 100.0*DBL_EPSILON*ampMax){pn1[k] = 1;}
                } // end check on location
            } // loop on x
        } // loop on y
        // Copy the points to be plotted
        if (nwork > 0)
        {
            beachBall->npolarity = (int) nwork;
            beachBall->xPolarity = (double *)
                                   calloc((size_t) nwork, sizeof(double));
            beachBall->yPolarity = (double *)
                                   calloc((size_t) nwork, sizeof(double));
            beachBall->polarity = (int *)
                                  calloc((size_t) nwork, sizeof(double));
            k = 0;
            for (ixy=0; ixy<nx*ny;  ixy++)
            {
                if (pn1[ixy] != 0)
                {
                    beachBall->xPolarity[k] = xw1[ixy];
                    beachBall->yPolarity[k] = yw1[ixy];
                    beachBall->polarity[k] = pn1[ixy];
                    k = k + 1;
                }
            }
        }
        free(pn1);
        free(xw1);
        free(yw1);
    }
    return;
}

void cliffsNodes_free(struct beachBall_struct *beachBall)
{
    if (beachBall->xNodalLine1 != NULL){free(beachBall->xNodalLine1);}
    if (beachBall->yNodalLine1 != NULL){free(beachBall->yNodalLine1);}
    if (beachBall->xNodalLine2 != NULL){free(beachBall->xNodalLine2);}
    if (beachBall->yNodalLine2 != NULL){free(beachBall->yNodalLine2);}
    if (beachBall->xFocalSphere != NULL){free(beachBall->xFocalSphere);}
    if (beachBall->yFocalSphere != NULL){free(beachBall->yFocalSphere);}
    if (beachBall->xPolarity != NULL){free(beachBall->xPolarity);}
    if (beachBall->yPolarity != NULL){free(beachBall->yPolarity);}
    if (beachBall->polarity != NULL){free(beachBall->polarity);}
    if (beachBall->nodalLine1Ptr != NULL){free(beachBall->nodalLine1Ptr);}
    if (beachBall->nodalLine2Ptr != NULL){free(beachBall->nodalLine2Ptr);}
    memset(beachBall, 0, sizeof(struct beachBall_struct));
    return;
}
//============================================================================//
/*!
 * @brief Finds simple rotation matrix R and transpose R^T for rotation
 *        of a vector by angle theta about x, y, or z.  This is based on
 *        Cliff Rohlich's rotmat.
 *
 * @param[in] naxis    rotation axis.  if 1 then x.  if 2 then y.  if 3 then z.
 * @param[in] theta    rotation angle (degrees)
 * @param[out] r       rotation matrix [3 x 3] in column major format.
 * @param[out] rt      tranpsose rotation matrix [3 x 3] in column major format.
 *
 * @author Ben Baker
 *
 * @copyright BSD
 *
 */
static void rotmat(const int naxis, const double theta,
                   double *__restrict__ r,
                   double *__restrict__ rt)
{
    double cosThetar, sinThetar, thetar;
    int i, j;
    const double degrad = M_PI/180.0;
    memset(r, 0, 9*sizeof(double));
    thetar = theta*degrad;
    cosThetar = cos(thetar);
    sinThetar = sin(thetar);
    r[0] = cosThetar;
    r[4] = cosThetar;
    r[8] = cosThetar;
    // naxis = x
    if (naxis == 1)
    {
        r[0] = 1.0;       // (1, 1)
        r[5] = sinThetar; // (3, 2)
        r[7] =-sinThetar; // (2, 3)
    }
    // naxis = y
    else if (naxis == 2)
    {
        r[2] = sinThetar; // (3, 1)
        r[4] = 1.0;       // (2, 2)
        r[6] =-sinThetar; // (1, 3)
    }
    // naxis = z
    else if (naxis == 3)
    {
        r[1] = sinThetar; // (2, 1)
        r[3] =-sinThetar; // (1, 2)
        r[8] = 1.0;       // (3, 3)
    }
    // Compute the transpose
    for (i=0; i<3; i++)
    {
        for (j=0; j<3; j++)
        {
            rt[3*i+j] = r[3*j+i];
        }
    }
    return;
}
//============================================================================//
/*!
 * @brief Find components of downward pointing unit vector pole from the
 *        strike and dip angles.  This is based on Cliff Frolich's vecstrd.
 *
 * @param[in] str    strike angle of pole (degrees)
 * @param[in] dip    dip angle of pole (degrees)
 *
 * @param[out] r     corresponding unit vector pole (3)
 *
 * @author Ben Baker
 *
 * @copyright BSD
 *
 */
static void vecstrd(const double str, const double dip, double r[3])
{
    double cosdip, cosstr, diprad, sindip, sinstr, strrad;
    const double degrad = M_PI/180.0;
    // convert to radians
    strrad = str*degrad;
    diprad = dip*degrad;
    // compute cosines and sines
    sinstr = sin(strrad);
    cosstr = cos(strrad);
    cosdip = cos(diprad);
    sindip = sin(diprad);
    // compute (x,y,z) from strike and dip for unit radius
    r[0] = sinstr*cosdip;
    r[1] = cosstr*cosdip;
    r[2] =-sindip;
    return;
}
//============================================================================//
static void isItNear(const double *__restrict__ pt,
                     double *howClose,
                     double *__restrict__ ptold)
{
    *howClose = dot3(ptold, pt); //cblas_ddot(3, ptold, 1, pt, 1);
    ptold[0] = pt[0];
    ptold[1] = pt[1];
    ptold[2] = pt[2];
    return;
}
/*!
 * @brief Finds the strike and dip of downward pointing unit vector.  This
 *        is based on Cliff Frolich's strdvec.
 *
 * @param[in,out] r      on input contains the downward pointing unit
 *                       vector.
 *                       if r[2] is > 0 then the unit vector is pointing
 *                       updward.  in this case then on output r3 will
 *                       be negated.
 *
 * @param[out] str       strike angle (degrees) \f$ s.t. \phi \in [0,360] \f$.
 * @param[out] dip       dip angle (degrees)
 *
 * @author Ben Baker
 *
 * @copyright BSD
 *
 */
static void strdvec(double r[3], double *str, double *dip)
{
    double rs;
    const double degradi = 180.0/M_PI;
    if (r[2] > 0.0)
    {
        r[0] =-r[0];
        r[1] =-r[1];
        r[2] =-r[2];
    }
    *str = atan2(r[0], r[1])*degradi;
    if (*str < 0.){*str = *str + 360.;}
    rs = sqrt(r[0]*r[0] + r[1]*r[1]);
    *dip=atan2(-r[2], rs)*degradi;
    return;
}
/*!
 * @brief Cross product of 2 length 3 vectors u and v.  Returns normal vector n.
 *
 * @param[in] u    first vector in cross product u x v [3]
 * @param[in] v    second vector in cross product u x v [3]
 *
 * @param[out] n   normal vector n = u x v [3]
 *
 * @author Ben Baker
 *
 * @copyright BSD
 *
 */
static void crossp(const double *__restrict__ u,
                   const double *__restrict__ v,
                   double *__restrict__ n)
{
    n[0] = u[1]*v[2] - u[2]*v[1];
    n[1] = u[2]*v[0] - u[0]*v[2];
    n[2] = u[0]*v[1] - u[1]*v[0];
    return;
}
/*!
 * @brief Finds strike and dip given components of unit vector.  This is
 *        based on Cliff Frohlich's strdv.
 *
 * @param[in] r      unit vector
 * @param[out] str   strike angle (degrees)
 * @param[out] dip   dip angle (degrees) s.t. dip down is positive
 *
 * @author Ben Baker
 *
 * @copyright BSD
 *
 */
static void strdv(const double r[3], double *str, double *dip)
{
    double rs;
    const double degradi = 180.0/M_PI;
    *str = atan2(r[0], r[1])*degradi;
    rs = sqrt(r[0]*r[0] + r[1]*r[1]);
    *dip = atan2(-r[2], rs)*degradi;
    return;
}

/*!
 * @brief Utility function for computing dot product of length 3 vector
 */
static double dot3(const double *__restrict__ x,
                   const double *__restrict__ y)
{
    double dot;
    dot = x[0]*y[0] + x[1]*y[1] + x[2]*y[2];
    return dot;
}

/*!
 * @brief Utility function for computing y = Ax for 3 x 3 matrix.
 */
static void gemv3(const double *__restrict__ A, const double *__restrict__ x,
                  double *__restrict__ y)
{
    y[0] = A[0]*x[0] + A[3]*x[1] + A[6]*x[2];
    y[1] = A[1]*x[0] + A[4]*x[1] + A[7]*x[2];
    y[2] = A[2]*x[0] + A[5]*x[1] + A[8]*x[2];
    return;
}


/*****************************************************************************
*
* Rokko: Integrated Interface for libraries of eigenvalue decomposition
*
* Copyright (C) 2015 by Rokko Developers https://github.com/t-sakashita/rokko
*
* Distributed under the Boost Software License, Version 1.0. (See accompanying
* file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
*
*****************************************************************************/

#ifndef ROKKO_LAPACK_H
#define ROKKO_LAPACK_H

#include <rokko/mangling.h>
#include <lapacke.h>

#ifdef __cplusplus

#include <boost/numeric/ublas/traits.hpp>

#define LAPACKE_GEQRF_DECL(NAME, TYPE) \
lapack_int LAPACKE_geqrf(int matrix_order, lapack_int m, lapack_int n, TYPE * a, lapack_int lda, TYPE * tau);
LAPACKE_GEQRF_DECL(sgeqrf, float);
LAPACKE_GEQRF_DECL(dgeqrf, double);
LAPACKE_GEQRF_DECL(cgeqrf, lapack_complex_float);
LAPACKE_GEQRF_DECL(zgeqrf, lapack_complex_double);
#undef LAPACKE_GEQRF_DECL

#define LAPACKE_GESVD_DECL(NAME, TYPE) \
lapack_int LAPACKE_gesvd(int matrix_order, char jobu, char jobvt, lapack_int m, lapack_int n, TYPE * a, lapack_int lda, boost::numeric::ublas::type_traits<TYPE>::real_type * s, TYPE * u, lapack_int ldu, TYPE * vt, lapack_int ldvt, boost::numeric::ublas::type_traits<TYPE>::real_type * superb);
LAPACKE_GESVD_DECL(sgesvd, float);
LAPACKE_GESVD_DECL(dgesvd, double);
LAPACKE_GESVD_DECL(cgesvd, lapack_complex_float);
LAPACKE_GESVD_DECL(zgesvd, lapack_complex_double);
#undef LAPACKE_GESVD_DECL

#define LAPACKE_HEEV_DECL(NAME, TYPE) \
lapack_int LAPACKE_heev(int matrix_order, char jobz, char uplo, lapack_int n, TYPE * a, lapack_int lda, boost::numeric::ublas::type_traits<TYPE>::real_type * w);
LAPACKE_HEEV_DECL(cheev, lapack_complex_float);
LAPACKE_HEEV_DECL(zheev, lapack_complex_double);
#undef LAPACKE_HEEV_DECL

#define LAPACKE_ORGQR_DECL(NAME, TYPE) \
lapack_int LAPACKE_orgqr(int matrix_order, lapack_int m, lapack_int n, lapack_int k, TYPE * a, lapack_int lda, const TYPE * tau); \
lapack_int LAPACKE_ungqr(int matrix_order, lapack_int m, lapack_int n, lapack_int k, TYPE * a, lapack_int lda, const TYPE * tau);
LAPACKE_ORGQR_DECL(sorgqr, float);
LAPACKE_ORGQR_DECL(dorgqr, double);
#undef LAPACKE_ORGQR_DECL

#define LAPACKE_SYEV_DECL(NAME, TYPE) \
lapack_int LAPACKE_syev(int matrix_order, char jobz, char uplo, lapack_int n, TYPE * a, lapack_int lda, TYPE * w ); \
lapack_int LAPACKE_heev(int matrix_order, char jobz, char uplo, lapack_int n, TYPE * a, lapack_int lda, TYPE * w );
LAPACKE_SYEV_DECL(ssyev, float);
LAPACKE_SYEV_DECL(dsyev, double);
#undef LAPACKE_SYEV_DECL

#define LAPACKE_UNGQR_DECL(NAME, TYPE) \
lapack_int LAPACKE_ungqr(int matrix_order, lapack_int m, lapack_int n, lapack_int k, TYPE * a, lapack_int lda, const TYPE * tau);
LAPACKE_UNGQR_DECL(cungqr, lapack_complex_float);
LAPACKE_UNGQR_DECL(zungqr, lapack_complex_double);
#undef LAPACKE_UNGQR_DECL

#define LAPACKE_SYGV_DECL(NAME, TYPE) \
lapack_int LAPACKE_sygv(int matrix_order, lapack_int itype, char jobz, char uplo, lapack_int n, TYPE * a, lapack_int lda, TYPE * b, lapack_int ldb, TYPE * w ); \
lapack_int LAPACKE_heev(int matrix_order, lapack_int itype, char jobz, char uplo, lapack_int n, TYPE * a, lapack_int lda, TYPE * b, lapack_int ldb, TYPE * w );
LAPACKE_SYGV_DECL(ssygv, float);
LAPACKE_SYGV_DECL(dsygv, double);
#undef LAPACKE_SYGV_DECL

#endif // __cplusplus

#endif // ROKKO_LAPACK_H


/*
** Implementation of LISA algorithm
** for statistical inference of fMRI images
**
** 1st level analysis using GLM (general linear model) with pre-coloring
**
** G.Lohmann, April 2017
*/
#include "viaio/Vlib.h"
#include "viaio/file.h"
#include "viaio/mu.h"
#include "viaio/option.h"
#include "viaio/os.h"
#include <viaio/VImage.h>

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_cdf.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_histogram.h>
#include <gsl/gsl_permutation.h>

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

#ifdef _OPENMP
#include <omp.h>
#endif /*_OPENMP*/

typedef struct TrialStruct {
  int   id;
  float onset;
  float duration;
  float height;
} Trial;

extern void VIsolatedVoxels(VImage src,float threshold);
extern void VHistogram(gsl_histogram *histogram,VString filename);
extern void VCheckImage(VImage src);
extern void FDR(VImage src,VImage dest,gsl_histogram *nullhist,gsl_histogram *realhist,double);
extern double ttest1(double *data1,int n);
extern void ImageStats(VImage src,double *,double *,double *hmin,double *hmax);
extern Trial *ReadDesign(VStringConst designfile,int *numtrials,int *nevents);
extern gsl_matrix *VCreateDesign(int ntimesteps,int nevents,int,VBoolean,gsl_matrix *);
extern void VHemoModel(Trial *trial,int ntrials,int nevents,int ntimesteps,double tr,int,VBoolean,gsl_matrix *,gsl_matrix *);
extern Trial *CopyTrials(Trial *trial,int numtrials);
extern void VGLM(gsl_matrix *Data,gsl_matrix *X,gsl_matrix *XInv,gsl_vector *con,VImage map,VImage zmap);
extern Trial *ConcatenateTrials(Trial **trial,int *numtrials,float *run_duration,int dlists,int sumtrials);

extern double VImageVar(VImage src);
extern void VImageCount(VImage src);
extern void VBilateralFilter(VImage src,VImage,int radius,double var1,double var2,int);
extern void VGetHistRange(VImage src,double *hmin,double *hmax);
extern void VZScale(VImage src,float,float stddev);
extern float VGetMode(VImage src);
extern void GlobalMean(gsl_matrix *Data,gsl_matrix *covariates,int column);
extern gsl_matrix *VReadCovariates(VString cfile,VBoolean normalize);

extern VImage VoxelMap(VAttrList list);
extern gsl_matrix *VReadImageData(VAttrList *list,int nlists);
extern void VGetTimeInfos(VAttrList *list,int nlists,double *mtr,float *run_duration);
extern void VRowNormalize(gsl_matrix *Data);
extern void CheckTrialLabels(Trial *trial,int numtrials);
extern void HistoUpdate(VImage,gsl_histogram *);
extern void PlotDesign(gsl_matrix *X,double tr,VString filename);

void XCheckImage(VImage src,char *filename)
{
  VAttrList out_list = VCreateAttrList();
  VAppendAttr(out_list,"image",NULL,VImageRepn,src);
  FILE *out_file = fopen(filename,"w");
  VWriteFile (out_file, out_list);
}

/* shuffle each run separately to ensure exchangebility, concatenate individual permtables */
int **genperm(gsl_rng *rx,int *numtrials,int sumtrials,int dlists,int numperm)
{
  int i,j,k;

  int **permtable = (int **) VCalloc(numperm,sizeof(int *));

  gsl_permutation **perm = (gsl_permutation **) VCalloc(dlists,sizeof(gsl_permutation *));
  for (k=0; k<dlists; k++) {
    perm[k] = gsl_permutation_alloc((size_t)numtrials[k]);
    gsl_permutation_init (perm[k]);
  }

  for (i = 0; i < numperm; i++) {
    permtable[i] = (int *) VCalloc(sumtrials,sizeof(int));
    int jj=0;
    for (k=0; k<dlists; k++) {
      gsl_ran_shuffle (rx, perm[k]->data,numtrials[k],sizeof(size_t));
      for (j=0; j<numtrials[k]; j++) {
	permtable[i][j+jj] = perm[k]->data[j] + jj;
      }
      jj += numtrials[k];
    }
  }

  for (k=0; k<dlists; k++) {
    gsl_permutation_free(perm[k]);
  }
  return permtable;
}

VDictEntry HemoDict[] = {
  { "gamma_0", 0 },
  { "gamma_1", 1 },
  { "gamma_2", 2 },
  { "gauss", 3 },
  { NULL }
};

int main (int argc, char *argv[])
{
  static VArgVector in_files;
  static VArgVector des_files;
  static VString  cova_filename="";
  static VString  out_filename="";
  static VString  plot_filename="";
  static VString  mask_filename="";
  static VShort   hemomodel = 0;
  static VBoolean firstcol = TRUE;
  static VArgVector contrast;
  static VFloat   alpha = 0.05;
  static VShort   radius = 2;
  static VFloat   rvar = 2.0;
  static VFloat   svar = 2.0;
  static VShort   numiter = 2;
  static VBoolean cleanup = TRUE;
  static VBoolean demean = TRUE;
  static VBoolean verbose = FALSE;
  static VBoolean globalmean = FALSE;
  static VShort   numperm = 5000;
  static VLong    seed = 99402622;
  static VShort   nproc = 0;
  static VOptionDescRec options[] = {
    {"in", VStringRepn, 0, & in_files, VRequiredOpt, NULL,"Input files" },
    {"out", VStringRepn, 1, & out_filename, VRequiredOpt, NULL,"Output file" },
    {"design", VStringRepn, 0, & des_files, VRequiredOpt, NULL,"Design files (1st level)" },
    {"contrast", VFloatRepn, 0, (VPointer) &contrast, VRequiredOpt, NULL, "Contrast vector"},
    {"nuisance", VStringRepn,  1, & cova_filename, VOptionalOpt, NULL,"Nuisance regressors" },
    {"demean",VBooleanRepn,1,(VPointer) &demean,VOptionalOpt,NULL,"Whether to subtract mean in nuisance regressors"},
    {"hemo", VShortRepn, 1, (VPointer) &hemomodel, VOptionalOpt, HemoDict,"Hemodynamic model" },
    {"col1", VBooleanRepn, 1, (VPointer) &firstcol, VOptionalOpt, NULL,"Whether to add a constant first column" },
    {"alpha",VFloatRepn,1,(VPointer) &alpha,VOptionalOpt,NULL,"FDR significance level"},
    {"perm",VShortRepn,1,(VPointer) &numperm,VOptionalOpt,NULL,"Number of permutations"},
    {"seed",VLongRepn,1,(VPointer) &seed,VOptionalOpt,NULL,"Seed for random number generation"},
    {"plotdesign", VStringRepn, 1, & plot_filename, VOptionalOpt, NULL,"Filename for plotting design matrix X" },
    {"radius",VShortRepn,1,(VPointer) &radius,VOptionalOpt,NULL,"Bilateral parameter (radius in voxels)"},
    {"rvar",VFloatRepn,1,(VPointer) &rvar,VOptionalOpt,NULL,"Bilateral parameter (radiometric)"},
    {"svar",VFloatRepn,1,(VPointer) &svar,VOptionalOpt,NULL,"Bilateral parameter (spatial)"},
    {"filteriterations",VShortRepn,1,(VPointer) &numiter,VOptionalOpt,NULL,"Bilateral parameter (number of iterations)"},
    {"cleanup",VBooleanRepn,1,(VPointer) &cleanup,VOptionalOpt,NULL,"Whether to remove isloated voxels"},
    {"mask", VStringRepn, 1, (VPointer) &mask_filename, VRequiredOpt, NULL, "Mask"},
    {"j",VShortRepn,1,(VPointer) &nproc,VOptionalOpt,NULL,"number of processors to use, '0' to use all"},
  };

  FILE *fp=NULL;
  VString in_filename;
  VAttrList out_list=NULL,geolist=NULL;
  int i;
  char *prg_name=GetLipsiaName("vlisa_precoloring");
  fprintf (stderr, "%s\n", prg_name);

  /* parse command line */
  if (! VParseCommand (VNumber (options), options, & argc, argv)) {
    VReportUsage (argv[0], VNumber (options), options, NULL);
    exit (EXIT_FAILURE);
  }
  if (argc > 1) {
    VReportBadArgs (argc, argv);
    exit (EXIT_FAILURE);
  }

  /* omp-stuff */
#ifdef _OPENMP
  int num_procs=omp_get_num_procs();
  if (nproc > 0 && nproc < num_procs) num_procs = nproc;
  fprintf(stderr," using %d cores\n",(int)num_procs);
  omp_set_num_threads(num_procs);
#endif /* _OPENMP */

  /* read functional image data */
  int nlists = in_files.number;
  if (nlists < 1) VError(" no input");

  VAttrList *list = (VAttrList *) VCalloc(nlists,sizeof(VAttrList));
  for (i=0; i<nlists; i++) {
    in_filename = ((VString *) in_files.vector)[i];
    fprintf(stderr," %3d:  %s\n",i,in_filename);
    list[i] = VReadAttrList(in_filename,0L,TRUE,FALSE);
    if (geolist == NULL) {
      geolist = VGetGeoInfo(list[i]);
      double *DGeo = VGetGeoDim(geolist,NULL);
      if (fabs(DGeo[0]-4.0) > 0.01) VError(" Input files must be 4D (not 3D)");
    }
  }

  /* get number og design files */
  int dlists = des_files.number;
  if (dlists != nlists) {
    VError(" number of input functional files (%d) and design files (%d) do not match",nlists,dlists);
  }

  /* apply brain mask or threshold */
  VMultMinval(list,nlists,mask_filename,0.0);

  /* read data and voxel map */
  double tr=0;
  float *run_duration = (float *) VCalloc(nlists,sizeof(float));
  VGetTimeInfos(list,nlists,&tr,run_duration);
  gsl_matrix *Data = VReadImageData(list,nlists);
  VImage map  = VoxelMap(list[0]);
  int nslices = VPixel(map,0,3,0,VShort);
  int nrows   = VPixel(map,0,3,1,VShort);
  int ncols   = VPixel(map,0,3,2,VShort);
  int ntimesteps = Data->size2;

  /* additional regressors, no task labels, not included in permutations */
  gsl_matrix *ctmp1=NULL;
  gsl_matrix *ctmp2=NULL;
  gsl_matrix *covariates=NULL;
  int cdim = 1;
  int nuisance_dim=0;
  if (strlen(cova_filename) > 1) {
    ctmp1 = VReadCovariates(cova_filename,demean);
    if (ctmp1->size1 != Data->size2) VError(" num timesteps in covariate file not consistent with data");
    nuisance_dim = ctmp1->size2;
  }
  if (globalmean) {
    if (ctmp1 != NULL) cdim = ctmp1->size2+1;
    ctmp2 = gsl_matrix_calloc(Data->size2,cdim);
    GlobalMean(Data,ctmp2,(int)(cdim-1));
  }
  if (ctmp1 != NULL && ctmp2 == NULL) covariates = ctmp1;
  if (ctmp2 != NULL) covariates = ctmp2;

  /* design files with task labels */
  Trial **trial = (Trial **) VCalloc(dlists,sizeof(Trial *));
  int *numtrials = (int *) VCalloc(dlists,sizeof(int *));
  int nevents = 0;
  int sumtrials = 0;
  for (i=0; i<dlists; i++) {
    in_filename = ((VString *) des_files.vector)[i];
    fprintf(stderr," %3d:  %s\n",i,in_filename);
    int kk=0,jj=0;
    trial[i] = ReadDesign(in_filename,&kk,&jj);
    numtrials[i] = kk;
    if (jj > nevents) nevents = jj;
    sumtrials += numtrials[i];
  }
  fprintf(stderr," Number of trials: %d, number of event types: %d\n",sumtrials,nevents);
  Trial *alltrials = ConcatenateTrials(trial,numtrials,run_duration,nlists,sumtrials);
  CheckTrialLabels(alltrials,sumtrials);

  /* read contrast vector */
  gsl_vector *cont = gsl_vector_alloc(contrast.number + nuisance_dim);
  gsl_vector_set_zero(cont);
  for (i=0; i < contrast.number; i++) {
    double u = ((VFloat *)contrast.vector)[i];
    gsl_vector_set(cont,i,u);
  }

  /* alloc initial design matrix X */
  gsl_matrix *X = VCreateDesign(ntimesteps,nevents,(int)hemomodel,firstcol,covariates);
  fprintf(stderr," Design file dimensions: %lu x %lu\n",X->size1,X->size2);

  gsl_matrix *XInv = gsl_matrix_calloc(X->size2,X->size1);
  if (X->size2 != cont->size) {
    VError(" dimension of contrast vector (%ld) does not match design matrix (%ld)",cont->size-1,X->size2);
  }

  /* ini random permutations */
  gsl_rng_env_setup();
  const gsl_rng_type *T = gsl_rng_default;
  gsl_rng *rx = gsl_rng_alloc(T);
  gsl_rng_set(rx,(unsigned long int)seed);
  if (verbose) fprintf(stderr," seed: %ld\n",(long)seed);
  int **permtable = genperm(rx,numtrials,sumtrials,dlists,(int)numperm);

  /* estimate null variance to adjust radiometric parameter, use first 30 permutations */
  int nperm=0;
  float stddev = 1.0;
  double meanvar = 0.0;
  if (numperm > 0) {
    int tstperm = 30;
    if (tstperm > numperm) tstperm = numperm;
    VImage zmap = VCreateImage(nslices,nrows,ncols,VFloatRepn);
    double varsum=0,nx=0;
    for (nperm = 0; nperm < tstperm; nperm++) {
      Trial *permtrials = CopyTrials(alltrials,sumtrials);
      int j=0;
      for (j=0; j<sumtrials; j++) {
	int j0 = permtable[nperm][j];
	permtrials[j].id = alltrials[j0].id;
      }
      gsl_matrix *X = VCreateDesign(ntimesteps,nevents,(int)hemomodel,firstcol,covariates);
      gsl_matrix *XInv = gsl_matrix_calloc(X->size2,X->size1);
      VHemoModel(permtrials,sumtrials,nevents,ntimesteps,tr,(int)hemomodel,firstcol,X,covariates);
      VGLM(Data,X,XInv,cont,map,zmap);
      varsum += VImageVar(zmap);
      nx++;

      gsl_matrix_free(X);
      gsl_matrix_free(XInv);
      VFree(permtrials);
    }
    meanvar = varsum/nx;
    stddev = (float)(sqrt(meanvar));  /* update stddev */
    VDestroyImage(zmap);
  }

  /* no permutation */
  VImage zmap1 = VCreateImage(nslices,nrows,ncols,VFloatRepn);
  VCopyImageAttrs (map,zmap1);
  VImage dst1 = VCreateImageLike (zmap1);
  VHemoModel(alltrials,sumtrials,nevents,ntimesteps,tr,(int)hemomodel,firstcol,X,covariates);
  if (strlen(plot_filename) > 0) PlotDesign(X,tr,plot_filename);
  VGLM(Data,X,XInv,cont,map,zmap1);

  if (numperm == 0) {
    double z = VImageVar(zmap1);
    stddev = sqrt(z); /* update stddev */
  }
  float mode=0;
  if (numperm > 0) VZScale(zmap1,mode,stddev);
  VBilateralFilter(zmap1,dst1,(int)radius,(double)rvar,(double)svar,(int)numiter);

  /* ini histograms */
  double hmin=0,hmax=0;
  VGetHistRange(dst1,&hmin,&hmax);
  size_t nbins = 20000;
  gsl_histogram *hist0 = gsl_histogram_alloc (nbins);
  gsl_histogram_set_ranges_uniform (hist0,hmin,hmax);
  gsl_histogram *histz = gsl_histogram_alloc (nbins);
  gsl_histogram_set_ranges_uniform (histz,hmin,hmax);
  HistoUpdate(dst1,histz);

  /* random permutations */
#pragma omp parallel for shared(Data) schedule(dynamic)
  for (nperm = 0; nperm < numperm; nperm++) {
    if (nperm%5 == 0) fprintf(stderr," perm  %4d  of  %d\r",nperm,(int)numperm);

    /* randomly shuffle trial labels */
    Trial *permtrials = CopyTrials(alltrials,sumtrials);
    int j=0;
    for (j=0; j<sumtrials; j++) {
      int j0 = permtable[nperm][j];
      permtrials[j].id = alltrials[j0].id;
    }

    /* hemodynamic model */
    gsl_matrix *X = VCreateDesign(ntimesteps,nevents,(int)hemomodel,firstcol,covariates);
    gsl_matrix *XInv = gsl_matrix_calloc(X->size2,X->size1);
    VHemoModel(permtrials,sumtrials,nevents,ntimesteps,tr,(int)hemomodel,firstcol,X,covariates);

    /* GLM */
    VImage zmap = VCreateImageLike(zmap1);
    VGLM(Data,X,XInv,cont,map,zmap);
    VZScale(zmap,mode,stddev);
    gsl_matrix_free(X);
    gsl_matrix_free(XInv);
    VFree(permtrials);

    /* bilateral filter */
    VImage dst = VCreateImageLike (zmap);
    VBilateralFilter(zmap,dst,(int)radius,(double)rvar,(double)svar,(int)numiter);

#pragma omp critical
    {
      HistoUpdate(dst,hist0);
    }
    VDestroyImage(dst);
    VDestroyImage(zmap);
  }

  /* apply fdr */
  VImage fdrimage = VCopyImage (dst1,NULL,VAllBands);
  if (numperm > 0) {
    FDR(dst1,fdrimage,hist0,histz,(double)alpha);
    if (cleanup && alpha < 1.0) {
      VIsolatedVoxels(fdrimage,(float)(1.0-alpha));
    }
  }

  /*
  ** output
  */
  out_list = VCreateAttrList ();
  VHistory(VNumber(options),options,prg_name,&list[0],&out_list);

  /* update geoinfo, 4D to 3D */
  if (geolist != NULL) {
    double *D = VGetGeoDim(geolist,NULL);
    D[0] = 3;
    D[4] = 1;
    VSetGeoDim(geolist,D);
  }
  VSetGeoInfo(geolist,out_list);
  VAppendAttr (out_list,"image",NULL,VImageRepn,fdrimage);
  fp = VOpenOutputFile (out_filename, TRUE);
  if (! VWriteFile (fp, out_list)) exit (1);
  fclose(fp);

  fprintf (stderr, "\n%s: done.\n", argv[0]);
  exit(0);
}


//Normalizes RMS of each vector in X according to dim.
//See C++ command-line code for more info.

#include <stdio.h>
#include <stdlib.h>
#include <float.h>
#include <math.h>
#include <lapacke.h>

#ifdef __cplusplus
namespace codee {
extern "C" {
#endif

int rms_scale_s (float *X, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t dim, const float sr, const float tau, const float target_dB_SPL, const float max_dB_SPL);
int rms_scale_d (double *X, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t dim, const double sr, const double tau, const double target_dB_SPL, const double max_dB_SPL);

int rms_scale_s (float *X, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t dim, const float sr, const float tau, const float target_dB_SPL, const float max_dB_SPL)
{
    if (dim>3u) { fprintf(stderr,"error in rms_scale_s: dim must be in [0 3]\n"); return 1; }
    if (sr<=0.0f) { fprintf(stderr,"error in rms_scale_s: sr (sample rate) must be positive\n"); return 1; }
    if (tau<=0.0f) { fprintf(stderr,"error in rms_scale_s: tau (time constant) must be positive\n"); return 1; }

    const size_t N = R*C*S*H;
    const size_t L = (dim==0u) ? R : (dim==1u) ? C : (dim==2u) ? S : H;

    if (N<2u || L<2u) {}
    else
    {
        const float rms_prctile = 0.9f;
        const float target_RMS = 20e-6f * powf(10.0f, target_dB_SPL/20.0f);
        const float max_RMS = 20e-6f * powf(10.0f, max_dB_SPL/20.0f);
        const float a1 = expf(-1.0f/(sr*tau));
        const float b0 = 1.0f - a1;
        const size_t tranz = (L>5u) ? 5u : 0u;  //nsamps to zero in case transient
        const size_t rms_p = (size_t)roundf((1.0f-rms_prctile)*(float)L);  //index for rms_prctile
        float rms, scale, scalemx;

        float *Y;
        if (!(Y=(float *)malloc(L*sizeof(float)))) { fprintf(stderr,"error in rms_scale_s: problem with malloc. "); perror("malloc"); return 1; }

        if (L==N)
        {
            *Y = b0 * *X * *X;
            ++X; ++Y;
            for (size_t l=L-1u; l>0u; --l, ++X, ++Y) { *Y = b0**X**X + a1**(Y-1); }
            Y -= L;
            for (size_t l=tranz; l>0u; --l, ++Y) { *Y = 0.0f; }  //in case transient
            Y -= tranz;
            if (LAPACKE_slasrt_work('D',(int)L,Y)) { fprintf(stderr,"error in rms_scale_s: problem with LAPACKE function\n"); }
            rms = sqrtf(*Y);
            scalemx = (rms>FLT_EPSILON) ? max_RMS/rms : 1.0f;
            rms = sqrtf(*(Y+rms_p));
            scale = (rms>FLT_EPSILON) ? target_RMS/rms : 1.0f;
            scale = (scale<scalemx) ? scale : scalemx;
            for (size_t l=L; l>0u; --l) { *--X *= scale; }
        }
        else
        {
            const size_t K = (iscolmajor) ? ((dim==0u) ? 1u : (dim==1u) ? R : (dim==2u) ? R*C : R*C*S) : ((dim==0u) ? C*S*H : (dim==1u) ? S*H : (dim==2u) ? H : 1u);
            const size_t B = (iscolmajor && dim==0u) ? C*S*H : K;
            const size_t V = N/L, G = V/B;

            if (K==1u && (G==1u || B==1u))
            {
                for (size_t v=V; v>0u; --v, X+=L)
                {
                    *Y = b0 * *X * *X;
                    ++X; ++Y;
                    for (size_t l=L-1u; l>0u; --l, ++X, ++Y) { *Y = b0**X**X + a1**(Y-1); }
                    Y -= L;
                    for (size_t l=tranz; l>0u; --l, ++Y) { *Y = 0.0f; }  //in case transient
                    Y -= tranz;
                    if (LAPACKE_slasrt_work('D',(int)L,Y)) { fprintf(stderr,"error in rms_scale_s: problem with LAPACKE function\n"); }
                    rms = sqrtf(*Y);
                    scalemx = (rms>FLT_EPSILON) ? max_RMS/rms : 1.0f;
                    rms = sqrtf(*(Y+rms_p));
                    scale = (rms>FLT_EPSILON) ? target_RMS/rms : 1.0f;
                    scale = (scale<scalemx) ? scale : scalemx;
                    for (size_t l=L; l>0u; --l) { *--X *= scale; }
                }
            }
            else
            {
                for (size_t g=G; g>0u; --g, X+=B*(L-1u))
                {
                    for (size_t b=B; b>0u; --b, ++X)
                    {
                        *Y = b0 * *X * *X;
                        X+=K; ++Y;
                        for (size_t l=L-1u; l>0u; --l, X+=K, ++Y) { *Y = b0**X**X + a1**(Y-1); }
                        Y -= L;
                        for (size_t l=tranz; l>0u; --l, ++Y) { *Y = 0.0f; }  //in case transient
                        Y -= tranz;
                        if (LAPACKE_slasrt_work('D',(int)L,Y)) { fprintf(stderr,"error in rms_scale_s: problem with LAPACKE function\n"); }
                        rms = sqrtf(*Y);
                        scalemx = (rms>FLT_EPSILON) ? max_RMS/rms : 1.0f;
                        rms = sqrtf(*(Y+rms_p));
                        scale = (rms>FLT_EPSILON) ? target_RMS/rms : 1.0f;
                        scale = (scale<scalemx) ? scale : scalemx;
                        for (size_t l=L; l>0u; --l) { X-=K; *X *= scale; }
                    }
                }
            }
        }

        free(Y);
    }

    return 0;
}

int rms_scale_d (double *X, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t dim, const double sr, const double tau, const double target_dB_SPL, const double max_dB_SPL)
{
    if (dim>3u) { fprintf(stderr,"error in rms_scale_d: dim must be in [0 3]\n"); return 1; }
    if (sr<=0.0) { fprintf(stderr,"error in rms_scale_d: sr (sample rate) must be positive\n"); return 1; }
    if (tau<=0.0) { fprintf(stderr,"error in rms_scale_d: tau (time constant) must be positive\n"); return 1; }

    const size_t N = R*C*S*H;
    const size_t L = (dim==0u) ? R : (dim==1u) ? C : (dim==2u) ? S : H;

    if (N<2u || L<2u) {}
    else
    {
        const double rms_prctile = 0.9;
        const double target_RMS = 20e-6 * pow(10.0, target_dB_SPL/20.0);
        const double max_RMS = 20e-6 * pow(10.0, max_dB_SPL/20.0);
        const double a1 = exp(-1.0/(sr*tau));
        const double b0 = 1.0 - a1;
        const size_t tranz = (L>5u) ? 5u : 0u;  //nsamps to zero in case transient
        const size_t rms_p = (size_t)round((1.0-rms_prctile)*(double)L);  //index for rms_prctile
        double rms, scale, scalemx;

        double *Y;
        if (!(Y=(double *)malloc(L*sizeof(double)))) { fprintf(stderr,"error in rms_scale_d: problem with malloc. "); perror("malloc"); return 1; }

        if (L==N)
        {
            *Y = b0 * *X * *X;
            ++X; ++Y;
            for (size_t l=L-1u; l>0u; --l, ++X, ++Y) { *Y = b0**X**X + a1**(Y-1); }
            Y -= L;
            for (size_t l=tranz; l>0u; --l, ++Y) { *Y = 0.0; }  //in case transient
            Y -= tranz;
            if (LAPACKE_dlasrt_work('D',(int)L,Y)) { fprintf(stderr,"error in rms_scale_d: problem with LAPACKE function\n"); }
            rms = sqrt(*Y);
            scalemx = (rms>DBL_EPSILON) ? max_RMS/rms : 1.0;
            rms = sqrt(*(Y+rms_p));
            scale = (rms>DBL_EPSILON) ? target_RMS/rms : 1.0;
            scale = (scale<scalemx) ? scale : scalemx;
            for (size_t l=L; l>0u; --l) { *--X *= scale; }
        }
        else
        {
            const size_t K = (iscolmajor) ? ((dim==0u) ? 1u : (dim==1u) ? R : (dim==2u) ? R*C : R*C*S) : ((dim==0u) ? C*S*H : (dim==1u) ? S*H : (dim==2u) ? H : 1u);
            const size_t B = (iscolmajor && dim==0u) ? C*S*H : K;
            const size_t V = N/L, G = V/B;

            if (K==1u && (G==1u || B==1u))
            {
                for (size_t v=V; v>0u; --v, X+=L)
                {
                    *Y = b0 * *X * *X;
                    ++X; ++Y;
                    for (size_t l=L-1u; l>0u; --l, ++X, ++Y) { *Y = b0**X**X + a1**(Y-1); }
                    Y -= L;
                    for (size_t l=tranz; l>0u; --l, ++Y) { *Y = 0.0; }  //in case transient
                    Y -= tranz;
                    if (LAPACKE_dlasrt_work('D',(int)L,Y)) { fprintf(stderr,"error in rms_scale_d: problem with LAPACKE function\n"); }
                    rms = sqrt(*Y);
                    scalemx = (rms>DBL_EPSILON) ? max_RMS/rms : 1.0;
                    rms = sqrt(*(Y+rms_p));
                    scale = (rms>DBL_EPSILON) ? target_RMS/rms : 1.0;
                    scale = (scale<scalemx) ? scale : scalemx;
                    for (size_t l=L; l>0u; --l) { *--X *= scale; }
                }
            }
            else
            {
                for (size_t g=G; g>0u; --g, X+=B*(L-1u))
                {
                    for (size_t b=B; b>0u; --b, ++X)
                    {
                        *Y = b0 * *X * *X;
                        X+=K; ++Y;
                        for (size_t l=L-1u; l>0u; --l, X+=K, ++Y) { *Y = b0**X**X + a1**(Y-1); }
                        Y -= L;
                        for (size_t l=tranz; l>0u; --l, ++Y) { *Y = 0.0; }  //in case transient
                        Y -= tranz;
                        if (LAPACKE_dlasrt_work('D',(int)L,Y)) { fprintf(stderr,"error in rms_scale_d: problem with LAPACKE function\n"); }
                        rms = sqrt(*Y);
                        scalemx = (rms>DBL_EPSILON) ? max_RMS/rms : 1.0;
                        rms = sqrt(*(Y+rms_p));
                        scale = (rms>DBL_EPSILON) ? target_RMS/rms : 1.0;
                        scale = (scale<scalemx) ? scale : scalemx;
                        for (size_t l=L; l>0u; --l) { X-=K; *X *= scale; }
                    }
                }
            }
        }

        free(Y);
    }

    return 0;
}

#ifdef __cplusplus
}
}
#endif


#ifndef _EM_DEFS_H_
#define _EM_DEFS_H_ 1

#include <petsc.h>

#include <Eigen/Core>
#include <Eigen/Dense>

#include <complex>
#include <tuple>

typedef std::complex<double> Complex;

typedef std::tuple<int, int> TupleII;

const double PI = 3.14159265358979323846;
const double MU = 4 * PI * 1E-7;
const Complex II = Complex(0.0, 1.0);

const double RTOD = 180.0 / PI;
const double DTOR = PI / 180.0;

const double EPS = 1.0E-6;

const int TX_SIZE = 7;

typedef Eigen::Vector3d Point;
typedef Eigen::Vector3d VectorD;
typedef Eigen::Vector3cd VectorZ;

typedef Eigen::Matrix3d Tensor;

struct PETScBlockVector {
  Vec re, im;
};

enum AnisotropicForm { Isotropic = 0, Vertical = 1, Triaxial = 2, Arbitrary = 3 };

enum PolarType { XY_POLAR = -1, YX_POLAR = -2};

enum RefineStrategy { FixedNumber = 0, FixedFraction = 1 };

enum InnerPCType { Mixed = 0, AMS = 1, Direct = 2 };

enum DirectSolverType { MUMPS = 0, SUPERLUDIST = 1 };

enum ObsType {
  F_EX_RI = 111,
  F_EY_RI = 121,
  F_EZ_RI = 131,
  F_HX_RI = 141,
  F_HY_RI = 151,
  F_HZ_RI = 161,
  F_EX_AP = 112,
  F_EY_AP = 122,
  F_EZ_AP = 132,
  F_HX_AP = 142,
  F_HY_AP = 152,
  F_HZ_AP = 162,
  R_XY_AP = 212,
  R_YX_AP = 222,
  Z_XX_RI = 311,
  Z_XY_RI = 321,
  Z_YX_RI = 331,
  Z_YY_RI = 341,
  T_ZX_RI = 351,
  T_ZY_RI = 361,
  Z_XX_AP = 312,
  Z_XY_AP = 322,
  Z_YX_AP = 332,
  Z_YY_AP = 342
};

#define EM_ERR_USER -1

#endif