#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <omp.h>
#include <math.h>
#include <gsl/gsl_integration.h>
#include <sys/time.h>

// ***************************************************************************************
// * Pre-Port Entwicklung und Testen eines OMP-parallisierten, adaptiven Simpson-integrators für
// * die fiesen doppelintgrale in imd_colrad.c
// ***************************************************************************************
int num_threads;
int simpson_error;
int funcevals;
int iter;

const double tolmax=1e-30;
#define INITIAL_STACK_SIZE 128; //128   /* initial size of new stacks */

const double alpha=0.816496580927726;
const double beta=0.447213595499958;
static const double xgkq[12] =
{
  0.0,
  -0.942882415695480,
  -0.816496580927726,
  -0.641853342345781,
  -0.447213595499958,
  -0.236383199662150,
  0.0,
  0.236383199662150,
  0.447213595499958,
  0.641853342345781,
  0.816496580927726,
  0.942882415695480
};

/* the stack structure */
struct stack_s{
  int el_count;            /* count of elements on stack */
  int el_size;             /* size of an element */
  int mem_reserve;         /* allocated memory for stack */
  void* elements;          /* pointer to begin of stack */
};

struct my_f_params { double ne; double T;double mu; double E;double DeltaE; int allowed;};

typedef struct _work_t{
  double a;
  double b;
  double tol;
  double S;
  double fa;
  double fb;
  double fm;
  double rec;
  int iter;
  struct my_f_params * p; //pointer auf params
} work_t;

typedef struct stack_s* stack_t;

typedef struct _work_t_gkq{
  double a;
  double b;
  double toler;
  double I_13;
  double fa;
  double fb;
  struct my_f_params * p; //pointer auf params
  double (*f)(double, struct my_f_params*);
  stack_t stack_inner;
  short int is_parent;
  short int subtask_nr; //inner task nr
  short int task_nr; //outer task nr
  short int subtasks_left; //erst pop'n wenn aller inner tasks (intergals) vollständig!
  double inner_integrals[5];
} work_gkq;

// **************************************** FUNCTION DEFS *************************************************
void create_stack(stack_t* stack, int element_size);
int empty_stack(stack_t stack);
void push_stack(stack_t stack, void* element);
void pop_stack(stack_t stack, void* element);

double
integral2(     double (*f)(double, struct my_f_params*), /* function to integrate */
     stack_t stack);

double gkq_adapt_double(stack_t stack);
double gkq_adapt_single(stack_t stack); //for single integral, first integral

double gkq_double(double (*f)(double, struct my_f_params*), double (*finner)(double, struct my_f_params*),
                  double a, double b,
                  double TOL, struct my_f_params* p,stack_t stack);

double gkq_adapt_serial(double (*f)(double, struct my_f_params*), double a, double b, double fa,double fb, double toler,double I_13, struct my_f_params* p);

work_gkq gkq_create_inner_task(double (*f)(double, struct my_f_params*), double a, double b, double TOL, struct my_f_params* p);
double gkq_single(double (*f)(double, struct my_f_params*), double a, double b,
                       double TOL, struct my_f_params* p,stack_t stack);

int terminate_serial;
int terminate_gkq;

// static double myfun(double x,struct my_f_params* p)
// {
//   double T=p->T;
//   double fermi=1/(1+exp(-x/T));
//   double fermi2=1- 1/(1+exp(-x/T));
//   double sigma=x/T*log(x/T*2)/pow(1/x,2.0); //einfach nur ärger machen

//   // return  exp(-x*x)/p->T+log(x*pow(p->T,2.0))/p->ne;
//   return fermi*fermi2*sigma;
// }

// static double myfun2(double x,void* pv)
// {
//   struct my_f_params *p= (struct my_f_params*) pv;

//   double T=p->T;
//   double fermi=1/(1+exp(-x/T));
//   double fermi2=1- 1/(1+exp(-x/T));
//   double sigma=x/T*log(x/T*2)/pow(1/x,2.0); //einfach nur ärger machen

//   // return  exp(-x*x)/p->T+log(x*pow(p->T,2.0))/p->ne;
//   return fermi*fermi2*sigma;
// }

static double inner_integrand(double x,void *pv)
{
  struct my_f_params *p= (struct my_f_params*) pv;
  double T=p->T;
  double fermi=1/(1+exp(-x/T));
  double fermi2=1- 1/(1+exp(-x/T));
  return exp(-x*x); //fermi*fermi2;
}
static double inner_integrand2(double x,struct my_f_params* p)
{
  double T=p->T;
  double fermi=1/(1+exp(-x/T));
  double fermi2=1- 1/(1+exp(-x/T));
  return fermi*fermi2;
}
void get_integ_bounds_inner(double *integ_bnd, double x, struct my_f_params *p)
{
  integ_bnd[0]=-4;
  integ_bnd[1]=4;
}

gsl_integration_workspace * gsinner;
gsl_integration_workspace * gsinner2;

static double myfun(double x,struct my_f_params* p)
{
  double T=p->T;
  double fermi=1/(1+exp(-x/T));
  double fermi2=1- 1/(1+exp(-x/T));
  double sigma=x/T*log(x/T*2)/pow(1/x,2.0); //einfach nur ärger machen

  // double result, err;
  // gsl_function gslfun;
  // gslfun.function=&inner_integrand;
  // gslfun.params=p;

  // gsl_integration_qag(&gslfun, x, 300, 0.0, 1e-3, 1000,1,
  //                      gsinner2, &result, &err);

  // gsl_integration_workspace_free(gsinner2);
  // stack_t stack2;
  // create_stack(&stack2, sizeof(work_gkq));

  //double result=gkq(inner_integrand2, x, 600, 1e-3, p);//, stack);
  // free(stack2->elements);

  return sigma;
  //return  exp(-x*x);
};

static double myfun_gsl(double x,void* pv)
{
  struct my_f_params *p= (struct my_f_params*) pv;

  double T=p->T;
  double fermi=1/(1+exp(-x/T));
  double fermi2=1- 1/(1+exp(-x/T));
  double sigma=x/T*log(x/T*2)/pow(1/x,2.0); //einfach nur ärger machen

  gsl_function gslfun;
  gslfun.function=&inner_integrand;
  gslfun.params=pv;
  double result, err;
  gsl_integration_qag(&gslfun, x, 600, 0.0, 1e-3, 1000,1,
                       gsinner, &result, &err);

  return sigma*result;
}

// static double myfun2(double x,void* pv)
// {
//   struct my_f_params *p= (struct my_f_params*) pv;
// static float sinfc(float x) { return sinf(x); }
// static double frand48c(double x) { funcevals++; return (double) drand48(); }

// *************************************************************************************************************
int main(int argc,char** argv)
{
  iter=0;
  num_threads=omp_get_num_threads();
  funcevals=0;
  simpson_error=0;

  struct timeval start, end;

  double gsltime, simpsontime;
  // ************************************************
  gsinner=   gsl_integration_workspace_alloc (1000);

  // double gslinteg=0.0;
  // double integ_err;

  // gsl_function gslfun;
  // gslfun.function=&myfun;

  // **********************************************

  double pi=3.141592653589793;
  double xmin, xmax;
  double answer = 0.0;

  int i;

  xmin=2;
  xmax=100;

  // ***********************************************
  double T0=1;
  double ne0=1e5;

  struct my_f_params ptest;
  ptest.T=T0;
  ptest.ne=ne0;

  // *************************************************
  int method=GSL_INTEG_GAUSS61;
  gsl_function gslfun;
  gslfun.function=&myfun_gsl;
  gslfun.params=&ptest;
  gsl_integration_workspace * gswork=NULL;
  gswork=   gsl_integration_workspace_alloc (1000);
  double gslinteg=0.0;
  double integ_err;

  gettimeofday(&start, NULL);
  gsl_integration_qag(&gslfun, xmin, xmax, 0.0, 1e-8, 1000,1,
                       gswork, &gslinteg, &integ_err);
  gettimeofday(&end, NULL);
  gsltime = ((end.tv_sec  - start.tv_sec) * 1000000u +
              end.tv_usec - start.tv_usec) /1.e6;

  printf("gslres:%.12e, gsltime:%.4e\n", gslinteg, gsltime);
  // *************************************************

  stack_t stack;  //global stack
  create_stack(&stack, sizeof(work_gkq));

  gettimeofday(&start, NULL);
  double integ=gkq_double(myfun,inner_integrand2, xmin, xmax, 1e-2, &ptest,stack);

  gettimeofday(&end, NULL);
  free(stack->elements);
  free(stack);

  double partime = ((end.tv_sec  - start.tv_sec) * 1000000u +
                     end.tv_usec - start.tv_usec) /1.e6;

  printf("my:%.12e, partime:%.4e\n", integ,partime);

  return 0;
}

/******************************************
 * create new stack
 ******************************************/
void create_stack(
             stack_t* stack,     /* stack to create */
             int element_size)   /* size of a stack element */
{
  int initial_size = INITIAL_STACK_SIZE;

  /* allocate memory for new stack struct */
  (*stack) = (stack_t) malloc(sizeof(struct stack_s));
  if (!(*stack)){
    fprintf(stderr, "error: could not allocate memory for stack.. Abort.\n");
    exit(1);
  }

  /* allocate memory for stack elements */
  (*stack)->elements = (void*) malloc(element_size * initial_size);
  (*stack)->mem_reserve = initial_size;
  if (!(*stack)->elements){
    fprintf(stderr, "error: could not allocate memory for stack.. Abort.\n");
    exit(1);
  }

  (*stack)->el_size = element_size;
  (*stack)->el_count = 0;

}

/*****************************************
 * check if the stack is empty
 *****************************************/
int empty_stack(stack_t stack)
{
  //MYMOD
  if(stack==NULL)
    return 1;
  //ENDOF MYMOD
  return stack->el_count <= 0;
}

/*****************************************
 * push a element on stack
 *****************************************/
void push_stack(
           stack_t stack,    /* target stack */
           void* element)    /* element to push */
{
  int i, new_reserve;
  int log2_count;

  /* check if we need more memory for stack */
  if (stack->el_count >= stack->mem_reserve)
  {

      /* calculate new size for the stack
         it should be a power of two */
      // for (i = stack->el_count, log2_count = 0; i > 0;  i>>1, log2_count++);
      //   new_reserve = 1 << log2_count;

      log2_count=0;
      for(i=stack->el_count;i>0; i=i*0.5) //i>>1)
      {
        log2_count++;
      }
      new_reserve= 1 << log2_count;

      /* reallocate memory for phase thread tables
         and nullify new values */
      stack->elements = (void *) realloc(stack->elements,
                      stack->el_size * new_reserve);
      if (!stack->elements){
        fprintf(stderr, "error: can't reallocate stack.. Aborting\n");
        exit(1);
      }

      stack->mem_reserve = new_reserve;
  }

  /* now push the element on top of the stack */
  memcpy((char*)stack->elements + stack->el_count*stack->el_size,
            element, stack->el_size);
  stack->el_count++;

}

/*****************************************
 * pop an element from stack
 *****************************************/
void pop_stack(
          stack_t stack,    /* target stack */
          void* element)    /* where poped el. should be stored */
{
  if (stack->el_count <= 0){
    fprintf(stderr, "error: trying to pop from empty stack.\n");
    exit(2);
  }

  stack->el_count--;
  memcpy(element,
         (char*)stack->elements + stack->el_count*stack->el_size,
         stack->el_size);

}

// ***************************************************************************
// *      Gauss-kronard quadrature
// ***************************************************************************
work_gkq gkq_create_inner_task(double (*f)(double, struct my_f_params*), double a, double b, double TOL, struct my_f_params* p)
{
  work_gkq work;

  work.f=f;
  struct my_f_params* pinner;
  pinner=(struct my_f_params*) malloc(sizeof(struct my_f_params)); //ACHTUNG: Muss wieder gefreed werden
  pinner->mu= p->mu;
  pinner->T=p->T;
  pinner->ne=p->ne;
  work.p=pinner;
  work.a=a;
  work.b=b;
  work.subtasks_left=0;//beliebig!

  double m=0.5*(a+b);
  double h=0.5*(b-a);
  double y[13];
  double fa=y[0]=f(a,p);
  double fb=y[12]=f(b,p);
  int i;
  for(i=1;i<12;i++)
    y[i]=f(m+xgkq[i]*h,p);

  double I_4= (h/6.0)*(y[0]+y[12]+5.0*(y[4]+y[8]));  // 4-point gauss-lobatto
  double I_7= (h/1470.0)*(77.0*(y[0]+y[12])+432.0*(y[2]+y[10])+  // 7-point kronrod
               625.0*(y[4]+y[8])+672.0*y[6]);

  double I_13= h*(0.0158271919734802*(y[0]+y[12])+0.0942738402188500*(y[1]+y[11])+0.155071987336585*(y[2]+y[10])+
                  0.188821573960182*(y[3]+y[9])+0.199773405226859*(y[4]+y[8])+0.224926465333340*(y[5]+y[7])+
                  0.242611071901408*y[6]);   //13-point Kronrod

  double Err1=fabs(I_7-I_13);
  double Err2=fabs(I_4-I_13);

  double r=(Err2 != 0.0) ? Err1/Err2 : 1.0;
  double toler=(r > 0.0 && r < 1.0) ? TOL/r : TOL;

  if(I_13 == 0)
    I_13=b-a;
  I_13=fabs(I_13);

// printf("create inner task: a:%f, b:%f, I4:%f,I7:%f,I13:%f\n",a,b,I_4,I_7,I_13);
  work.toler = toler;
  work.I_13=I_13;
  work.fa=fa;
  work.fb=fb;

  work.is_parent=0;
  for(i=0;i<5;i++)
    work.inner_integrals[i]=0.0;
  return work;

}

