/* ************************** 
   galaxy_fitting.c

   Created on: July 7, 2011
   Author: Michael Hirsch
   ************************** */
#ifndef GALAXY_FITTING_C
#define GALAXY_FITTING_C

#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <complex.h>
#include <sys/time.h>
#include <string.h>

#include "utils.h"
#include "lbfgsb.h"
#include "galaxy_fitting.h"

void check_gradient(function func, int N, real * x, void * auxdata){
  printf("\n==================Checking gradient===================\n");
  printf("Query point: Computed gradient vs. Estimated gradient:\n");
  printf("------------------------------------------------------\n");

  real objective, gradient[N], gradient_estimated[N];
  real objective_minus, gradient_minus[N];
  real objective_plus , gradient_plus[N];
  real z_minus[N], z_plus[N];
  real epsilon = 1e-5;

  func(x, &objective, gradient, auxdata);

  for (int i=0; i < N; i++){
    // Clone query point
    for (int j=0; j < N; j++){ z_minus[j] = z_plus[j] = x[j]; }
    // Add epsilon to ith component of query point
    z_minus[i] -= epsilon;
    z_plus[i]  += epsilon;
    //Evaluate objective function at query points
    func(z_minus, &objective_minus, gradient_minus, auxdata);
    func(z_plus,  &objective_plus , gradient_plus , auxdata);
    // Approximate gradient with finite differences
    gradient_estimated[i] = (objective_plus-objective_minus) / (2*epsilon); 
    printf("%.06f     %.06f     %.06f\n", x[i], gradient[i], gradient_estimated[i]);
  }
  printf("======================================================\n\n");
}

void dot2x2(real * x, real * y, real * z){
  /* Computing matrix-matrix multilication for 2x2 matrices 
     represented as vectors containing 4 elements */

  z[0] = x[0] * y[0] + x[1] * y[2];
  z[1] = x[0] * y[1] + x[1] * y[3];
  z[2] = x[2] * y[0] + x[3] * y[2];
  z[3] = x[2] * y[1] + x[3] * y[3]; 

}

void q2p(real * q, real * p){
  /* Transforming the square root of a covariance matrix to a precision matrix */

  /* Computing the covariance matrix first cov = q * q */
  real covxx = q[0] * q[0] + q[2] * q[2];
  real covxy = q[0] * q[1] + q[2] * q[3]; 
  real covyy = q[1] * q[1] + q[3] * q[3]; 

  /* Computing the determinant of covariance matrix for the matrix inverse */
  real det = covxx * covyy - covxy * covxy;

  /* Computing the matrix inverse of a 2x2 matrix */
  p[0] = covyy / det;
  p[1] = p[2]= -covxy / det;
  p[3] = covxx / det;
}

void q2cov(real * q, real * cov){
  /* Transforming the square root of a covariance matrix to a covariance matrix */

  /* Computing the covariance matrix first cov = q * q */
  cov[0] = q[0] * q[0] + q[2] * q[2];
  cov[1] = cov[2] = q[0] * q[1] + q[2] * q[3]; 
  cov[3] = q[1] * q[1] + q[3] * q[3]; 
}

/* ===================================== */
/*    Single Gaussian fitting routines   */
/* ===================================== */

void fg_x2parameters(real * x, real * a, real * x0, real * y0, real * q){
  /* x is the optimization variable comprising of amplitude, 
     mean and square root of covariance matrix */
  *a  = x[0];       // amplitude                       
  *x0 = x[1];       // centroid in x direction         
  *y0 = x[2];       // centroid in y direction         
  q[0] = x[3];      // square root of covariance matrix 
  q[1] = x[4];      // -- " --                          
  q[2] = x[5];      // -- " --                          
  q[3] = x[6];      // -- " --                          
}

