/*
 * Copyright (C) 2009-2010 Sébastien Villemot <sebastien.villemot@ens.fr>
 *
 * 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, see <http://www.gnu.org/licenses/>.
 */

#include <iostream>
#include <cassert>
#include <cmath>
#include <cstring>

#include <gsl/gsl_blas.h>
#include <gsl/gsl_deriv.h>

#include "mat.h"

#include "MMJSolution.hh"

MMJSolution::MMJSolution(const ModelSpec &modspec_arg, const std::string &matdir_arg) throw (UnsupportedSpec)
  : ModelSolution(modspec_arg), matdir(matdir_arg)
{
  const int s = modspec.spec_no();
  const int n = modspec.n;

  switch(s)
    {
    case 1:
    case 5:
      if (n != 2 && n != 4 && n != 6 && n != 8 && n != 10)
        throw UnsupportedSpec();
      break;
    case 2:
    case 6:
      if (n != 2 && n != 4 && n != 6 && n != 8)
        throw UnsupportedSpec();
      break;
    case 3:
    case 4:
    case 7:
    case 8:
      if (n != 2 && n != 4 && n != 6)
        throw UnsupportedSpec();
      break;
    default:
      throw UnsupportedSpec();
    }

  // Retrieve MATLAB arrays
  char filename[50];
  snprintf(filename, 50, "mmj/%s/M%dN%d.mat", matdir.c_str(), s, n);

  MATFile *mfp = matOpen(filename, "r");
  if (mfp == NULL)
    {
      std::cerr << "MMJSolution::MMJSolution: can't open " << filename << std::endl;
      exit(EXIT_FAILURE);
    }

  mxArray *V_mx = matGetVariable(mfp, "V");
  mxArray *z0_mx = matGetVariable(mfp, "z0");
  mxArray *VI_mx = matGetVariable(mfp, "VI");
  mxArray *meanx_mx = matGetVariable(mfp, "meanx");
  mxArray *sigz0_mx = matGetVariable(mfp, "sigz0");
  mxArray *stdx_mx = matGetVariable(mfp, "stdx");
  mxArray *stdz_mx = matGetVariable(mfp, "stdz");
  mxArray *v_mx = matGetVariable(mfp, "v");
  mxArray *NC_mx = matGetVariable(mfp, "NC");

  mxArray *Cgrid_mx, *Cfunc_mx, *VIL_mx;

  if (s == 1 || s == 5)
    {
      Cgrid_mx = matGetVariable(mfp, "Cgrid");
      Cfunc_mx = matGetVariable(mfp, "Cfunc");
    }
  else
    VIL_mx = matGetVariable(mfp, "VIL");

  matClose(mfp);

  // First deal with NC (needed for matrices dimensions)
  assert(NC_mx != NULL && mxGetClassID(NC_mx) == mxDOUBLE_CLASS && mxGetNumberOfDimensions(NC_mx) == 2
         && mxGetDimensions(NC_mx)[0] == 1 && mxGetDimensions(NC_mx)[1] == 1);
  NC = (int) mxGetPr(NC_mx)[0];

  // Test array types and dimensions
  assert(V_mx != NULL && z0_mx != NULL && VI_mx != NULL && meanx_mx != NULL
         && sigz0_mx != NULL && stdx_mx != NULL && stdz_mx != NULL && v_mx != NULL);
  assert(mxGetClassID(V_mx) == mxDOUBLE_CLASS && mxGetNumberOfDimensions(V_mx) == 2
         && mxGetDimensions(V_mx)[0] == 2*n && mxGetDimensions(V_mx)[1] == 2*n);
  assert(mxGetClassID(z0_mx) == mxDOUBLE_CLASS && mxGetNumberOfDimensions(z0_mx) == 2
         && mxGetDimensions(z0_mx)[0] == NC && mxGetDimensions(z0_mx)[1] == 2*n);
  assert(mxGetClassID(VI_mx) == mxDOUBLE_CLASS && mxGetNumberOfDimensions(VI_mx) == 3
         && (mxGetDimensions(VI_mx)[0] == 2*n+1 || mxGetDimensions(VI_mx)[0] == 1+2*n+2*n*(2*n+1)/2)
         && mxGetDimensions(VI_mx)[1] == n && mxGetDimensions(VI_mx)[2] == NC);
  assert(mxGetClassID(meanx_mx) == mxDOUBLE_CLASS && mxGetNumberOfDimensions(meanx_mx) == 2
         && mxGetDimensions(meanx_mx)[0] == 1 && mxGetDimensions(meanx_mx)[1] == 2*n);
  assert(mxGetClassID(sigz0_mx) == mxDOUBLE_CLASS && mxGetNumberOfDimensions(sigz0_mx) == 2
         && mxGetDimensions(sigz0_mx)[0] == NC && mxGetDimensions(sigz0_mx)[1] == 1);
  assert(mxGetClassID(stdx_mx) == mxDOUBLE_CLASS && mxGetNumberOfDimensions(stdx_mx) == 2
         && mxGetDimensions(stdx_mx)[0] == 1 && mxGetDimensions(stdx_mx)[1] == 2*n);
  assert(mxGetClassID(stdz_mx) == mxDOUBLE_CLASS && mxGetNumberOfDimensions(stdz_mx) == 2
         && mxGetDimensions(stdz_mx)[0] == 1 && mxGetDimensions(stdz_mx)[1] == 2*n);
  assert(mxGetClassID(v_mx) == mxDOUBLE_CLASS && mxGetNumberOfDimensions(v_mx) == 2
         && mxGetDimensions(v_mx)[0] == 1 && mxGetDimensions(v_mx)[1] == 1);

  if (s == 1 || s == 5)
    {
      assert(Cgrid_mx != NULL && Cfunc_mx != NULL);
      assert(mxGetClassID(Cgrid_mx) == mxDOUBLE_CLASS && mxGetNumberOfDimensions(Cgrid_mx) == 2
             && mxGetDimensions(Cgrid_mx)[0] == 1 && mxGetDimensions(Cgrid_mx)[1] == 1000);
      assert(mxGetClassID(Cfunc_mx) == mxDOUBLE_CLASS && mxGetNumberOfDimensions(Cfunc_mx) == 2
             && mxGetDimensions(Cfunc_mx)[0] == 1 && mxGetDimensions(Cfunc_mx)[1] == 1000);
    }
  else
    {
      assert(VIL_mx != NULL);
      VIL_dim2 = (s == 2 || s == 6 ? 1 : n);
      assert(mxGetClassID(VIL_mx) == mxDOUBLE_CLASS && mxGetNumberOfDimensions(VIL_mx) == 3
             && (mxGetDimensions(VIL_mx)[0] == 2*n+1 || mxGetDimensions(VIL_mx)[0] == 1+2*n+2*n*(2*n+1)/2)
             && mxGetDimensions(VIL_mx)[1] == VIL_dim2 && mxGetDimensions(VIL_mx)[2] == NC);
    }

  // Construct GSL structures
  V_prime = gsl_matrix_alloc(2*n, 2*n);
  gsl_matrix_memcpy(V_prime, &gsl_matrix_view_array(mxGetPr(V_mx), 2*n, 2*n).matrix);
  z0_prime = gsl_matrix_alloc(2*n, 3);
  gsl_matrix_memcpy(z0_prime, &gsl_matrix_view_array(mxGetPr(z0_mx), 2*n, 3).matrix);
  VI_dim1 = mxGetDimensions(VI_mx)[0];
  VI = new double[VI_dim1*n*NC];
  memcpy(VI, mxGetPr(VI_mx), VI_dim1*n*NC*sizeof(double));

  meanx = gsl_vector_alloc(2*n);
  gsl_vector_memcpy(meanx, &gsl_vector_view_array(mxGetPr(meanx_mx), 2*n).vector);
  sigz0 = gsl_vector_alloc(3);
  gsl_vector_memcpy(sigz0, &gsl_vector_view_array(mxGetPr(sigz0_mx), 3).vector);
  stdx = gsl_vector_alloc(2*n);
  gsl_vector_memcpy(stdx, &gsl_vector_view_array(mxGetPr(stdx_mx), 2*n).vector);
  stdz = gsl_vector_alloc(2*n);
  gsl_vector_memcpy(stdz, &gsl_vector_view_array(mxGetPr(stdz_mx), 2*n).vector);
  v = mxGetPr(v_mx)[0];

  if (s == 1 || s == 5)
    {
      const ModelSpecA1A5 &ms = dynamic_cast<const ModelSpecA1A5 &>(modspec);
      bool all_gamma_ones = true;
      for(int j = 0; j < n; j++)
        if (ms.gammas[j] != 1.0)
          {
            all_gamma_ones = false;
            break;
          }

      if (all_gamma_ones)
        {
          acc = NULL;
          spline = NULL;
        }
      else
        {
          acc = gsl_interp_accel_alloc();
          spline = gsl_spline_alloc(gsl_interp_linear, 1000);
          gsl_spline_init(spline, mxGetPr(Cgrid_mx), mxGetPr(Cfunc_mx), 1000);
        }
    }
  else
    {
      VIL = new double[VI_dim1*VIL_dim2*NC];
      memcpy(VIL, mxGetPr(VIL_mx), VI_dim1*VIL_dim2*NC*sizeof(double));
    }

  // Free MX arrays
  mxDestroyArray(V_mx);
  mxDestroyArray(z0_mx);
  mxDestroyArray(VI_mx);
  mxDestroyArray(meanx_mx);
  mxDestroyArray(sigz0_mx);
  mxDestroyArray(stdx_mx);
  mxDestroyArray(stdz_mx);
  mxDestroyArray(v_mx);
  mxDestroyArray(NC_mx);
  if (s == 1 || s == 5)
    {
      mxDestroyArray(Cgrid_mx);
      mxDestroyArray(Cfunc_mx);
    }
  else
    mxDestroyArray(VIL_mx);

  // Temporary variables
  xnorm = gsl_vector_alloc(2*n);
  znorm = gsl_vector_alloc(2*n);
  crbf = gsl_vector_alloc(NC);
  dist_z_z0 = gsl_vector_alloc(NC);
  rbfn = gsl_vector_alloc(NC);
  z1 = gsl_matrix_alloc(NC, 2*n);
  vi = gsl_matrix_alloc(VI_dim1, n);
  VI_tmp = gsl_matrix_alloc(VI_dim1, NC);
  instrs = gsl_vector_alloc(VI_dim1);
  ord2_vec = gsl_vector_alloc(2*n);
  ord2_mat = gsl_matrix_alloc(2*n, 2*n);

  if (s != 1 && s != 5)
    {
      vil = gsl_matrix_alloc(VI_dim1, VIL_dim2);
      VIL_tmp = gsl_matrix_alloc(VI_dim1, NC);
    }
}