double gkq_double(double (*f)(double, struct my_f_params*), double (*finner)(double, struct my_f_params*),
                  double a, double b,
                  double TOL, struct my_f_params* p,stack_t stack)
{
  //1st integration
  //create parent task

  double result=0.0;
// *********************************************

  double m=0.5*(a+b);
  double h=0.5*(b-a);
  int i;

  double y[13];

  stack_t st;
  create_stack(&st, sizeof(work_gkq));

  double integ_bnd[2];

  get_integ_bounds_inner(integ_bnd, a, p);
  y[0]=f(a,p)* gkq_single(finner,integ_bnd[0],integ_bnd[1],1e-3, p, st);

  for(i=1;i<12;i++)
  {
    get_integ_bounds_inner(integ_bnd, m+xgkq[i]*h, p);
    y[i]=f(m+xgkq[i]*h,p)*gkq_single(finner,integ_bnd[0],integ_bnd[1],1e-3,p,st);
  }

  get_integ_bounds_inner(integ_bnd, b, p);
  y[12]=f(b,p)*gkq_single(finner,integ_bnd[0],integ_bnd[1],1e-3, p, st);

  free(st->elements);
  free(st);

  double fa=y[0];
  double fb=y[12];

  double I_4= (h/6.0)*(y[0]+y[12]+5.0*(y[4]+y[8]));  // 4-point gauss-lobatto
  double I_7= (h/1470.0)*(77.0*(y[0]+y[12])+432.0*(y[2]+y[10])+  // 7-point kronrod
               625.0*(y[4]+y[8])+672.0*y[6]);

  double I_13= h*(0.0158271919734802*(y[0]+y[12])+0.0942738402188500*(y[1]+y[11])+0.155071987336585*(y[2]+y[10])+
                  0.188821573960182*(y[3]+y[9])+0.199773405226859*(y[4]+y[8])+0.224926465333340*(y[5]+y[7])+
                  0.242611071901408*y[6]);   //13-point Kronrod

  double Err1=fabs(I_7-I_13);
  double Err2=fabs(I_4-I_13);

  double r=(Err2 != 0.0) ? Err1/Err2 : 1.0;
  double toler=(r > 0.0 && r < 1.0) ? TOL/r : TOL;

  if(I_13 == 0)
    I_13=b-a;
  I_13=fabs(I_13);

  printf("First approx I_13:%.4e\n",I_13);

  //Prepare work and push onto stack
  work_gkq work;
  work.a = a;
  work.b = b;
  work.toler = toler;
  work.I_13=I_13;
  work.fa=fa;
  work.fb=fb;

  work.p=p;
  work.f=f;
  work.is_parent=1;
  work.task_nr=0; //fange bei 0 das zählen an

  for(i=0;i<5;i++)
    work.inner_integrals[i]=0.0;

  // ALLOC INNER WS FOR INTEGR.
  // gsl_integration_workspace *gsinner2=  gsl_integration_workspace_alloc (1000);

  stack_t stack_inner;
  create_stack(&stack_inner, sizeof(work_gkq));
  work.stack_inner=stack_inner;

  //push work on inner stack
  //for comp of 5 inner intgrals
  work_gkq winner;
  double m_inner, h_inner, mll_inner,ml_inner, mr_inner, mrr_inner;

  m_inner=  (work.a+work.b)/2;
  h_inner=  (work.a+work.b)/2;
  mll_inner=(m_inner-alpha*h_inner);
  ml_inner= (m_inner-beta*h_inner);
  mr_inner= (m_inner+beta*h_inner);
  mrr_inner=(m_inner+alpha*h_inner);

  //1st subtask
  get_integ_bounds_inner(integ_bnd, mll_inner, work.p);
  winner=gkq_create_inner_task(inner_integrand2,integ_bnd[0], integ_bnd[1],1e-3, work.p);
  winner.subtask_nr=0;
  winner.task_nr=work.task_nr;

  push_stack(work.stack_inner,&winner);

  //2nd subtask
  get_integ_bounds_inner(integ_bnd, ml_inner, work.p);
  winner=gkq_create_inner_task(inner_integrand2,integ_bnd[0], integ_bnd[1],1e-3, work.p);
  winner.subtask_nr=1;
  winner.task_nr=work.task_nr;
  push_stack(work.stack_inner,&winner);

  //3rd subtask
  get_integ_bounds_inner(integ_bnd, m_inner, work.p);
  winner=gkq_create_inner_task(inner_integrand2,integ_bnd[0], integ_bnd[1],1e-3, work.p);
  winner.subtask_nr=2;
  winner.task_nr=work.task_nr;
  push_stack(work.stack_inner,&winner);

  //4th subtask
  get_integ_bounds_inner(integ_bnd, mr_inner, work.p);
  winner=gkq_create_inner_task(inner_integrand2,integ_bnd[0], integ_bnd[1],1e-3, work.p);
  winner.subtask_nr=3;
  winner.task_nr=work.task_nr;
  push_stack(work.stack_inner,&winner);

  //5th subtask
  get_integ_bounds_inner(integ_bnd, mrr_inner, work.p);
  winner=gkq_create_inner_task(inner_integrand2,integ_bnd[0], integ_bnd[1],1e-3, work.p);
  winner.subtask_nr=4;
  winner.task_nr=work.task_nr;
  push_stack(work.stack_inner,&winner);

  //push outer integtand work on outer stack
  work.subtasks_left=5;
  push_stack(stack, &work);

/*
  int elemsinner=work.stack_inner->el_count;
  int elout=stack->el_count;

  work_gkq* top=(work_gkq*) stack->elements +(stack->el_count-1); //get top element

  int inner2=0;
  inner2=top->stack_inner->el_count;
*/
  // printf("elemsinner:%d,ou:%d,in2:%d\n",elemsinner,elout, inner2);

  // int in2=  top->stack_inner->el_count;

  result=gkq_adapt_double(stack);//,stack);

  // result=gkq_adapt_serial(f,a,b,fa,fb,toler,I_13, p);
  // gsl_integration_workspace_free(gsinner2);

  // free(stack->elements);

  return result;
}

double gkq_single(double (*f)(double, struct my_f_params*), double a, double b,
                       double TOL, struct my_f_params* p,stack_t stack)
{
  double result=0.0;
// *********************************************

  double m=0.5*(a+b);
  double h=0.5*(b-a);
  int i;

  double y[13];
  double fa=y[0]=f(a,p);
  double fb=y[12]=f(b,p);

  for(i=1;i<12;i++)           //hier müssen direkt 13 integrale berechnet werden!!!
    y[i]=f(m+xgkq[i]*h,p);    //auch das sollte parallel erfolgen (pragma parfor?)

  double I_4= (h/6.0)*(y[0]+y[12]+5.0*(y[4]+y[8]));  // 4-point gauss-lobatto
  double I_7= (h/1470.0)*(77.0*(y[0]+y[12])+432.0*(y[2]+y[10])+  // 7-point kronrod
               625.0*(y[4]+y[8])+672.0*y[6]);

  double I_13= h*(0.0158271919734802*(y[0]+y[12])+0.0942738402188500*(y[1]+y[11])+0.155071987336585*(y[2]+y[10])+
                  0.188821573960182*(y[3]+y[9])+0.199773405226859*(y[4]+y[8])+0.224926465333340*(y[5]+y[7])+
                  0.242611071901408*y[6]);   //13-point Kronrod

  double Err1=fabs(I_7-I_13);
  double Err2=fabs(I_4-I_13);

  double r=(Err2 != 0.0) ? Err1/Err2 : 1.0;
  double toler=(r > 0.0 && r < 1.0) ? TOL/r : TOL;

  if(I_13 == 0)
    I_13=b-a;
  I_13=fabs(I_13);

  //Prepare work and push onto stack
  work_gkq work;
  work.a = a;
  work.b = b;
  work.toler = toler;
  work.I_13=I_13;
  work.fa=fa;
  work.fb=fb;
  work.p=p;
  work.a=a;
  work.f=f;

  push_stack(stack, &work);
  result=gkq_adapt_single(stack);//,stack);

  return result;
}

double gkq_adapt_serial(double (*f)(double, struct my_f_params*), double a, double b,
                        double fa,double fb, double toler,double I_13, struct my_f_params* p)
{
  double m = (a+b)/2;
  double h = (b-a)/2;
  double mll=m-alpha*h;
  double ml=m-beta*h;
  double mr=m+beta*h;
  double mrr=m+alpha*h;

  double fmll=f(mll,p);
  double fml=f(ml,p);
  double fm=f(m,p);
  double fmr=f(mr,p);
  double fmrr=f(mrr,p);
  double I_4=h/6.0*(fa+fb+5.0*(fml+fmr));   // 4-point Gauss-Lobatto formula.
  double I_7=h/1470.0*(77.0*(fa+fb)+432.0*(fmll+fmrr)+625.0*(fml+fmr)+672.0*fm);

  if (fabs(I_7-I_4) <= toler*I_13 || mll <= a || b <= mrr)
  {
    if ((mll <= a || b <= mrr) && !terminate_serial) //Error
     {
      // out_of_tolerance=true; // Interval contains no more machine numbers
      printf("OUT OF TOLERANCE !!!, mll:%.4e, a:%.4e, b:%.4e, mrr:%.4e\n", mll,b,b,mrr);
      terminate_serial=1;
     }

// printf("me ok:%d, a:%f,b:%f, tler:%.5e,I_4:%f,I_7:%f\n", omp_get_thread_num(), a,b,toler,I_4,I_7);

    return I_7;
  }
  else
  {

    // printf("me NOOOO:%d, a:%f,b:%f, tler:%.5e,I_4:%f,I_7:%f\n", omp_get_thread_num(), a,b,toler,I_4,I_7);

    return  gkq_adapt_serial(f, a,mll,fa,fmll,toler,I_13,p) +
            gkq_adapt_serial(f, mll,ml,fmll,fml,toler,I_13,p) +
            gkq_adapt_serial(f, ml,m,fml,fm,toler,I_13,p) +
            gkq_adapt_serial(f, m,mr,fm,fmr,toler,I_13,p) +
            gkq_adapt_serial(f, mr,mrr,fmr,fmrr,toler,I_13,p) +
            gkq_adapt_serial(f, mrr,b,fmrr,fb,toler,I_13,p);
  }

}

double gkq_adapt_single(stack_t stack)
{
  work_gkq work;
  // work.iter=0;
  int ready, idle, busy;
  double integral_result = 0.0;

  busy = 0;
  terminate_gkq=0;

#pragma omp parallel default(none) \
    shared(stack, integral_result,busy) \
    private(work, idle, ready)
  {
// printf("me:%d, err:%d\n",omp_get_thread_num(),simpson_error);

    ready = 0;
    idle = 1;

    while(!ready) // && !terminate_gkq)//  && !simpson_error) //<-- so NICHT!
    {
      #pragma omp critical (stack)
      {
        if (!empty_stack(stack))
        {
          /* we have new work */
          pop_stack(stack, &work);
          if (idle)
          {
            /* say others i'm busy */
            busy += 1;
            idle = 0;
          }
        }
        else
        {
          /* no new work on stack */
          if (!idle){
            busy -= 1;
            idle = 1;
          }

          /* nobody has anything to do; let us leave the loop */
          if (busy == 0)
          {
            ready = 1;
          }
        }
      } /* end critical(stack) */

      if (idle)
        continue; //if ready==1 --> leave loop
// double I_prev=work.I_prev;

double (*f)(double, struct my_f_params*)=work.f;

      double a = work.a;
      double b = work.b;
      double toler = work.toler;
      double I_13=work.I_13;
      double fa=work.fa;
      double fb=work.fb;
      // int iter=work.iter;
      // double *y= work.y; // brauch ich nicht!
      struct my_f_params * p = work.p;

      double m = (a+b)/2;
      double h = (b -a)/2;
      double mll=m-alpha*h;
      double ml=m-beta*h;
      double mr=m+beta*h;
      double mrr=m+alpha*h;

      double fmll=f(mll,p);
      double fml=f(ml,p);
      double fm=f(m,p);
      double fmr=f(mr,p);
      double fmrr=f(mrr,p);
      double I_4=h/6.0*(fa+fb+5.0*(fml+fmr));   // 4-point Gauss-Lobatto formula.
      double I_7=h/1470.0*(77.0*(fa+fb)+432.0*(fmll+fmrr)+625.0*(fml+fmr)+672.0*fm);

// if(myid==1)
//   printf("I_7:%.4e, I_13:%.4e,I_4:%.4e, minus:%.4e, to:%.4e\n",I_7,I_13,I_4,I_7-I_4, toler*I_13);
// int maxiter=50; //max. subdivisions
// double abstol=1e-30;
// work.I_prev=I_7; // für abstolcheck in nächster recursion

      if (fabs(I_7-I_4) <= toler*I_13 || mll <= a || b <= mrr) // || iter > maxiter || fabs(I_7-I_prev) < abstol )
      {
        if ((mll <= a || b <= mrr)) //Error
         {
           // out_of_tolerance=true; // Interval contains no more machine numbers
           // printf("OUT OF TOLERANCE !!!, mll:%.4e, a:%.4e, b:%.4e, mrr:%.4e,I_7-I_4:%.4e, tol:%.4e,I_13:%.4e\n",
           //        mll,b,b,mrr,I_7-I_4, toler*I_13,I_13);

         }
        #pragma omp critical (integral_result)
        {
          integral_result += I_7;      //Terminate recursion.
        }
      }
      else  //subdivide interval and push new work on stack
      {
        #pragma omp critical (stack)
        {
          // work.iter=iter+1;

          work.a=a;
          work.b=mll;
          work.fa=fa;
          work.fb=fmll;
          push_stack(stack, &work);

          work.a=mll;
          work.b=ml;
          work.fa=fmll;
          work.fb=fml;
          push_stack(stack, &work);

          work.a=ml;
          work.b=m;
          work.fa=fml;
          work.fb=fm;
          push_stack(stack, &work);

          work.a=m;
          work.b=mr;
          work.fa=fm;
          work.fb=fmr;
          push_stack(stack, &work);

          work.a=mr;
          work.b=mrr;
          work.fa=fmr;
          work.fb=fmrr;
          push_stack(stack, &work);

          work.a=mrr;
          work.b=b;
          work.fa=fmrr;
          work.fb=fb;

          push_stack(stack, &work);

        } // pragma critical stack
      }   // else ..non-acceptable error
    } // while
  } /* end omp parallel */
  return integral_result;
}