void gaussian(real a, real x0, real y0, real * p, matrix * out){
  /* Compute single gaussian centered at (x0,y0) with amplitude a and precision matrix p */

  out->mat = (real*) malloc(out->size.m * out->size.n * sizeof(out->mat));

  for (int i=0; i < out->size.m; i++){
    real dx  = i - x0;
    real dx2 = p[0] * dx * dx;
    for (int j=0; j < out->size.n; j++){
      real dy = j - y0;
      real arg = dx2 + (p[1] + p[2]) * dy * dx + p[3] * dy * dy;
      out->mat[IDX(i,j,out->size.n)] = a * exp(-0.5 * arg);
    }
  }
}

void fg_function(real * x, real * objective, real * gradient, void * yp){
  /* Computes objective value and gradient wrt to model parameters */

  real a, x0, y0, q[4], p[4], cov[4];
  matrix * y = yp;

  fg_x2parameters(x, &a, &x0, &y0, q);
  q2p(q, p);
  q2cov(q, cov);

  /* Compute gaussian with model parameters x */
  matrix z;
  z.size = y->size;
  gaussian(a, x0, y0, p, &z);
 
  /* Compute likelihood */
  *objective = 0;
  real grad_a = 0;
  real grad_x0 = 0;
  real grad_y0 = 0;
  real grad_q[4] = {0.,0.,0.,0.};

  for (int i=0; i < y->size.m; i++){
    real dx  = i - x0;
    for (int j=0; j < y->size.n; j++){
      real dy = j - y0;
      real dij = y->mat[IDX( i,j,y->size.n )] - z.mat[IDX( i,j,z.size.n )];
      // Objective value
      *objective += 0.5 * dij * dij;
      // Gradient wrt amplitude
      real dijz = dij * z.mat[IDX( i,j,z.size.n )];
      grad_a  -= dijz / a;
      // Gradient wrt centroid
      grad_x0 -= (p[0] * dx + p[1] * dy) * dijz;
      grad_y0 -= (p[2] * dx + p[3] * dy) * dijz;
      // Gradient wrt square root of covariance matrix
      grad_q[0] += (dx * dx * dijz);
      grad_q[1] += (dx * dy * dijz);
      grad_q[2] += (dy * dx * dijz);
      grad_q[3] += (dy * dy * dijz);
      //Gradient for normalized Gaussian where a = sqrt(det(P))/(2*pi)
      //grad_q[0] += ((dx * dx - cov[0]) * dijz);
      //grad_q[1] += ((dx * dy - cov[1]) * dijz);
      //grad_q[2] += ((dy * dx - cov[2]) * dijz);
      //grad_q[3] += ((dy * dy - cov[3]) * dijz);
    }
  }

  real tmp1[4], tmp2[4];
  dot2x2(p, grad_q, tmp1);
  dot2x2(tmp1, p, tmp2);
  dot2x2(q, tmp2, grad_q);

  gradient[0] =  grad_a;
  gradient[1] =  grad_x0;
  gradient[2] =  grad_y0;
  gradient[3] = -grad_q[0];
  gradient[4] = -grad_q[1];
  gradient[5] = -grad_q[2];
  gradient[6] = -grad_q[3];

  free(z.mat);
}


