#include <stdio.h>
#include "sweeny_uf.h"
#include <ctype.h>
#include <strings.h>
#include <stdio.h>
#include "../src/uf.h"
#include "../src/queue.h"
#include <math.h>
#include <gsl/gsl_rng.h> //Verwendung von Mersenne-Twister Pseudozufallszahlengenerator
static __s8 dN;

#define MIN(a,b) a <= b ? a: b
#define MAX(a,b) a >= b? a: b

static char setup=0;
static char verbose=0;
static double rcWeights[4]; // array with precalculated mc weights
static double p_min_del,p_max_del, p_min_ins,p_max_ins;
static struct queue collect1,collect2;
static struct queue_node *todo_pool, *collect_pool;
static __u32 offset_1=1,offset_2=2;
static __u64 *sec_cs_moment,*four_cs_moment;
static __u32 *size_giant;
static __u32 *num_bonds;
static __u32 *num_cluster;
static __u32 DX;
static __u32 N;
static __u32 seed;
static double q;
static double coupling;
static double beta;
static double v;
static double K;
static __u32 steps;
static __s8 *bonds;
static gsl_rng * r;
static __u32 edge[2];
static __s32 adj1[4];
static __s32 adj2[4];
static __s32 *uf1;
static __u32 cs1,cs2;
static __u32 activeEdges=0;
static __u32 cutoff;
/******************************************************************************
 *****************************************************************************/
static char init(void)
{
  K=coupling*beta;
  v = exp(K) - 1.0;
  N = DX*DX;
  rcWeights[0] = MIN(pow(v,-1),1); //db==-1,dN==0
  rcWeights[1] = MIN(pow(v,-1)*q,1); //db==-1,dN=1
  rcWeights[2] = MIN(v,1); //db==1,dN==0
  rcWeights[3] = MIN(v*pow(q,-1),1); //db==1,dN==-1
  p_min_del = MIN(rcWeights[0],rcWeights[1]);
  p_max_del = MAX(rcWeights[0],rcWeights[1]);
  p_min_ins = MIN(rcWeights[2],rcWeights[3]);
  p_max_ins = MAX(rcWeights[2],rcWeights[3]);
  if(verbose) {
	  printf("p_min_del = %f\n",p_min_del);
	  printf("p_max_del = %f\n",p_max_del);
	  printf("p_min_ins = %f\n",p_min_ins);
	  printf("p_max_ins = %f\n",p_max_ins);

  }
  r = gsl_rng_alloc(gsl_rng_mt19937);
  if(!r)
      return 0;
  gsl_rng_set(r,seed);
  __u32 i;
  uf1 = initUF(N);
  todo_pool = malloc(N*sizeof(struct queue_node));
  if(!todo_pool) {
      return 0;
  }
  collect_pool = malloc(N*sizeof(struct queue_node));
  if(!collect_pool) {
      return 0;
  }
  for(i=0;i<N;++i) {

      collect_pool[i].next = todo_pool[i].next = NULL;
      collect_pool[i].data = todo_pool[i].data= i;
      collect_pool[i].visited = todo_pool[i].visited = 0;
    }

  bonds = malloc(sizeof(*bonds)*2*N);
  if(!bonds) {
    return 0;
  }
  for(i=0;i<2*N;i++) bonds[i] = -1;
  return 1;
}
/******************************************************************************
 *****************************************************************************/
static void destroy(void)
{
  destroyUF(uf1);
  free(bonds);
  gsl_rng_free(r);
  free(collect_pool);
  free(todo_pool);
}
/******************************************************************************
 *****************************************************************************/
static inline __u32 ltcXnext(__u32 idx)
{
  if((idx+1) % DX)
    return idx+1;
  else
    return idx +1- DX ;
}
/******************************************************************************
 *****************************************************************************/
static inline __u32 ltcXprev(__u32 idx)
{
  if((idx%DX))
    return idx-1;
  else
    return idx + DX  -1;
}
/******************************************************************************
 *****************************************************************************/
static inline __u32 ltcYnext(__u32 idx)
{

  if(idx <= N -1 && idx >= N - DX)
    return idx +DX- N ;
  else
    return idx+DX;
}
/******************************************************************************
 *****************************************************************************/
static inline __u32 ltcYprev(__u32 idx)
{
  if(idx <= DX - 1)
    return idx + N - DX;
  else
    return idx - DX;

}
/******************************************************************************
 *****************************************************************************/
static void Adjacent(__u32 bidx)
{

  if(bidx%2) {
    bidx = (bidx -1)/2;
    edge[0] = bidx;
    edge[1] = ltcXnext(bidx);
  }
  else {
    bidx = bidx/2;
    edge[0] = bidx;
    edge[1] = ltcYprev(bidx);
  }

}
/******************************************************************************
 *****************************************************************************/
static void Adjacent2(__u32 idx, __u8 a) {
  if(a == 1)
  {
    if(bonds[2*idx] == 1)  adj1[0] = ltcYprev(idx);
    else  adj1[0] = -1;
    if(bonds[(2*idx)+1] == 1)  adj1[1] = ltcXnext(idx);
    else adj1[1] = -1;
    if(bonds[2*ltcYnext(idx)] == 1)	adj1[2] = ltcYnext(idx);
    else adj1[2] = -1;
    if(bonds[(2*ltcXprev(idx))+1] ==1)adj1[3] = ltcXprev(idx);
    else adj1[3] = -1;
  }
  else {
    if(bonds[2*idx] == 1)adj2[0] = ltcYprev(idx);
    else adj2[0] = -1;
    if(bonds[(2*idx)+1] == 1)adj2[1] = ltcXnext(idx);
    else adj2[1] = -1;
    if(bonds[2*ltcYnext(idx)] == 1)	adj2[2] = ltcYnext(idx);
    else adj2[2] = -1;
    if(bonds[(2*ltcXprev(idx))+1] == 1)adj2[3] = ltcXprev(idx);
    else adj2[3] = -1;
  }

}
/******************************************************************************
 *****************************************************************************/
static __u8 breadthFirstSearch(__u32 start, __u32 goal,__u8 accept_split)
{

    static struct queue todo1,todo2;
  __u32 i=0,activeP1=0,activeP2=0;
  init_queue(&todo1,todo_pool);
  init_queue(&todo2,todo_pool);
  init_queue(&collect1,collect_pool);
  init_queue(&collect2,collect_pool);
  cs1=0;
  cs2=0;
  enqueue(&todo1,start); // Put starting point onto queue 1
  enqueue(&collect1,start);
  cs1++; // Increase cluster size of bfs 1
  collect_pool[start].visited = offset_1;
  enqueue(&todo2,goal); // Put goal point onto queue 2
  enqueue(&collect2,goal);
  collect_pool[goal].visited = offset_2;
  cs2++; // increase cluster size of bfs 2
    while(!queue_empty_p(&todo1) && !queue_empty_p(&todo2)) {
        dequeue(&todo2,&activeP2); // get next vertex of bfs 2
        Adjacent2(activeP2,2);	 // get all adjacent vertices of current vertex
        for(i=0;i<4;i++) {
            if(adj2[i] != -1) { // if accessable (edge exists)
                if(collect_pool[adj2[i]].visited == offset_2) continue; // already visited
                if((__u32)adj2[i] == start || collect_pool[adj2[i]].visited == offset_1) { // reconnected
                    while(!queue_empty_p(&todo1)) dequeue(&todo1,&activeP1); // empty queue 1
                    while(!queue_empty_p(&todo2)) dequeue(&todo2,&activeP2); // empty queue 2
                    while(!queue_empty_p(&collect1)) dequeue(&collect1,&activeP1);
                    while(!queue_empty_p(&collect2)) dequeue(&collect2,&activeP2);
                    offset_1+=2;
                    offset_2+=2;
                    return 1; // 1 indicates success
                }
                enqueue(&todo2,adj2[i]);
                enqueue(&collect2,adj2[i]);
                collect_pool[adj2[i]].visited = offset_2; // mark as visited
                cs2++; // increase cluster size
            }
        }
        dequeue(&todo1,&activeP1);
        Adjacent2(activeP1,1);
        for(i=0;i<4;i++) {
            if(adj1[i] != -1) {
                if(collect_pool[adj1[i]].visited == offset_1) continue;
                if((__u32)adj1[i] == goal || collect_pool[adj1[i]].visited == offset_2) {
                    while(!queue_empty_p(&todo1)) dequeue(&todo1,&activeP1);
                    while(!queue_empty_p(&todo2)) dequeue(&todo2,&activeP2);
                    while(!queue_empty_p(&collect1)) dequeue(&collect1,&activeP1);
                    while(!queue_empty_p(&collect2)) dequeue(&collect2,&activeP2);
                    offset_1+=2;
                    offset_2+=2;
                    return 1;
                }
                enqueue(&todo1,adj1[i]);
                enqueue(&collect1,adj1[i]);
                collect_pool[adj1[i]].visited = offset_1;
                cs1++;
            }
        }
    }
    /* Only if a deletion of a cutting edge has to be accepted,
        * completely traverse the unfinished cluster to
        * be able to update the UF datastructure afterwards based on
        * the collect1 and collect2 queues.
    */
    if(accept_split) {
        while(!queue_empty_p(&todo1)) {
            dequeue(&todo1,&activeP1);
            Adjacent2(activeP1,1);
            for(i=0;i<4;i++) {
                if(adj1[i] != -1) {
                    if(collect_pool[adj1[i]].visited == offset_1) continue;
                    enqueue(&todo1,adj1[i]);
                    enqueue(&collect1,adj1[i]);
                    collect_pool[adj1[i]].visited = offset_1;
                    cs1++;
                }
            }

        }
        while(!queue_empty_p(&todo2)) {
            dequeue(&todo2,&activeP2);
            Adjacent2(activeP2,2);
            for(i=0;i<4;i++) {
                if(adj2[i] != -1) {
                    if(collect_pool[adj2[i]].visited == offset_2) continue;
                    enqueue(&todo2,adj2[i]);
                    enqueue(&collect2,adj2[i]);
                    collect_pool[adj2[i]].visited = offset_2;
                    cs2++;
                }
            }
        }
    }
    else {
        while(!queue_empty_p(&todo1)) dequeue(&todo1,&activeP1);
        while(!queue_empty_p(&todo2)) dequeue(&todo2,&activeP2);
        while(!queue_empty_p(&collect1)) dequeue(&collect1,&activeP1);
        while(!queue_empty_p(&collect2)) dequeue(&collect2,&activeP2);
    }
    offset_1+=2;
    offset_2+=2;

    return 0;
}
/******************************************************************************
 *****************************************************************************/
static __u8 removeBond(__u32 a, __u32 b,__u8 accept_split,__u8 accept_non_split) {
    __u32 vertex;
    if(breadthFirstSearch(a,b,accept_split)) //replacement edge found
        return accept_non_split;
    else {  // no replacement edge available and hence cluster will be splitted if move accepted
        if(accept_split) {  // accept cluster splitting case? if yes rebuild uf
            while(!queue_empty_p(&collect1)) {
                dequeue(&collect1,&vertex);
                uf1[vertex] = a;
            }
            uf1[a] = -1*cs1;
            while(!queue_empty_p(&collect2)) {
                dequeue(&collect2,&vertex);
                uf1[vertex] = b;
            }
            uf1[b] = -1*cs2;
            return 1;
        } // not accepted
        else {
            return 0;
        }
    }

}
/******************************************************************************
 *****************************************************************************/
static inline __u8 acceptChange(__s8 d_numCl, __s8 d_numAB, double randV)
{
  return d_numAB == -1 ? (randV < rcWeights[d_numCl]) : (randV < rcWeights[2 - d_numCl]);
}
/******************************************************************************
 *****************************************************************************/
static inline void   mcStep(void)
{

  static __u32 bidx;
  static double rnd_num;
  bidx = gsl_rng_uniform_int(r,2*N);
  rnd_num  = gsl_rng_uniform(r);
  Adjacent(bidx);
  if(bonds[bidx] == 1) { //edge is active hence delete it
    if(rnd_num < p_min_del) {
   	  bonds[bidx] = -1;
      if(removeBond(edge[0],edge[1],1,1))
        activeEdges--;
      else {
          if(verbose)fprintf(stderr,"ERROR!\n");
          exit(-1);
      }
    }
    else {
	if(rnd_num < p_max_del) {
	  bonds[bidx] = -1;
	  if(removeBond(edge[0],edge[1], acceptChange(1,-1,rnd_num),acceptChange(0,-1,rnd_num)))
          activeEdges--;
      else
          bonds[bidx] = 1;
    }

    }

  }
  else {
    if(rnd_num < p_min_ins) {
        bonds[bidx]  = 1;
        activeEdges++;
        unite(edge[0],edge[1],uf1);
      }
    else {
    if(rnd_num < p_max_ins) {
      dN = connected(edge[0],edge[1],uf1) ? 0: -1;
      if(acceptChange(dN,1,rnd_num)) {
        bonds[bidx]=1;
        activeEdges++;
        if(dN == -1)
            unite(edge[0],edge[1],uf1);
      }
    }

  }
  }
}
/******************************************************************************
 *****************************************************************************/
static void extract_observables(__u32 cnt)
{
  __u32 i=0,tn=0,nclust=0;
  size_giant[cnt] = 0;
  sec_cs_moment[cnt] = 0;
  four_cs_moment[cnt] = 0;
  for(i=0;i<N && tn <= N;i++) {
    if(uf1[i] < 0) {
      tn -= uf1[i];
      nclust+=1;
      sec_cs_moment[cnt]+= (__u64)pow(uf1[i],2);
      four_cs_moment[cnt]+= (__u64)pow(uf1[i],4);
      if((__u32)(-uf1[i]) > size_giant[cnt]) size_giant[cnt] = (__u32)(-uf1[i]);
    }
  }
  num_bonds[cnt] = activeEdges;
  num_cluster[cnt] = nclust;
}
/******************************************************************************
 *****************************************************************************/