double gkq_adapt_double(stack_t stack)
{
  work_gkq work;
  work_gkq* pwork_outer;

  int ready, idle, busy;
  double integral_result = 0.0;
  busy = 0;
  int myid;
  int elcnt; //nr of outer task elements on stack

#pragma omp parallel default(none)  \
    shared(stack, integral_result,busy,iter,elcnt) \
    private(work, idle, ready,myid,pwork_outer)
  {

    myid=omp_get_thread_num();

    ready = 0;
    idle = 1;

    while(!ready) // && !terminate_gkq)//  && !simpson_error) //<-- so NICHT!
    {

// printf("\n NEXT, curapprox:%.4e\n",integral_result);
      #pragma omp critical (stack)
      {
        //pointer to outer work element
        //work_gkq* pwork=(work_gkq*) stack->elements + stack->el_count*stack->el_size;

        elcnt=stack->el_count;

// if(elcnt>1)
//  getchar();

        stack_t stack_inner;
        if(elcnt>0)
        {
          pwork_outer=(work_gkq*) stack->elements +(elcnt-1); //get top element

          //IST WOHL SUBOPTIMAL
            while(pwork_outer->task_nr > 0 && pwork_outer->subtasks_left==0) //this task is complete, goto next outer task
            {
              printf("myid:%d, outer task nr:%d, complete:subs:%d, decrement\n",myid,pwork_outer->task_nr,pwork_outer->subtasks_left);
              pwork_outer--;
            }
            stack_inner=pwork_outer->stack_inner;
        }
        else
        {
          stack_inner=NULL;
          pwork_outer=NULL;
        }

        printf("myid:%d,elcnt:%d,sinner:%p,pwout:%p\n",myid,elcnt,stack_inner,pwork_outer);

        { //stackinner gehört zu work, und work ist private!
          if(!empty_stack(stack_inner))
          {
            printf("myid:%d,iter:%d, inner stack not empty,pop..\n",myid,iter);
            pop_stack(stack_inner, &work);
//if letztes inner-work elem, blockiere outer work elem bis ich das integral hab

            iter++;

            if (idle)
            {
              busy += 1;
              idle = 0;
            }
          }
          else //inner stack is empty, pop from outer
          {
            printf("myid:%d,iter:%d, inner stack is empty?\n",myid,iter);
            if (!empty_stack(stack)) //work elem ist blockiert solange inner integs nicht vollständig
            {
              // printf("myid:%d,iter:%d, inner stack empty. work on outer with cnt=%d\n",myid,iter,pwork_outer->subtasks_left);
              if(pwork_outer->subtasks_left==0)
              {
                printf("myid:%d,iter:%d, outer stack not empty\n",myid,iter);
                pop_stack(stack, &work);
                iter++;
                if (idle)
                {
                  busy += 1;
                  idle = 0;
                }
              }
              else
              {
                printf("myid:%d,iter:%d, inner integs not complete:%d\n",myid,iter,pwork_outer->subtasks_left);
                if (!idle)
                {
                  busy -= 1;
                  idle = 1;
                }

              }
            }
            else //auch outer stack ist leer
            {
              printf("myid:%d,iter:%d, outer stack empty too\n",myid,iter);
              if (!idle)
              {
                busy -= 1;
                idle = 1;
              }
              if (busy == 0)
              {
                ready = 1;
              }
            }
          }
        } // critical inner stack
      } //critical outer stack

      if (idle)
      {
        printf("myid:%d,iter:%d, noth8ing to do\n",myid,iter);
        continue; //if ready==1 --> leave loop
      }

      //work on inner tasks first,  if available
      if(work.is_parent==0)
      {
        printf("myid:%d,iter:%d,tasknr:%d,innertask:%d, left subs:%d\n",
                myid,iter,work.task_nr,work.subtask_nr,pwork_outer->subtasks_left); //subtasks nr nicht immer aktuell!

// getchar();

        double (*f)(double, struct my_f_params*) = work.f;
        double a = work.a;
        double b = work.b;
        double toler = work.toler;
        double I_13=work.I_13;
        double fa=work.fa;
        double fb=work.fb;
        // double *y= work.y; // brauch ich nicht!
        struct my_f_params * p = work.p;

        double m = (a+b)/2;
        double h = (b -a)/2;
        double mll=m-alpha*h;
        double ml=m-beta*h;
        double mr=m+beta*h;
        double mrr=m+alpha*h;

        double fmll=f(mll,p);
        double fml=f(ml,p);
        double fm=f(m,p);
        double fmr=f(mr,p);
        double fmrr=f(mrr,p);
        double I_4=h/6.0*(fa+fb+5.0*(fml+fmr));   // 4-point Gauss-Lobatto formula.
        double I_7=h/1470.0*(77.0*(fa+fb)+432.0*(fmll+fmrr)+625.0*(fml+fmr)+672.0*fm);

// printf("myid:%d,iter:%d non parent checkpoint 1 passed\n",myid,iter);

        if (fabs(I_7-I_4) <= toler*I_13 || mll <= a || b <= mrr)
        {
          if ((mll <= a || b <= mrr))// && !terminate_gkq) //Error
           {
             printf("OUT OF TOLERANCE !!!, mll:%.4e, a:%.4e, b:%.4e, mrr:%.4e\n", mll,a,b,mrr);
           }

// printf("myid:%d,iter:%d non parent checkpoint 2.1 passed\n",myid,iter);

          int tasknr=work.subtask_nr;

          printf("myid:%d,iter:%d, winner error acceptable: I_7:%.4e, I_4:%.4e, I_13:%.4e,toler*I_13:%.4e,toler:%.4e\n",
                  myid,iter,I_7,I_4, I_13,toler*I_13,toler);

          #pragma omp critical (innerinteg) // workers greifen auf selbes array zu.nur an anderer Stelle..evtl. besser 6 versch.doubles statt array?
          {
            double *inner_integrals=pwork_outer->inner_integrals;
            inner_integrals[tasknr]+=I_7;
            pwork_outer->subtasks_left=pwork_outer->subtasks_left-1; //hier sollte subtasks_left aktuell sein

            printf("myid:%d,iter:%d, inner_integral[%d]:%.4e added:%.4e,subtask left:%d\n",
                    myid,iter,tasknr,inner_integrals[tasknr],I_7,pwork_outer->subtasks_left);
          }
        }
        else  //subdivide interval and push new work on stack
        {

          stack_t stack_inner;
          #pragma omp critical //(stack) //(stack_inner)
          {
            stack_inner =  pwork_outer->stack_inner;
            pwork_outer->subtasks_left=pwork_outer->subtasks_left-1;//weil dieser stack durch 6 andere ersetzt wird

            work.task_nr=pwork_outer->task_nr; //inner task soll auch wissen wer sein outer task is (bisher nur für debug)
            //subtasknr bleibt dieselbe (bezogen auf parent task)
            work.a=a;
            work.b=mll;
            work.fa=fa;
            work.fb=fmll;

            push_stack(stack_inner, &work);

            work.a=mll;
            work.b=ml;
            work.fa=fmll;
            work.fb=fml;

            push_stack(stack_inner, &work);

            work.a=ml;
            work.b=m;
            work.fa=fml;
            work.fb=fm;

            push_stack(stack_inner, &work);

            work.a=m;
            work.b=mr;
            work.fa=fm;
            work.fb=fmr;

            push_stack(stack_inner, &work);

            work.a=mr;
            work.b=mrr;
            work.fa=fmr;
            work.fb=fmrr;

            push_stack(stack_inner, &work);

            work.a=mrr;
            work.b=b;
            work.fa=fmrr;
            work.fb=fb;

            push_stack(stack_inner, &work);

            pwork_outer->subtasks_left=pwork_outer->subtasks_left+6;

            printf("myid:%d,iter:%d, inner_integral approx not accurate..subdivided: I_4:%f,I_7:%f,I_13:%f,left subs:%d\n",
                    myid,iter,I_4,I_7,I_13,pwork_outer->subtasks_left);

          } // pragma critical stack_inner
        }   // else ..non-acceptable error
      }// !isparent
      else  //parent task without any inner tasks
      {

printf("myid:%d,iter:%d, work is parent,subs left:%d\n",myid,iter,work.subtasks_left);

        // stack_t stack_inner = work.stack_inner; //brauch ich hier nicht
        // free(stack_inner->elements);
        // free(stack_inner);

        double (*f)(double, struct my_f_params*) = work.f;
        double a = work.a;
        double b = work.b;
        double toler = work.toler;
        double I_13=work.I_13;
        double fa=work.fa;
        double fb=work.fb;
        // double *y= work.y; // brauch ich nicht!
        struct my_f_params * p = work.p;

        double m = (a+b)/2;
        double h = (b -a)/2;
        double mll=m-alpha*h;
        double ml=m-beta*h;
        double mr=m+beta*h;
        double mrr=m+alpha*h;

        //the inner intergrals are pre-calculated and saved in an array
        //the function only calculates a prefactor...
        double fmll=f(mll,p)*work.inner_integrals[0];
        double fml=  f(ml,p)*work.inner_integrals[1];
        double fm=   f(m,p)*work.inner_integrals[2];
        double fmr=  f(mr,p)*work.inner_integrals[3];
        double fmrr= f(mrr,p)*work.inner_integrals[4];

        double I_4=h/6.0*(fa+fb+5.0*(fml+fmr));   // 4-point Gauss-Lobatto formula.
        double I_7=h/1470.0*(77.0*(fa+fb)+432.0*(fmll+fmrr)+625.0*(fml+fmr)+672.0*fm);

        if (fabs(I_7-I_4) <= toler*I_13 || mll <= a || b <= mrr)
        {
          if ((mll <= a || b <= mrr))// && !terminate_gkq) //Error
           {
             // out_of_tolerance=true; // Interval contains no more machine numbers
             printf("outer task OUT OF TOLERANCE !!!, mll:%.4e, a:%.4e, b:%.4e, mrr:%.4e\n", mll,b,b,mrr);
           }
           //#pragma omp critical(integal_result)
           {
              #pragma omp atomic
              integral_result += I_7;
              printf("myid:%d,iter:%d, I7 added, integs: 0=%f,1=%f,2=%f,3=%f,4=%f\n", myid,iter,
                    work.inner_integrals[0],work.inner_integrals[1],work.inner_integrals[2],
                    work.inner_integrals[3],work.inner_integrals[4]);
           }
        }
        else  //subdivide interval and push new work on stack
        {

printf("myid:%d,iter:%d, iouter_integral approx not accurate: I_7=%.4e,I_4=%.4e,I_13=%.4e, toler*I_13:%.4e\n"
        "integs: 0=%f,1=%f,2=%f,3=%f,4=%f..subdivide\n",
        myid,iter,I_7,I_4,I_13,I_13*toler,work.inner_integrals[0],work.inner_integrals[1],work.inner_integrals[2],
                    work.inner_integrals[3],work.inner_integrals[4] );

          stack_t stack_inner = work.stack_inner;
          #pragma omp critical (stack)
          {
            //create sub-stack for inner-integrals

            work.subtasks_left=5; //for all new outer work elements

            create_stack(&stack_inner, sizeof(work_gkq));
            work.is_parent=1;

            work_gkq winner;

            //outer task elem
            work.a=a;
            work.b=mll;
            work.fa=fa;
            work.fb=fmll;

            double m_inner=  (work.a+work.b)/2;
            double h_inner=  (work.a+work.b)/2;
            double mll_inner=(m_inner-alpha*h_inner);
            double ml_inner= (m_inner-beta*h_inner);
            double mr_inner= (m_inner+beta*h_inner);
            double mrr_inner=(m_inner+alpha*h_inner);

            //Jedes subinterval bekommt 5 inner tasks für die inneren integrale!

            int curcnt=stack->el_count;
            work.task_nr=curcnt; //weil ich bei 0 anfange zu zählen und push_stack gleich el_count incrementiert

            double integ_bnd[2];

            //1st subtask
            get_integ_bounds_inner(integ_bnd, mll_inner, work.p);
            winner=gkq_create_inner_task(inner_integrand2, integ_bnd[0],integ_bnd[1],1e-3, work.p);
            winner.task_nr=work.task_nr;
            winner.subtask_nr=0;
            push_stack(work.stack_inner,&winner);

            //2nd subtask
            get_integ_bounds_inner(integ_bnd, ml_inner, work.p);
            winner=gkq_create_inner_task(inner_integrand2, integ_bnd[0],integ_bnd[1],1e-3, work.p);
            winner.subtask_nr=1;
            winner.task_nr=work.task_nr;
            push_stack(work.stack_inner,&winner);

            //3rd subtask
            get_integ_bounds_inner(integ_bnd, m_inner, work.p);
            winner=gkq_create_inner_task(inner_integrand2, integ_bnd[0],integ_bnd[1],1e-3, work.p);
            winner.subtask_nr=2;
            winner.task_nr=work.task_nr;
            push_stack(work.stack_inner,&winner);

            //4th subtask
            get_integ_bounds_inner(integ_bnd, mr_inner, work.p);
            winner=gkq_create_inner_task(inner_integrand2, integ_bnd[0],integ_bnd[1],1e-3, work.p);
            winner.subtask_nr=3;
            winner.task_nr=work.task_nr;
            push_stack(work.stack_inner,&winner);

            //5th subtask
            get_integ_bounds_inner(integ_bnd, mrr_inner, work.p);
            winner=gkq_create_inner_task(inner_integrand2, integ_bnd[0],integ_bnd[1],1e-3, work.p);
            winner.subtask_nr=4;
            winner.task_nr=work.task_nr;
            push_stack(work.stack_inner,&winner);

            push_stack(stack, &work);

            // *************************
            // *   2nd subinterval
            // **************************
            work.a=mll;
            work.b=ml;
            work.fa=fmll;
            work.fb=fml;
            work.task_nr++;

            m_inner=  (work.a+work.b)/2;
            h_inner=  (work.a+work.b)/2;
            mll_inner=(m_inner-alpha*h_inner);
            ml_inner= (m_inner-beta*h_inner);
            mr_inner= (m_inner+beta*h_inner);
            mrr_inner=(m_inner+alpha*h_inner);

            //1st subtask
            get_integ_bounds_inner(integ_bnd, mll_inner, work.p);
            winner=gkq_create_inner_task(inner_integrand2, integ_bnd[0],integ_bnd[1],1e-3, work.p);
            winner.subtask_nr=0;
            winner.task_nr=work.task_nr;
            push_stack(work.stack_inner,&winner);

            //2nd subtask
            get_integ_bounds_inner(integ_bnd, ml_inner, work.p);
            winner=gkq_create_inner_task(inner_integrand2, integ_bnd[0],integ_bnd[1],1e-3, work.p);
            winner.subtask_nr=1;
            winner.task_nr=work.task_nr;
            push_stack(work.stack_inner,&winner);

            //3rd subtask
            get_integ_bounds_inner(integ_bnd, m_inner, work.p);
            winner=gkq_create_inner_task(inner_integrand2, integ_bnd[0],integ_bnd[1],1e-3, work.p);
            winner.subtask_nr=2;
            winner.task_nr=work.task_nr;
            push_stack(work.stack_inner,&winner);

            //4th subtask
            get_integ_bounds_inner(integ_bnd, mr_inner, work.p);
            winner=gkq_create_inner_task(inner_integrand2, integ_bnd[0],integ_bnd[1],1e-3, work.p);
            winner.subtask_nr=3;
            winner.task_nr=work.task_nr;
            push_stack(work.stack_inner,&winner);

            //5th subtask
            get_integ_bounds_inner(integ_bnd, mrr_inner, work.p);
            winner=gkq_create_inner_task(inner_integrand2, integ_bnd[0],integ_bnd[1],1e-3, work.p);
            winner.subtask_nr=4;
            winner.task_nr=work.task_nr;
            push_stack(work.stack_inner,&winner);

            // #pragma omp critical (stack)
            // {
              push_stack(stack, &work);
            // }

            // *************************
            // *   3rd subinterval
            // **************************
            work.a=ml;
            work.b=m;
            work.fa=fml;
            work.fb=fm;
            work.task_nr++;

            m_inner=  (work.a+work.b)/2;
            h_inner=  (work.a+work.b)/2;
            mll_inner=(m_inner-alpha*h_inner);
            ml_inner= (m_inner-beta*h_inner);
            mr_inner= (m_inner+beta*h_inner);
            mrr_inner=(m_inner+alpha*h_inner);

            //1st subtask
            get_integ_bounds_inner(integ_bnd, mll_inner, work.p);
            winner=gkq_create_inner_task(inner_integrand2, integ_bnd[0],integ_bnd[1],1e-3, work.p);
            winner.subtask_nr=0;
            winner.task_nr=work.task_nr;
            push_stack(work.stack_inner,&winner);

            //2nd subtask
            get_integ_bounds_inner(integ_bnd, ml_inner, work.p);
            winner=gkq_create_inner_task(inner_integrand2, integ_bnd[0],integ_bnd[1],1e-3, work.p);
            winner.subtask_nr=1;
            winner.task_nr=work.task_nr;
            push_stack(work.stack_inner,&winner);

            //3rd subtask
            get_integ_bounds_inner(integ_bnd, m_inner, work.p);
            winner=gkq_create_inner_task(inner_integrand2, integ_bnd[0],integ_bnd[1],1e-3, work.p);
            winner.subtask_nr=2;
            winner.task_nr=work.task_nr;
            push_stack(work.stack_inner,&winner);

            //4th subtask
            get_integ_bounds_inner(integ_bnd, mr_inner, work.p);
            winner=gkq_create_inner_task(inner_integrand2, integ_bnd[0],integ_bnd[1],1e-3, work.p);
            winner.subtask_nr=3;
            winner.task_nr=work.task_nr;
            push_stack(work.stack_inner,&winner);

            //5th subtask
            get_integ_bounds_inner(integ_bnd, mrr_inner, work.p);
            winner=gkq_create_inner_task(inner_integrand2, integ_bnd[0],integ_bnd[1],1e-3, work.p);
            winner.subtask_nr=4;
            winner.task_nr=work.task_nr;
            push_stack(work.stack_inner,&winner);

            // #pragma omp critical (stack)
            // {
              push_stack(stack, &work);
            // }

            // *************************
            // *   4th subinterval
            // **************************
            work.a=m;
            work.b=mr;
            work.fa=fm;
            work.fb=fmr;
            work.task_nr++;

            m_inner=  (work.a+work.b)/2;
            h_inner=  (work.a+work.b)/2;
            mll_inner=(m_inner-alpha*h_inner);
            ml_inner= (m_inner-beta*h_inner);
            mr_inner= (m_inner+beta*h_inner);
            mrr_inner=(m_inner+alpha*h_inner);

            //1st subtask
            get_integ_bounds_inner(integ_bnd, mll_inner, work.p);
            winner=gkq_create_inner_task(inner_integrand2,integ_bnd[0],integ_bnd[1],1e-3, work.p);
            winner.subtask_nr=0;
            winner.task_nr=work.task_nr;
            push_stack(work.stack_inner,&winner);

            //2nd subtask
            get_integ_bounds_inner(integ_bnd, ml_inner, work.p);
            winner=gkq_create_inner_task(inner_integrand2,integ_bnd[0],integ_bnd[1],1e-3, work.p);
            winner.subtask_nr=1;
            winner.task_nr=work.task_nr;
            push_stack(work.stack_inner,&winner);

            //3rd subtask
            get_integ_bounds_inner(integ_bnd, m_inner, work.p);
            winner=gkq_create_inner_task(inner_integrand2,integ_bnd[0],integ_bnd[1],1e-3, work.p);
            winner.subtask_nr=2;
            winner.task_nr=work.task_nr;
            push_stack(work.stack_inner,&winner);

            //4th subtask
            get_integ_bounds_inner(integ_bnd, mr_inner, work.p);
            winner=gkq_create_inner_task(inner_integrand2,integ_bnd[0],integ_bnd[1],1e-3, work.p);
            winner.subtask_nr=3;
            winner.task_nr=work.task_nr;
            push_stack(work.stack_inner,&winner);

            //5th subtask
            get_integ_bounds_inner(integ_bnd, mrr_inner, work.p);
            winner=gkq_create_inner_task(inner_integrand2,integ_bnd[0],integ_bnd[1],1e-3, work.p);
            winner.subtask_nr=4;
            winner.task_nr=work.task_nr;
            push_stack(work.stack_inner,&winner);

            // #pragma omp critical (stack)
            {
              push_stack(stack, &work);
            }

            // *************************
            // *   5th subinterval
            // **************************
            work.a=mr;
            work.b=mrr;
            work.fa=fmr;
            work.fb=fmrr;
            work.task_nr++;

            m_inner=  (work.a+work.b)/2;
            h_inner=  (work.a+work.b)/2;
            mll_inner=(m_inner-alpha*h_inner);
            ml_inner= (m_inner-beta*h_inner);
            mr_inner= (m_inner+beta*h_inner);
            mrr_inner=(m_inner+alpha*h_inner);

            //1st subtask
            get_integ_bounds_inner(integ_bnd, mll_inner, work.p);
            winner=gkq_create_inner_task(inner_integrand2,integ_bnd[0],integ_bnd[1],1e-3, work.p);
            winner.subtask_nr=0;
            winner.task_nr=work.task_nr;
            push_stack(work.stack_inner,&winner);

            //2nd subtask
            get_integ_bounds_inner(integ_bnd, ml_inner, work.p);
            winner=gkq_create_inner_task(inner_integrand2,integ_bnd[0],integ_bnd[1],1e-3, work.p);
            winner.subtask_nr=1;
            winner.task_nr=work.task_nr;
            push_stack(work.stack_inner,&winner);

            //3rd subtask
            get_integ_bounds_inner(integ_bnd, m_inner, work.p);
            winner=gkq_create_inner_task(inner_integrand2,integ_bnd[0],integ_bnd[1],1e-3, work.p);
            winner.subtask_nr=2;
            winner.task_nr=work.task_nr;
            push_stack(work.stack_inner,&winner);

            //4th subtask
            get_integ_bounds_inner(integ_bnd, mr_inner, work.p);
            winner=gkq_create_inner_task(inner_integrand2,integ_bnd[0],integ_bnd[1],1e-3, work.p);
            winner.subtask_nr=3;
            winner.task_nr=work.task_nr;
            push_stack(work.stack_inner,&winner);

            //5th subtask
            get_integ_bounds_inner(integ_bnd, mrr_inner, work.p);
            winner=gkq_create_inner_task(inner_integrand2,integ_bnd[0],integ_bnd[1],1e-3, work.p);
            winner.subtask_nr=4;
            winner.task_nr=work.task_nr;
            push_stack(work.stack_inner,&winner);

            // #pragma omp critical (stack)
            {
              push_stack(stack, &work);
            }

            // *************************
            // *   6th subinterval
            // **************************
            work.a=mrr;
            work.b=b;
            work.fa=fmrr;
            work.fb=fb;
            work.task_nr++;

            m_inner=  (work.a+work.b)/2;
            h_inner=  (work.a+work.b)/2;
            mll_inner=(m_inner-alpha*h_inner);
            ml_inner= (m_inner-beta*h_inner);
            mr_inner= (m_inner+beta*h_inner);
            mrr_inner=(m_inner+alpha*h_inner);

            //1st subtask
            get_integ_bounds_inner(integ_bnd, mll_inner, work.p);
            winner=gkq_create_inner_task(inner_integrand2,integ_bnd[0],integ_bnd[1],1e-3, work.p);
            winner.subtask_nr=0;
            winner.task_nr=work.task_nr;
            push_stack(work.stack_inner,&winner);

            //2nd subtask
            get_integ_bounds_inner(integ_bnd, ml_inner, work.p);
            winner=gkq_create_inner_task(inner_integrand2,integ_bnd[0],integ_bnd[1],1e-3, work.p);
            winner.subtask_nr=1;
            winner.task_nr=work.task_nr;
            push_stack(work.stack_inner,&winner);

            //3rd subtask
            get_integ_bounds_inner(integ_bnd, m_inner, work.p);
            winner=gkq_create_inner_task(inner_integrand2,integ_bnd[0],integ_bnd[1],1e-3, work.p);
            winner.subtask_nr=2;
            winner.task_nr=work.task_nr;
            push_stack(work.stack_inner,&winner);

            //4th subtask
            get_integ_bounds_inner(integ_bnd, mr_inner, work.p);
            winner=gkq_create_inner_task(inner_integrand2,integ_bnd[0],integ_bnd[1],1e-3, work.p);
            winner.subtask_nr=3;
            winner.task_nr=work.task_nr;
            push_stack(work.stack_inner,&winner);

            //5th subtask
            get_integ_bounds_inner(integ_bnd, mrr_inner, work.p);
            winner=gkq_create_inner_task(inner_integrand2,integ_bnd[0],integ_bnd[1],1e-3, work.p);
            winner.subtask_nr=4;
            winner.task_nr=work.task_nr;
            push_stack(work.stack_inner,&winner);

            // #pragma omp critical (stack)
            {
              push_stack(stack, &work);
            }

            printf("myid:%d,iter:%d, 6 subinvtervals with 5 subtasks each pushed to stack outer\n",myid,iter);

          } // pragma critical
        }   // else ..non-acceptable error

      }

    } // while
  } /* end omp parallel */

  return integral_result;
}