void fg(real * x, matrix * y){
  
  int N = 7;
  //real x[7] = {1., y->size.m/2, y->size.n/2, 10., 0., 0., 10.};
  real x_init[7];
  for (int i=0; i<N; i++){ x_init[i] = x[i]; };

  // Setting lower and upper bounds
  //real lb[7] = {-INFINITY, -INFINITY, -INFINITY, -INFINITY, 
  //		-INFINITY, -INFINITY, -INFINITY};
  real lb[7] = {0, 0, 0, 0, 0, 0, 0};
  real ub[7] = {INFINITY, INFINITY, INFINITY, INFINITY, 
		INFINITY, INFINITY, INFINITY, };

  void * auxdata = y;

  int optres = lbf_solve(N, x, lb, ub, fg_function, auxdata, 1, 500, 1.0e-16);

  real a, x0, y0, q[4], p[4], cov[4];

  printf("\nBefore minimization\n");
  printf("Value of amplitude: %f\n",  x_init[0]);
  printf("Value of centroid x: %f\n", x_init[1]);
  printf("Value of centroid y: %f\n", x_init[2]);
  printf("Value of q[0]: %f\n",	      x_init[3]);
  printf("Value of q[1]: %f\n",	      x_init[4]);
  printf("Value of q[2]: %f\n",	      x_init[5]);
  printf("Value of q[3]: %f\n",	      x_init[6]);

  printf("\nAfter minimization\n");
  printf("Value of amplitude: %f\n",  x[0]);
  printf("Value of centroid x: %f\n", x[1]);
  printf("Value of centroid y: %f\n", x[2]);
  printf("Value of q[0]: %f\n",       x[3]);
  printf("Value of q[1]: %f\n",	      x[4]);
  printf("Value of q[2]: %f\n",	      x[5]);
  printf("Value of q[3]: %f\n",	      x[6]);

  fg_x2parameters(x, &a, &x0, &y0, q);
  q2p(q, p);
  q2cov(q, cov);
	
}

/* ====================================================== */
/*    Single Gaussian fitting routines incl. convolution  */
/* ====================================================== */

void fcg_function(real * x, real * objective, real * gradient, void * auxdata){
  /* Computes objective value and gradient wrt to model parameters */

  generative_model * gm;
  gm = auxdata;

  real a, x0, y0, q[4], p[4], cov[4];
  matrix * y = gm->data;

  fg_x2parameters(x, &a, &x0, &y0, q);
  q2p(q, p);
  q2cov(q, cov);

  /* Compute gaussian with model parameters x */
  matrix z;
  z.size = y->size;
  gaussian(a, x0, y0, p, &z);

  /* Compute forward model */
  matrix * ytmp = gm->forward(gm->model, &z);

  /* Compare model with data */
  matrix diff;
  diff.size = y->size;
  diff.mat = (real*) malloc(diff.size.m * diff.size.n * sizeof(diff.mat));
  for (int i=0; i < y->size.m; i++){
    for (int j=0; j < y->size.n; j++){
      diff.mat[IDX( i,j,y->size.n )] = y->mat[IDX( i,j,y->size.n )] - ytmp->mat[IDX( i,j,z.size.n )];
    }
  }
  matrix * diff_tp = gm->transpose(gm->model, &diff);
 
  /* Compute likelihood */
  *objective = 0;
  real grad_a = 0;
  real grad_x0 = 0;
  real grad_y0 = 0;
  real grad_q[4] = {0.,0.,0.,0.};

  for (int i=0; i < y->size.m; i++){
    real dx  = i - x0;
    for (int j=0; j < y->size.n; j++){
      real dy = j - y0;
      real dij = diff.mat[IDX( i,j,diff.size.n )];
      real dij_tp = diff_tp->mat[IDX( i,j,z.size.n )];
      // Objective value
      *objective += 0.5 * dij * dij;
      // Gradient wrt amplitude
      real dijz = dij_tp * z.mat[IDX( i,j,z.size.n )];
      grad_a  -= dijz / a;
      // Gradient wrt centroid
      grad_x0 -= (p[0] * dx + p[1] * dy) * dijz;
      grad_y0 -= (p[2] * dx + p[3] * dy) * dijz;
      // Gradient wrt square root of covariance matrix
      grad_q[0] += (dx * dx * dijz);
      grad_q[1] += (dx * dy * dijz);
      grad_q[2] += (dy * dx * dijz);
      grad_q[3] += (dy * dy * dijz);
      //Gradient for normalized Gaussian where a = sqrt(det(P))/(2*pi)
      //grad_q[0] += ((dx * dx - cov[0]) * dijz);
      //grad_q[1] += ((dx * dy - cov[1]) * dijz);
      //grad_q[2] += ((dy * dx - cov[2]) * dijz);
      //grad_q[3] += ((dy * dy - cov[3]) * dijz);
    }
  }

  real tmp1[4], tmp2[4];
  dot2x2(p, grad_q, tmp1);
  dot2x2(tmp1, p, tmp2);
  dot2x2(q, tmp2, grad_q);

  gradient[0] =  grad_a;
  gradient[1] =  grad_x0;
  gradient[2] =  grad_y0;
  gradient[3] = -grad_q[0];
  gradient[4] = -grad_q[1];
  gradient[5] = -grad_q[2];
  gradient[6] = -grad_q[3];

  free(z.mat);
  free(ytmp->mat);
  free(diff.mat);
  free(diff_tp->mat);
}