MMJSolution::~MMJSolution()
{
  const int s = modspec.spec_no();

  gsl_matrix_free(V_prime);
  gsl_matrix_free(z0_prime);
  delete[] VI;
  gsl_vector_free(meanx);
  gsl_vector_free(sigz0);
  gsl_vector_free(stdx);
  gsl_vector_free(stdz);
  if (s == 1 || s == 5)
    {
      if (spline)
        gsl_spline_free(spline);
      if (acc)
        gsl_interp_accel_free(acc);
    }
  else
    delete[] VIL;

  // Temporary variables
  gsl_vector_free(xnorm);
  gsl_vector_free(znorm);
  gsl_vector_free(crbf);
  gsl_vector_free(dist_z_z0);
  gsl_vector_free(rbfn);
  gsl_vector_free(ord2_vec);
  gsl_matrix_free(z1);
  gsl_matrix_free(vi);
  gsl_matrix_free(VI_tmp);
  gsl_matrix_free(ord2_mat);
  gsl_vector_free(instrs);

  if (s != 1 && s != 5)
    {
      gsl_matrix_free(vil);
      gsl_matrix_free(VIL_tmp);
    }
}

void
MMJSolution::policy_func_internal(const gsl_vector *y_prev, const gsl_vector *shocks, gsl_vector *y_curr)
{
  const int n = modspec.n;
  const int s = modspec.spec_no();

  assert(y_prev->stride == 1 && y_curr->stride == 1);

  double *c_curr = gsl_vector_ptr(y_curr, 0);
  double *l_curr = gsl_vector_ptr(y_curr, n);
  double *i_curr = gsl_vector_ptr(y_curr, 2*n);
  double *k_curr = gsl_vector_ptr(y_curr, 3*n);
  double *a_curr = gsl_vector_ptr(y_curr, 4*n);

  const double *k_prev = gsl_vector_const_ptr(y_prev, 3*n);
  const double *a_prev = gsl_vector_const_ptr(y_prev, 4*n);

  // Set tech shock in y_curr
  for(int j = 0; j < n; j++)
    a_curr[j] = exp(modspec.rho*log(a_prev[j]) + modspec.sigma*(gsl_vector_get(shocks, j) + gsl_vector_get(shocks, n)));

  // Compute capital stocks
  for(int j = 0; j < n; j++)
    {
      gsl_vector_set(xnorm, j, k_prev[j]);
      gsl_vector_set(xnorm, j+n, a_curr[j]);
    }
  gsl_vector_sub(xnorm, meanx);
  gsl_vector_div(xnorm, stdx);

  gsl_blas_dgemv(CblasNoTrans, 1.0, V_prime, xnorm, 0.0, znorm);
  gsl_vector_div(znorm, stdz);

  gsl_vector_set_all(crbf, -0.5*v*v);
  gsl_vector_div(crbf, sigz0);
  gsl_vector_div(crbf, sigz0);

  for(int j = 0; j < NC; j++)
    gsl_matrix_set_row(z1, j, znorm);

  gsl_vector_set_zero(dist_z_z0);
  for(int j = 0; j < NC; j++)
    for(int k = 0; k < 2*n; k++)
      *gsl_vector_ptr(dist_z_z0, j) += gsl_matrix_get(z1, j, k) * gsl_matrix_get(z1, j, k) + gsl_matrix_get(z0_prime, k, j) * gsl_matrix_get(z0_prime, k, j) - 2 * gsl_matrix_get(z1, j, k) * gsl_matrix_get(z0_prime, k, j);

  for(int j = 0; j < NC; j++)
    gsl_vector_set(rbfn, j, exp(gsl_vector_get(crbf, j) * gsl_vector_get(dist_z_z0, j)));

  double sum = 0;
  for(int j = 0; j < NC; j++)
    sum += gsl_vector_get(rbfn, j);
  gsl_vector_scale(rbfn, 1/sum);

  for(int j = 0; j < n; j++)
    {
      for(int i = 0; i < VI_dim1; i++)
        for(int k = 0; k < NC; k++)
          gsl_matrix_set(VI_tmp, i, k, VI[i + VI_dim1*(j + n * k)]);
      gsl_blas_dgemv(CblasNoTrans, 1.0, VI_tmp, rbfn, 0.0, &gsl_matrix_column(vi, j).vector);
    }

  gsl_vector_set(instrs, 0, 1.0);
  gsl_vector_memcpy(&gsl_vector_subvector(instrs, 1, n).vector, &gsl_vector_const_view_array(k_prev, n).vector);
  gsl_vector_memcpy(&gsl_vector_subvector(instrs, n+1, n).vector, &gsl_vector_view_array(a_curr, n).vector);

  if (VI_dim1 > 2*n+1)
    {
      gsl_vector_memcpy(&gsl_vector_subvector(ord2_vec, 0, n).vector, &gsl_vector_const_view_array(k_prev, n).vector);
      gsl_vector_memcpy(&gsl_vector_subvector(ord2_vec, n, n).vector, &gsl_vector_view_array(a_curr, n).vector);
      gsl_matrix_set_zero(ord2_mat);
      gsl_blas_dsyr(CblasLower, 1.0, ord2_vec, ord2_mat);
      int k = 2*n+1;
      for(int j = 0; j < 2*n; j++)
        for(int i = j; i < 2*n; i++)
          gsl_vector_set(instrs, k++, gsl_matrix_get(ord2_mat, i, j));
    }
    
  gsl_blas_dgemv(CblasTrans, 1.0, vi, instrs, 0.0, &gsl_vector_view_array(k_curr, n).vector);

  // Set investment
  for(int j = 0; j < n; j++)
    i_curr[j] = k_curr[j] - (1.0-modspec.delta)*k_prev[j];

  // Compute labor and consumption
  if (s == 1 || s == 5)
    {
      // Set labor to NAN
      gsl_vector_set_all(&gsl_vector_view_array(l_curr, n).vector, NAN);

      // Compute aggregate consumption
      double C;
      double f[n], fk[n], fl[n];
      modspec.production(y_prev->data, y_curr->data, f, fk, fl);
      C = 0;
      for(int j = 0; j < n; j++)
        C += a_curr[j] * f[j] - k_curr[j] + k_prev[j] - modspec.phi/2.0*k_prev[j]*pow(k_curr[j]/k_prev[j]-1.0, 2.0);

      if (!spline)
        {
          // All gammas equal to one
          for(int j = 0; j < n; j++)
            c_curr[j] = C / ((double) n);
        }
      else
        {
          // Interpolate consumption of first country
          gsl_spline_eval_e(spline, C, acc, c_curr);

          // Compute consumption for other countries and labor for all
          const ModelSpecA1A5 &ms = dynamic_cast<const ModelSpecA1A5 &>(modspec);

          double dif = 1;

          while(dif > 1e-10)
            {
              double ec = C;

              for(int j = 1; j < n; j++)
                {
                  c_curr[j] = pow(modspec.taus[0] / modspec.taus[j] * pow(c_curr[0], -1.0/ms.gammas[0]), -ms.gammas[j]);
                  ec -= c_curr[j];
                }

              c_curr[0] = 0.99*c_curr[0] + 0.01*ec;
              dif = fabs(1.0-ec/c_curr[0]);
            }
        }
    }
  else
    {
      // Initialize labor
      for(int j = 0; j < VIL_dim2; j++)
        {
          for(int i = 0; i < VI_dim1; i++)
            for(int k = 0; k < NC; k++)
              gsl_matrix_set(VIL_tmp, i, k, VIL[i + VI_dim1*(j + VIL_dim2 * k)]);
          gsl_blas_dgemv(CblasNoTrans, 1.0, VIL_tmp, rbfn, 0.0, &gsl_matrix_column(vil, j).vector);
        }

      if (s == 3 || s == 4 || s == 7 || s == 8)
        gsl_blas_dgemv(CblasTrans, 1.0, vil, instrs, 0.0, &gsl_vector_view_array(l_curr, n).vector);
      else // A2 or A6
        gsl_blas_ddot(instrs, &gsl_matrix_column(vil, 0).vector, l_curr); // Only labor supply of first country

      double dif = 1;

      // Loop to compute consumption and labor
      while(dif > 1e-10)
        {
          if (s == 2 || s == 6)
            {
              const ModelSpecA2A6 &ms = dynamic_cast<const ModelSpecA2A6 &>(modspec);

              // Compute labor supply of other countries
              for(int j = 1; j < n; j++)
                l_curr[j] = pow(a_curr[j] * pow(k_prev[j], modspec.alpha) / a_curr[0]
                                / pow(k_prev[0], modspec.alpha) * modspec.taus[0] * ms.b[0]
                                / modspec.taus[j] / ms.b[j], ms.etas[j] / (1+modspec.alpha*ms.etas[j]))
                  * pow(l_curr[0], (ms.etas[j]+modspec.alpha*ms.etas[j]*ms.etas[0])
                        / (ms.etas[0]+modspec.alpha*ms.etas[j]*ms.etas[0]));

              // Compute consumption of all countries
              for(int j = 0; j < n; j++)
                c_curr[j] = pow(ms.b[j]*pow(l_curr[j], 1/ms.etas[j])/modspec.A/(1.0-modspec.alpha)
                                / a_curr[j] / pow(k_prev[j], modspec.alpha) / pow(l_curr[j], -modspec.alpha),
                                -ms.gammas[j]);

              double el = 0;
              for(int j = 0; j < n; j++)
                el += c_curr[j] + k_curr[j] - k_prev[j] + modspec.phi/2.0*pow(k_curr[j]/k_prev[j]-1.0, 2.0)*k_prev[j];
              for(int j = 1; j < n; j++)
                el -= modspec.A * pow(k_prev[j], modspec.alpha) * pow(l_curr[j], 1.0-modspec.alpha) * a_curr[j];
              el /= modspec.A * pow(k_prev[0], modspec.alpha) * a_curr[0];
              el = pow(el, 1.0/(1.0-modspec.alpha));

              l_curr[0] = 0.99*l_curr[0] + 0.01*el;
              dif = fabs(1.0-el/l_curr[0]);
            }
          else if (s == 3 || s == 7)
            {
              const ModelSpecA3A7 &ms = dynamic_cast<const ModelSpecA3A7 &>(modspec);

              for(int j = 0; j < n; j++)
                c_curr[j] = ms.psi/(1.0-ms.psi)*modspec.A*(1.0-modspec.alpha)*a_curr[j]*pow(k_prev[j], modspec.alpha)*pow(l_curr[j], -modspec.alpha)*(ms.time_endowment - l_curr[j]);

              double el[n];
              el[n-1] = 0.0;
              for(int j = 0; j < n; j++)
                el[n-1] += c_curr[j] + k_curr[j] - k_prev[j] + modspec.phi/2.0*pow(k_curr[j]/k_prev[j]-1.0, 2.0)*k_prev[j];
              for(int j = 0; j < n-1; j++)
                el[n-1] -= modspec.A * pow(k_prev[j], modspec.alpha) * pow(l_curr[j], 1.0-modspec.alpha) * a_curr[j];
              el[n-1] = pow(el[n-1] / (modspec.A * pow(k_prev[n-1], modspec.alpha) * a_curr[n-1]), 1.0/(1.0-modspec.alpha));

              for(int j = 0; j < n-1; j++)
                el[j] = ms.time_endowment - pow(ms.taus[n-1]/ms.taus[j]*pow(c_curr[n-1], ms.psi*(1.0-1.0/ms.gammas[n-1])-1.0)/pow(c_curr[j], ms.psi*(1.0-1.0/ms.gammas[j])-1.0)*pow(ms.time_endowment-l_curr[n-1], (1.0-ms.psi)*(1.0-1.0/ms.gammas[n-1])), 1.0/(1.0-ms.psi)/(1.0-1.0/ms.gammas[j]));

              dif = 0.0;
              for(int j = 0; j < n; j++)
                {
                  l_curr[j] = 0.99*l_curr[j]+0.01*el[j];
                  dif += fabs(1.0-el[j]/l_curr[j]);
                }
              dif /= (double) n;
            }
          else // A4 or A8
            {
              const ModelSpecA4A8 &ms = dynamic_cast<const ModelSpecA4A8 &>(modspec);

              for(int j = 0; j < n; j++)
                c_curr[j] = pow((1.0-modspec.alpha)*modspec.A*a_curr[j]*pow(l_curr[j], ms.mus[j]-1.0)
                                *pow(modspec.alpha*pow(k_prev[j], ms.mus[j])
                                     + (1.0-modspec.alpha)*pow(l_curr[j], ms.mus[j]), 1.0/ms.mus[j]-1.0)/ms.b[j], ms.xis[j]) * (ms.time_endowment - l_curr[j]);

              double el[n];
              el[0] = 0;
              for(int j = 0; j < n; j++)
                el[0] += c_curr[j] + k_curr[j] - k_prev[j] + modspec.phi/2.0*pow(k_curr[j]/k_prev[j]-1.0, 2.0)*k_prev[j];
              for(int j = 1; j < n; j++)
                el[0] -= ms.A * a_curr[j] * pow(ms.alpha * pow(k_prev[j], ms.mus[j]) + (1.0-ms.alpha) * pow(l_curr[j], ms.mus[j]), 1.0/ms.mus[j]);

              el[0] = pow((pow(el[0]/a_curr[0]/ms.A, ms.mus[0]) - ms.alpha*pow(k_prev[0], ms.mus[0]))/(1.0-ms.alpha), 1.0/ms.mus[0]);

              for(int j = 1; j < n; j++)
                el[j] = ms.time_endowment-pow((pow(ms.taus[0]/ms.taus[j]*pow(pow(c_curr[0], 1.0-1.0/ms.xis[0])+ms.b[0]*pow(ms.time_endowment-l_curr[0], 1.0-1.0/ms.xis[0]), (1.0/ms.xis[0]-1.0/ms.gammas[0])/(1.0-1.0/ms.xis[0]))*pow(c_curr[0], -1.0/ms.xis[0])/pow(c_curr[j], -1.0/ms.xis[j]), (1.0-1.0/ms.xis[j])/(1.0/ms.xis[j]-1.0/ms.gammas[j]))-pow(c_curr[j], 1.0-1.0/ms.xis[j]))/ms.b[j], 1.0/(1.0-1.0/ms.xis[j]));

              dif = 0.0;
              for(int j = 0; j < n; j++)
                {
                  l_curr[j] = 0.99*l_curr[j]+0.01*el[j];
                  dif += fabs(1.0-el[j]/l_curr[j]);
                }
              dif /= (double) n;
            }
        }
    }

  // Set lambda to the mean of marginal utilites of consumption
  double uc_curr[n], ul_curr[n];
  gsl_vector_set(y_curr, 5*n, 0.0);
  modspec.marginal_utilities(y_curr->data, uc_curr, ul_curr);
  for(int j = 0; j < n; j++)
    *gsl_vector_ptr(y_curr, 5*n) += uc_curr[j] * modspec.taus[j];
  *gsl_vector_ptr(y_curr, 5*n) /= (double) n;
}