//SIMPSON

  // *****************************************************************************************************************************************
// double integral2(double (*f)(double, struct my_f_params*), stack_t stack)
// {
//   work_t work;
//   int ready, idle, busy;
//   double integral_result = 0.0;

//   busy = 0;

// #pragma omp parallel default(none) \
//     shared(stack, integral_result,f,busy,simpson_error) \
//     private(work, idle, ready)
//   {

// // printf("me:%d, err:%d\n",omp_get_thread_num(),simpson_error);

//     ready = 0;
//     idle = 1;

//     while(!ready  && !simpson_error) //<-- so NICHT!
//     {
// #pragma omp critical (stack)
//       {
//         if (!empty_stack(stack))
//         {
//           /* we have new work */
//           pop_stack(stack, &work);
//           if (idle)
//           {
//             /* say others i'm busy */
//             busy += 1;
//             idle = 0;
//           }
//         }
//         else{
//           /* no new work on stack */
//           if (!idle){
//             busy -= 1;
//             idle = 1;
//           }

//           /* nobody has anything to do; let us leave the loop */
//           if (busy == 0)
//           {
//             ready = 1;
//           }
//         }
//       } /* end critical(stack) */

//       if (idle)
//         continue; //if ready==1 --> leave loop

//       double b = work.b;
//       double a = work.a;
//       double tol = work.tol;
//       double S=work.S; // previous TOTAL integral
//       double fa=work.fa;
//       double fb=work.fb;
//       double fm=work.fm;
//       int    rec=work.rec;

//       double h = (b - a)/2;
//       double mid = (a+b)/2;
//       double lm=(a+mid)/2;
//       double rm=(mid+b)/2;

//       double flm=f(lm,work.p);
//       double frm=f(rm,work.p);

//       double Sl=h/6*(fa+4*flm+fm);
//       double Sr=h/6*(fm+4*frm+fb);

//       double delta=Sl+Sr-S;