void fcg(real * x, matrix * y, generative_model * gm){
  
  int N = 7;
  real x_init[7];
  for (int i=0; i<N; i++){ x_init[i] = x[i]; };

  // Setting lower and upper bounds
  //real lb[7] = {-INFINITY, -INFINITY, -INFINITY, -INFINITY, 
  //		-INFINITY, -INFINITY, -INFINITY};
  real lb[7] = {0, 0, 0, 0, 0, 0, 0};
  real ub[7] = {INFINITY, INFINITY, INFINITY, INFINITY, 
		INFINITY, INFINITY, INFINITY, };

  gm->data = y;
  void * auxdata = gm;

  int optres = lbf_solve(N, x, lb, ub, fcg_function, auxdata, 1, 500, 1.0e-16);

  real a, x0, y0, q[4], p[4], cov[4];

  printf("\nBefore minimization\n");
  printf("Value of amplitude: %f\n",  x_init[0]);
  printf("Value of centroid x: %f\n", x_init[1]);
  printf("Value of centroid y: %f\n", x_init[2]);
  printf("Value of q[0]: %f\n",	      x_init[3]);
  printf("Value of q[1]: %f\n",	      x_init[4]);
  printf("Value of q[2]: %f\n",	      x_init[5]);
  printf("Value of q[3]: %f\n",	      x_init[6]);

  printf("\nAfter minimization\n");
  printf("Value of amplitude: %f\n",  x[0]);
  printf("Value of centroid x: %f\n", x[1]);
  printf("Value of centroid y: %f\n", x[2]);
  printf("Value of q[0]: %f\n",       x[3]);
  printf("Value of q[1]: %f\n",	      x[4]);
  printf("Value of q[2]: %f\n",	      x[5]);
  printf("Value of q[3]: %f\n",	      x[6]);

  fg_x2parameters(x, &a, &x0, &y0, q);
  q2p(q, p);
  q2cov(q, cov);
	
}

/* ===================================== */
/* Generalized Gaussian fitting routines */
/* ===================================== */

void fgg_x2parameters(real * x, real * a, real * x0, real * y0, real * q, real * beta){
  /* x is the optimization variable comprising of amplitude, 
     mean and square root of covariance matrix */
  *a	= x[0];	     // amplitude
  *x0	= x[1];	     // centroid in x direction
  *y0	= x[2];	     // centroid in y direction	
  q[0]  = x[3];      // square root of covariance matrix	
  q[1]  = x[4];      // -- " --	
  q[2]  = x[5];      // -- " --	
  q[3]  = x[6];      // -- " --	
  *beta = x[7];      // index of exponentiation	
}

void generalized_gaussian(real a, real x0, real y0, real * p, real beta, matrix * out){
  /* Compute single gaussian centered at (x0,y0) with amplitude a and precision matrix p */

  out->mat = (real*) malloc(out->size.m * out->size.n * sizeof(out->mat));

  for (int i=0; i < out->size.m; i++){
    real dx  = i - x0;
    real dx2 = p[0] * dx * dx;
    for (int j=0; j < out->size.n; j++){
      real dy = j - y0;
      real arg = dx2 + (p[1] + p[2]) * dy * dx + p[3] * dy * dy;
      out->mat[IDX(i,j,out->size.n)] = a * exp(-0.5 * pow(arg, beta));
    }
  }
}