static void generateTimeSeries(void)
{
  __u32 i=0,j=0;
  for(i=0;i<cutoff;i++){
      for(j=0;j<2*N;j++)mcStep();
      // reset visited array
      // otherwise it will overflow at some point
      // could also be done less frequent
      for(j=0;j<N;j++)collect_pool[j].visited=0;
  }
  for(i=0;i<steps;i++) {
    for(j=0;j<2*N;j++)
      mcStep();
      extract_observables(i);
      for(j=0;j<N;j++)collect_pool[j].visited=0;
    }
}
/******************************************************************************
 *****************************************************************************/
char init_sweeny_uf(double _q,unsigned int _l,double _beta,double _coupl,
        unsigned int _cutoff,unsigned _tslength,unsigned int rng_seed,
        void *ts_0,void *ts_1,void * ts_2,void *ts_3, void *ts_4) {
    q = _q;
    DX = _l;
    beta = _beta;
    coupling = _coupl;
    cutoff = _cutoff;
    steps = _tslength;
    seed = rng_seed;
    num_bonds = (__u32 *)ts_0;
    num_cluster = (__u32 *)ts_1;
    size_giant = (__u32 *)ts_2;
    sec_cs_moment = (__u64 *)ts_3;
    four_cs_moment = (__u64 *)ts_4;
    return setup=init();

}
/******************************************************************************
 *****************************************************************************/
void destroy_sweeny_uf(void) {
    if(setup)
        destroy();
    setup=0;
}
/******************************************************************************
 *****************************************************************************/
char simulate_sweeny_uf(void) {
    if(!setup)return 0;
    generateTimeSeries();
    return 1;
}
/******************************************************************************
 *****************************************************************************/


static char help[] = "Solve the p-Laplacian equation in 2D using Q^1 FEM.\n"
"Implements an objective function and a residual (gradient) function, but\n"
"no Jacobian.  Defaults to p=4 and quadrature degree n=2.  Run as one of:\n"
"   ./plap -snes_fd_color             [default]\n"
"   ./plap -snes_mf\n"
"   ./plap -snes_fd                   [does not scale]\n"
"   ./plap -snes_fd_function -snes_fd [does not scale]\n"
"Uses a manufactured solution.\n"
"This is NOT a recommended example for further work because of the weird way\n"
"it handles the Dirichlet boundary values.\n\n";

#include <petsc.h>
#include "../../quadrature.h"

#define COMM PETSC_COMM_WORLD

/* cool dendritic failure:
    ./plap -snes_fd_color -snes_converged_reason -ksp_converged_reason -pc_type mg -plap_p 10.0 -da_refine 6 -snes_monitor_solution draw
succeeds in 11 snes iterations (on fine grid) with grid sequencing:
    ./plap -snes_fd_color -snes_converged_reason -ksp_converged_reason -pc_type mg -plap_p 10.0 -snes_grid_sequence 6 -snes_monitor_solution draw
*/

//STARTCTX
typedef struct {
    double    p, eps, alpha;
    int       quaddegree;
    PetscBool no_residual;
} PLapCtx;
//ENDCTX

PetscErrorCode ConfigureCtx(PLapCtx *user) {
    PetscErrorCode ierr;
    user->p = 4.0;
    user->eps = 0.0;
    user->alpha = 1.0;
    user->quaddegree = 2;
    user->no_residual = PETSC_FALSE;
    ierr = PetscOptionsBegin(COMM,"plap_","p-laplacian solver options",""); CHKERRQ(ierr);
    ierr = PetscOptionsReal("-p","exponent p with  1 <= p < infty",
                      "plap.c",user->p,&(user->p),NULL); CHKERRQ(ierr);
    if (user->p < 1.0) { SETERRQ(COMM,1,"p >= 1 required"); }
    ierr = PetscOptionsReal("-eps","regularization parameter eps",
                      "plap.c",user->eps,&(user->eps),NULL); CHKERRQ(ierr);
    ierr = PetscOptionsReal("-alpha","parameter alpha in exact solution",
                      "plap.c",user->alpha,&(user->alpha),NULL); CHKERRQ(ierr);
    ierr = PetscOptionsInt("-quaddegree","quadrature degree n (= 1,2,3 only)",
                     "plap.c",user->quaddegree,&(user->quaddegree),NULL); CHKERRQ(ierr);
    if ((user->quaddegree < 1) || (user->quaddegree > 3)) {
        SETERRQ(COMM,2,"quadrature degree n=1,2,3 only"); }
    ierr = PetscOptionsBool("-no_residual","do not set the residual evaluation function",
                      "plap.c",user->no_residual,&(user->no_residual),NULL);CHKERRQ(ierr);
    ierr = PetscOptionsEnd(); CHKERRQ(ierr);
    return 0;
}

//STARTEXACT
double Uexact(double x, double y, double alpha) {
    return 0.5 * (x+alpha)*(x+alpha) * (y+alpha)*(y+alpha);
}

double Frhs(double x, double y, PLapCtx *user) {
    const double alf = user->alpha,
                 XX = (x+alf)*(x+alf),
                 YY = (y+alf)*(y+alf),
                 D2 = XX + YY,
                 C = PetscPowScalar(XX * YY * D2, (user->p - 2.0) / 2.0),
                 gamma1 = 1.0/(x+alf) + (x+alf)/D2,
                 gamma2 = 1.0/(y+alf) + (y+alf)/D2;
    return - (user->p - 2.0) * C * (gamma1*(x+alf)*YY + gamma2*XX*(y+alf))
           - C * D2;
}
//ENDEXACT

//STARTINITIALITERATE
PetscErrorCode InitialIterateLocal(DMDALocalInfo *info, Vec u, PLapCtx *user) {
    PetscErrorCode ierr;
    const double hx = 1.0 / (info->mx+1), hy = 1.0 / (info->my+1);
    double       x, y, **au;
    int          i, j;
    ierr = DMDAVecGetArray(info->da,u,&au); CHKERRQ(ierr);
    for (j = info->ys; j < info->ys + info->ym; j++) {
        y = hy * (j + 1);
        for (i = info->xs; i < info->xs + info->xm; i++) {
            x = hx * (i + 1);
            au[j][i] = (1.0 - x) * Uexact(0.0,y,user->alpha)
                       + x * Uexact(1.0,y,user->alpha);
        }
    }
    ierr = DMDAVecRestoreArray(info->da,u,&au); CHKERRQ(ierr);
    return 0;
}
//ENDINITIALITERATE

PetscErrorCode GetUexactLocal(DMDALocalInfo *info, Vec uex, PLapCtx *user) {
    PetscErrorCode ierr;
    const double hx = 1.0 / (info->mx+1), hy = 1.0 / (info->my+1);
    double       x, y, **auex;
    int          i, j;
    ierr = DMDAVecGetArray(info->da,uex,&auex); CHKERRQ(ierr);
    for (j = info->ys; j < info->ys + info->ym; j++) {
        y = hy * (j + 1);
        for (i = info->xs; i < info->xs + info->xm; i++) {
            x = hx * (i + 1);
            auex[j][i] = Uexact(x,y,user->alpha);
        }
    }
    ierr = DMDAVecRestoreArray(info->da,uex,&auex); CHKERRQ(ierr);
    return 0;
}

//STARTFEM
static double xiL[4]  = { 1.0, -1.0, -1.0,  1.0},
              etaL[4] = { 1.0,  1.0, -1.0, -1.0};

double chi(int L, double xi, double eta) {
    return 0.25 * (1.0 + xiL[L] * xi) * (1.0 + etaL[L] * eta);
}

// evaluate v(xi,eta) on reference element using local node numbering
double eval(const double v[4], double xi, double eta) {
    double sum = 0.0;
    int    L;
    for (L=0; L<4; L++)
        sum += v[L] * chi(L,xi,eta);
    return sum;
}

typedef struct {
    double  xi, eta;
} gradRef;

gradRef dchi(int L, double xi, double eta) {
    const gradRef result = {0.25 * xiL[L]  * (1.0 + etaL[L] * eta),
                            0.25 * etaL[L] * (1.0 + xiL[L]  * xi)};
    return result;
}

// evaluate partial derivs of v(xi,eta) on reference element
gradRef deval(const double v[4], double xi, double eta) {
    gradRef sum = {0.0,0.0}, tmp;
    int     L;
    for (L=0; L<4; L++) {
        tmp = dchi(L,xi,eta);
        sum.xi += v[L] * tmp.xi;  sum.eta += v[L] * tmp.eta;
    }
    return sum;
}
//ENDFEM

//STARTTOOLS
void GetUorG(DMDALocalInfo *info, int i, int j, double **au, double *u,
             PLapCtx *user) {
    const double hx = 1.0 / (info->mx+1),  hy = 1.0 / (info->my+1),
                 x = hx * (i + 1),  y = hy * (j + 1);
    u[0] = ((i == info->mx) || (j == info->my))
             ? Uexact(x,   y,   user->alpha) : au[j][i];
    u[1] = ((i == 0)  || (j == info->my))
             ? Uexact(x-hx,y,   user->alpha) : au[j][i-1];
    u[2] = ((i == 0)  || (j == 0))
             ? Uexact(x-hx,y-hy,user->alpha) : au[j-1][i-1];
    u[3] = ((i == info->mx) || (j == 0))
             ? Uexact(x,   y-hy,user->alpha) : au[j-1][i];
}

double GradInnerProd(DMDALocalInfo *info, gradRef du, gradRef dv) {
    const double hx = 1.0 / (info->mx+1),  hy = 1.0 / (info->my+1),
                 cx = 4.0 / (hx * hx),  cy = 4.0 / (hy * hy);
    return cx * du.xi  * dv.xi + cy * du.eta * dv.eta;
}

double GradPow(DMDALocalInfo *info, gradRef du, double P, double eps) {
    return PetscPowScalar(GradInnerProd(info,du,du) + eps * eps, P / 2.0);
}
//ENDTOOLS

//STARTOBJECTIVE
double ObjIntegrandRef(DMDALocalInfo *info,
                       const double f[4], const double u[4],
                       double xi, double eta, double p, double eps) {
    const gradRef du = deval(u,xi,eta);
    return GradPow(info,du,p,eps) / p - eval(f,xi,eta) * eval(u,xi,eta);
}

PetscErrorCode FormObjectiveLocal(DMDALocalInfo *info, double **au,
                                  double *obj, PLapCtx *user) {
  PetscErrorCode ierr;
  const double hx = 1.0 / (info->mx+1),  hy = 1.0 / (info->my+1);
  const Quad1D q = gausslegendre[user->quaddegree-1];
  const int    XE = info->xs + info->xm,  YE = info->ys + info->ym;
  double       x, y, lobj = 0.0, u[4];
  int          i,j,r,s;
  MPI_Comm     com;

  // loop over all elements
  for (j = info->ys; j <= YE; j++) {
      y = hy * (j + 1);
      for (i = info->xs; i <= XE; i++) {
          x = hx * (i + 1);
          // owned elements include "right" and "top" edges of grid
          if (i < XE || j < YE || i == info->mx || j == info->my) {
              const double f[4] = {Frhs(x,   y,   user),
                                   Frhs(x-hx,y,   user),
                                   Frhs(x-hx,y-hy,user),
                                   Frhs(x,   y-hy,user)};
              GetUorG(info,i,j,au,u,user);
              // loop over quadrature points
              for (r = 0; r < q.n; r++) {
                  for (s = 0; s < q.n; s++) {
                      lobj += q.w[r] * q.w[s]
                              * ObjIntegrandRef(info,f,u,q.xi[r],q.xi[s],
                                                user->p,user->eps);
                  }
              }
          }
      }
  }
  lobj *= 0.25 * hx * hy;

  ierr = PetscObjectGetComm((PetscObject)(info->da),&com); CHKERRQ(ierr);
  ierr = MPI_Allreduce(&lobj,obj,1,MPI_DOUBLE,MPI_SUM,com); CHKERRQ(ierr);
  return 0;
}
//ENDOBJECTIVE

//STARTFUNCTION
double FunIntegrandRef(DMDALocalInfo *info, int L,
                       const double f[4], const double u[4],
                       double xi, double eta, double p, double eps) {
  const gradRef du    = deval(u,xi,eta),
                dchiL = dchi(L,xi,eta);
  return GradPow(info,du,p - 2.0,eps) * GradInnerProd(info,du,dchiL)
         - eval(f,xi,eta) * chi(L,xi,eta);
}

PetscErrorCode FormFunctionLocal(DMDALocalInfo *info, double **au,
                                 double **FF, PLapCtx *user) {
  const double hx = 1.0 / (info->mx+1),  hy = 1.0 / (info->my+1);
  const Quad1D q = gausslegendre[user->quaddegree-1];
  const int    XE = info->xs + info->xm,  YE = info->ys + info->ym,
               li[4] = {0,-1,-1,0},  lj[4] = {0,0,-1,-1};
  double       x, y, u[4];
  int          i,j,l,r,s,PP,QQ;

  // clear residuals
  for (j = info->ys; j < YE; j++)
      for (i = info->xs; i < XE; i++)
          FF[j][i] = 0.0;

  // loop over all elements
  for (j = info->ys; j <= YE; j++) {
      y = hy * (j + 1);
      for (i = info->xs; i <= XE; i++) {
          x = hx * (i + 1);
          const double f[4] = {Frhs(x,   y,   user),
                               Frhs(x-hx,y,   user),
                               Frhs(x-hx,y-hy,user),
                               Frhs(x,   y-hy,user)};
          GetUorG(info,i,j,au,u,user);
          // loop over corners of element i,j
          for (l = 0; l < 4; l++) {
              PP = i + li[l];
              QQ = j + lj[l];
              // only update residual if we own node
              if (PP >= info->xs && PP < XE
                  && QQ >= info->ys && QQ < YE) {
                  // loop over quadrature points
                  for (r = 0; r < q.n; r++) {
                      for (s = 0; s < q.n; s++) {
                         FF[QQ][PP]
                             += 0.25 * hx * hy * q.w[r] * q.w[s]
                                * FunIntegrandRef(info,l,f,u,
                                                  q.xi[r],q.xi[s],
                                                  user->p,user->eps);
                      }
                  }
              }
          }
      }
  }
  return 0;
}
//ENDFUNCTION