//       // serious numerical trouble: it won't converge
//       if ((tol/2 == tol) || fabs(a-lm) <= tol) // || tol < tolmax)
//       {
//              simpson_error = 1;
// #pragma omp critical (integral_result)
//              integral_result = S;
//       }
//       //need something against spurious convergence
//       //for now: iter > 5 (c.f. numerical recipes)
//       if( work.iter > 5 &&  (rec <= 0 || fabs(delta) <= 15*tol))  //error acceptable
//       {
// #pragma omp critical (integral_result)
//         integral_result += Sl+Sr+delta/15;
//       }
//       else // error not acceptable
//       {
//         //push new subintervals to stack
//         work.a = a;
//         work.b = mid;
//         work.tol = tol/2;
//         work.S = Sl;
//         work.fa=fa;
//         work.fb=fm;
//         work.fm=flm;
//         work.rec=rec-1;
//         work.iter=work.iter+1;

//   #pragma omp critical (stack)
//         {
//           //LEFT
//           push_stack(stack, &work);
//           //prepare RIGHT side and push to stack
//           work.a = mid;
//           work.b = b;
//           work.tol = tol/2;
//           work.S=Sr;
//           work.fa=fm;
//           work.fb=fb;
//           work.fm=frm;
//           work.rec=rec-1;
//           push_stack(stack, &work);

//         }
//       }
//     } /* while */
//   } /* end omp parallel */

//   return integral_result;
// }


/* specfunc/bessel_Yn.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 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.
 */

/* Author:  G. Jungman */

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_sf_gamma.h>
#include <gsl/gsl_sf_psi.h>
#include <gsl/gsl_sf_bessel.h>

#include "error.h"

#include "bessel.h"
#include "bessel_amp_phase.h"
#include "bessel_olver.h"

/*-*-*-*-*-*-*-*-*-*-*-* Private Section *-*-*-*-*-*-*-*-*-*-*-*/

/* assumes n >= 1 */
static int bessel_Yn_small_x(const int n, const double x, gsl_sf_result * result)
{
  int k;
  double y = 0.25 * x * x;
  double ln_x_2 = log(0.5*x);
  gsl_sf_result ln_nm1_fact;
  double k_term;
  double term1, sum1, ln_pre1;
  double term2, sum2, pre2;

  gsl_sf_lnfact_e((unsigned int)(n-1), &ln_nm1_fact);

  ln_pre1 = -n*ln_x_2 + ln_nm1_fact.val;
  if(ln_pre1 > GSL_LOG_DBL_MAX - 3.0) GSL_ERROR ("error", GSL_EOVRFLW);

  sum1 = 1.0;
  k_term = 1.0;
  for(k=1; k<=n-1; k++) {
    k_term *= y/(k * (n-k));
    sum1 += k_term;
  }
  term1 = -exp(ln_pre1) * sum1 / M_PI;

  pre2 = -exp(n*ln_x_2) / M_PI;
  if(fabs(pre2) > 0.0) {
    const int KMAX = 20;
    gsl_sf_result psi_n;
    gsl_sf_result npk_fact;
    double yk = 1.0;
    double k_fact  = 1.0;
    double psi_kp1 = -M_EULER;
    double psi_npkp1;
    gsl_sf_psi_int_e(n, &psi_n);
    gsl_sf_fact_e((unsigned int)n, &npk_fact);
    psi_npkp1 = psi_n.val + 1.0/n;
    sum2 = (psi_kp1 + psi_npkp1 - 2.0*ln_x_2)/npk_fact.val;
    for(k=1; k<KMAX; k++) {
      psi_kp1   += 1./k;
      psi_npkp1 += 1./(n+k);
      k_fact   *= k;
      npk_fact.val *= n+k;
      yk *= -y;
      k_term = yk*(psi_kp1 + psi_npkp1 - 2.0*ln_x_2)/(k_fact*npk_fact.val);
      sum2 += k_term;
    }
    term2 = pre2 * sum2;
  }
  else {
    term2 = 0.0;
  }

  result->val  = term1 + term2;
  result->err  = GSL_DBL_EPSILON * (fabs(ln_pre1)*fabs(term1) + fabs(term2));
  result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);

  return GSL_SUCCESS;
}

/*-*-*-*-*-*-*-*-*-*-*-* Functions with Error Codes *-*-*-*-*-*-*-*-*-*-*-*/

int
gsl_sf_bessel_Yn_e(int n, const double x, gsl_sf_result * result)
{
  int sign = 1;

  if(n < 0) {
    /* reduce to case n >= 0 */
    n = -n;
    if(GSL_IS_ODD(n)) sign = -1;
  }

  /* CHECK_POINTER(result) */

  if(n == 0) {
    int status = gsl_sf_bessel_Y0_e(x, result);
    result->val *= sign;
    return status;
  }
  else if(n == 1) {
    int status = gsl_sf_bessel_Y1_e(x, result);
    result->val *= sign;
    return status;
  }
  else {
    if(x <= 0.0) {
      DOMAIN_ERROR(result);
    }
    if(x < 5.0) {
      int status = bessel_Yn_small_x(n, x, result);
      result->val *= sign;
      return status;
    }
    else if(GSL_ROOT3_DBL_EPSILON * x > (n*n + 1.0)) {
      int status = gsl_sf_bessel_Ynu_asympx_e((double)n, x, result);
      result->val *= sign;
      return status;
    }
    else if(n > 50) {
      int status = gsl_sf_bessel_Ynu_asymp_Olver_e((double)n, x, result);
      result->val *= sign;
      return status;
    }
    else {
      double two_over_x = 2.0/x;
      gsl_sf_result r_by;
      gsl_sf_result r_bym;
      int stat_1 = gsl_sf_bessel_Y1_e(x, &r_by);
      int stat_0 = gsl_sf_bessel_Y0_e(x, &r_bym);
      double bym = r_bym.val;
      double by  = r_by.val;
      double byp;
      int j;

      for(j=1; j<n; j++) {
        byp = j*two_over_x*by - bym;
        bym = by;
        by  = byp;
      }
      result->val  = sign * by;
      result->err  = fabs(result->val) * (fabs(r_by.err/r_by.val) + fabs(r_bym.err/r_bym.val));
      result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);

      return GSL_ERROR_SELECT_2(stat_1, stat_0);
    }
  }
}

int
gsl_sf_bessel_Yn_array(const int nmin, const int nmax, const double x, double * result_array)
{
  /* CHECK_POINTER(result_array) */

  if(nmin < 0 || nmax < nmin || x <= 0.0) {
    int j;
    for(j=0; j<=nmax-nmin; j++) result_array[j] = 0.0;
    GSL_ERROR ("error", GSL_EDOM);
  }
  else {
    gsl_sf_result r_Ynm1;
    gsl_sf_result r_Yn;
    int stat_nm1 = gsl_sf_bessel_Yn_e(nmin,   x, &r_Ynm1);
    int stat_n   = gsl_sf_bessel_Yn_e(nmin+1, x, &r_Yn);
    double Ynp1;
    double Yn   = r_Yn.val;
    double Ynm1 = r_Ynm1.val;
    int n;

    int stat = GSL_ERROR_SELECT_2(stat_nm1, stat_n);

    if(stat == GSL_SUCCESS) {
      for(n=nmin+1; n<=nmax+1; n++) {
        result_array[n-nmin-1] = Ynm1;
        Ynp1 = -Ynm1 + 2.0*n/x * Yn;
        Ynm1 = Yn;
        Yn   = Ynp1;
      }
    }
    else {
      for(n=nmin; n<=nmax; n++) {
        result_array[n-nmin] = 0.0;
      }
    }

    return stat;
  }
}

/*-*-*-*-*-*-*-*-*-* Functions w/ Natural Prototypes *-*-*-*-*-*-*-*-*-*-*/

#include "eval.h"

double gsl_sf_bessel_Yn(const int n, const double x)
{
  EVAL_RESULT(gsl_sf_bessel_Yn_e(n, x, &result));
}


/* movstat/movmad.c
 *
 * Copyright (C) 2018 Patrick Alken
 *
 * 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.
 */

#include <config.h>
#include <stdlib.h>
#include <math.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_movstat.h>
#include <gsl/gsl_sort.h>
#include <gsl/gsl_statistics.h>

/*
gsl_movstat_mad()
  Apply a moving MAD to an input vector (with scale factor to make an
unbiased estimate of sigma)

Inputs: endtype - how to handle end points
        x       - input vector, size n
        xmedian - (output) vector of median values of x, size n
                  xmedian_i = median of window centered on x_i
        xmad    - (output) vector of estimated standard deviations of x, size n
                  xmad_i = MAD of i-th window: 1.4826 * median(|x_i - xmedian_i|)
        w       - workspace
*/

int
gsl_movstat_mad(const gsl_movstat_end_t endtype, const gsl_vector * x, gsl_vector * xmedian, gsl_vector * xmad,
                gsl_movstat_workspace * w)
{
  if (x->size != xmedian->size)
    {
      GSL_ERROR("x and xmedian vectors must have same length", GSL_EBADLEN);
    }
  else if (x->size != xmad->size)
    {
      GSL_ERROR("x and xmad vectors must have same length", GSL_EBADLEN);
    }
  else
    {
      double scale = 1.482602218505602;
      int status = gsl_movstat_apply_accum(endtype, x, gsl_movstat_accum_mad, &scale, xmedian, xmad, w);
      return status;
    }
}

/*
gsl_movstat_mad0()
  Apply a moving MAD to an input vector (without scale factor to make an
unbiased estimate of sigma)

Inputs: endtype - how to handle end points
        x       - input vector, size n
        xmedian - (output) vector of median values of x, size n
                  xmedian_i = median of window centered on x_i
        xmad    - (output) vector of estimated standard deviations of x, size n
                  xmad_i = MAD of i-th window: median(|x_i - xmedian_i|)
        w       - workspace
*/

int
gsl_movstat_mad0(const gsl_movstat_end_t endtype, const gsl_vector * x, gsl_vector * xmedian, gsl_vector * xmad,
                 gsl_movstat_workspace * w)
{
  if (x->size != xmedian->size)
    {
      GSL_ERROR("x and xmedian vectors must have same length", GSL_EBADLEN);
    }
  else if (x->size != xmad->size)
    {
      GSL_ERROR("x and xmad vectors must have same length", GSL_EBADLEN);
    }
  else
    {
      double scale = 1.0;
      int status = gsl_movstat_apply_accum(endtype, x, gsl_movstat_accum_mad, &scale, xmedian, xmad, w);
      return status;
    }
}