void fgg_function(real * x, real * objective, real * gradient, void * yp){
  /* Computes objective value and gradient wrt to model parameters */

  real a, x0, y0, beta, q[4], p[4], cov[4];
  matrix * y = yp;

  fgg_x2parameters(x, &a, &x0, &y0, q, &beta);
  q2p(q, p);
  q2cov(q, cov);

  /* Compute generalized gaussian with model parameters x */
  matrix z; 
  z.size = y->size;
  generalized_gaussian(a, x0, y0, p, beta, &z);
 
  /* Compute likelihood */
  *objective = 0.0;
  real grad_a = 0.0;
  real grad_x0 = 0.0;
  real grad_y0 = 0.0;
  real grad_beta = 0.0;
  real grad_q[4] = {0.,0.,0.,0.};

  for (int i=0; i < y->size.m; i++){
    real dx  = i - x0;
    real dx2 = p[0] * dx * dx;
    for (int j=0; j < y->size.n; j++){
      real dy = j - y0;
      real dij = y->mat[IDX( i,j,y->size.n )] - z.mat[IDX( i,j,z.size.n )];
      // Objective value
      *objective += 0.5 * dij * dij;
      // Gradient wrt amplitude
      real dijz = dij * z.mat[IDX( i,j,z.size.n )];
      grad_a  -= dijz / a;
      // Gradient wrt centroid
      real arg = dx2 + (p[1] + p[2]) * dy * dx + p[3] * dy * dy;
      grad_x0 -= beta * (p[0] * dx + p[1] * dy) * dijz * pow(arg, beta-1);
      grad_y0 -= beta * (p[2] * dx + p[3] * dy) * dijz * pow(arg, beta-1);
      // Gradient wrt square root of covariance matrix
      grad_q[0] += (beta * pow(arg, beta-1) * dx * dx * dijz);
      grad_q[1] += (beta * pow(arg, beta-1) * dx * dy * dijz);
      grad_q[2] += (beta * pow(arg, beta-1) * dy * dx * dijz);
      grad_q[3] += (beta * pow(arg, beta-1) * dy * dy * dijz);   
      //Gradient for normalized generalized Gaussian distribution
      //grad_q[0] += ((beta * pow(arg, beta-1) * dx * dx - cov[0]) * dijz);
      //grad_q[1] += ((beta * pow(arg, beta-1) * dx * dy - cov[1]) * dijz);
      //grad_q[2] += ((beta * pow(arg, beta-1) * dy * dx - cov[2]) * dijz);
      //grad_q[3] += ((beta * pow(arg, beta-1) * dy * dy - cov[3]) * dijz);
      // Here goes the implementation of the gradient wrt beta
      grad_beta = 0.0;
    }
  }
  
  real tmp1[4], tmp2[4];
  dot2x2(p, grad_q, tmp1);
  dot2x2(tmp1, p, tmp2);
  dot2x2(q, tmp2, grad_q);

  gradient[0] =  grad_a;
  gradient[1] =  grad_x0;
  gradient[2] =  grad_y0;
  gradient[3] = -grad_q[0];
  gradient[4] = -grad_q[1];
  gradient[5] = -grad_q[2];
  gradient[6] = -grad_q[3];
  gradient[7] =  grad_beta;

  free(z.mat);
}