int main(int argc,char **argv) {
  PetscErrorCode ierr;
  DM             da, da_after;
  SNES           snes;
  Vec            u_initial, u, u_exact;
  PLapCtx        user;
  DMDALocalInfo  info;
  double         err, hx, hy;

  PetscInitialize(&argc,&argv,NULL,help);
  ierr = ConfigureCtx(&user); CHKERRQ(ierr);

  ierr = DMDACreate2d(COMM,
               DM_BOUNDARY_NONE, DM_BOUNDARY_NONE, DMDA_STENCIL_BOX,
               3,3,PETSC_DECIDE,PETSC_DECIDE,1,1,NULL,NULL,&da); CHKERRQ(ierr);
  ierr = DMSetFromOptions(da); CHKERRQ(ierr);
  ierr = DMSetUp(da); CHKERRQ(ierr);
  ierr = DMSetApplicationContext(da,&user);CHKERRQ(ierr);

  ierr = DMDAGetLocalInfo(da,&info); CHKERRQ(ierr);
  hx = 1.0 / (info.mx+1);
  hy = 1.0 / (info.my+1);
  ierr = DMDASetUniformCoordinates(da,hx,1.0-hx,hy,1.0-hy,0.0,1.0); CHKERRQ(ierr);

  ierr = SNESCreate(COMM,&snes); CHKERRQ(ierr);
  ierr = SNESSetDM(snes,da); CHKERRQ(ierr);
  ierr = DMDASNESSetObjectiveLocal(da,
             (DMDASNESObjective)FormObjectiveLocal,&user); CHKERRQ(ierr);
  if (!user.no_residual) {
      ierr = DMDASNESSetFunctionLocal(da,INSERT_VALUES,
                 (DMDASNESFunction)FormFunctionLocal,&user); CHKERRQ(ierr);
  }
  ierr = SNESSetFromOptions(snes); CHKERRQ(ierr);

  ierr = DMCreateGlobalVector(da,&u_initial);CHKERRQ(ierr);
  ierr = InitialIterateLocal(&info,u_initial,&user); CHKERRQ(ierr);
  ierr = SNESSolve(snes,NULL,u_initial); CHKERRQ(ierr);
  ierr = VecDestroy(&u_initial); CHKERRQ(ierr);
  ierr = DMDestroy(&da); CHKERRQ(ierr);

  ierr = SNESGetSolution(snes,&u); CHKERRQ(ierr);
  ierr = VecDuplicate(u,&u_exact); CHKERRQ(ierr);
  ierr = SNESGetDM(snes,&da_after); CHKERRQ(ierr);
  ierr = DMDAGetLocalInfo(da_after,&info); CHKERRQ(ierr);
  ierr = PetscPrintf(COMM,
            "grid of %d x %d = %d interior nodes\n",
            info.mx,info.my,info.mx*info.my); CHKERRQ(ierr);
  ierr = GetUexactLocal(&info,u_exact,&user); CHKERRQ(ierr);
  ierr = VecAXPY(u,-1.0,u_exact); CHKERRQ(ierr);    // u <- u + (-1.0) uexact
  ierr = VecNorm(u,NORM_INFINITY,&err); CHKERRQ(ierr);
  ierr = PetscPrintf(COMM,"numerical error:  |u-u_exact|_inf = %.3e\n",
           err); CHKERRQ(ierr);

  VecDestroy(&u_exact);  SNESDestroy(&snes);
  return PetscFinalize();
}


/* linalg/choleskyc.c
 *
 * Copyright (C) 2007, 2019 Patrick Alken
 * Copyright (C) 2010 Huan Wu
 *
 * 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 <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_complex.h>
#include <gsl/gsl_complex_math.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_errno.h>

#include "recurse.h"

/*
 * This module contains routines related to the Cholesky decomposition
 * of a complex Hermitian positive definite matrix.
 */

static void cholesky_complex_conj_vector(gsl_vector_complex *v);
static int complex_cholesky_decomp_L2(gsl_matrix_complex * A);
static int complex_cholesky_decomp_L3(gsl_matrix_complex * A);

/*
gsl_linalg_complex_cholesky_decomp()
  Perform the Cholesky decomposition on a Hermitian positive definite
matrix using Level 3 BLAS.

Inputs: A - (input/output) complex positive definite matrix

Return: success or error

Notes:
1) The lower triangle of A is overwritten with the Cholesky decomposition
*/

int
gsl_linalg_complex_cholesky_decomp(gsl_matrix_complex *A)
{
  const size_t N = A->size1;

  if (N != A->size2)
    {
      GSL_ERROR("Cholesky decomposition requires square matrix", GSL_ENOTSQR);
    }
  else
    {
      return complex_cholesky_decomp_L3(A);
    }
}

/*
gsl_linalg_complex_cholesky_solve()
  Solve A x = b where A is in cholesky form
*/

int
gsl_linalg_complex_cholesky_solve (const gsl_matrix_complex * cholesky,
                                   const gsl_vector_complex * b,
                                   gsl_vector_complex * x)
{
  if (cholesky->size1 != cholesky->size2)
    {
      GSL_ERROR ("cholesky matrix must be square", GSL_ENOTSQR);
    }
  else if (cholesky->size1 != b->size)
    {
      GSL_ERROR ("matrix size must match b size", GSL_EBADLEN);
    }
  else if (cholesky->size2 != x->size)
    {
      GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
    }
  else
    {
      gsl_vector_complex_memcpy (x, b);
      return gsl_linalg_complex_cholesky_svx(cholesky, x);
    }
}

/*
gsl_linalg_complex_cholesky_svx()
  Solve A x = b in place where A is in cholesky form
*/

int
gsl_linalg_complex_cholesky_svx (const gsl_matrix_complex * cholesky,
                                 gsl_vector_complex * x)
{
  if (cholesky->size1 != cholesky->size2)
    {
      GSL_ERROR ("cholesky matrix must be square", GSL_ENOTSQR);
    }
  else if (cholesky->size2 != x->size)
    {
      GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
    }
  else
    {
      /* solve for y using forward-substitution, L y = b */
      gsl_blas_ztrsv (CblasLower, CblasNoTrans, CblasNonUnit, cholesky, x);

      /* perform back-substitution, L^H x = y */
      gsl_blas_ztrsv (CblasLower, CblasConjTrans, CblasNonUnit, cholesky, x);

      return GSL_SUCCESS;
    }
}

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

gsl_linalg_complex_cholesky_invert()
  Compute the inverse of an Hermitian positive definite matrix in
  Cholesky form.

Inputs: LLT - matrix in cholesky form on input
              A^{-1} = L^{-H} L^{-1} on output

Return: success or error
******************************************************************************/

int
gsl_linalg_complex_cholesky_invert(gsl_matrix_complex * LLT)
{
  if (LLT->size1 != LLT->size2)
    {
      GSL_ERROR ("cholesky matrix must be square", GSL_ENOTSQR);
    }
  else
    {
      int status;
      size_t N = LLT->size1;
      size_t i, j;

      /* invert the lower triangle of LLT */
      status = gsl_linalg_complex_tri_invert(CblasLower, CblasNonUnit, LLT);
      if (status)
        return status;

      /* compute A^{-1} = L^{-H} L^{-1} */
      status = gsl_linalg_complex_tri_LHL(LLT);
      if (status)
        return status;

      /* copy the Hermitian lower triangle to the upper triangle */
      for (i = 1; i < N; ++i)
        {
          for (j = 0; j < i; ++j)
            {
              gsl_complex z = gsl_matrix_complex_get(LLT, i, j);
              gsl_matrix_complex_set(LLT, j, i, gsl_complex_conjugate(z));
            }
        }

      return GSL_SUCCESS;
    }
}

/********************************************
 *           INTERNAL ROUTINES              *
 ********************************************/

static void
cholesky_complex_conj_vector(gsl_vector_complex *v)
{
  size_t i;

  for (i = 0; i < v->size; ++i)
    {
      gsl_complex * vi = gsl_vector_complex_ptr(v, i);
      GSL_IMAG(*vi) = -GSL_IMAG(*vi);
    }
}

/*
complex_cholesky_decomp_L2()
  Perform the Cholesky decomposition on a Hermitian positive definite
matrix using Level 2 BLAS. See Golub & Van Loan, "Matrix Computations" (3rd ed),
algorithm 4.2.2.

Inputs: A - (input/output) complex postive definite matrix

Return: success or error

Notes:
1) The lower triangle of A is overwritten with the Cholesky decomposition
*/

static int
complex_cholesky_decomp_L2(gsl_matrix_complex * A)
{
  const size_t N = A->size1;

  if (N != A->size2)
    {
      GSL_ERROR("Cholesky decomposition requires square matrix", GSL_ENOTSQR);
    }
  else
    {
      size_t j;
      gsl_complex z;
      double ajj;

      for (j = 0; j < N; ++j)
        {
          z = gsl_matrix_complex_get(A, j, j);
          ajj = GSL_REAL(z);

          if (j > 0)
            {
              gsl_vector_complex_const_view aj =
                gsl_matrix_complex_const_subrow(A, j, 0, j);

              gsl_blas_zdotc(&aj.vector, &aj.vector, &z);
              ajj -= GSL_REAL(z);
            }

          if (ajj <= 0.0)
            {
              GSL_ERROR("matrix is not positive definite", GSL_EDOM);
            }

          ajj = sqrt(ajj);
          GSL_SET_COMPLEX(&z, ajj, 0.0);
          gsl_matrix_complex_set(A, j, j, z);

          if (j < N - 1)
            {
              gsl_vector_complex_view av =
                gsl_matrix_complex_subcolumn(A, j, j + 1, N - j - 1);

              if (j > 0)
                {
                  gsl_vector_complex_view aj =
                    gsl_matrix_complex_subrow(A, j, 0, j);
                  gsl_matrix_complex_view am =
                    gsl_matrix_complex_submatrix(A, j + 1, 0, N - j - 1, j);

                  cholesky_complex_conj_vector(&aj.vector);

                  gsl_blas_zgemv(CblasNoTrans,
                                 GSL_COMPLEX_NEGONE,
                                 &am.matrix,
                                 &aj.vector,
                                 GSL_COMPLEX_ONE,
                                 &av.vector);

                  cholesky_complex_conj_vector(&aj.vector);
                }

              gsl_blas_zdscal(1.0 / ajj, &av.vector);
            }
        }

      return GSL_SUCCESS;
    }
}

/*
complex_cholesky_decomp_L3()
  Perform the Cholesky decomposition on a Hermitian positive definite
matrix using Level 3 BLAS.

Inputs: A - (input/output) complex postive definite matrix

Return: success or error

Notes:
1) The lower triangle of A is overwritten with the Cholesky decomposition

2) Based on ReLAPACK recursive variant with Level 3 BLAS
*/

static int
complex_cholesky_decomp_L3(gsl_matrix_complex * A)
{
  const size_t N = A->size1;

  if (N != A->size2)
    {
      GSL_ERROR("Cholesky decomposition requires square matrix", GSL_ENOTSQR);
    }
  else if (N <= CROSSOVER_CHOLESKY)
    {
      /* use unblocked Level 2 algorithm */
      return complex_cholesky_decomp_L2(A);
    }
  else
    {
      /*
       * partition matrix:
       *
       * A11 A12
       * A21 A22
       *
       * where A11 is N1-by-N1
       */
      int status;
      const size_t N1 = GSL_LINALG_SPLIT_COMPLEX(N);
      const size_t N2 = N - N1;
      gsl_matrix_complex_view A11 = gsl_matrix_complex_submatrix(A, 0, 0, N1, N1);
      gsl_matrix_complex_view A21 = gsl_matrix_complex_submatrix(A, N1, 0, N2, N1);
      gsl_matrix_complex_view A22 = gsl_matrix_complex_submatrix(A, N1, N1, N2, N2);

      /* recursion on A11 */
      status = complex_cholesky_decomp_L3(&A11.matrix);
      if (status)
        return status;

      /* A21 = A21 * A11^{-1} */
      gsl_blas_ztrsm(CblasRight, CblasLower, CblasConjTrans, CblasNonUnit, GSL_COMPLEX_ONE, &A11.matrix, &A21.matrix);

      /* A22 -= A21 A21^H */
      gsl_blas_zherk(CblasLower, CblasNoTrans, -1.0, &A21.matrix, 1.0, &A22.matrix);

      /* recursion on A22 */
      status = complex_cholesky_decomp_L3(&A22.matrix);
      if (status)
        return status;

      return GSL_SUCCESS;
    }
}


/*
** 1st level statistical inference using LISA
** GLM (general linear modeling) using precoloring
**
** G.Lohmann, MPI-KYB, 2018
*/
#include <viaio/Vlib.h>
#include <viaio/VImage.h>
#include <viaio/mu.h>
#include <viaio/option.h>

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

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_math.h>

extern gsl_matrix *PseudoInv(gsl_matrix *A,gsl_matrix *B);
extern void printmat(gsl_matrix *R,char *str);
extern void printvec(gsl_vector *x,char *str);