/*****************************************************************************
*
* Rokko: Integrated Interface for libraries of eigenvalue decomposition
*
* Copyright (C) 2012-2020 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_CSCALAPACK_H
#define ROKKO_CSCALAPACK_H

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

#ifdef __cplusplus
extern "C" {
#endif

int cscalapack_descinit(int* desc, int m, int n, int mb, int nb, int irsrc, int icsrc,
                        int ictxt, int lld);

int cscalapack_indxg2p(int indxglob, int nb, int iproc, int isrcproc, int nprocs);

int cscalapack_numroc(int n, int nb, int iproc, int isrcproc, int nprocs);

double cscalapack_pdelget(char scope, char top, const double* A, int ia, int ja, const int* descA);

void cscalapack_pdelset(double* A, int ia, int ja, const int* descA, double alpha);

double cscalapack_pdlamch(int icnt, char cmch);

double cscalapack_pdlange(char norm, int m, int n, const double* A, const int* descA);

double cscalapack_pdlange_work(char norm, int m, int n, const double* A, const int* descA,
                               double* work);

void cscalapack_pdlaprnt(int m, int n, const double* A, int ia, int ja, const int* descA,
                         int irprnt, int icprnt, const char* cmatnm, int nout, double* work);

#define CSCALAPACK_PSYEV_DECL(NAMES, NAMEL, TYPE, TYPE_REAL) \
int cscalapack_ ## NAMES ## _work (char jobz, char uplo, int n, \
                                   TYPE* A, int ia, int ja, const int* descA, \
                                   TYPE_REAL* w, TYPE* Z, int iz, int jz, const int* descZ, \
                                   TYPE* work, int lwork); \
int cscalapack_ ## NAMES (char jobz, char uplo, int n, \
                          TYPE* A, int ia, int ja, const int* descA, \
                          TYPE_REAL* w, TYPE* Z, int iz, int jz, const int* descZ);

CSCALAPACK_PSYEV_DECL(pssyev, PSSYEV, float, float);
CSCALAPACK_PSYEV_DECL(pdsyev, PDSYEV, double, double);
CSCALAPACK_PSYEV_DECL(pcheev, PCHEEV, lapack_complex_float, float);
CSCALAPACK_PSYEV_DECL(pzheev, PZHEEV, lapack_complex_double, double);

#undef CSCALAPACK_PSYEV_DECL

#define CSCALAPACK_PSYEVD_DECL(NAMES, NAMEL, TYPE, TYPE_REAL) \
int cscalapack_ ## NAMES ## _work (char jobz, char uplo, int n, \
                                   TYPE* A, int ia, int ja, const int* descA, \
                                   TYPE_REAL* w, TYPE* Z, int iz, int jz, const int* descZ, \
                                   TYPE* work, int lwork, int* iwork, int liwork); \
int cscalapack_ ## NAMES (char jobz, char uplo, int n, \
                          TYPE* A, int ia, int ja, const int* descA, \
                          TYPE_REAL* w, TYPE* Z, int iz, int jz, const int* descZ);

CSCALAPACK_PSYEVD_DECL(pssyevd, PSSYEVD, float, float);
CSCALAPACK_PSYEVD_DECL(pdsyevd, PDSYEVD, double, double);
CSCALAPACK_PSYEVD_DECL(pcheevd, PCHEEVD, lapack_complex_float, float);
CSCALAPACK_PSYEVD_DECL(pzheevd, PZHEEVD, lapack_complex_double, double);

#undef CSCALAPACK_PSYEVD_DECL

#define CSCALAPACK_PSYEVX_DECL(NAMES, NAMEL, TYPE, TYPE_REAL) \
int cscalapack_ ## NAMES ## _work(char jobz, char range, char uplo, int n, \
                                  TYPE* A, int ia, int ja, const int* descA, \
                                  TYPE_REAL vl, TYPE_REAL vu, int il, int iu, \
                                  TYPE_REAL abstol, int* m, int* nZ, TYPE_REAL* w, TYPE_REAL orfac, \
                                  TYPE* Z, int iz, int jz, const int* descZ, \
                                  TYPE* work, int lwork, int* iwork, int liwork, \
                                  int* ifail, int* iclustr, TYPE_REAL* gap); \
int cscalapack_ ## NAMES (char jobz, char range, char uplo, int n, \
                          TYPE* A, int ia, int ja, const int* descA, \
                          TYPE_REAL vl, TYPE_REAL vu, int il, int iu, \
                          TYPE_REAL abstol, int* m, int* nZ, TYPE_REAL* w, TYPE_REAL orfac, \
                          TYPE* Z, int iz, int jz, const int* descZ, \
                          int* ifail, int* iclustr, TYPE_REAL* gap);

CSCALAPACK_PSYEVX_DECL(pssyevx, PSSYEVX, float, float);
CSCALAPACK_PSYEVX_DECL(pdsyevx, PDSYEVX, double, double);
CSCALAPACK_PSYEVX_DECL(pcheevx, PCHEEVX, lapack_complex_float, float);
CSCALAPACK_PSYEVX_DECL(pzheevx, PZHEEVX, lapack_complex_double, double);

#undef CSCALAPACK_PSYEVX_DECL

#ifdef ROKKO_HAVE_SCALAPACK_PDSYEVR
#define CSCALAPACK_PSYEVR_DECL(NAMES, NAMEL, TYPE, TYPE_REAL) \
int cscalapack_## NAMES ##_work(char jobz, char range, char uplo, int n, \
                                TYPE* A, int ia, int ja, const int* descA, \
                                TYPE_REAL vl, TYPE_REAL vu, int il, int iu, \
                                int* m, int* nz, \
                                TYPE_REAL* w, TYPE* Z, int iz, int jz, const int* descZ, \
                                TYPE* work, int lwork, int* iwork, int liwork); \
int cscalapack_## NAMES (char jobz, char range, char uplo, int n, \
                         TYPE* A, int ia, int ja, const int* descA, \
                         TYPE_REAL vl, TYPE_REAL vu, int il, int iu, \
                         int* m, int* nz, \
                         TYPE_REAL* w, TYPE* Z, int iz, int jz, const int* descZ);

CSCALAPACK_PSYEVR_DECL(pssyevr, PSSYEVR, float, float);
CSCALAPACK_PSYEVR_DECL(pdsyevr, PDSYEVR, double, double);
CSCALAPACK_PSYEVR_DECL(pcheevr, PCHEEVR, lapack_complex_float, float);
CSCALAPACK_PSYEVR_DECL(pzheevr, PZHEEVR, lapack_complex_double, double);

#undef CSCALAPACK_PSYEVR_DECL
#endif

float cscalapack_pselget(char scope, char top, const float* A, int ia, int ja, const int* descA);

void cscalapack_pselset(float* A, int ia, int ja, const int* descA, float alpha);

float cscalapack_pslamch(int icnt, char cmch);

float cscalapack_pslange(char norm, int m, int n, const float* A, const int* descA);

float cscalapack_pslange_work(char norm, int m, int n, const float* A, const int* descA,
                              float* work);

void cscalapack_pslaprnt(int m, int n, const float* A, int ia, int ja, const int* descA,
                         int irprnt, int icprnt, const char* cmatnm, int nout, float* work);

#define CSCALAPACK_PSTEBZ_DECL(NAMES, NAMEL, TYPE) \
int cscalapack_## NAMES ##_work(int ictxt, char range, char order, int n, \
                                TYPE vl, TYPE vu, int il, int iu, \
                                TYPE abstol, const TYPE* d, const TYPE* e, int* m, int* nsplit, \
                                TYPE* w, int* iblock, int* isplit, \
                                TYPE* work, int lwork, int* iwork, int liwork); \
int cscalapack_## NAMES (int ictxt, char range, char order, int n, \
                         TYPE vl, TYPE vu, int il, int iu, \
                         TYPE abstol, const TYPE* d, const TYPE* e, int* m, int* nsplit, \
                         TYPE* w, int* iblock, int* isplit);

CSCALAPACK_PSTEBZ_DECL(psstebz, PSSTEBZ, float);
CSCALAPACK_PSTEBZ_DECL(pdstebz, PDSTEBZ, double);

#undef CSCALAPACK_PSTEBZ_DECL

#define CSCALAPACK_PSTEIN_DECL(NAMES, NAMEL, TYPE, TYPE_REAL) \
int cscalapack_## NAMES ##_work(int n, const TYPE_REAL* d, const TYPE_REAL* e, int m, \
                                TYPE_REAL* w, const int* iblock, const int* isplit, TYPE_REAL orfac, \
                                TYPE* Z, const int* iZ, const int* jZ, const int* descZ, \
                                TYPE* work, int lwork, int* iwork, int liwork, \
                                int* ifail, int* iclustr, TYPE_REAL* gap); \
int cscalapack_## NAMES (int n, const TYPE_REAL* d, const TYPE_REAL* e, int m, \
                         TYPE_REAL* w, const int* iblock, const int* isplit, TYPE_REAL orfac, \
                         TYPE* Z, const int* iZ, const int* jZ, const int* descZ, \
                         int* ifail, int* iclustr, TYPE_REAL* gap);

CSCALAPACK_PSTEIN_DECL(psstein, PSSTEIN, float, float);
CSCALAPACK_PSTEIN_DECL(pdstein, PDSTEIN, double, double);
CSCALAPACK_PSTEIN_DECL(pcstein, PCSTEIN, lapack_complex_float, float);
CSCALAPACK_PSTEIN_DECL(pzstein, PZSTEIN, lapack_complex_double, double);

#undef CSCALAPACK_PSTEIN_DECL

#define CSCALAPACK_PSTEDC_DECL(NAMES, NAMEL, TYPE) \
int cscalapack_## NAMES ##_work(char compz, int n, TYPE* d, TYPE* e, \
                                TYPE* Q, int iq, int jq, const int* descQ, \
                                TYPE* work, int lwork, int* iwork, int liwork); \
int cscalapack_## NAMES (char compz, int n, TYPE* d, TYPE* e, \
                         TYPE* Q, int iq, int jq, const int* descQ);

CSCALAPACK_PSTEDC_DECL(psstedc, PSSTEDC, float);
CSCALAPACK_PSTEDC_DECL(pdstedc, PDSTEDC, double);

#undef CSCALAPACK_PSTEDC_DECL

#ifdef __cplusplus
}
#endif

#endif // ROKKO_CSCALAPACK_H


/* chii - CHIsq test integers belonging to a given contiguous range. C
 * not R because a) a second opinion is good and b) I'm too lazy to
 * figure out how to make R honor sparse input, where a particular
 * bucket might have no values. otherwise, use the "chisq.test" in R or
 * the "equichisq" arlet of my r-fu script
 *
 * the following should fail, as there are six buckets (implicit 0 for
 * minimum through 5, inclusive) and the Perl only produces numbers for
 * five of those buckets (0..4)
 *
 *   perl -E 'for (1..1000) { say int rand 5}' | chii -M 5
 *
 * (it could also fail for reasons unrelated to the missing bucket, or
 * it could pass if there are not enough trials to reveal the lack)
 *
 * if you are actually testing the fitness of non-cryptographic hash
 * functions, consider instead the "smhasher" project */

#include <ctype.h>
#include <err.h>
#include <errno.h>
#include <getopt.h>
#include <locale.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sysexits.h>
#include <unistd.h>

// https://github.com/thrig/goptfoo
#include <goptfoo.h>

// GNU Science Library
#include <gsl/gsl_cdf.h>

double Flag_Fitness = 0.05;     // -F @ 95% confidence by default
long long Flag_Min;             // -m
long long Flag_Max;             // -M

double chisq(long long valuec, long long *values, long long minv,
             long long maxv, long long sum);
void emit_help(void);

int main(int argc, char *argv[])
{
    int ch;

    char *ep;
    long long number_of_nums = 0;
    long long range, *numstore, value;

    char *line = NULL;
    char *lp;
    FILE *fh;
    size_t linesize = 0;
    ssize_t linelen;
    ssize_t lineno = 1;

    double pvalue;

#ifdef __OpenBSD__
    if (pledge("stdio", NULL) == -1)
        err(1, "pledge failed");
#endif

    setlocale(LC_ALL, "C");

    while ((ch = getopt(argc, argv, "F:h?m:M:")) != -1) {
        switch (ch) {
        case 'F':
            Flag_Fitness = flagtod(ch, optarg, 0.0, 1.0);
            break;
        case 'm':
            Flag_Min = flagtoll(ch, optarg, LLONG_MIN, LLONG_MAX);
            break;
        case 'M':
            Flag_Max = flagtoll(ch, optarg, LLONG_MIN, LLONG_MAX);
            break;
        case 'h':
        case '?':
        default:
            emit_help();
            /* NOTREACHED */
        }
    }
    argc -= optind;
    argv += optind;

    if ((Flag_Min == 0 && Flag_Max == 0) || Flag_Min == Flag_Max
        || Flag_Min > Flag_Max)
        emit_help();

    if (argc == 0 || strncmp(*argv, "-", (size_t) 2) == 0) {
        fh = stdin;
    } else {
        if (!(fh = fopen(*argv, "r")))
            err(EX_IOERR, "could not open '%s'", *argv);
    }

    // inclusive, need buckets for all possible numbers
    range = Flag_Max - Flag_Min + 1;

    if (!(numstore = calloc((size_t) range, sizeof(long long))))
        err(EX_OSERR, "could not calloc() for %lld long longs", range);

    while ((linelen = getline(&line, &linesize, fh)) != -1) {
        // so can parse multiple numbers per line, assuming they are
        // isspace(3) delimited (downside: complicates the parsing)
        lp = line;
        while (*lp != '\0') {
            errno = 0;
            value = strtoll(lp, &ep, 10);
            if (*lp == '\0')
                errx(EX_DATAERR, "could not parse long long at line %ld",
                     lineno);
            if (errno == ERANGE && (value == LLONG_MIN || value == LLONG_MAX))
                errx(EX_DATAERR, "value out of range at line %ld", lineno);
            if (value < Flag_Min || value > Flag_Max)
                errx(EX_DATAERR, "value beyond bounds at line %ld", lineno);

            numstore[value - Flag_Min]++;
            number_of_nums++;

            // nom up not-strtoll() material so parser does not get wedged
            // on trailing material such as `echo -n 0 1 2 3 4" "`
            while (*ep != '+' && *ep != '-' && !isdigit(*ep)) {
                if (*ep == '\0' || *ep == '\n') {
                    *ep = '\0';
                    break;
                }
                ep++;
            }
            lp = ep;
        }
        lineno++;
    }
    if (number_of_nums == 0)
        errx(EX_DATAERR, "could not parse any numbers");
    if (ferror(fh))
        err(EX_IOERR, "ferror() reading input");

    pvalue = chisq(range, numstore, Flag_Min, Flag_Max, number_of_nums);
    printf("p-value %.6g%s\n", pvalue, pvalue <= Flag_Fitness ? " FAIL" : "");

    exit(EXIT_SUCCESS);
}

double chisq(long long valuec, long long *values, long long minv,
             long long maxv, long long sum)
{
    unsigned long delta, v;
    double expected, sum_of_squares;

    delta = maxv - minv + 1;
    expected = sum / (double) delta;

    sum_of_squares = 0.0;
    for (v = 0; v < valuec; v++) {
        sum_of_squares += pow(values[v] - expected, 2.0) / expected;
    }
    return gsl_cdf_chisq_Q(sum_of_squares, (double) delta - 1.0);
}

void emit_help(void)
{
    fputs("Usage: chii [-F fit] -m min -M max [file|-]\n", stderr);
    exit(EX_USAGE);
}


/*
 *  emu.c
 *  Edited and renamed to fr_emu.c for cosmo_pmc by Martin Kilbinger 2013.
 *
 *
 *  Created by Earl Lawrence on 9/17/09.
 *  Update 10/9/2012
 *
 *  This program was prepared by Los Alamos National Security, LLC at Los Alamos National Laboratory (LANL)
 *  under contract No. DE-AC52-06NA25396 with the U.S. Department of Energy (DOE). All rights in the program
 *  are reserved by the DOE and Los Alamos National Security, LLC.  Permission is granted to the public to
 *  copy and use this software without charge, provided that this Notice and any statement of authorship are
 *  reproduced on all copies.  Neither the U.S. Government nor LANS makes any warranty, express or implied,
 *  or assumes any liability or responsibility for the use of this software.
 *
 *
 */

#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_errno.h>

#include "coyote.h"
#include "fr_constants.h"

/* Copy parameters to emulator input array */
void fill_xstar6_wo_z(double omega_m, double omega_b, double n_spec, double sigma_8, double w0_de, double h_100,
                      double xstar[])
{
   /* Orders reversed:  (omega_b <-> omega_m), (w0_de <-> sigma_8), compared to v1 */
   xstar[0] = omega_b;
   xstar[1] = omega_m;
   xstar[2] = n_spec;
   xstar[3] = h_100 * 100;
   xstar[4] = w0_de;
   xstar[5] = sigma_8;
}

/* ============================================================ *
 * Returns the Coyote non-linear power spectrum. k is in units  *
 * of h/Mpc, h_100 is now an independent parameter (Coyote v2). *
 * It is also used to transform it to [1/Mpc] as                *
 * needed by the Coyote emulator.                               *
 * Six parameters, for coyote13.				*
 * ============================================================ */