void fgg(real * x, matrix * y){
  
  int N = 8;
  real x_init[8];
  for (int i=0; i<N; i++){ x_init[i] = x[i]; };

  // Setting lower and upper bounds
  //real lb[8] = {-INFINITY, -INFINITY, -INFINITY, -INFINITY, 
  //		-INFINITY, -INFINITY, -INFINITY, -INFINITY};
  real lb[8] = {0, 0, 0, 0, 0, 0, 0, 0};
  real ub[8] = {INFINITY, INFINITY, INFINITY, INFINITY, 
		INFINITY, INFINITY, INFINITY, INFINITY};

  void * auxdata = y;

  int optres = lbf_solve(N, x, lb, ub, fgg_function, auxdata, 1, 500, 1.0e-16);

  real a, x0, y0, beta, q[4], p[4], cov[4];

  printf("\nBefore minimization\n");
  printf("Value of amplitude: %f\n",  x_init[0]);
  printf("Value of centroid x: %f\n", x_init[1]);
  printf("Value of centroid y: %f\n", x_init[2]);
  printf("Value of q[0]: %f\n",	      x_init[3]);
  printf("Value of q[1]: %f\n",	      x_init[4]);
  printf("Value of q[2]: %f\n",	      x_init[5]);
  printf("Value of q[3]: %f\n",	      x_init[6]);
  printf("Value of beta: %f\n",	      x_init[7]);

  printf("\nAfter minimization\n");
  printf("Value of amplitude: %f\n",  x[0]);
  printf("Value of centroid x: %f\n", x[1]);
  printf("Value of centroid y: %f\n", x[2]);
  printf("Value of q[0]: %f\n",       x[3]);
  printf("Value of q[1]: %f\n",	      x[4]);
  printf("Value of q[2]: %f\n",	      x[5]);
  printf("Value of q[3]: %f\n",	      x[6]);
  printf("Value of beta: %f\n",	      x[7]);

  fgg_x2parameters(x, &a, &x0, &y0, q, &beta);
  q2p(q, p);
  q2cov(q, cov);	
}


/* ======================================================== */
/* Generalized Gaussian fitting routines incl. convolution  */
/* ======================================================== */

void fcgg_function(real * x, real * objective, real * gradient, void * auxdata){
  /* Computes objective value and gradient wrt to model parameters */

  generative_model * gm;
  gm = auxdata;

  real a, x0, y0, beta, q[4], p[4], cov[4];
  matrix * y = gm->data;

  fgg_x2parameters(x, &a, &x0, &y0, q, &beta);
  q2p(q, p);
  q2cov(q, cov);

  /* Compute gaussian with model parameters x */
  matrix z;
  z.size = y->size;
  generalized_gaussian(a, x0, y0, p, beta, &z);

  /* Compute forward model */
  matrix * ytmp = gm->forward(gm->model, &z);

  /* Compare model with data */
  matrix diff;
  diff.size = y->size;
  diff.mat = (real*) malloc(diff.size.m * diff.size.n * sizeof(diff.mat));
  for (int i=0; i < y->size.m; i++){
    for (int j=0; j < y->size.n; j++){
      diff.mat[IDX( i,j,y->size.n )] = y->mat[IDX( i,j,y->size.n )] - ytmp->mat[IDX( i,j,z.size.n )];
    }
  }
  matrix * diff_tp = gm->transpose(gm->model, &diff);
 
  /* Compute likelihood */
  *objective = 0;
  real grad_a = 0;
  real grad_x0 = 0;
  real grad_y0 = 0;
  real grad_beta = 0.0;
  real grad_q[4] = {0.,0.,0.,0.};

  for (int i=0; i < y->size.m; i++){
    real dx  = i - x0;
    real dx2 = p[0] * dx * dx;
   for (int j=0; j < y->size.n; j++){
      real dy = j - y0;
      real dij = diff.mat[IDX( i,j,diff.size.n )];
      real dij_tp = diff_tp->mat[IDX( i,j,z.size.n )];
      // Objective value
      *objective += 0.5 * dij * dij;
      // Gradient wrt amplitude
      real dijz = dij_tp * z.mat[IDX( i,j,z.size.n )];
      grad_a  -= dijz / a;
      // Gradient wrt centroid
      real arg = dx2 + (p[1] + p[2]) * dy * dx + p[3] * dy * dy;
      grad_x0 -= beta * (p[0] * dx + p[1] * dy) * dijz * pow(arg, beta-1);;
      grad_y0 -= beta * (p[2] * dx + p[3] * dy) * dijz * pow(arg, beta-1);;
      // Gradient wrt square root of covariance matrix
      grad_q[0] += (beta * pow(arg, beta-1) * dx * dx * dijz);
      grad_q[1] += (beta * pow(arg, beta-1) * dx * dy * dijz);
      grad_q[2] += (beta * pow(arg, beta-1) * dy * dx * dijz);
      grad_q[3] += (beta * pow(arg, beta-1) * dy * dy * dijz);
      //Gradient for normalized Gaussian where a = sqrt(det(P))/(2*pi)
      //grad_q[0] += ((dx * dx - cov[0]) * dijz);
      //grad_q[1] += ((dx * dy - cov[1]) * dijz);
      //grad_q[2] += ((dy * dx - cov[2]) * dijz);
      //grad_q[3] += ((dy * dy - cov[3]) * dijz);
      grad_beta = 0.0;
    }
  }

  real tmp1[4], tmp2[4];
  dot2x2(p, grad_q, tmp1);
  dot2x2(tmp1, p, tmp2);
  dot2x2(q, tmp2, grad_q);

  gradient[0] =  grad_a;
  gradient[1] =  grad_x0;
  gradient[2] =  grad_y0;
  gradient[3] = -grad_q[0];
  gradient[4] = -grad_q[1];
  gradient[5] = -grad_q[2];
  gradient[6] = -grad_q[3];
  gradient[7] =  grad_beta;

  free(z.mat);
  free(ytmp->mat);
  free(diff.mat);
  free(diff_tp->mat);
}