/*
** general linear model (GLM) using pecoloring
*/
void VGLM(gsl_matrix *Data,gsl_matrix *X,gsl_matrix *XInv,gsl_vector *con,VImage map,VImage zmap)
{
  int i;
  int m = Data->size2;
  int n = con->size;
  gsl_set_error_handler_off();
  gsl_vector *y = gsl_vector_calloc (m);
  gsl_vector *beta = gsl_vector_calloc (n);

  /* compute pseudoinverse */
  XInv = PseudoInv(X,XInv);

  /* main loop */
  VFillImage(zmap,VAllBands,0);
  size_t nvox=0;
  for (nvox=0; nvox < Data->size1; nvox++) {

    y->data = gsl_matrix_ptr(Data,nvox,0);
    gsl_blas_dgemv(CblasNoTrans,1.0,XInv,y,0.0,beta);

    /* contrast image */
    double sum=0;
    for (i=0; i<beta->size; i++) {
      sum += (beta->data[i]*con->data[i]);
    }
    if (gsl_isnan(sum) || gsl_isinf(sum)) continue;
    int b = VPixel(map,0,0,nvox,VShort);
    int r = VPixel(map,0,1,nvox,VShort);
    int c = VPixel(map,0,2,nvox,VShort);
    VPixel(zmap,b,r,c,VFloat) = sum;
  }

  gsl_vector_free(beta);
  gsl_vector_free(y);
}


#include <stdio.h>
#include <string.h>
#include <gsl/gsl_statistics.h>
#include "tz_error.h"
#include "tz_constant.h"
#include "tz_image_lib.h"
#include "tz_stack_lib.h"
#include "tz_objdetect.h"
#include "tz_imatrix.h"
#include "tz_stack_math.h"
#include "tz_stack_bwdist.h"
#include "tz_stack_bwmorph.h"
#include "tz_voxel_linked_list.h"
#include "tz_stack_sampling.h"
#include "tz_stack_attribute.h"
#include "tz_dimage_lib.h"
#include "tz_arrayview.h"
#include "tz_dmatrix.h"

INIT_EXCEPTION_MAIN(e)

int main(int argc, char* argv[]) {
  char neuron_name[100];
  if (argc == 1) {
    strcpy(neuron_name, "fly_neuron");
  } else {
    strcpy(neuron_name, argv[1]);
  }

  char file_path[100];

#if 0 /* local maxima */
  sprintf(file_path, "../data/%s/mask.tif", neuron_name);
  Stack *stack1 = Read_Stack(file_path);

  sprintf(file_path, "../data/%s/highpass_locmin.tif", neuron_name);
  Stack *stack2 = Read_Stack(file_path);

  Stack_Add(stack1, stack2, stack1);
  sprintf(file_path, "../data/%s/cell_mark.tif", neuron_name);
  Write_Stack(file_path, stack1);
#endif

#if 0
  sprintf(file_path, "../data/%s.tif", neuron_name);
  Stack *stack1 = Read_Stack(file_path);

  DMatrix *filter = Mexihat_2D_D(2.0, NULL);
  filter->ndim = 3;
  filter->dim[2] = 1;

  DMatrix *out = Filter_Stack_Fast_D(stack1, filter, NULL, 0);

# if 1
  int nvoxel = Stack_Voxel_Number(stack1);
  int i;
  for (i = 0; i < nvoxel; i++) {
    out->array[i] = fabs(out->array[i]);
  }
# endif

  Stack *stack2 = Scale_Double_Stack(out->array, out->dim[0], out->dim[1],
				   out->dim[2], GREY);

  sprintf(file_path, "../data/%s/edge.tif", neuron_name);
  Write_Stack(file_path, stack2);

  Kill_Stack(stack1);
  Kill_Stack(stack2);
#endif

#if 1
  sprintf(file_path, "../data/%s/mask.tif", neuron_name);
  Stack *stack2 = Read_Stack(file_path);
  sprintf(file_path, "../data/%s/seeds.pa", neuron_name);
  Pixel_Array *pa = Pixel_Array_Read(file_path);
  double *pa_array = (double *) pa->array;
  double max_dist = gsl_stats_max(pa_array, 1, pa->size);
  double mean_dist = gsl_stats_mean(pa_array, 1, pa->size);
  double std_dist = sqrt(gsl_stats_variance(pa_array, 1, pa->size));

  int erode_size = (int) (mean_dist + std_dist * 2 + 0.5);
  Struct_Element *se = Make_Ball_Se(erode_size);
  Stack *stack3 = Stack_Erode_Fast(stack2, NULL, se);
  int dilate_size = (int) (max_dist + 0.5);
  se = Make_Ball_Se(dilate_size);
  Stack *stack4 = Stack_Dilate(stack3, NULL, se);

  Stack_Sub(stack2, stack4, stack2);
  Write_Stack("../data/test.tif", stack2);

  Kill_Stack(stack2);
  Kill_Stack(stack3);
  Kill_Stack(stack4);
  Kill_Pixel_Array(pa);

#endif

  return 0;
}


/*
 *  Player - One Hell of a Robot Server
 *  Copyright (C) 2000  Brian Gerkey   &  Kasper Stoy
 *                      gerkey@usc.edu    kaspers@robotics.usc.edu
 *
 *  This library is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU Lesser General Public
 *  License as published by the Free Software Foundation; either
 *  version 2.1 of the License, or (at your option) any later version.
 *
 *  This library 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
 *  Lesser General Public License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public
 *  License along with this library; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */
/**************************************************************************
 * Desc: Useful pdf functions
 * Author: Andrew Howard
 * Date: 10 Dec 2002
 * CVS: $Id: pf_pdf.c 6348 2008-04-17 02:53:17Z gerkey $
 *************************************************************************/

#include <assert.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
//#include <gsl/gsl_rng.h>
//#include <gsl/gsl_randist.h>

#include "amcl/pf/pf_pdf.h"

// Random number generator seed value
static unsigned int pf_pdf_seed;

/**************************************************************************
 * Gaussian
 *************************************************************************/

// Create a gaussian pdf
pf_pdf_gaussian_t *pf_pdf_gaussian_alloc(pf_vector_t x, pf_matrix_t cx)
{
  pf_matrix_t cd;
  pf_pdf_gaussian_t *pdf;

  pdf = calloc(1, sizeof(pf_pdf_gaussian_t));

  pdf->x = x;
  pdf->cx = cx;
  //pdf->cxi = pf_matrix_inverse(cx, &pdf->cxdet);

  // Decompose the convariance matrix into a rotation
  // matrix and a diagonal matrix.
  pf_matrix_unitary(&pdf->cr, &cd, pdf->cx);
  pdf->cd.v[0] = sqrt(cd.m[0][0]);
  pdf->cd.v[1] = sqrt(cd.m[1][1]);
  pdf->cd.v[2] = sqrt(cd.m[2][2]);

  // Initialize the random number generator
  //pdf->rng = gsl_rng_alloc(gsl_rng_taus);
  //gsl_rng_set(pdf->rng, ++pf_pdf_seed);
  srand48(++pf_pdf_seed);

  return pdf;
}

// Destroy the pdf
void pf_pdf_gaussian_free(pf_pdf_gaussian_t *pdf)
{
  //gsl_rng_free(pdf->rng);
  free(pdf);
  return;
}

/*
// Compute the value of the pdf at some point [x].
double pf_pdf_gaussian_value(pf_pdf_gaussian_t *pdf, pf_vector_t x)
{
  int i, j;
  pf_vector_t z;
  double zz, p;

  z = pf_vector_sub(x, pdf->x);

  zz = 0;
  for (i = 0; i < 3; i++)
    for (j = 0; j < 3; j++)
      zz += z.v[i] * pdf->cxi.m[i][j] * z.v[j];

  p =  1 / (2 * M_PI * pdf->cxdet) * exp(-zz / 2);

  return p;
}
*/

// Generate a sample from the pdf.
pf_vector_t pf_pdf_gaussian_sample(pf_pdf_gaussian_t *pdf)
{
  int i, j;
  pf_vector_t r;
  pf_vector_t x;

  // Generate a random vector
  for (i = 0; i < 3; i++)
  {
    //r.v[i] = gsl_ran_gaussian(pdf->rng, pdf->cd.v[i]);
    r.v[i] = pf_ran_gaussian(pdf->cd.v[i]);
  }

  for (i = 0; i < 3; i++)
  {
    x.v[i] = pdf->x.v[i];
    for (j = 0; j < 3; j++)
      x.v[i] += pdf->cr.m[i][j] * r.v[j];
  }

  return x;
}

// Draw randomly from a zero-mean Gaussian distribution, with standard
// deviation sigma.
// We use the polar form of the Box-Muller transformation, explained here:
//   http://www.taygeta.com/random/gaussian.html
double pf_ran_gaussian(double sigma)
{
  double x1, x2, w, r;

  do
  {
    do { r = drand48(); } while (r==0.0);
    x1 = 2.0 * r - 1.0;
    do { r = drand48(); } while (r==0.0);
    x2 = 2.0 * r - 1.0;
    w = x1*x1 + x2*x2;
  } while(w > 1.0 || w==0.0);

  return(sigma * x2 * sqrt(-2.0*log(w)/w));
}


#ifndef PRIMAL_DUAL_LOOPLESS_H
#define PRIMAL_DUAL_LOOPLESS_H

#include "problem_data.h"

#include <string>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <stdio.h>      /* printf */
#include <time.h>
#include <fstream>
#include <algorithm>
#include <iomanip>
#include <ctime>
#include <sstream>
#include <omp.h>

//This class implements the loopless variance reduced type methods with arbitrary sampling

/*
The optimization problem to solve is:

\sum_{i=1}^n \lambda_i\phi_i(A_i^{\top} w)+ g(w)
Assumption 1: For each i, \phi_i is 1-smooth
By default, lambda_i=1/n for all i.
*
* The dual problem is
*         g*(-sum_{i=1}^n \lambda_i \alpha_i A_i)+\sum_{i=1}^n \lambda_i \phi*_i(\alpha_i)
*/

template<typename L, typename D>
class Primal_Dual_LOOPLESS: public problem_data<L, D>
{

private:

  // involved variables

  std::vector<D> proba_vector;

  std::vector<D>  tilde_proba_vector;

  std::vector<D> group_C;

  std::vector<D> index_group_C;

  std::vector<D> maxp_group_C;

  std::vector<D> isolated_I;

  std::vector<D> sump_group_C;
  vector<D> theta_S;   //theta_S in the paper

  std::vector<D> Li;

  std::vector<D> gk;     // the vector g^k in the paper

  // auxiliary variables
  std::vector<D> deltagk;
  std::vector<D> index_to_do_prox;
  std::vector<D> whether_or_not_to_do_prox;

  L nb_of_iters_per_loop;

  D primal_value;

  D dual_value;

  D epsilon;

  L max_nb_loops;

  D max_p;

  D maxv;

  D lambda_1;
  D lambda_2;

  D running_time;

  L nb_loops;

  L print_every_N;

  D delta;

  vector<L> batch_i;
  vector<L> my_batch;
  vector<D> batch_deltaalphai;

  L batch_size;

  L nb_groups;    //number of groups in group sampling

protected:

  std::vector<D> primal_x;     // the x variable

  string uniform;

  D Lf;

  D L2;

  D L1;

  D sumLi;

  D p; // the probability of changing w to x as in the paper
  L tau;  //number of threads on each node/computer

  D scaler;

  std::vector<D> dual_alpha;  //  dual_alpha[i]=\phi_i'(A_i't)

  std::vector<D> lambda_f;    // lambda_f[i]=lambda_i
  std::vector<D> baralpha;   // baralpha=sum_{i=1}^{nsamples} lambda_f[i]* dual_alpha[i]*A_i

  L current_nb_iters;

  L nb_iters;

  std::vector<D> last_update_i;

  ofstream samp;

public:

  gsl_rng * rng;
  virtual inline D gradient_of_phi_i(D, L){return D(NULL);}
  virtual inline D gradient_of_gstar_j(D, L){return D(NULL);}
  virtual inline D value_of_phi_i(D, L) {return D(NULL);}
  virtual inline D value_of_g_j(D, L){return D(NULL);}
  virtual inline D value_of_phistar_i(D,L) {return D(NULL);}
  virtual inline D value_of_gstar(vector<D> &){return D(NULL);}
  virtual inline D value_of_gstar_minus(vector<D> &, D){return D(NULL);}
  virtual inline D feasible_dual(vector<D> &){return D(NULL);}
  virtual inline D compute_delta_alpha(D,D,L){return D(NULL);}
  virtual inline void set_auxiliary_v(){}
  virtual inline D get_lambda1(){return D(NULL);}
  virtual inline D get_lambda2(){return D(NULL);}
  virtual inline D compute_current_xj_value(D , L, L , L){return D(NULL);}
  virtual inline D compute_current_xj_value_without_update(D , L, L , L){return D(NULL);}
  virtual inline void set_stepsize(){}
  virtual inline void update_x(D,L){}
  virtual inline void update_baralpha(){}

  Primal_Dual_LOOPLESS()
  : problem_data<L,D>()
  {

  }

  Primal_Dual_LOOPLESS(const char* matrix_file, const char* vector_file)
  : problem_data<L,D>(matrix_file, vector_file)
  {

  }

  Primal_Dual_LOOPLESS(const char* matrix_file)
  : problem_data<L,D>(matrix_file)
  {

  }

  void set_rng()
  {
    gsl_rng_env_setup();
    const gsl_rng_type * T;
    T = gsl_rng_default;
    rng = gsl_rng_alloc(T);
    gsl_rng_set(rng,time(NULL));
    //gsl_rng_set(rng, 27432042);

  }

  void set_print_every_N(L i){print_every_N=i;}

  void set_L1(L p){
    if(p==0){ //batch sampling mode  (stochastic process in the paper)
      L1=(1-1./tau)*Lf+L2;
    }else{ //group sampling mode
      L1=Lf+L2;
    }
    //  alpha_l=1/(4.*L1+2*L2);
  }