double P_NL_coyote6(double omega_m, double omega_b, double n_spec, double sigma_8, double w0_de,
		    double h_100, double a, double k, double **ystar_allz, error **err)
{
   double xstar[p], ystar[2*fr_nsim], val, k_invMpc;
   int ihi, ilo, i;

   /* Copy cosmological parameters */
   fill_xstar6_wo_z(omega_m, omega_b, n_spec, sigma_8, w0_de, h_100, xstar);

   /* Redshift */
   xstar[p] = 1.0/a - 1.0;

   /* Input k is [h/Mpc], Coyote interpolates on k_invMpc in [1/Mpc] */
   k_invMpc  = k * h_100;

   /* Check k-range */
   testErrorRetVA(k_invMpc < fr_ksim[0] || k_invMpc > fr_ksim[fr_nsim-1], coyote_range,
           "Fourier mode k=%g 1/Mpc out of range [%g;%g]",
           *err, __LINE__, -1.0, k_invMpc, fr_ksim[0], fr_ksim[fr_nsim-1]);

   if (*ystar_allz == NULL) {
      *ystar_allz = malloc_err(sizeof(double) * fr_rs * fr_nsim, err);
      forwardError(*err, __LINE__, -1.0);

      /* The actual emulator, filling the array in (k, z) */
      //testErrorRet(ystar_allz == NULL, io_null, "Coyote13: array for (k,z) 'ystar_allz' not initialised",
      //	   *err, __LINE__,);
      fr_fill_ystar_allz(*ystar_allz, xstar, err);
      forwardError(*err, __LINE__, -1.0);
   }

   /* ystar: first half = k, second half = P(k) */
   fr_emu(xstar, ystar, *ystar_allz, err);
   forwardError(*err, __LINE__, -1.0);

   /* Looking for right k-index with bisection */
   ilo = 0;
   ihi = fr_nsim-1;
   while (ihi - ilo > 1) {
      i = (ihi + ilo) >> 1;
      if (ystar[i] > k_invMpc) ihi = i;
      else ilo = i;
   }
   testErrorRetVA(ihi == ilo, math_wrongValue, "Bisection failed, both indices equal (%d)", *err, __LINE__, -1.0, ihi);

   /* Linear interpolation */
   val = (k_invMpc - ystar[ilo]) / (ystar[ihi] - ystar[ilo]) * (ystar[fr_nsim + ihi]
         - ystar[fr_nsim + ilo]) + ystar[fr_nsim + ilo];

   /* Coyote P(k) [Mpc^3] -> output P(k) [(Mpc/h)^3] */
   val *= h_100 * h_100 * h_100;

   return val;
}

void fr_check_range(const double *xstar, error **err)
{
   int i;

   for(i=0; i<p; i++) {
      testErrorRetVA((xstar[i] < fr_xmin[i]) || (xstar[i] > fr_xmin[i]+fr_xrange[i]), coyote_range,
		     "Coyote FrankenEmu parameter #%d (%g) out of range [%g; %g]",
		     *err, __LINE__,, i, xstar[i], fr_xmin[i], fr_xmin[i] + fr_xrange[i]);
   } // for(i=0; i<p; i++)

   // Check redshift to make sure we're interpolating
   testErrorRetVA((xstar[p] < 0) || (xstar[p] > 4), coyote_range,
		  "Redshift %g out of range [%g;%g]", *err, __LINE__,, xstar[p], 0.0, 4.0);
}

/* ============================================================ *
 * The actual emulation. Fills the array ystar_allz for (k, z). *
 * Cosmological parameters, placeholder for the output, type of *
 * output.
 * xstar[p] = redshift is not used here, only the p=6 cosmo.    *
 * parameters.							*
 * ============================================================ */
void fr_fill_ystar_allz(double *ystar_allz, const double *xstar, error **err)
{
   const double sd = 0.16002;
   int i, j, k;
   double xstarstd[p], wstar[peta], Sigmastar[peta][m], ystaremu[neta];
   double logc;

   // Interpolation stuff for k
   gsl_spline *lininterp_k = gsl_spline_alloc(gsl_interp_linear, neta/rs);
   gsl_interp_accel *accel = gsl_interp_accel_alloc();

   //fprintf(stderr, "MKDEBUG: Filling ystar_allz for (%g, %g, %g, %g, %g, %g)\n",
   //	   xstar[0], xstar[1], xstar[2], xstar[3], xstar[4], xstar[5]);

    // Standardize the inputs
    for(i=0; i<p; i++) {
        xstarstd[i] = (xstar[i] - fr_xmin[i]) / fr_xrange[i];
    }

    // Compute the covariances between the new input and sims for all PCs
    for(i=0; i<peta; i++) {
        for(j=0; j<m; j++) {
            logc = 0.0;
            for(k=0; k<p; k++) {
                logc -= fr_beta[i][k]*dsqr(fr_x[j][k]-xstarstd[k]);
            }
            Sigmastar[i][j] = exp(logc)/fr_lamz[i];
        }
    }

    // Compute wstar, the predicted PC weights for the new input
    for(i=0; i<peta; i++) {
        wstar[i]=0.0;
        for(j=0; j<m; j++) {
            wstar[i] += Sigmastar[i][j] * fr_KrigBasis[i][j];
        }
    }

    // Compute ystar, the new output
    for(i=0; i<neta; i++) {
        ystaremu[i] = 0.0;
        for(j=0; j<peta; j++) {
            ystaremu[i] += fr_K[i][j]*wstar[j];
        }
        ystaremu[i] = ystaremu[i]*sd + fr_mean[i];
    }

    // Interpolate the emulated output onto the original domain.
    for(i=0; i<rs; i++) {
        gsl_spline_init(lininterp_k, fr_kemu, &ystaremu[i*neta/rs], neta/rs);
        for(j=0; j<fr_nsim; j++) {
            ystar_allz[i*fr_nsim+j] = gsl_spline_eval(lininterp_k, fr_ksim[j], accel);
        }
        gsl_interp_accel_reset(accel);
    }

    gsl_spline_free(lininterp_k);
    gsl_interp_accel_free(accel);
}

/* ============================================================ *
 * This used to be the actual emulation, now fr_fill_ystar_allz *
 * is called from here.						*
 * xstar[p+1] contains the six cosmological parameters plus the *
 * redshift.							*
 * ============================================================ */
void fr_emu(const double *xstar, double *ystar, const double *ystar_allz, error **err)
{
    int i, j;
    //double ystar_allz[rs*fr_nsim];
    double zemu[rs], ybyz[rs];

    /* CosmoPMC version of Coyote: no initialisation, no static variables */

    // Interpolation stuff z
    gsl_spline *lininterp_z = gsl_spline_alloc(gsl_interp_linear, rs);
    gsl_interp_accel *accel = gsl_interp_accel_alloc();

    // Check the inputs to make sure we're interpolating.
    fr_check_range(xstar, err);
    forwardError(*err, __LINE__,);

    // Fill in the k values for the final output; first half of ystar
    for(i=0; i<fr_nsim; i++) {
        ystar[i] = fr_ksim[i];
    }

    // Interpolate on to the desired redshift
    // The order needs to be reversed here.
    for(i=0; i<rs; i++) {
        zemu[i] = (1/fr_aemu[rs-i-1]) - 1.0;
    }
    for(i=0; i<fr_nsim; i++) {
        // Build an array with the values of y for a given value of z
        // Reverse the order
        for(j=0; j<rs; j++) {
            ybyz[rs-j-1] = ystar_allz[j*fr_nsim+i];
        }
        gsl_spline_init(lininterp_z, zemu, ybyz, rs);
        ystar[fr_nsim+i] = gsl_spline_eval(lininterp_z, xstar[p], accel);
        gsl_interp_accel_reset(accel);
    }

    //printf("z interped\n");

    // Transform to P(k); second half of ystar
    for(j=0; j<fr_nsim; j++) {
       ystar[fr_nsim+j] = ystar[fr_nsim+j] - 1.5*log10(fr_ksim[j]);
       ystar[fr_nsim+j] = pow(10.0,ystar[fr_nsim+j])*2.0*M_PI*M_PI;
    }

    // Free some stuff.  I always forget this.
    // Thanks to Tim Eifler for discovering it (apparently the hard way).
    gsl_spline_free(lininterp_z);
    gsl_interp_accel_free(accel);
}

/* Undefine constants from fr_constants.h */
#undef m
#undef neta
#undef p
#undef peta
#undef rs


#include <stdio.h>
#include <string.h>
#include <string>
#include <time.h>
#include <math.h>
#include <vector>
#include <algorithm>
#include <iterator>
#include <sys/stat.h>
//#define GSL_DLL
//#include <gsl/gsl_cdf.h> //use gaussian p-value function
using namespace std;
void printProgress(double percentage);


/**
 *
 * @file testing_zgetmi.c
 *
 *  PLASMA testings module
 *  PLASMA is a software package provided by Univ. of Tennessee,
 *  Univ. of California Berkeley and Univ. of Colorado Denver
 *
 *  This program tests the implementation of the inplace format
 *  conversion based on the GKK algorithm by Gustavson, Karlsson,
 *  Kagstrom.
 *
 * @version 2.6.0
 * @author Mathieu Faverge
 * @date 2010-11-15
 *
 * @precisions normal z -> c d s
 *
 * Purpose :
 *    Test the in place matrix transposition with the 6 different
 *    formats of input.
 **/

#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <lapacke.h>
#include <plasma.h>
#include "testing_zmain.h"

static int check_solution(int m, int n, int mb, int nb,
                   PLASMA_Complex64_t *A, PLASMA_Complex64_t *B,
                   int (*mapA)(int, int, int, int, int, int)) {
    int i, j;

    for( j=0; j<n; j++) {
        for (i=0; i<m; i++) {
            if (A[ mapA(m, n, mb, nb, i, j) ] != B[ mapA(n, m, mb, nb, j, i) ] ) {
                return -1;
            }
        }
    }
    return 0;
}

int testing_zgetmi(int argc, char **argv){

    PLASMA_Complex64_t *A, *B;
    int m, n, mb, nb;
    int i, ret, size;

    /* Check for number of arguments*/
    if (argc != 4){
        USAGE("GETMI", "M N MB NB ntdbypb with \n",
              "   - M       : the number of rows of the matrix    \n"
              "   - N       : the number of columns of the matrix \n"
              "   - MB      : the number of rows of each block    \n"
              "   - NB      : the number of columns of each block \n");
        return -1;
    }

    m      = atoi(argv[0]);
    n      = atoi(argv[1]);
    mb     = atoi(argv[2]);
    nb     = atoi(argv[3]);

    size = m*n*sizeof(PLASMA_Complex64_t);
    A = (PLASMA_Complex64_t *)malloc(size);
    B = (PLASMA_Complex64_t *)malloc(size);
    LAPACKE_zlarnv_work(1, ISEED, m*n, A);

    for(i=0; i<6; i++) {
        memcpy(B, A, size);

        printf(" - TESTING ZGETMI (%4s) ...", formatstr[i]);
        ret = PLASMA_zgetmi( m, n, A, format[i], mb, nb );

        if (ret != PLASMA_SUCCESS) {
            printf("Failed\n");
            continue;
        }

        if ( check_solution(m, n, mb, nb, B, A,
                            (int (*)(int, int, int, int, int, int))formatmap[i]) == 0 )
            printf("............ PASSED !\n");
        else
            printf("... FAILED !\n");
    }

    free( A ); free( B );

    return 0;
}


/******************************************************************************
 *                                                                            *
 * PROBLEM.C                                                                  *
 *                                                                            *
 * INITIAL CONDITIONS FOR BONDI INFLOW                                        *
 *                                                                            *
 ******************************************************************************/

#include "decs.h"

// Local functions
void coord_transform(double *Pr, int i, int j);
double lfish_calc(double rmax) ;

void set_problem_params()
{
}

// Rootfinding for analytic Bondi solution
#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_roots.h>
struct params {
  double r, mdot, n, C1, C2;
};
double rfunc(double T, void *params)
{
  struct params *p = (struct params *) params;
  //double r = p->r;
  double r = MY_MAX(2.5, p->r); // Solution breaks inside event horizon
  double mdot = p->mdot;
  double n = p->n;
  double C1 = p->C1;
  double C2 = p->C2;

  double resid = (pow(1. + (1. + n)*T,2)*(1. - 2.*mdot/r + pow(C1,2)/
         (pow(r,4)*pow(T,2*n))) - C2);

  //printf("r = %e T = %e resid = %e\n", r, T, resid);
  return resid;
  //return (pow(1. + (1. + n)*T,2)*(1. - 2.*mdot/r + pow(C1,2)/
  //       (pow(r,4)*pow(T,2*n))) - C2);
}

double C1, C2, T_bondi[N1+2*NG][N2+2*NG], n;

// Adapted from M. Chandra
double get_Tfunc(double T, double r)
{
  return pow(1.+(1.+n)*T,2.)*(1.-2./r+pow(C1/r/r/pow(T,n),2.))-C2;
}

double get_T(double r)
{
  double rtol = 1.e-12;
  double ftol = 1.e-14;
  double Tmin = 0.6*(sqrt(C2) - 1.)/(n + 1);
  double Tmax = pow(C1*sqrt(2./r/r/r),1./n);
  double f0, f1, fh;
  double T0, T1, Th;
  T0 = 0.6*Tmin;
  f0 = get_Tfunc(T0, r);
  T1 = Tmax;
  f1 = get_Tfunc(T1, r);

  if (f0*f1 > 0.) {
    printf("Failed solving for T at r = %e C1 = %e C2 = %e\n", r, C1, C2);
    exit(-1);
  }

  Th = (f1*T0 - f0*T1)/(f1 - f0);
  fh = get_Tfunc(Th, r);
  double epsT = rtol*(Tmin + Tmax);
  while (fabs(Th - T0) > epsT && fabs(Th - T1) > epsT && fabs(fh) > ftol) {
    if (fh*f0 < 0.) {
      T0 = Th;
      f0 = fh;
    } else {
      T1 = Th;
      f1 = fh;
    }

    Th = (f1*T0 - f0*T1)/(f1 - f0);
    fh = get_Tfunc(Th, r);
  }

  return Th;
}