void fcgg(real * x, matrix * y, generative_model * gm){
  
  int N = 8;
  real x_init[8];
  for (int i=0; i<N; i++){ x_init[i] = x[i]; };

  // Setting lower and upper bounds
  //real lb[7] = {-INFINITY, -INFINITY, -INFINITY, -INFINITY, 
  //		-INFINITY, -INFINITY, -INFINITY};
  real lb[7] = {0, 0, 0, 0, 0, 0, 0};
  real ub[7] = {INFINITY, INFINITY, INFINITY, INFINITY, 
		INFINITY, INFINITY, INFINITY, };

  gm->data = y;
  void * auxdata = gm;

  int optres = lbf_solve(N, x, lb, ub, fcgg_function, auxdata, 1, 500, 1.0e-16);

  real a, x0, y0, beta, q[4], p[4], cov[4];

  printf("\nBefore minimization\n");
  printf("Value of amplitude: %f\n",  x_init[0]);
  printf("Value of centroid x: %f\n", x_init[1]);
  printf("Value of centroid y: %f\n", x_init[2]);
  printf("Value of q[0]: %f\n",	      x_init[3]);
  printf("Value of q[1]: %f\n",	      x_init[4]);
  printf("Value of q[2]: %f\n",	      x_init[5]);
  printf("Value of q[3]: %f\n",	      x_init[6]);
  printf("Value of beta: %f\n",	      x_init[7]);

  printf("\nAfter minimization\n");
  printf("Value of amplitude: %f\n",  x[0]);
  printf("Value of centroid x: %f\n", x[1]);
  printf("Value of centroid y: %f\n", x[2]);
  printf("Value of q[0]: %f\n",       x[3]);
  printf("Value of q[1]: %f\n",	      x[4]);
  printf("Value of q[2]: %f\n",	      x[5]);
  printf("Value of q[3]: %f\n",	      x[6]);
  printf("Value of beta: %f\n",	      x[8]);

  fgg_x2parameters(x, &a, &x0, &y0, q, &beta);
  q2p(q, p);
  q2cov(q, cov);
	
}

#endif