  void set_Li_Lf()
  {
    sumLi=0;
    maxv=0;
    D minv=std::numeric_limits<double>::max();
    Li.resize(this->nsamples,0);
    for(L i=0;i<this->nsamples;i++)
    {
      D vi=0;
      for (L k = this->ptr[i]; k < this->ptr[i + 1];k++)
      {
        vi+=lambda_f[i]*this->nsamples*this->A[k]*this->A[k];
      }
      Li[i]=vi;
      if(maxv<vi) maxv=vi;
      if(minv>vi) minv=vi;
    }
    for(L i=0;i<this->nsamples;i++){
      if(Li[i]<maxv*1e-15)
      {
        for (L k = this->ptr[i]; k < this->ptr[i + 1];k++)
        { this->A[k]=0;
          L j=this->row_idx[k];
          for (L kj = this->ptr_t[j]; kj < this->ptr_t[j + 1];	kj++)
          {
            L i2=this->col_idx[kj];
            if(i2==i) this->A_t[kj]=0;
          }
        }
        Li[i]=maxv*1e-15;
      }
      sumLi+=Li[i];
    }
    Lf=compute_lambda_max(10);
    cout<<"  max of v: "<<maxv<<"  min of v: "<<minv<<" sumof Li="<<sumLi<<" Lf= "<<Lf<<endl;
  }

  //compute the maximal eigenvalue of sum_{i=1}^m \lambda_f[i]*A_i*A_i' by power iteration
  D compute_lambda_max(L K){

    std::vector<D> bk(this->nfeatures);
    for (L j=0;j<this->nfeatures;j++)
    {
      bk[j]=1;
    }
    std::vector<D> yk(this->nsamples);
    D normk;
    D tmp;
    for(L kk=0;kk<K;kk++){
      for (L i=0;i<this->nsamples;i++){
        tmp=0;
        for (L k = this->ptr[i]; k < this->ptr[i + 1];	k++)
        {
          L j=this->row_idx[k];
          tmp+=this->A[k]*bk[j];
        }
        yk[i]=tmp;
      }
      normk=0;
      for (L j=0;j<this->nfeatures;j++){
        bk[j]=0;
        for (L k = this->ptr_t[j]; k < this->ptr_t[j + 1];	k++)
        {
          L i=this->col_idx[k];
          bk[j]+=this->A_t[k]*yk[i]*lambda_f[i];
        }
        normk+=bk[j]*bk[j];
      }
      normk=sqrt(normk);
      for (L j=0;j<this->nfeatures;j++)
      {bk[j]=bk[j]/normk; }
    }
    cout<<endl;
    D res=0;
    normk=0;
    for (L i=0;i<this->nsamples;i++){
      tmp=0;
      for (L k = this->ptr[i]; k < this->ptr[i + 1];	k++)
      {
        L j=this->row_idx[k];
        tmp+=this->A[k]*bk[j];
      }
      yk[i]=tmp;
      normk+=yk[i]*yk[i];
    }
    std::vector<D> bk2(this->nfeatures);
    for (L j=0;j<this->nfeatures;j++){
      bk2[j]=0;
      for (L k = this->ptr_t[j]; k < this->ptr_t[j + 1];	k++)
      {
        L i=this->col_idx[k];
        bk2[j]+=this->A_t[k]*yk[i]*lambda_f[i];
      }
    }
    for (L j=0;j<this->nfeatures;j++)
    res+=bk2[j]*bk[j];
    return res;
  }

  void set_optimal_probability()
  {
    tilde_proba_vector.clear();
    tilde_proba_vector.resize(this->nsamples,0);
    proba_vector.clear();
    proba_vector.resize(this->nsamples,0);
    D sum=0;
    for(L i=0; i<this->nsamples;i++)
    {
      tilde_proba_vector[i]=Li[i];
      sum+=Li[i];
    }
    max_p=0;
    for(L i=0; i<this->nsamples;i++)
    {
      tilde_proba_vector[i]=tilde_proba_vector[i]/sum;
      proba_vector[i]=1-pow(1-tilde_proba_vector[i],tau);
      if(max_p<tilde_proba_vector[i])
      {
        max_p=tilde_proba_vector[i];
      }
    }
    cout<<"sum="<<sum<<"; proba"<<tilde_proba_vector[0]<<endl;
  }

  void set_group_sampling_probability(){
    proba_vector.clear();
    proba_vector.resize(this->nsamples,0);
    std::vector<D> q(this->nsamples);
    D sumq=0;
    cout<<"start setting group sampling probablity"<<endl;
    for(L i=0; i<this->nsamples;i++)
    {
      q[i]=Li[i];
      sumq+=q[i];
    }
    D maxq=0;
    L nb=0;
    D tmp=0;
    for(L i=0; i<this->nsamples;i++)
    {
      q[i]=q[i]/sumq*tau;
      if(q[i]>maxq) maxq=q[i];
      if(q[i]>1) {
        nb++; //count the number of elements larger than 1
        tmp+=q[i]-1;
      }
    }
    cout<<"maxq="<<maxq<<endl;
    if(maxq<=1){
      for(L i=0; i<this->nsamples;i++)
      {
        proba_vector[i]=q[i];
      }
    }else{
      for(L i=0; i<this->nsamples;i++)
      {
        if(q[i]>1) q[i]=1;
        else {
          D deltaq=min(1-q[i],tmp);
          tmp=tmp-deltaq;
          q[i]+=deltaq;
        }
        proba_vector[i]=q[i];
      }
    }
  }

  void set_L2(L p_mod, L u){
    if(p_mod==0){ //batch sampling mode
      D tmp=0;
      D st;
      for(L i=0;i<this->nsamples;i++){
        st=Li[i]/tilde_proba_vector[i];
        if (st>tmp) tmp=st;
      }
      L2=tmp/this->nsamples/tau;
      cout<<"L2="<<L2<<"; "<<tilde_proba_vector[0]<<endl;
    }
    else{ //group sampling mode
      D tmp=0;
      D st;
      for(L i=0;i<this->nsamples;i++){
        st=Li[i]/proba_vector[i];
        if(isolated_I[i]==1) st=st-Li[i];
        if(st>tmp) tmp=st;
      }
      L2=tmp/this->nsamples;
    }
    cout<<"set_L2="<<L2<<endl;
  }

  void set_uniform_probability(L p_mod)
  {
    if(p_mod==0) {
      //batch sampling (stochastic process as defined in the paper)
      proba_vector.clear();
      tilde_proba_vector.clear();
      tilde_proba_vector.resize(this->nsamples,1./this->nsamples);
      proba_vector.resize(this->nsamples,1-pow(1-1.0/this->nsamples,tau));
      max_p=1.0/this->nsamples;
      cout<<"uniform="<<tilde_proba_vector[0]<<endl;
    }
    else{ //group sampling
      D pi=(tau*this->c+0.0)/this->nsamples;
      proba_vector.clear();
      proba_vector.resize(this->nsamples,pi);
    }
  }

  void sort_p(){
    vector<pair<D,L> >a;
    for (L i = 0 ;i < this->nsamples ; i++) {
      a.push_back(make_pair(proba_vector[i],i)); // k = value, i = original index
    }
    sort(a.begin(),a.end());
    group_C.clear();
    group_C.resize(this->nsamples);
    isolated_I.clear();
    isolated_I.resize(this->nsamples,0);
    D tmp=0;
    D maxpi=0;
    D previous_i=0;
    index_group_C.clear();
    index_group_C.push_back(0);
    maxp_group_C.clear();
    sump_group_C.clear();

    for (L i = 0 ;i < this->nsamples ; i++){
      group_C[i]=a[this->nsamples-1-i].second;
      D pi=a[this->nsamples-1-i].first;
      if(tmp+pi<=1){
        if(pi>maxpi) maxpi=pi;
        tmp+=pi;
      }
      else{
        index_group_C.push_back(i);
        maxp_group_C.push_back(maxpi);
        sump_group_C.push_back(tmp);
        tmp=pi;
        maxpi=pi;
        if(i-previous_i==1) isolated_I[group_C[previous_i]]=1;
        previous_i=i;
      }
    }
    index_group_C.push_back(this->nsamples);
    maxp_group_C.push_back(maxpi);
    sump_group_C.push_back(tmp);
    nb_groups=sump_group_C.size();
    if(previous_i==this->nsamples-1) isolated_I[group_C[previous_i]]=1;
    cout<<"size of group="<<nb_groups<<endl;
  }

  inline L sampling(L n)
  {
    L i=(floor)(gsl_rng_uniform(rng)*n);
    D y=gsl_rng_uniform(rng);
    while(y*max_p>tilde_proba_vector[i])
    {
      i=(floor)(gsl_rng_uniform(rng)*n);
      y=gsl_rng_uniform(rng);
    }
    return i;
  }

  void set_initial_dual(){
    for(L i=0;i<this->nsamples;i++)
    {
      D res=0;
      for (L k = this->ptr[i]; k < this->ptr[i + 1];k++)
      {
        L j=this->row_idx[k];
        res+=this->A[k]*primal_x[j];
      }
      dual_alpha[i]=gradient_of_phi_i(res,i);
    }
  }

  void update_primal()
  {
    L nb_indices=0;
    //#pragma omp parallel for
    for(L i_t=0;i_t<batch_size;i_t++)
    {
      L i=batch_i[i_t];
      D deltaalphai=batch_deltaalphai[i_t];
      for (L k = this->ptr[i]; k < this->ptr[i + 1];k++)
      {
        L j=this->row_idx[k];
        if (whether_or_not_to_do_prox[j]==0) {
          //#pragma omp critical
          {
          whether_or_not_to_do_prox[j]=1;
          index_to_do_prox[nb_indices]=j;
          nb_indices++;
          deltagk[j]=0;
          }
        }
        deltagk[j]+=lambda_f[i]*deltaalphai*this->A[k]*theta_S[i];
      }
    }
    #pragma omp parallel for
    for(L i_d=0;i_d<nb_indices;i_d++){
      L j=index_to_do_prox[i_d];
      D xj=compute_current_xj_value(baralpha[j]+deltagk[j], j,last_update_i[j],current_nb_iters);
      //update_x(xj,j);
      last_update_i[j]=current_nb_iters;
      whether_or_not_to_do_prox[j]=0;
      index_to_do_prox[i_d]=-1;
    }

  }

  void update_baralpha(L  i, D deltaalphai)
  {
    for (L k = this->ptr[i]; k < this->ptr[i + 1];	k++)
    {
      L j=this->row_idx[k];
      baralpha[j]+=lambda_f[i]*deltaalphai*this->A[k];
    }
  }

  D compute_AiTxk(L i) //This function compute A_i^\top x^k
  {
    D res=0;
    D xj;
    for (L k = this->ptr[i]; k < this->ptr[i + 1];k++)
    {
      L j=this->row_idx[k];
      if(current_nb_iters!=last_update_i[j])
      {
        xj=compute_current_xj_value(baralpha[j], j,last_update_i[j],current_nb_iters);
        //update_x(xj,j);
      }
      res+=this->A[k]*primal_x[j];
      last_update_i[j]=current_nb_iters;
    }
    return res;
  }

  D compute_AiTxk_without_update(L i) //This function compute A_i^\top x^k  without updating x^k
  {
    D res=0;
    D xj;
    //#pragma omp parallel for reduction(+:res)
    for (L k = this->ptr[i]; k < this->ptr[i + 1];k++)
    {
      L j=this->row_idx[k];
      if(current_nb_iters!=last_update_i[j])
      {
        xj=compute_current_xj_value_without_update(baralpha[j], j,last_update_i[j],current_nb_iters);
      }
      else {
        xj=primal_x[j];
      }
      res+=this->A[k]*xj;
    }
    return res;
  }

  void compute_dual_value()
  {
    D res=0;
    //D alphaAx= 0;
    std::vector<D> alpha_tmp(this->nsamples,0);
    std::vector<D> baralpha_tmp(this->nfeatures,0);
    //#pragma omp parallel for schedule(dynamic)
    for(L i=0;i<this->nsamples;i++)
    {
      D aitx=compute_AiTxk_without_update(i);
      alpha_tmp[i]=gradient_of_phi_i(aitx,i);
      //alphaAx+= lambda_f[i]*alpha_tmp[i]*aitx;
      for (L k = this->ptr[i]; k < this->ptr[i + 1];	k++)
      {
        L j=this->row_idx[k];
        baralpha_tmp[j]+=lambda_f[i]*alpha_tmp[i]*this->A[k];
      }
    }

    D scal=feasible_dual(baralpha_tmp);
    //#pragma omp parallel for reduction(-:res)
    for(L i=0;i<this->nsamples;i++)
    {
      res-=value_of_phistar_i(scal*alpha_tmp[i],i)*lambda_f[i];
    }
    res-=value_of_gstar_minus(baralpha_tmp,scal); // should have used value_of_gstar(-baralpha)
    dual_value=res;
  }

  void compute_primal_value()
  {
    D res=0;
    //#pragma omp parallel for reduction(+:res)
    for(L i=0;i<this->nsamples;i++)
    {
      D aitx=compute_AiTxk_without_update(i);
      res+=lambda_f[i]*value_of_phi_i(aitx,i);
    }

    D res2=0;
    //#pragma omp parallel for reduction(+:res2)
    for(L j=0; j<this->nfeatures; j++)
    {
      D xj=compute_current_xj_value_without_update(baralpha[j], j,last_update_i[j],current_nb_iters);
      D gj=value_of_g_j(xj,j);
      res2+=gj;
    }
    primal_value= res+res2;
  }

  void compute_initial_dual_value()
  {

    D res=0;
    D scal=feasible_dual(baralpha);
    #pragma omp parallel for reduction(-:res)
    for(L i=0;i<this->nsamples;i++)
    {
      res-=value_of_phistar_i(scal*dual_alpha[i],i)*lambda_f[i];
    }
    //cout<< "d_1(x)= "<< res<< " d_2(x)= "<< value_of_gstar_minus(baralpha,scal)<< endl;
    res-=value_of_gstar_minus(baralpha,scal);  // should have used value_of_gstar(-baralpha)
    dual_value=res;
  }