void bl_to_ks(double X[NDIM], double ucon_bl[NDIM], double ucon_ks[NDIM])
{
  double r, th;
  bl_coord(X, &r, &th);

  double trans[NDIM][NDIM];
  DLOOP2 trans[mu][nu] = 0.;
  DLOOP1 trans[mu][mu] = 1.;
  trans[0][1] = 2.*r/(r*r - 2.*r + a*a);
  trans[3][1] = a/(r*r - 2.*r + a*a);

  DLOOP1 ucon_ks[mu] = 0.;
  DLOOP2 ucon_ks[mu] += trans[mu][nu]*ucon_bl[nu];
}

void fourvel_to_prim(double ucon[NDIM], double prim[NVAR],
  struct of_geom *geom)
{
  double alpha, beta[NDIM], gamma;
  alpha = 1.0/sqrt(-geom->gcon[0][0]);
  beta[1] = alpha*alpha*geom->gcon[0][1];
  beta[2] = alpha*alpha*geom->gcon[0][2];
  beta[3] = alpha*alpha*geom->gcon[0][3];
  gamma = ucon[0]*alpha;

  prim[U1] = ucon[1] + beta[1]*gamma/alpha;
  prim[U2] = ucon[2] + beta[2]*gamma/alpha;
  prim[U3] = ucon[3] + beta[3]*gamma/alpha;
}

void set_ut(double ucon[NDIM], struct of_geom *geom)
{
  double AA, BB, CC;

  AA = geom->gcov[0][0];
  BB = 2.*(geom->gcov[0][1]*ucon[1] +
           geom->gcov[0][2]*ucon[2] +
           geom->gcov[0][3]*ucon[3]);
  CC = 1. + geom->gcov[1][1]*ucon[1]*ucon[1] +
            geom->gcov[2][2]*ucon[2]*ucon[2] +
            geom->gcov[3][3]*ucon[3]*ucon[3] +
       2. *(geom->gcov[1][2]*ucon[1]*ucon[2] +
            geom->gcov[1][3]*ucon[1]*ucon[3] +
            geom->gcov[2][3]*ucon[2]*ucon[3]);

  double discr = BB*BB - 4.*AA*CC;
  ucon[0] = (-BB - sqrt(discr))/(2.*AA);
}

void get_prim_bondi(int i, int j, int k, double P[NVAR])
{
  double r, th, X[NDIM];
  coord(i, j, k, CENT, X);
  bl_coord(X, &r, &th);

  while (r < Reh) {
    i++;
    coord(i, j, k, CENT, X);
    bl_coord(X, &r, &th);
  }

  double T = T_bondi[i][j];
  double ur = -C1/(pow(T,n)*pow(r,2));
  double rho = pow(T,n);
  double u = rho*T/(gam - 1.);
  double ucon_bl[NDIM], ucon_ks[NDIM], ucon_mks[NDIM];
  struct of_geom geom_bl, *geom_mks;

  blgset(i, j, &geom_bl);

  DLOOP1 {
    ucon_bl[mu] = 0.;
    ucon_ks[mu] = 0.;
    ucon_mks[mu] = 0.;
  }
  ucon_bl[1] = ur;

  set_ut(ucon_bl, &geom_bl);
  bl_to_ks(X, ucon_bl, ucon_ks);

  double dxdX[NDIM][NDIM], dXdx[NDIM][NDIM];
  set_dxdX(X, dxdX);
  invert(&dxdX[0][0], &dXdx[0][0]);
  DLOOP2 {
    ucon_mks[mu] += dXdx[mu][nu]*ucon_ks[nu];
  }

  geom_mks = get_geometry(i, j, k, CENT);
  fourvel_to_prim(ucon_mks, P, geom_mks);
  //printf("[%i %i %i] r = %e ucons = %e %e %e vr = %e\n",
  //  i, j, k, r, ucon_bl[1], ucon_ks[1], ucon_mks[1], P[U1]);

  P[RHO] = rho;
  P[UU] = u;
  P[B1] = 0.;
  P[B2] = 0.;
  P[B3] = 0.;

  /*if (j == N2/2){
    printf("\nu[1]: %e %e %e\n", ucon_bl[1], ucon_ks[1], ucon_mks[1]);
  printf("T rho ur u = %e %e %e %e\n", T, rho, ur, u);
  printf("P[] = %e %e %e %e\n", P[0], P[1], P[2], P[3]);
  }*/

  /*double gcov[NDIM][NDIM];
  bl_gcov_func(r, th, gcov);
  double ut = -sqrt((-1. - gcov[1][1]*ur*ur)/gcov[0][0]);
  double ucon[NDIM] = {ut, ur, 0, 0};

//  if ( j == NG)
//  printf("ucon[] = %e %e %e %e\n", ucon[0], ucon[1], ucon[2], ucon[3]);

  double trans[NDIM][NDIM];
  // transform to Kerr-Schild
  // Make transform matrix
  memset(trans, 0, 16*sizeof(double));
  for (int mu = 0; mu < NDIM; mu++) {
    trans[mu][mu] = 1.;
  }
  trans[0][1] = 2. * r / (r * r - 2. * r + a * a);
  trans[3][1] = a / (r * r - 2. * r + a * a);

  // Transform from BL to KS coordinates
  double tmp[NDIM];
  for (int mu = 0; mu < NDIM; mu++) tmp[mu] = 0.;
  for (int mu = 0; mu < NDIM; mu++) {
    for (int nu = 0; nu < NDIM; nu++) {
    tmp[mu] += trans[mu][nu]*ucon[nu];
    }
  }
  for (int mu = 0; mu < NDIM; mu++) ucon[mu] = tmp[mu];

  // Transform from KS to MKS coordinates
  set_dxdX(X, trans);
  for (int mu = 0; mu < NDIM; mu++) tmp[mu] = 0.;
  for (int mu = 0; mu < NDIM; mu++) {
    for (int nu = 0; nu < NDIM; nu++) {
    tmp[mu] += trans[mu][nu]*ucon[nu];
    }
  }
  for (int mu = 0; mu < NDIM; mu++) ucon[mu] = tmp[mu];

  struct of_geom *geom = get_geometry(i, j, k, CENT);
  double alpha = 1.0 / sqrt( -geom->gcon[0][0] ) ;
  double gamma = ucon[0] * alpha;

  double beta[NDIM];
  beta[1] = alpha * alpha * geom->gcon[0][1];
  beta[2] = alpha * alpha * geom->gcon[0][2];
  beta[3] = alpha * alpha * geom->gcon[0][3];

  P[RHO] = rho;
  P[UU] = u;
  P[U1] = ucon[1] + beta[1] * gamma / alpha;
  P[U2] = 0.;
  P[U3] = 0.;
  P[B1] = 0.;
  P[B2] = 0.;
  P[B3] = 0.;
  if (r < 5.) P[U1] = 0.;

  //P[U1] *= 0.;*/
}

/*void get_bondi_prim(double r, double P[NDIM])
{
  double T, rho, ur, ut;
  if (r > 2. + 1.e-8) {
    T = get_T(r);
    rho = pow(T, n);
    ur = -C1/r/r/pow(T,n);
    ut = (2.*ur/r+sqrt(ur*ur+1.-2./r))/(1.-2./r);
  } else {
    T = 1.;
    rho = RHOMIN;
    ur = 0.;
    ut = 1.;
  }

  P[RHO] = rho;
  P[UU] = n*rho*T;
  P[U1] = (ur + 2.*ut/(r + 2.))/r;
  P[U2] = 0.;
  P[U3] = 0.;
  P[B1] = 0.;
  P[B2] = 0.;
  P[B3] = 0.;
}*/

void init_prob()
{
  double r, th, X[NDIM];

  double mdot = 1.;
  double rs = 8.;
  n = 1./(gam - 1.);

  // Solution constants
  double uc = sqrt(mdot/(2.*rs));
  double Vc = -sqrt(pow(uc,2)/(1. - 3.*pow(uc,2)));
  double Tc = -n*pow(Vc,2)/((n + 1.)*(n*pow(Vc,2) - 1.));
  C1 = uc*pow(rs,2)*pow(Tc,n);
  C2 = pow(1. + (1. + n)*Tc,2)*(1. - 2.*mdot/rs + pow(C1,2)/
       (pow(rs,4)*pow(Tc,2*n)));

  printf("a = %e Reh = %e\n", a, Reh);

  printf("mdot = %e\n", mdot);
  printf("rs   = %e\n", rs);
  printf("n    = %e\n", n);
  printf("uc   = %e\n", uc);
  printf("Vc   = %e\n", Vc);
  printf("Tc   = %e\n", Tc);
  printf("C1   = %e\n", C1);
  printf("C2   = %e\n", C2);

  ZSLOOP(-NG,N1+NG-1,-NG,N2+NG-1,-NG,N3+NG-1) {
    coord(i, j, k, CENT, X);
    bl_coord(X, &r, &th);

    T_bondi[i][j] = get_T(r);
  }

  ZLOOP {
    get_prim_bondi(i, j, k, P[i][j][k]);
  } // ZSLOOP
}

// Convert Boyer-Lindquist four-velocity to MKS 3-velocity
void coord_transform(double *Pr, int ii, int jj)
{
  double X[NDIM], r, th, ucon[NDIM], trans[NDIM][NDIM], tmp[NDIM];
  double AA, BB, CC, discr;
  double alpha, gamma, beta[NDIM];
  struct of_geom *geom, blgeom;

  coord(ii, jj, 0, CENT, X);
  bl_coord(X, &r, &th);
  blgset(ii, jj, &blgeom);

  ucon[1] = Pr[U1];
  ucon[2] = Pr[U2];
  ucon[3] = Pr[U3];

  AA = blgeom.gcov[0][0];
  BB = 2. * (blgeom.gcov[0][1] * ucon[1] +
       blgeom.gcov[0][2] * ucon[2] +
       blgeom.gcov[0][3] * ucon[3]);
  CC = 1. +
      blgeom.gcov[1][1] * ucon[1] * ucon[1] +
      blgeom.gcov[2][2] * ucon[2] * ucon[2] +
      blgeom.gcov[3][3] * ucon[3] * ucon[3] +
      2. * (blgeom.gcov[1][2] * ucon[1] * ucon[2] +
      blgeom.gcov[1][3] * ucon[1] * ucon[3] +
      blgeom.gcov[2][3] * ucon[2] * ucon[3]);

  discr = BB * BB - 4. * AA * CC;
  ucon[0] = (-BB - sqrt(discr)) / (2. * AA);
  // This is ucon in BL coords

  // transform to Kerr-Schild
  // Make transform matrix
  memset(trans, 0, 16*sizeof(double));
  for (int mu = 0; mu < NDIM; mu++) {
    trans[mu][mu] = 1.;
  }
  trans[0][1] = 2. * r / (r * r - 2. * r + a * a);
  trans[3][1] = a / (r * r - 2. * r + a * a);

  // Transform from BL to KS coordinates
  for (int mu = 0; mu < NDIM; mu++) tmp[mu] = 0.;
  for (int mu = 0; mu < NDIM; mu++) {
    for (int nu = 0; nu < NDIM; nu++) {
     tmp[mu] += trans[mu][nu]*ucon[nu];
    }
  }
  for (int mu = 0; mu < NDIM; mu++) ucon[mu] = tmp[mu];

  // Transform from KS to MKS coordinates
  set_dxdX(X, trans);
  for (int mu = 0; mu < NDIM; mu++) tmp[mu] = 0.;
  for (int mu = 0; mu < NDIM; mu++) {
    for (int nu = 0; nu < NDIM; nu++) {
     tmp[mu] += trans[mu][nu]*ucon[nu];
    }
  }
  for (int mu = 0; mu < NDIM; mu++) ucon[mu] = tmp[mu];

  //ucon[1] *= (1. / (r - R0));
  //ucon[1] /= dr_dx(X[1]);
  //ucon[2] *=
  //    (1. / (M_PI + (1. - hslope) * M_PI * cos(2. * M_PI * X[2])));
  //ucon[2] /= dth_dx(X[2]);

  // Solve for v. Use same u^t, unchanged under KS -> KS'
  geom = get_geometry(ii, jj, 0, CENT) ;
  alpha = 1.0 / sqrt( -geom->gcon[0][0] ) ;
  gamma = ucon[0] * alpha;

  beta[1] = alpha * alpha * geom->gcon[0][1];
  beta[2] = alpha * alpha * geom->gcon[0][2];
  beta[3] = alpha * alpha * geom->gcon[0][3];

  Pr[U1] = ucon[1] + beta[1] * gamma / alpha;
  Pr[U2] = ucon[2] + beta[2] * gamma / alpha;
  Pr[U3] = ucon[3] + beta[3] * gamma / alpha;
}

double lfish_calc(double r)
{
  return (((pow(a, 2) - 2. * a * sqrt(r) + pow(r, 2)) *
     ((-2. * a * r *
       (pow(a, 2) - 2. * a * sqrt(r) +
        pow(r,
      2))) / sqrt(2. * a * sqrt(r) + (-3. + r) * r) +
      ((a + (-2. + r) * sqrt(r)) * (pow(r, 3) + pow(a, 2) *
      (2. + r))) / sqrt(1 + (2. * a) / pow (r, 1.5) - 3. / r)))
    / (pow(r, 3) * sqrt(2. * a * sqrt(r) + (-3. + r) * r) *
       (pow(a, 2) + (-2. + r) * r))
      );
}

void bound_gas_prob_x1r(int i, int j, int k, grid_prim_type P)
{
  get_prim_bondi(i, j, k, P[i][j][k]);
}