  void compute_initial_primal_value()
  {
    D res=0;
    #pragma omp parallel for reduction(+:res)
    for(L j=0; j<this->nfeatures; j++)
    {
      res+=value_of_g_j(primal_x[j],j);
    }
    D res2=0;
    #pragma omp parallel for reduction(+:res2)
    for(L i=0;i<this->nsamples;i++)
    {
      D aitx=0;
      for (L k = this->ptr[i]; k < this->ptr[i + 1];	k++)
      {
        L j=this->row_idx[k];
        aitx+=this->A[k]*primal_x[j];
      }
      res2+=lambda_f[i]*value_of_phi_i(aitx,i);
    }
    //cout<< "p_1(x)= "<< res<< " p_2(x)= "<< res2<< endl;
    primal_value= res+res2;
  }

  void set_p(D scal_p){

    if(scal_p==4.6){
      p=sqrt(this->mu/L1*tau/this->nsamples);
    }
    else if (scal_p==5.6){
      p=this->mu/L1;
    }
    else{
      p=scal_p*tau/(0.0+this->nsamples);
    }
    cout<<"changing probablity: "<<p<<endl;
  }

  void initialize(vector<D> & x0, vector<D> & w0, vector<D> & L_phi, D val_mu, D val_epsilon, L max_nb, L nb_tau, L nb_c, L u, L p_mod, D scal_p)
  {
    cout<<"start initializing"<<" u="<<u<<endl;
    this->distributed_set_up(nb_c);
    if(nb_tau>this->noverc) perror("tau should be less than n over c");
    tau=nb_tau;
    nb_of_iters_per_loop=floor(this->nsamples/(this->c*(tau+0.0)));
    batch_i.clear();
    batch_deltaalphai.clear();
    batch_i.resize(this->nsamples,0);
    batch_deltaalphai.resize(this->nsamples,0);

    deltagk.clear();
    deltagk.resize(this->nfeatures,0);
    index_to_do_prox.clear();
    index_to_do_prox.resize(this->nfeatures,-1);
    whether_or_not_to_do_prox.clear();
    whether_or_not_to_do_prox.resize(this->nfeatures,0);
    gk.clear();
    gk.resize(this->nfeatures,0);

    /**setup parameters**/
    epsilon=val_epsilon;
    max_nb_loops=max_nb;
    this->mu=val_mu;
    cout<<"mu="<<this->mu<<endl;
    lambda_f.clear();
    lambda_f.resize(this->nsamples,0); //L_phi is the Lipschitz constant of the function \phi_i.
    for(int i=0;i< this->nsamples; i++){
      lambda_f[i]= L_phi[i]/this->nsamples;
    }

    set_rng();
    set_auxiliary_v();

    lambda_1=get_lambda1();
    lambda_2=get_lambda2();

    set_Li_Lf();

    running_time=0;
    /**setup probability**/
    if(p_mod==0)   // batch sampling (stochastic process as defined in the paper)
    {
      if(u==0)   // uniform sampling
      {
        set_uniform_probability(p_mod);
        this->uniform="uniform";
      }
      else{
        set_optimal_probability();
        this->uniform="nonuniform";
      }
    }
    else{  //group sampling
      if(u==0){
        set_uniform_probability(p_mod);
        this->uniform="uniform";
      }else{
        this->set_group_sampling_probability();
        this->uniform="nonuniform";
      }
      sort_p();
      cout<<"sort p"<<endl;
    }

    set_L2(p_mod,u);
    scaler=scal_p;

    set_L1(p_mod);
    cout<<"set L1="<<L1<<endl;
    set_p(scal_p);
    set_thetaS(p_mod);
    cout<<"setthetaS"<<endl;

    primal_x=w0;
    dual_alpha=x0;
    set_stepsize();
    cout<<"set stepsize"<<endl;

    set_initial_dual();

    baralpha.clear();
    baralpha.resize(this->nfeatures,0);
    last_update_i.clear();
    last_update_i.resize(this->nfeatures,0);
    for(L i=0;i<this->nsamples;i++)
    {
      update_baralpha(i, dual_alpha[i]);
    }

    current_nb_iters=0;
    D start_time_initial=std::clock();
    compute_initial_primal_value();
    compute_initial_dual_value();
    D end_time_initial=std::clock();
    cout<<"computation of initial primal dual value time="<<(end_time_initial-start_time_initial) / (double) CLOCKS_PER_SEC<<endl;

    cout<<"primal value="<<primal_value<<endl;
    cout<<"dual_value="<<dual_value<<endl;

    cout<<"Initialization is finished!"<<endl;
  }

  void result_record()
  {
    cout<<primal_value<<endl;
    cout<<dual_value<<endl;
    ofstream result;
    result.open("results/x0.dat");
    for (L j=0;j<this->nfeatures;j++){
      result <<setprecision(20)<< primal_x[j] << " ";
      //cout<< primal_x[j] << " ";
    }
    result << endl;
    ofstream resulta;
    resulta.open("results/alpha0.dat");
    for (L i=0;i<this->nsamples;i++)
    resulta << setprecision(20)<<dual_alpha[i] << " ";
    resulta << endl;
    result.close();
  }

  void read_w0(){
    ifstream recordw0("results/x0.dat");
    for(L j=0;j<this->nfeatures;j++)
    {
      recordw0>>primal_x[j];
    }
    recordw0.close();

    ifstream recordalpha0("results/alpha0.dat");
    for(L i=0;i<this->nsamples;i++)
    {
      recordalpha0>>dual_alpha[i];
    }
    recordalpha0.close();
  }

  void compute_and_record_result()
  {
    if(nb_loops%print_every_N==0){
      D start = std::clock();
      compute_primal_value();
      D endt=std::clock();
      cout<<"computation of primal value time="<<(endt-start) / (double) CLOCKS_PER_SEC<<endl;
      compute_dual_value();
      delta=primal_value-dual_value;
      cout<<setprecision(16)<<floor(((0.0+nb_iters)*this->c*tau/(this->nsamples)))<<";;;  "<<running_time<<"; primal dual gap="<<delta<<" primal value="<<primal_value<<"; dual value="<<dual_value<<endl;
      samp<<floor(((0.0+nb_iters)*this->c*tau/(this->nsamples)))<<" "<<delta<<" "<<running_time<<" "<<primal_value<<endl;
    }

  }

  void batch_sampling()
  {
    batch_size=0;
    L i;
    for(L it_t=0;it_t<tau;it_t++)
    {
      i=sampling(this->nsamples);
      batch_i[batch_size]=i;
      batch_size++;
    }
  }

  void set_thetaS(L p_mod){
    theta_S.clear();
    theta_S.resize(this->nsamples,0);
    if(p_mod==0){
      for(L i=0;i<this->nsamples;i++)
      theta_S[i]=1.0/(tau*tilde_proba_vector[i]);
    }
    else{
      for(L i=0;i<this->nsamples;i++)
      theta_S[i]=1.0/proba_vector[i];
    }
  }

  void group_sampling(){
    batch_size=0;
    std::vector<L> sampled_groups;
    for(L i=0;i<nb_groups;i++){
      D y=gsl_rng_uniform(rng);
      if(y<sump_group_C[i])
      {
        sampled_groups.push_back(i);
        batch_size++;
      }
    }
    for(L t=0;t<batch_size;t++){
      L group_i=sampled_groups[t];
      L s1=index_group_C[group_i];
      L s2=index_group_C[group_i+1];
      L i=s1+(floor)(gsl_rng_uniform(rng)*(s2-s1));
      D y=gsl_rng_uniform(rng);
      D maxpi=maxp_group_C[group_i];
      while(y*maxpi>proba_vector[group_C[i]])
      {
        i=s1+(floor)(gsl_rng_uniform(rng)*(s2-s1));
        y=gsl_rng_uniform(rng);
      }
      batch_i[t]=group_C[i];
    }
  }

  void loopless(vector<D> & x0, vector<D> & w0, string filename, vector<D> & L_phi, D val_mu, D val_epsilon, L max_nb, L nb_tau, L nb_c, L u, L p_mod, D scal_p)
  {
    initialize(x0,  w0,  L_phi, val_mu, val_epsilon,  max_nb,  nb_tau, nb_c,  u, p_mod, scal_p);
    string sampname="results/L_"+filename+uniform;
    if(p_mod==0) sampname=sampname+"_batch";
    else sampname=sampname+"_group";
    string scal_str;
    stringstream  scal_convert;
    scal_convert<<scal_p;
    scal_str=scal_convert.str();
    sampname+=scal_str;
    cout<<"running Loopless"<<" ; "<<sampname<<endl;
    samp.open(sampname.c_str());

    delta=primal_value-dual_value;
    nb_loops=0;
    nb_iters=0;

    cout<<setprecision(9)<<"initial: "<<" delta: "<<delta<<" primal: "<<primal_value<<"; dual: "<<dual_value<<"epsilon="<<epsilon<<endl;
    samp<<((0.0+nb_iters)*this->c*tau/(this->nsamples))<<" "<<delta<<running_time<<" "<<primal_value<<endl;
    srand48(27432042);
    //srand(time(NULL));
    printf("There are %d threads\n",omp_get_num_threads());

    D start;
    while(delta>=epsilon && nb_loops<max_nb_loops)
    {
      compute_and_record_result();

      if(delta<epsilon) break;
      start = std::clock();

      nb_loops++;

      //cout<<"before the loop time elapsed="<<duration<<endl;
      start = std::clock();

      //cout<<"nb_of_iters_per_loop="<<nb_of_iters_per_loop<<endl;
      for(L it=0;it<nb_of_iters_per_loop;it++)
      {
        if(p_mod==0)
        batch_sampling();
        else
        group_sampling();

        //start = std::clock();
        start=omp_get_wtime();
        #pragma omp parallel
        {
          #pragma omp for
          for(L i_t=0;i_t<batch_size;i_t++)
          {
          L i=batch_i[i_t];
          D aitg=compute_AiTxk(i);
          D deltaalphai=(gradient_of_phi_i(aitg,i)-dual_alpha[i]);
          batch_deltaalphai[i_t]=deltaalphai;
         }
      }
        //cout<<omp_get_wtime()-start<<endl;
        current_nb_iters++;
        update_primal();
        update_baralpha();
        nb_iters++;
        D end_time = omp_get_wtime();
        //running_time+= ( std::clock() - start ) / (double) CLOCKS_PER_SEC;
        running_time+=end_time-start;

      }

      //cout<<"after the loop time elapsed="<<duration<<endl;
    }
    if(delta>=epsilon) {compute_primal_value();
      compute_dual_value();
      delta=primal_value-dual_value;
      cout<<setprecision(9)<<floor(((0.0+nb_iters)*this->c*tau/(this->nsamples)))<<";  "<<running_time<<"; primal dual gap="<<delta<<" primal value="<<primal_value<<"; dual value="<<dual_value<<endl;
      samp<<floor(((0.0+nb_iters)*this->c*tau/(this->nsamples)))<<" "<<delta<<" "<<running_time<<" "<<primal_value<<endl;
    }
    cout<<setprecision(9)<<"nb_loops: "<<nb_loops<<"nb_iters"<<floor(((0.0+nb_iters)*this->c*tau/(this->nsamples)))<<";  delta: "<<delta<<" primal: "<<primal_value<<"; dual: "<<dual_value<<endl;
    samp.close();
  }

  void loopless2(vector<D> & x0, vector<D> & w0, string filename, vector<D> & L_phi, D val_mu, D val_epsilon, L max_nb, L nb_tau, L nb_c, L u, L p_mod, D scal_p)
  {
    initialize(x0,  w0,  L_phi, val_mu, val_epsilon,  max_nb,  nb_tau, nb_c,  u, p_mod, scal_p);
    string sampname="results/L_"+filename+uniform;
    if(p_mod==0) sampname=sampname+"_batch";
    else sampname=sampname+"_group";
    string scal_str;
    stringstream  scal_convert;
    scal_convert<<scal_p;
    scal_str=scal_convert.str();
    sampname+=scal_str;
    cout<<"running Loopless"<<" ; "<<sampname<<endl;
    samp.open(sampname.c_str());

    delta=primal_value-dual_value;
    nb_loops=0;
    nb_iters=0;

    cout<<setprecision(9)<<"initial: "<<" delta: "<<delta<<" primal: "<<primal_value<<"; dual: "<<dual_value<<endl;
    samp<<((0.0+nb_iters)*this->c*tau/(this->nsamples))<<" "<<delta<<running_time<<" "<<primal_value<<endl;
    //srand48(27432042);
    srand(time(NULL));

    D start;
    while(nb_loops<max_nb_loops)
    {
      compute_and_record_result();

      start = std::clock();

      nb_loops++;

      //cout<<"before the loop time elapsed="<<duration<<endl;
      start = std::clock();
      //cout<<"nb_of_iters_per_loop="<<nb_of_iters_per_loop<<endl;
      for(L it=0;it<nb_of_iters_per_loop;it++)
      {
        if(p_mod==0)
        batch_sampling();
        else
        group_sampling();

        start = std::clock();
        for(L i_t=0;i_t<batch_size;i_t++)
        {
          L i=batch_i[i_t];
          D aitg=compute_AiTxk(i);
          D deltaalphai=(gradient_of_phi_i(aitg,i)-dual_alpha[i]);
          batch_deltaalphai[i_t]=deltaalphai;
        }
        current_nb_iters++;
        update_primal();
        update_baralpha();
        nb_iters++;
        running_time+= ( std::clock() - start ) / (double) CLOCKS_PER_SEC;
      }
      //cout<<"after the loop time elapsed="<<duration<<endl;
    }
    cout<<setprecision(9)<<"nb_loops: "<<floor(((0.0+nb_iters)*this->c*tau/(this->nsamples)))<<";  delta: "<<delta<<" primal: "<<primal_value<<"; dual: "<<dual_value<<endl;
    samp.close();
  }

};

#endif /* MIN_SMOOTH_CONVEX_H */


/*
 # Copyright (C) 2009-2013 Thierry Sousbie
 # University of Tokyo / CNRS, 2009
 #
 # This file is part of porject DisPerSE
 #
 #  Author          : Thierry Sousbie
 #  Contact         : tsousbie@gmail.com
 #
 #  Licenses        : This file is 'dual-licensed', you have to choose one
 #                    of the two licenses below to apply.
 #
 #                    CeCILL-C
 #                    The CeCILL-C license is close to the GNU LGPL.
 #                    ( http://www.cecill.info/licences/Licence_CeCILL-C_V1-en.html )
 #
 #                or  CeCILL v2.0
 #                    The CeCILL license is compatible with the GNU GPL.
 #                    ( http://www.cecill.info/licences/Licence_CeCILL_V2-en.html )
 #
 #  This software is governed either by the CeCILL or the CeCILL-C license
 #  under French law and abiding by the rules of distribution of free software.
 #  You can  use, modify and or redistribute the software under the terms of
 #  the CeCILL or CeCILL-C licenses as circulated by CEA, CNRS and INRIA
 #  at the following URL : "http://www.cecill.info".
 #
 #  As a counterpart to the access to the source code and  rights to copy,
 #  modify and redistribute granted by the license, users are provided only
 #  with a limited warranty  and the software's author,  the holder of the
 #  economic rights,  and the successive licensors  have only  limited
 #  liability.
 #
 #  In this respect, the user's attention is drawn to the risks associated
 #  with loading,  using,  modifying and/or developing or reproducing the
 #  software by the user in light of its specific status of free software,
 #  that may mean  that it is complicated to manipulate,  and  that  also
 #  therefore means  that it is reserved for developers  and  experienced
 #  professionals having in-depth computer knowledge. Users are therefore
 #  encouraged to load and test the software's suitability as regards their
 #  requirements in conditions enabling the security of their systems and/or
 #  data to be ensured and,  more generally, to use and operate it in the
 #  same conditions as regards security.
 #
 #  The fact that you are presently reading this means that you have had
 #  knowledge of the CeCILL and CeCILL-C licenses and that you accept its terms.
*/
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include "dtfi.h"

#ifdef HAVE_GSL
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

// mu is the average value
unsigned int randomPoisson(double mu)
{
  static int first_call = 1;
  static const gsl_rng_type *T;
  static gsl_rng * r;

  if (first_call) {
    first_call=0;
    gsl_rng_env_setup();
    T = gsl_rng_default;
    r = gsl_rng_alloc(T);
  }

  return gsl_ran_poisson (r, mu);
}

void randomSimplexCoords(double **coord, int ndims, double *result, double *P)
{
  static int first_call = 1;
  static const gsl_rng_type *T;
  static gsl_rng * r;
  int i,j,k;
  double u;
  //double P[50];

  if (first_call) {
    first_call=0;
    gsl_rng_env_setup();
    T = gsl_rng_default;
    r = gsl_rng_alloc (T);
  }

  result[0]=gsl_rng_uniform (r);

  if (ndims==3)
    {
      u=gsl_rng_uniform (r);
      if (u<result[0])
	{
	  result[1]=result[0];
	  result[0]=u;
	}
      else result[1]=u;

      u=gsl_rng_uniform (r);
      if (u<result[0])
	{
	  result[2]=result[1];
	  result[1]=result[0];
	  result[0]=u;
	}
      else if (u<result[1])
	{
	  result[2]=result[1];
	  result[1]=u;
	}
      else result[2]=u;

    }
  else
    {
      for (i=1;i<ndims;i++)
	{
	  u = gsl_rng_uniform (r);

	  for (j=0;j<i;j++)
	    if (u<result[j])
	      {
		result[i]=result[j];
		result[j]=u;
		break;
	      }

	  if (j==i) result[i]=u;
	  else {
	    j++;
	    for (;j<i;j++)
	      for (k=j+1;k<=i;k++)
		{
		  if (result[j]>result[k])
		    {
		      double tmp = result[k];
		      result[k]=result[j];
		      result[j]=tmp;
		    }
		}
	  }
	}
    }

  P[0]=result[0];
  for (i=1;i<ndims;i++) P[i]= result[i]-result[i-1];
  P[ndims]=1.-result[ndims-1];

  for (i=0;i<ndims;i++) result[i]=P[0]*coord[0][i];

  for (i=1;i<ndims+1;i++)
    for (j=0;j<ndims;j++)
      result[j]+=P[i]*coord[i][j];

}

void genDTFIInSimplex(double **coord, double *Proba, int ngen, int ndims, double **result_p)
{

  if (*result_p==NULL) {
    *result_p=(double*)malloc(sizeof(double)*ngen*ndims);
  }

  double *result = *result_p;
  double P[ndims];
  int i,n;
  double keepProba;
  double pmax_inv=0;

  static int first_call = 1;
  static const gsl_rng_type *T;
  static gsl_rng * r;

  //if (ngen>100) printf("negen = %d\n",ngen);

  if (first_call) {
    first_call=0;
    gsl_rng_env_setup();
    T = gsl_rng_default;
    r = gsl_rng_alloc (T);
  }

  if (Proba!=NULL) {
    pmax_inv=0;
    for (i=0;i<ndims+1;i++)
      if (Proba[i]>pmax_inv) pmax_inv=Proba[i];
    pmax_inv=(double)1./pmax_inv;
  }

  for (n=0;n<ngen;)
    {
      randomSimplexCoords(coord,ndims,&(result[n*ndims]),P);
      if (Proba!=NULL) {
	keepProba=Proba[0]*pmax_inv*P[0];
	for (i=1;i<ndims+1;i++) keepProba+=Proba[i]*pmax_inv*P[i];

	if (gsl_rng_uniform (r)<=keepProba) n++;
      } else n++;
    }

}

#else

unsigned int randomPoisson(double mu)
{
  fprintf(stderr,"ERROR: Function 'randomPoisson' needs GSL.\n");
  fprintf(stderr,"Please, recompile with GSL.\n");

  return -1;
}

void randomSimplexCoords(double **coord, int ndims, double *result, double *P)
{
  fprintf(stderr,"ERROR: Function 'randomSimplexCoords' needs GSL.\n");
  fprintf(stderr,"Please, recompile with GSL.\n");

  return -1;
}

void genDTFIInSimplex(double **coord, double *Proba, int ngen, int ndims, double **result_p)
{
  fprintf(stderr,"ERROR: Function 'genDTFIInSimplex' needs GSL.\n");
  fprintf(stderr,"Please, recompile with GSL.\n");
}

#endif


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

  Library   : Image Registration Toolkit (IRTK)
  Module    : $Id$
  Copyright : Imperial College, Department of Computing
              Visual Information Processing (VIP), 2008 onwards
  Date      : $Date$
  Version   : $Revision$
  Changes   : $Author$

Copyright (c) 1999-2014 and onwards, Imperial College London
All rights reserved.
See LICENSE for details

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

#ifndef _IRTKVECTOR_H

#define _IRTKVECTOR_H

#ifdef USE_VXL
// VXL header file was loaded in irtkMatrix.h
#include <vnl/vnl_diag_matrix.h>
#else
#include <gsl/gsl_vector.h>
#endif

#include <iostream>
#include <cmath>

#include <irtkObject.h>

/**

  Vector class.

*/

class irtkVector : public irtkObject
{

protected:

  /// Number of rows
  int _rows;

  /// Data
  double *_vector;

public:

  /// Default constructor
  irtkVector();

  /// Constructor for given row dimensions
  irtkVector(int);

  /// Copy constructor
  irtkVector(const irtkVector &);

  /// Destructor
  ~irtkVector();

  /// Intialize matrix with number of rows
  void Initialize(int);

  //
  // Vector access functions
  //

  /// Returns number of rows
  int Rows() const;

  /// Puts vector value
  void   Put(int, double);

  /// Gets vector value
  double Get(int) const;

  //
  // Operators for vector access
  //

  /// Puts vector value
  double &operator()(int);

  /// Gets vector value
  double  operator()(int) const;

  //
  // Vector operators for doubles
  //

  /// Subtraction of a double
  irtkVector& operator-=(double);

  /// Addition of a double
  irtkVector& operator+=(double);

  /// Multiplication with a double
  irtkVector& operator*=(double);

  /// Division by a double
  irtkVector& operator/=(double);

  /// Return result of subtraction of a double
  irtkVector  operator- (double);

  /// Return result of addition of a double
  irtkVector  operator+ (double);

  /// Return result of multiplication with a double
  irtkVector  operator* (double);

  /// Return result of division by a double
  irtkVector  operator/ (double);

  //
  // Vector operators for vectors
  //

  /// Vector copy operator
  irtkVector& operator =(const irtkVector&);

  /// Vector subtraction operator
  irtkVector& operator-=(const irtkVector&);

  /// Vector addition operator
  irtkVector& operator+=(const irtkVector&);

  /// Vector componentwise multiplication operator (no scalar nor cross product)
  irtkVector& operator*=(const irtkVector&);

  /// Vector componentwise division operator
  irtkVector& operator/=(const irtkVector&);

  /// Return result for vector subtraction
  irtkVector  operator- (const irtkVector&);

  /// Return result for vector addition
  irtkVector  operator+ (const irtkVector&);

  /// Return result for componentwise vector multiplication (no scalar nor cross product)
  irtkVector  operator* (const irtkVector&);

  /// Return result for componentwise vector division
  irtkVector  operator/ (const irtkVector&);

  /// Comparison operator ==
  bool operator==(const irtkVector &);

#ifndef USE_STL
  /// Comparison operator != (if USE_STL is defined, negate == operator)
  bool operator!=(const irtkVector &);
#endif

  /// Comparison operator <
  bool operator<(const irtkVector &);

  /// Scalar/dot product
  double ScalarProduct(const irtkVector&);

  /// Vector/cross product
  irtkVector CrossProduct(const irtkVector&);

  /// Returns norm of a vector
  double Norm(void) const;

  /// Vector normalization
  void   Normalize(void);

  //
  // Vector in- and output
  //

  /// Interface to output stream
  friend std::ostream& operator<< (std::ostream&, const irtkVector&);

  /// Interface to input stream
  friend std::istream& operator>> (std::istream&, irtkVector&);

  /// Print vector
  void Print();

  /// Read vector from file
  void Read (char *);

  /// Write vector to file
  void Write(char *);

#ifdef USE_VXL
  template <class T> void Vector2Vnl(vnl_diag_matrix<T>*) const;

  template <class T> void Vnl2Vector(vnl_diag_matrix<T>*);

#else
  /// Conversion to GSL vector (memory must be allocated)
  void Vector2GSL(gsl_vector *) const;

  /// Conversion from GSL vector
  void GSL2Vector(gsl_vector *);
#endif

};

//
// Access operators
//

inline int irtkVector::Rows() const
{
  return _rows;
}

inline void irtkVector::Put(int rows, double vector)
{
  _vector[rows] = vector;
}

inline double irtkVector::Get(int rows) const
{
  return _vector[rows];
}

inline double& irtkVector::operator()(int rows)
{
  return _vector[rows];
}

inline double irtkVector::operator()(int rows) const
{
  return _vector[rows];
}

//
// Vector operators for doubles
//

inline irtkVector& irtkVector::operator-=(double x)
{
  int i;

  for (i = 0; i < _rows; i++) {
    _vector[i] -= x;
  }

  return *this;
}

inline irtkVector& irtkVector::operator+=(double x)
{
  int i;

  for (i = 0; i < _rows; i++) {
    _vector[i] += x;
  }

  return *this;
}

inline irtkVector& irtkVector::operator*=(double x)
{
  int i;

  for (i = 0; i < _rows; i++) {
    _vector[i] *= x;
  }

  return *this;
}

inline irtkVector& irtkVector::operator/=(double x)
{
  int i;

  for (i = 0; i < _rows; i++) {
    _vector[i] /= x;
  }

  return *this;
}

inline irtkVector irtkVector::operator- (double x)
{
  int i;

  irtkVector m(_rows);
  for (i = 0; i < _rows; i++) {
    m._vector[i] = _vector[i] - x;
  }
  return m;
}

inline irtkVector irtkVector::operator+ (double x)
{
  int i;

  irtkVector m(_rows);
  for (i = 0; i < _rows; i++) {
    m._vector[i] = _vector[i] + x;
  }
  return m;
}

inline irtkVector irtkVector::operator* (double x)
{
  int i;

  irtkVector m(_rows);
  for (i = 0; i < _rows; i++) {
    m._vector[i] = _vector[i] * x;
  }
  return m;
}

inline irtkVector irtkVector::operator/ (double x)
{
  int i;

  irtkVector m(_rows);
  for (i = 0; i < _rows; i++) {
    m._vector[i] = _vector[i] / x;
  }
  return m;
}

//
// Vector operators for vectors
//

inline irtkVector& irtkVector::operator =(const irtkVector& v)
{
  int i;

  // Copy size
  this->Initialize(v._rows);

  // Copy vector
  for (i = 0; i < _rows; i++) {
    _vector[i] = v._vector[i];
  }
  return *this;
}

inline irtkVector& irtkVector::operator-=(const irtkVector& v)
{
  int i;

  if (_rows != v._rows) {
    std::cerr << "irtkVector::operator-=: Size mismatch" << std::endl;
    exit(1);
  }
  for (i = 0; i < _rows; i++) {
    _vector[i] -= v._vector[i];
  }

  return *this;
}

inline irtkVector& irtkVector::operator+=(const irtkVector& v)
{
  int i;

  if (_rows != v._rows) {
    std::cerr << "irtkVector::operator+=: Size mismatch" << std::endl;
    exit(1);
  }
  for (i = 0; i < _rows; i++) {
    _vector[i] += v._vector[i];
  }

  return *this;
}

inline irtkVector& irtkVector::operator*=(const irtkVector& v)
{
  int i;

  if (_rows != v._rows) {
    std::cerr << "irtkVector::operator*=: Size mismatch" << std::endl;
    exit(1);
  }
  for (i = 0; i < _rows; i++) {
    _vector[i] *= v._vector[i];
  }

  return *this;
}

inline irtkVector& irtkVector::operator/=(const irtkVector& v)
{
  int i;

  if (_rows != v._rows) {
    std::cerr << "irtkVector::operator/=: Size mismatch" << std::endl;
    exit(1);
  }
  for (i = 0; i < _rows; i++) {
    _vector[i] /= v._vector[i];
  }

  return *this;
}

inline irtkVector irtkVector::operator- (const irtkVector& v)
{
  int i;

  if (_rows != v._rows) {
    std::cerr << "irtkVector::operator-: Size mismatch" << std::endl;
    exit(1);
  }
  irtkVector m(_rows);
  for (i = 0; i < _rows; i++) {
    m._vector[i] = _vector[i] - v._vector[i];
  }
  return m;
}

inline irtkVector irtkVector::operator+ (const irtkVector& v)
{
  int i;

  if (_rows != v._rows) {
    std::cerr << "irtkVector::operator+: Size mismatch" << std::endl;
    exit(1);
  }
  irtkVector m(_rows);
  for (i = 0; i < _rows; i++) {
    m._vector[i] = _vector[i] + v._vector[i];
  }
  return m;
}

inline irtkVector irtkVector::operator* (const irtkVector& v)
{
  int i;

  if (_rows != v._rows) {
    std::cerr << "irtkVector::operator*: Size mismatch" << std::endl;
    exit(1);
  }
  irtkVector m(_rows);
  for (i = 0; i < _rows; i++) {
    m._vector[i] = _vector[i] * v._vector[i];
  }
  return m;
}

inline irtkVector irtkVector::operator/ (const irtkVector& v)
{
  int i;

  if (_rows != v._rows) {
    std::cerr << "irtkVector::operator/: Size mismatch" << std::endl;
    exit(1);
  }
  irtkVector m(_rows);
  for (i = 0; i < _rows; i++) {
    m._vector[i] = _vector[i] / v._vector[i];
  }
  return m;
}

//
// Comparison
//

inline bool  irtkVector::operator==(const irtkVector &v)
{
  if (_rows != v._rows) {
    return false;
  }
  for (int i = 0; i < _rows; i++) {
    if (_vector[i] != v._vector[i]) return false;
  }
  return true;

}

#ifndef USE_STL
inline bool  irtkVector::operator!=(const irtkVector &v)
{
  if (_rows != v._rows) {
    return true;
  }
  for (int i = 0; i < _rows; i++) {
    if (_vector[i] != v._vector[i]) return true;
  }
  return false;
}
#endif

inline bool  irtkVector::operator<(const irtkVector &v)
{
  if (_rows > v._rows) {
    return false;
  }
  for (int i = 0; i < _rows; i++) {
    if (_vector[i] >= v._vector[i]) return false;
  }
  return true;

}

//
// Vector products
//

inline double irtkVector::ScalarProduct(const irtkVector &v)
{
  int i;
  double scalar_product=0;

  if (_rows != v._rows) {
    std::cerr << "irtkVector::ScalarProduct: Size mismatch" << std::endl;
    exit(1);
  }

  for (i = 0; i < _rows; i++) {
    scalar_product += _vector[i]*v._vector[i];
  }
  return scalar_product;
}

inline irtkVector irtkVector::CrossProduct(const irtkVector &v)
{
  int i;

  if (_rows != v._rows) {
    std::cerr << "irtkVector::CrossProduct: Size mismatch" << std::endl;
    exit(1);
  }

  irtkVector m(_rows);
  for (i = 0; i < _rows; i++) {
    m._vector[i] = (_vector[(i+1)%_rows]*v._vector[(i+2)%_rows]-
                    _vector[(i+2)%_rows]*v._vector[(i+1)%_rows]);
  }
  return m;
}

//
// Functions
//

inline double irtkVector::Norm(void) const
{
  double norm = 0;

  for (int i = 0; i < _rows; i++) {
    norm += _vector[i]*_vector[i];
  }
  return std::sqrt(norm);
}

inline void irtkVector::Normalize(void)
{
  double norm = Norm();

  if (norm != 0) {
    *this /= norm;
  }
}

#ifdef USE_VXL

template <class T>
inline void irtkVector::Vector2Vnl(vnl_diag_matrix<T>* m) const
{
  unsigned i;

  for (i = 0; i < (unsigned) _rows; i++) {
    (*m)(i) = (T) _vector[i];
  }
}

template <class T>
inline void irtkVector::Vnl2Vector(vnl_diag_matrix<T>* m)
{
  unsigned i;

  for (i = 0; i < (unsigned) _rows; i++) {
    _vector[i] = (float) (*m)(i);
  }
}

#endif

#endif


/**
 * A metropolis hasting sampler for bremsstrahlung and pair production cross sections
 *
 * copyright  (C) 2007
 * the icecube collaboration
 *
 * $Id: I3MetropolisHastings.h 143040 2016-03-11 17:35:25Z nega $
 *
 * @version $Revision: 143040 $
 * @date $LastChangedDate: 2016-03-11 10:35:25 -0700 (Fri, 11 Mar 2016) $
 * @author Bernhard Voigt <bernhard.voigt@desy.de>   $LastChangedBy$
 */

#ifndef I3METROPOLISHASTINGS_h_
#define I3METROPOLISHASTINGS_h_

// icetray includes
#include "icetray/I3Logging.h"
#include "cmc/I3CascadeMCCommon.h"

// gsl includes
#include <gsl/gsl_rng.h>

// c++ includes
#include <string>

/**
 * @brief This class implements a Metropolis Hastings sampler
 *
 * It uses a pdf and a proposal distribution to efficiently
 * produce random samples from the pdf
 *
 * The proposal distribution used is a beta distribution, which
 * matches almost the shape of the bremsstrahlung and pair production
 * cross sections, using the right parameters. See I3CascadeSimulation
 * for the usage.
 *
 * For details on the algorithm, take a look at :
 * http://en.wikipedia.org/wiki/Metropolis-Hastings_algorithm
 */
class I3MetropolisHastings : public I3CascadeMCCommon::Sampler {

  // set logging name for icetray
  SET_LOGGER("I3MetropolisHastings");

 public:

  /**
   * Default Constructor
   */
  I3MetropolisHastings() {}

  /**
   * Constructor
   *
   * @param rng      a gsl_random instance
   * @param pdf      a function with two parameters, ie. the energy and
   *                 fractional energy of the cross section
   * @param proposal a gsl random instance sampling from the proposal
   *                 distribution which is a beta distribution
   * @param proposalDistribution a beta function in x
   * @param name     name of this instance for logging purposes
   *
   * The proposal gsl random instance as well as the proposalDistribution are implemented
   * in a struct beta_dist in the header file I3CascadeSimulation.h
   */
  I3MetropolisHastings(I3RandomServicePtr rng,
                       double (*pdf)(const double&, const double&),
                       double (*proposal)(const gsl_rng*),
                       double (*proposalDistribution)(const double&),
                       std::string name);

  /**
   * Get the total sample loop count
   */
  unsigned long GetCounter() {
    return counter_;
  }

  /**
   * Reset the total sample loop count
   */
  void ResetCounter() {
    counter_ = 0;
  }

  /**
   * Get the number of returned samples
   */
  unsigned long GetYield() {
    return yield_;
  }

  /**
   * Reset the number of returned samples
   */
  void ResetYield() {
    yield_ = 0;
  }

  /**
   * Samples n times starting at start position
   *
   * Used to forget the initial state
   *
   * @param energy parameter of underlying pdf
   * @param start starting position
   * @param n number of samples
   */
  void BurnIn(double energy, double start, unsigned int n=100);

  /**
   * Samples the underlying pdf in the given range
   *
   * @param energy parameter to the underlying pdf
   * @param lower edge of sample range
   * @param higher edge of sample range
   * @return random number
   */
  double Sample(double energy, double lower=0, double higher=1);

 private:

  /**
   * @brief pdf, ie a cross section with energy and fractional energy parameters
   */
  double (*pdf_)(const double&, const double&);

  /**
   * @brief a gls random instance drawing samples from the proposal distribution
   */
  double (*proposal_)(const gsl_rng*);

  /**
   * @brief a proposal distribution, in this case a beta distribution in x
   */
  double (*proposalDistribution_)(const double&);

  /**
   * @brief loop counter
   */
  unsigned long counter_;

  /**
   * @brief counts how many successful attemps have been made to draw a new random number
   */
  unsigned long yield_;

  /**
   * @brief name of this instance, for logging purpose
   */
  std::string name_;

  /**
   * @brief last drawn sample
   */
  double x_;

  /**
   * @brief last value of pdf(x)
   */
  double y_;

  /**
   * @brief last value of proposalDist(x)
   */
  double propDistY_;

};

#endif


/* sum/levin_utrunc.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2007 Gerard Jungman, Brian Gough
 *
 * 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_test.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_sum.h>

int
gsl_sum_levin_utrunc_accel (const double *array,
                            const size_t array_size,
                            gsl_sum_levin_utrunc_workspace * w,
                            double *sum_accel, double *abserr_trunc)
{
  return gsl_sum_levin_utrunc_minmax (array, array_size,
                                      0, array_size - 1,
                                      w, sum_accel, abserr_trunc);
}

int
gsl_sum_levin_utrunc_minmax (const double *array,
                             const size_t array_size,
                             const size_t min_terms,
                             const size_t max_terms,
                             gsl_sum_levin_utrunc_workspace * w,
                             double *sum_accel, double *abserr_trunc)
{
  if (array_size == 0)
    {
      *sum_accel = 0.0;
      *abserr_trunc = 0.0;
      w->sum_plain = 0.0;
      w->terms_used = 0;
      return GSL_SUCCESS;
    }
  else if (array_size == 1)
    {
      *sum_accel = array[0];
      *abserr_trunc = GSL_POSINF;
      w->sum_plain = array[0];
      w->terms_used = 1;
      return GSL_SUCCESS;
    }
  else
    {
      const double SMALL = 0.01;
      const size_t nmax = GSL_MAX (max_terms, array_size) - 1;
      double trunc_n = 0.0, trunc_nm1 = 0.0;
      double actual_trunc_n = 0.0, actual_trunc_nm1 = 0.0;
      double result_n = 0.0, result_nm1 = 0.0;
      size_t n;
      int better = 0;
      int before = 0;
      int converging = 0;
      double least_trunc = GSL_DBL_MAX;
      double result_least_trunc;

      /* Calculate specified minimum number of terms. No convergence
         tests are made, and no truncation information is stored. */

      for (n = 0; n < min_terms; n++)
        {
          const double t = array[n];

          result_nm1 = result_n;
          gsl_sum_levin_utrunc_step (t, n, w, &result_n);
        }

      /* Assume the result after the minimum calculation is the best. */

      result_least_trunc = result_n;

      /* Calculate up to maximum number of terms. Check truncation
         condition. */

      for (; n <= nmax; n++)
        {
          const double t = array[n];

          result_nm1 = result_n;
          gsl_sum_levin_utrunc_step (t, n, w, &result_n);

          /* Compute the truncation error directly */

          actual_trunc_nm1 = actual_trunc_n;
          actual_trunc_n = fabs (result_n - result_nm1);

          /* Average results to make a more reliable estimate of the
             real truncation error */

          trunc_nm1 = trunc_n;
          trunc_n = 0.5 * (actual_trunc_n + actual_trunc_nm1);

          /* Determine if we are in the convergence region. */

          better = (trunc_n < trunc_nm1 || trunc_n < SMALL * fabs (result_n));
          converging = converging || (better && before);
          before = better;

          if (converging)
            {
              if (trunc_n < least_trunc)
                {
                  /* Found a low truncation point in the convergence
                     region. Save it. */

                  least_trunc = trunc_n;
                  result_least_trunc = result_n;
                }

              if (fabs (trunc_n / result_n) < 10.0 * GSL_MACH_EPS)
                break;
            }
        }

      if (converging)
        {
          /* Stopped in the convergence region. Return result and
             error estimate. */

          *sum_accel = result_least_trunc;
          *abserr_trunc = least_trunc;
          w->terms_used = n;
          return GSL_SUCCESS;
        }
      else
        {
          /* Never reached the convergence region. Use the last
             calculated values. */

          *sum_accel = result_n;
          *abserr_trunc = trunc_n;
          w->terms_used = n;
          return GSL_SUCCESS;
        }
    }
}

int
gsl_sum_levin_utrunc_step (const double term,
                           const size_t n,
                           gsl_sum_levin_utrunc_workspace * w, double *sum_accel)
{
  if (term == 0.0)
    {
      /* This is actually harmless when treated in this way. A term
         which is exactly zero is simply ignored; the state is not
         changed. We return GSL_EZERODIV as an indicator that this
         occured. */

      return GSL_EZERODIV;
    }
  else if (n == 0)
    {
      *sum_accel = term;
      w->sum_plain = term;
      w->q_den[0] = 1.0 / term;
      w->q_num[0] = 1.0;
      return GSL_SUCCESS;
    }
  else
    {
      double factor = 1.0;
      double ratio = (double) n / (n + 1.0);
      int j;

      w->sum_plain += term;
      w->q_den[n] = 1.0 / (term * (n + 1.0) * (n + 1.0));
      w->q_num[n] = w->sum_plain * w->q_den[n];

      for (j = n - 1; j >= 0; j--)
        {
          double c = factor * (j + 1) / (n + 1);
          factor *= ratio;
          w->q_den[j] = w->q_den[j + 1] - c * w->q_den[j];
          w->q_num[j] = w->q_num[j + 1] - c * w->q_num[j];
        }

      *sum_accel = w->q_num[0] / w->q_den[0];
      return GSL_SUCCESS;
    }
}