SparseMatrix.cc

/*
 * Copyright (C) 2007-2009 Dynare Team
 *
 * This file is part of Dynare.
 *
 * Dynare 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.
 *
 * Dynare 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 Dynare.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <cstring>
#include <ctime>
#include <sstream>
#include "SparseMatrix.hh"

#ifdef _MSC_VER
unsigned long _nan[2] = { 0xffffffff, 0x7fffffff };
double NAN = *((double *) _nan);
#endif

SparseMatrix::SparseMatrix()
{
  pivotva = NULL;
  g_save_op = NULL;
  g_nop_all = 0;
  mem_mngr.init_Mem();
  symbolic = true;
  alt_symbolic = false;
  alt_symbolic_count = 0;
  max_u = 0;
  min_u = 0x7FFFFFFF;
  res1a = 9.0e60;
  tbreak_g = 0;
  start_compare = 0;
  restart = 0;
  IM_i.clear();
}

int
SparseMatrix::NRow(int r)
{
  return NbNZRow[r];
}

int
SparseMatrix::NCol(int c)
{
  return NbNZCol[c];
}

int
SparseMatrix::At_Row(int r, NonZeroElem **first)
{
  (*first) = FNZE_R[r];
  return NbNZRow[r];
}

int
SparseMatrix::Union_Row(int row1, int row2)
{
  NonZeroElem *first1, *first2;
  int n1 = At_Row(row1, &first1);
  int n2 = At_Row(row2, &first2);
  int i1 = 0, i2 = 0, nb_elem = 0;
  while (i1 < n1 && i2 < n2)
    {
      if (first1->c_index == first2->c_index)
        {
          nb_elem++;
          i1++;
          i2++;
          first1 = first1->NZE_R_N;
          first2 = first2->NZE_R_N;
        }
      else if (first1->c_index < first2->c_index)
        {
          nb_elem++;
          i1++;
          first1 = first1->NZE_R_N;
        }
      else
        {
          nb_elem++;
          i2++;
          first2 = first2->NZE_R_N;
        }
    }
  return nb_elem;
}

int
SparseMatrix::At_Pos(int r, int c, NonZeroElem **first)
{
  (*first) = FNZE_R[r];
  while ((*first)->c_index != c)
    (*first) = (*first)->NZE_R_N;
  return NbNZRow[r];
}

int
SparseMatrix::At_Col(int c, NonZeroElem **first)
{
  (*first) = FNZE_C[c];
  return NbNZCol[c];
}

int
SparseMatrix::At_Col(int c, int lag, NonZeroElem **first)
{
  (*first) = FNZE_C[c];
  int i = 0;
  while ((*first)->lag_index != lag && (*first))
    (*first) = (*first)->NZE_C_N;
  if ((*first))
    {
      NonZeroElem *firsta = (*first);
      if (!firsta->NZE_C_N)
        i++;
      else
        {
          while (firsta->lag_index == lag && firsta->NZE_C_N)
            {
              firsta = firsta->NZE_C_N;
              i++;
            }
          if (firsta->lag_index == lag) i++;
        }
    }
  return i;
}

void
SparseMatrix::Delete(const int r, const int c)
{
	NonZeroElem *first = FNZE_R[r], *firsta = NULL;

  while (first->c_index != c)
    {
      firsta = first;
      first = first->NZE_R_N;
    }
  if (firsta != NULL)
    firsta->NZE_R_N = first->NZE_R_N;
  if (first == FNZE_R[r])
    FNZE_R[r] = first->NZE_R_N;
  NbNZRow[r]--;

  first = FNZE_C[c];
  firsta = NULL;
  while (first->r_index != r)
    {
      firsta = first;
      first = first->NZE_C_N;
    }

  if (firsta != NULL)
    firsta->NZE_C_N = first->NZE_C_N;
  if (first == FNZE_C[c])
		FNZE_C[c] = first->NZE_C_N;

  u_liste.push_back(first->u_index);
  mem_mngr.mxFree_NZE(first);
  NbNZCol[c]--;
}

void
SparseMatrix::Print(int Size, int *b)
{
  int a, i, j, k, l;
  mexPrintf("   ");
  for (k = 0; k < Size*periods; k++)
    mexPrintf("%-2d ", k);
  mexPrintf("    |    ");
  for (k = 0; k < Size*periods; k++)
    mexPrintf("%8d", k);
  mexPrintf("\n");
  for (i = 0; i < Size*periods; i++)
    {
      NonZeroElem *first = FNZE_R[i];
      j = NbNZRow[i];
      mexPrintf("%-2d ", i);
      a = 0;
      for (k = 0; k < j; k++)
        {
          for (l = 0; l < (first->c_index-a); l++)
            mexPrintf("   ");
          mexPrintf("%-2d ", first->u_index);
          a = first->c_index+1;
          first = first->NZE_R_N;
        }
      for (k = a; k < Size*periods; k++)
        mexPrintf("   ");
      mexPrintf("%-2d ", b[i]);

      first = FNZE_R[i];
      j = NbNZRow[i];
      mexPrintf(" | %-2d ", i);
      a = 0;
      for (k = 0; k < j; k++)
        {
          for (l = 0; l < (first->c_index-a); l++)
            mexPrintf("        ");
          mexPrintf("%8.4f", double (u[first->u_index]));
          a = first->c_index+1;
          first = first->NZE_R_N;
        }
      for (k = a; k < Size*periods; k++)
        mexPrintf("        ");
      mexPrintf("%8.4f", double (u[b[i]]));
      mexPrintf("\n");
    }
}

void
SparseMatrix::Insert(const int r, const int c, const int u_index, const int lag_index)
{
  NonZeroElem *firstn, *first, *firsta, *a;
  firstn = mem_mngr.mxMalloc_NZE();
  first = FNZE_R[r];
  firsta = NULL;
  while (first->c_index < c && (a = first->NZE_R_N))
    {
      firsta = first;
      first = a;
    }
  firstn->u_index = u_index;
  firstn->r_index = r;
  firstn->c_index = c;
  firstn->lag_index = lag_index;
  if (first->c_index > c)
    {
      if (first == FNZE_R[r])
        FNZE_R[r] = firstn;
      if (firsta != NULL)
        firsta->NZE_R_N = firstn;
      firstn->NZE_R_N = first;
    }
  else
    {
      first->NZE_R_N = firstn;
      firstn->NZE_R_N = NULL;
    }
  NbNZRow[r]++;
  first = FNZE_C[c];
  firsta = NULL;
  while (first->r_index < r && (a = first->NZE_C_N))
    {
      firsta = first;
      first = a;
    }
  if (first->r_index > r)
    {
      if (first == FNZE_C[c])
        FNZE_C[c] = firstn;
      if (firsta != NULL)
        firsta->NZE_C_N = firstn;
      firstn->NZE_C_N = first;
    }
  else
    {
      first->NZE_C_N = firstn;
      firstn->NZE_C_N = NULL;
    }

  NbNZCol[c]++;
}

void
SparseMatrix::Read_SparseMatrix(string file_name, const int Size, int periods, int y_kmin, int y_kmax, bool steady_state, bool two_boundaries)
{
  unsigned int eq, var;
  int i, j, lag;
  filename = file_name;
  mem_mngr.fixe_file_name(file_name);
  if (!SaveCode.is_open())
    {
    	if(steady_state)
        SaveCode.open((file_name + "_static.bin").c_str(), ios::in | ios::binary);
			else
			  SaveCode.open((file_name + "_dynamic.bin").c_str(), ios::in | ios::binary);
      if (!SaveCode.is_open())
        {
        	if(steady_state)
            mexPrintf("Error : Can't open file \"%s\" for reading\n", (file_name + "_static.bin").c_str());
					else
					  mexPrintf("Error : Can't open file \"%s\" for reading\n", (file_name + "_dynamic.bin").c_str());
          mexEvalString("st=fclose('all');clear all;");
          mexErrMsgTxt("Exit from Dynare");
        }
    }
  IM_i.clear();
	if(two_boundaries)
	  {
	  	for (i = 0; i < u_count_init-Size; i++)
        {
          SaveCode.read(reinterpret_cast<char *>(&eq), sizeof(eq));
          SaveCode.read(reinterpret_cast<char *>(&var), sizeof(var));
          SaveCode.read(reinterpret_cast<char *>(&lag), sizeof(lag));
          SaveCode.read(reinterpret_cast<char *>(&j), sizeof(j));
          IM_i[make_pair(make_pair(eq, var), lag)] = j;
        }
      for (j=0;j<Size;j++)
        IM_i[make_pair(make_pair(j, Size*(periods+y_kmax)), 0)] = j;
	  }
	else
	  {
	  	for (i = 0; i < u_count_init; i++)
        {
          SaveCode.read(reinterpret_cast<char *>(&eq), sizeof(eq));
          SaveCode.read(reinterpret_cast<char *>(&var), sizeof(var));
          SaveCode.read(reinterpret_cast<char *>(&lag), sizeof(lag));
          SaveCode.read(reinterpret_cast<char *>(&j), sizeof(j));
          IM_i[make_pair(make_pair(eq, var), lag)] = j;
        }
	  }
  index_vara = (int *) mxMalloc(Size*(periods+y_kmin+y_kmax)*sizeof(int));
  for (j = 0; j < Size; j++)
    SaveCode.read(reinterpret_cast<char *>(&index_vara[j]), sizeof(*index_vara));
  if (periods+y_kmin+y_kmax > 1)
    {
      for (i = 1; i < periods+y_kmin+y_kmax; i++)
        {
          for (j = 0; j < Size; j++)
            {
              index_vara[j+Size*i] = index_vara[j+Size*(i-1)]+y_size;
            }
        }
    }
  index_equa = (int *) mxMalloc(Size*sizeof(int));
  for (j = 0; j < Size; j++)
    {
      SaveCode.read(reinterpret_cast<char *>(&index_equa[j]), sizeof(*index_equa));
    }
}

void
SparseMatrix::Simple_Init(int it_, int y_kmin, int y_kmax, int Size, map<pair<pair<int, int>, int>, int> &IM)
{
  int i, eq, var, lag;
  map<pair<pair<int, int>, int>, int>::iterator it4;
  NonZeroElem *first;
  pivot = (int *) mxMalloc(Size*sizeof(int));
  pivot_save = (int *) mxMalloc(Size*sizeof(int));
  pivotk = (int *) mxMalloc(Size*sizeof(int));
  pivotv = (double *) mxMalloc(Size*sizeof(double));
  pivotva = (double *) mxMalloc(Size*sizeof(double));
  b = (int *) mxMalloc(Size*sizeof(int));
  line_done = (bool *) mxMalloc(Size*sizeof(bool));

  mem_mngr.init_CHUNK_BLCK_SIZE(u_count);
  g_save_op = NULL;
  g_nop_all = 0;
  i = Size*sizeof(NonZeroElem *);
  FNZE_R = (NonZeroElem **) mxMalloc(i);
  FNZE_C = (NonZeroElem **) mxMalloc(i);
  NonZeroElem **temp_NZE_R = (NonZeroElem **) mxMalloc(i);
  NonZeroElem **temp_NZE_C = (NonZeroElem **) mxMalloc(i);
  i = Size*sizeof(int);
  NbNZRow = (int *) mxMalloc(i);
  NbNZCol = (int *) mxMalloc(i);
  i = Size*sizeof(*b);
  it4 = IM.begin();
  eq = -1;
  //#pragma omp parallel for num_threads(atoi(getenv("DYNARE_NUM_THREADS")))
  for (i = 0; i < Size; i++)
    {
      b[i] = 0;
      line_done[i] = 0;
      FNZE_C[i] = 0;
      FNZE_R[i] = 0;
      temp_NZE_C[i] = 0;
      temp_NZE_R[i] = 0;
      NbNZRow[i] = 0;
      NbNZCol[i] = 0;
    }
  int u_count1 = Size;
  while (it4 != IM.end())
    {
      var = it4->first.first.second;
      eq = it4->first.first.first;
      lag = it4->first.second;
      if (lag == 0)   /*Build the index for sparse matrix containing the jacobian : u*/
        {
          NbNZRow[eq]++;
          NbNZCol[var]++;
          first = mem_mngr.mxMalloc_NZE();
          first->NZE_C_N = NULL;
          first->NZE_R_N = NULL;
          first->u_index = u_count1;
          first->r_index = eq;
          first->c_index = var;
          first->lag_index = lag;
          if (FNZE_R[eq] == NULL)
            FNZE_R[eq] = first;
          if (FNZE_C[var] == NULL)
            FNZE_C[var] = first;
          if (temp_NZE_R[eq] != NULL)
            temp_NZE_R[eq]->NZE_R_N = first;
          if (temp_NZE_C[var] != NULL)
            temp_NZE_C[var]->NZE_C_N = first;
          temp_NZE_R[eq] = first;
          temp_NZE_C[var] = first;
          u_count1++;
        }
      it4++;
    }
  //#pragma omp parallel for num_threads(atoi(getenv("DYNARE_NUM_THREADS")))
  for (i = 0; i < Size; i++)
		b[i] = i;
  mxFree(temp_NZE_R);
  mxFree(temp_NZE_C);
  u_count = u_count1;
}

void
SparseMatrix::Init(int periods, int y_kmin, int y_kmax, int Size, map<pair<pair<int, int>, int>, int> &IM)
{
  int t, i, eq, var, lag, ti_y_kmin, ti_y_kmax;
  double tmp_b = 0.0;
  map<pair<pair<int, int>, int>, int>::iterator it4;
  NonZeroElem *first;
  pivot = (int *) mxMalloc(Size*periods*sizeof(int));
  pivot_save = (int *) mxMalloc(Size*periods*sizeof(int));
  pivotk = (int *) mxMalloc(Size*periods*sizeof(int));
  pivotv = (double *) mxMalloc(Size*periods*sizeof(double));
  pivotva = (double *) mxMalloc(Size*periods*sizeof(double));
  b = (int *) mxMalloc(Size*periods*sizeof(int));
  line_done = (bool *) mxMalloc(Size*periods*sizeof(bool));
  mem_mngr.init_CHUNK_BLCK_SIZE(u_count);
  g_save_op = NULL;
  g_nop_all = 0;
  i = (periods+y_kmax+1)*Size*sizeof(NonZeroElem*);
  FNZE_R = (NonZeroElem **) mxMalloc(i);
  FNZE_C = (NonZeroElem **) mxMalloc(i);
  NonZeroElem **temp_NZE_R = (NonZeroElem **) mxMalloc(i);
  NonZeroElem **temp_NZE_C = (NonZeroElem **) mxMalloc(i);
  i = (periods+y_kmax+1)*Size*sizeof(int);
  NbNZRow = (int *) mxMalloc(i);
  NbNZCol = (int *) mxMalloc(i);

  //#pragma omp parallel for num_threads(atoi(getenv("DYNARE_NUM_THREADS")))
  for (i = 0; i < periods*Size; i++)
    {
      b[i] = 0;
      line_done[i] = 0;
    }
  //#pragma omp parallel for num_threads(atoi(getenv("DYNARE_NUM_THREADS")))
  for (i = 0; i < (periods+y_kmax+1)*Size; i++)
    {
      FNZE_C[i] = 0;
      FNZE_R[i] = 0;
      temp_NZE_C[i] = NULL;
      temp_NZE_R[i] = NULL;
      NbNZRow[i] = 0;
      NbNZCol[i] = 0;
    }

  //#pragma omp parallel for num_threads(atoi(getenv("DYNARE_NUM_THREADS"))) ordered private(it4, ti_y_kmin, ti_y_kmax, eq, var, lag) schedule(dynamic)
  for (t = 0; t < periods; t++)
    {
      ti_y_kmin = -min(t, y_kmin);
      ti_y_kmax = min(periods-(t+1), y_kmax);
      it4 = IM.begin();
      eq = -1;
      //#pragma omp ordered
      while (it4 != IM.end())
        {
          var = it4->first.first.second;
          if (eq != it4->first.first.first+Size*t)
            tmp_b = 0;
          eq = it4->first.first.first+Size*t;
          lag = it4->first.second;
          if (var < (periods+y_kmax)*Size)
            {
              lag = it4->first.second;
              if (lag <= ti_y_kmax && lag >= ti_y_kmin)   /*Build the index for sparse matrix containing the jacobian : u*/
                {
                  var += Size*t;
                  NbNZRow[eq]++;
                  NbNZCol[var]++;
                  first = mem_mngr.mxMalloc_NZE();
                  first->NZE_C_N = NULL;
                  first->NZE_R_N = NULL;
                  first->u_index = it4->second+u_count_init*t;
                  first->r_index = eq;
                  first->c_index = var;
                  first->lag_index = lag;
                  if (FNZE_R[eq] == NULL)
                    FNZE_R[eq] = first;
                  if (FNZE_C[var] == NULL)
                    FNZE_C[var] = first;
                  if (temp_NZE_R[eq] != NULL)
                    temp_NZE_R[eq]->NZE_R_N = first;
                  if (temp_NZE_C[var] != NULL)
                    temp_NZE_C[var]->NZE_C_N = first;
                  temp_NZE_R[eq] = first;
                  temp_NZE_C[var] = first;
                }
              else       /*Build the additive terms ooutside the simulation periods related to the first lags and the last leads...*/
                {
                  if (lag < ti_y_kmin)
                    {
                      tmp_b += u[it4->second+u_count_init*t]*y[index_vara[var+Size*(y_kmin+t)]];
                    }
                  else
                    {
                      tmp_b += u[it4->second+u_count_init*t]*y[index_vara[var+Size*(y_kmin+t)]];

                    }
                }
            }
          else           /* ...and store it in the u vector*/
            {
              b[eq] = it4->second+u_count_init*t;
              u[b[eq]] += tmp_b;
              tmp_b = 0;
            }
          it4++;
        }
    }
  mxFree(temp_NZE_R);
  mxFree(temp_NZE_C);
}

void
SparseMatrix::ShortInit(int periods, int y_kmin, int y_kmax, int Size, map<pair<pair<int, int>, int>, int> &IM)
{
  int t, eq, var, lag, ti_y_kmin, ti_y_kmax;
  double tmp_b = 0.0;
  map<pair<pair<int, int>, int>, int>::iterator it4;
  //#pragma omp parallel for num_threads(atoi(getenv("DYNARE_NUM_THREADS"))) ordered private(it4, ti_y_kmin, ti_y_kmax, eq, var, lag, tmp_b) schedule(dynamic)
  for (t = 0; t < periods; t++)
    {
      ti_y_kmin = -min(t, y_kmin);
      ti_y_kmax = min(periods-(t+1), y_kmax);
      it4 = IM.begin();
      eq = -1;
      while (it4 != IM.end())
        {
          var = it4->first.first.second;
          if (eq != it4->first.first.first+Size*t)
            tmp_b = 0;
          eq = it4->first.first.first+Size*t;
          if (var < (periods+y_kmax)*Size)
            {
              lag = it4->first.second;
              if (lag <= ti_y_kmax && lag >= ti_y_kmin)
                {
                  var += Size*t;
                }
              else
                {
                  tmp_b += u[it4->second+u_count_init*t]*y[index_vara[var+Size*(y_kmin+t)]];
                }
            }
          else
            {
              b[eq] = it4->second+u_count_init*t;
              u[b[eq]] += tmp_b;
            }
          it4++;
        }
    }
}

int
SparseMatrix::Get_u()
{
  if (!u_liste.empty())
    {
      int i = u_liste.back();
      u_liste.pop_back();
      return i;
    }
  else
    {
      if (u_count < u_count_alloc)
        {
          int i = u_count;
          u_count++;
          return i;
        }
      else
        {
          u_count_alloc += 5*u_count_alloc_save;
          u = (double *) mxRealloc(u, u_count_alloc*sizeof(double));
          if (!u)
            {
              mexPrintf("Error in Get_u: memory exhausted (realloc(%d))\n", u_count_alloc*sizeof(double));
              mexEvalString("st=fclose('all');clear all;");
              mexErrMsgTxt("Exit from Dynare");
            }
          int i = u_count;
          u_count++;
          return i;
        }
    }
}

void
SparseMatrix::Delete_u(int pos)
{
  u_liste.push_back(pos);
}

void
SparseMatrix::Clear_u()
{
  u_liste.clear();
}

void
SparseMatrix::Print_u()
{
  for (unsigned int i = 0; i < u_liste.size(); i++)
    mexPrintf("%d ", u_liste[i]);
}

void
SparseMatrix::End(int Size)
{
  mem_mngr.Free_All();
  mxFree(FNZE_R);
  mxFree(FNZE_C);
  mxFree(NbNZRow);
  mxFree(NbNZCol);
  mxFree(b);
  mxFree(line_done);
  mxFree(pivot);
  mxFree(pivot_save);
  mxFree(pivotk);
  mxFree(pivotv);
  mxFree(pivotva);
}

bool
SparseMatrix::compare(int *save_op, int *save_opa, int *save_opaa, int beg_t, int periods, long int nop4,  int Size)
{
  long int i, j, nop = nop4/2, t, k;
  double r = 0.0;
  bool OK = true;
  clock_t t001;
  t_save_op_s *save_op_s, *save_opa_s, *save_opaa_s;
  int *diff1, *diff2;
  t001 = clock();
  diff1 = (int *) mxMalloc(nop*sizeof(int));
  diff2 = (int *) mxMalloc(nop*sizeof(int));
  int max_save_ops_first = -1;
  j = k = i = 0;
  while (i < nop4 && OK)
    {
      save_op_s = (t_save_op_s *) &(save_op[i]);
      save_opa_s = (t_save_op_s *) &(save_opa[i]);
      save_opaa_s = (t_save_op_s *) &(save_opaa[i]);
      diff1[j] = save_op_s->first-save_opa_s->first;
      if (max_save_ops_first < save_op_s->first+diff1[j]*(periods-beg_t))
        {
          max_save_ops_first = save_op_s->first+diff1[j]*(periods-beg_t);
        }
      switch (save_op_s->operat)
        {
        case IFLD:
        case IFDIV:
          OK = (save_op_s->operat == save_opa_s->operat && save_opa_s->operat == save_opaa_s->operat
                && diff1[j] == (save_opa_s->first-save_opaa_s->first));
          i += 2;
          break;
        case IFLESS:
        case IFSUB:
          diff2[j] = save_op_s->second-save_opa_s->second;
          OK = (save_op_s->operat == save_opa_s->operat && save_opa_s->operat == save_opaa_s->operat
                && diff1[j] == (save_opa_s->first-save_opaa_s->first)
                && diff2[j] == (save_opa_s->second-save_opaa_s->second));
          i += 3;
          break;
        default:
          mexPrintf("unknown operator = %d ", save_op_s->operat);
          mexEvalString("st=fclose('all');clear all;");
          filename += " stopped";
          mexErrMsgTxt(filename.c_str());
          break;
        }
      j++;
    }
  // the same pivot for all remaining periods
  if (OK)
    {
      //#pragma omp parallel for  num_threads(atoi(getenv("DYNARE_NUM_THREADS"))) ordered private(j) schedule(dynamic)
      for (i = beg_t; i < periods; i++)
        {
          for (j = 0; j < Size; j++)
            {
              ///#pragma omp ordered
              pivot[i*Size+j] = pivot[(i-1)*Size+j]+Size;
            }
        }
      if (max_save_ops_first >= u_count_alloc)
        {
          u_count_alloc += max_save_ops_first;
          u = (double *) mxRealloc(u, u_count_alloc*sizeof(double));
          if (!u)
            {
              mexPrintf("Error in Get_u: memory exhausted (realloc(%d))\n", u_count_alloc*sizeof(double));
              mexEvalString("st=fclose('all');clear all;");
              mexErrMsgTxt("Exit from Dynare");
            }
        }
      double *up;
      for (t = 1; t < periods-beg_t-y_kmax; t++)
        {
          i = j = 0;
          while (i < nop4)
            {
              save_op_s = (t_save_op_s *) (&(save_op[i]));
              up = &u[save_op_s->first+t*diff1[j]];
              switch (save_op_s->operat)
                {
                case IFLD:
                  r = *up;
                  i += 2;
                  break;
                case IFDIV:
                  *up /= r;
                  i += 2;
                  break;
                case IFSUB:
                  *up -= u[save_op_s->second+t*diff2[j]]*r;;
                  i += 3;
                  break;
                case IFLESS:
                  *up = -u[save_op_s->second+t*diff2[j]]*r;
                  i += 3;
                  break;
                }
              j++;
            }
        }
      int t1 = max(1, periods-beg_t-y_kmax);
      int periods_beg_t = periods-beg_t;
      for (t = t1; t < periods_beg_t; t++)
        {
          i = j = 0;
          while (i < nop4)
            {
              save_op_s = (t_save_op_s *) (&(save_op[i]));
              if (save_op_s->lag < (periods_beg_t-t))
                {
                  up = &u[save_op_s->first+t*diff1[j]];
                  switch (save_op_s->operat)
                    {
                    case IFLD:
                      r = *up;
                      i += 2;
                      break;
                    case IFDIV:
                      *up /= r;
                      i += 2;
                      break;
                    case IFSUB:
                      *up -= u[save_op_s->second+t*diff2[j]]*r;
                      i += 3;
                      break;
                    case IFLESS:
                      *up = -u[save_op_s->second+t*diff2[j]]*r;
                      i += 3;
                      break;
                    }
                }
              else
                {
                  switch (save_op_s->operat)
                    {
                    case IFLD:
                    case IFDIV:
                      i += 2;
                      break;
                    case IFSUB:
                    case IFLESS:
                      i += 3;
                      break;
                    }
                }
              j++;
            }
        }
    }
  mxFree(diff1);
  mxFree(diff2);
  return OK;
}

int
SparseMatrix::complete(int beg_t, int Size, int periods, int *b)
{
  long int i, j, k, nop, nopa, nop1, cal_y, nb_var, pos, t, ti, max_var, min_var;
  NonZeroElem *first;
  int *save_code;
  int *diff;
  double yy = 0.0, err;

  int size_of_save_code = (1+y_kmax)*Size*(Size+1+4)/2*4;
  save_code = (int *) mxMalloc(size_of_save_code*sizeof(int));
  int size_of_diff = (1+y_kmax)*Size*(Size+1+4);
  diff = (int *) mxMalloc(size_of_diff*sizeof(int));
  cal_y = y_size*y_kmin;

  i = (beg_t+1)*Size-1;
  nop = 0;
  for (j = i; j > i-Size; j--)
    {
      pos = pivot[j];
      nb_var = At_Row(pos, &first);
      first = first->NZE_R_N;
      nb_var--;
      save_code[nop] = IFLDZ;
      save_code[nop+1] = 0;
      save_code[nop+2] = 0;
      save_code[nop+3] = 0;
      if ((nop+3) >= size_of_save_code)
        mexPrintf("out of save_code[%d] (bound=%d)\n", nop+2, size_of_save_code);
      nop += 4;
      for (k = 0; k < nb_var; k++)
        {
          save_code[nop] = IFMUL;
          save_code[nop+1] = index_vara[first->c_index]+cal_y;
          save_code[nop+2] = first->u_index;
          save_code[nop+3] = first->lag_index;
          if ((nop+3) >= size_of_save_code)
            mexPrintf("out of save_code[%d] (bound=%d)\n", nop+2, size_of_save_code);
          nop += 4;
          first = first->NZE_R_N;
        }
      save_code[nop] = IFADD;
      save_code[nop+1] = b[pos];
      save_code[nop+2] = 0;
      save_code[nop+3] = 0;
      if ((nop+3) >= size_of_save_code)
        mexPrintf("out of save_code[%d] (bound=%d)\n", nop+2, size_of_save_code);
      nop += 4;
      save_code[nop] = IFSTP;
      save_code[nop+1] = index_vara[j]+y_size*y_kmin;
      save_code[nop+2] = 0;
      save_code[nop+3] = 0;
      if ((nop+2) >= size_of_save_code)
        mexPrintf("out of save_code[%d] (bound=%d)\n", nop+2, size_of_save_code);
      nop += 4;
    }
  i = beg_t*Size-1;
  nop1 = nopa = 0;
  for (j = i; j > i-Size; j--)
    {
      pos = pivot[j];
      nb_var = At_Row(pos, &first);
      first = first->NZE_R_N;
      nb_var--;
      diff[nopa] = 0;
      diff[nopa+1] = 0;
      nopa += 2;
      nop1 += 4;
      for (k = 0; k < nb_var; k++)
        {
          diff[nopa] = save_code[nop1+1]-(index_vara[first->c_index]+cal_y);
          diff[nopa+1] = save_code[nop1+2]-(first->u_index);
          if ((nop1+2) >= size_of_save_code)
            mexPrintf("out of save_code[%d] (bound=%d)\n", nop1+2, size_of_save_code);
          if ((nopa+1) >= size_of_diff)
            mexPrintf("out of diff[%d] (bound=%d)\n", nopa+2, size_of_diff);
          nopa += 2;
          nop1 += 4;
          first = first->NZE_R_N;
        }
      diff[nopa] = save_code[nop1+1]-(b[pos]);
      diff[nopa+1] = 0;
      if ((nop1+3) >= size_of_save_code)
        mexPrintf("out of save_code[%d] (bound=%d)\n", nop1+2, size_of_save_code);
      if ((nopa+1) >= size_of_diff)
        mexPrintf("out of diff[%d] (bound=%d)\n", nopa+2, size_of_diff);
      nopa += 2;
      nop1 += 4;
      diff[nopa] = save_code[nop1+1]-(index_vara[j]+y_size*y_kmin);
      diff[nopa+1] = 0;
      if ((nop1+4) >= size_of_save_code)
        mexPrintf("out of save_code[%d] (bound=%d)\n", nop1+2, size_of_save_code);
      if ((nopa+1) >= size_of_diff)
        mexPrintf("out of diff[%d] (bound=%d)\n", nopa+2, size_of_diff);
      nopa += 2;
      nop1 += 4;
    }
  max_var = (periods+y_kmin)*y_size;
  min_var = y_kmin*y_size;
  int k1 = 0;
  for (t = periods+y_kmin-1; t >= beg_t+y_kmin; t--)
    {
      j = 0;
      ti = t-y_kmin-beg_t;
      for (i = 0; i < nop; i += 4)
        {
          switch (save_code[i])
            {
            case IFLDZ:
              yy = 0;
              break;
            case IFMUL:
              k = save_code[i+1]+ti*diff[j];
              if (k < max_var && k > min_var)
                {
                  yy += y[k]*u[save_code[i+2]+ti*diff[j+1]];
                }
              break;
            case IFADD:
              yy = -(yy+u[save_code[i+1]+ti*diff[j]]);
              break;
            case IFSTP:
              k = save_code[i+1]+ti*diff[j];
              k1 = k;
              err = yy - y[k];
              y[k] += slowc*(err);
              break;
            }
          j += 2;
        }
    }
  mxFree(save_code);
  mxFree(diff);
  return (beg_t);
}

double
SparseMatrix::bksub(int tbreak, int last_period, int Size, double slowc_l)
{
  NonZeroElem *first;
  int i, j, k;
  double yy;
  res1 = res2 = max_res = 0;
  for (i = 0; i < y_size*(periods+y_kmin); i++)
    y[i] = ya[i];
  if (symbolic && tbreak)
    last_period = complete(tbreak, Size, periods, b);
  else
    last_period = periods;
  for (int t = last_period+y_kmin-1; t >= y_kmin; t--)
    {
      int ti = (t-y_kmin)*Size;
      int cal = y_kmin*Size;
      int cal_y = y_size*y_kmin;
      for (i = ti-1; i >= ti-Size; i--)
        {
          j = i+cal;
          int pos = pivot[i+Size];
          int nb_var = At_Row(pos, &first);
          first = first->NZE_R_N;
          nb_var--;
          int eq = index_vara[j]+y_size;
          yy = 0;
          ostringstream tmp_out;
          tmp_out.clear();
          for (k = 0; k < nb_var; k++)
            {
              yy += y[index_vara[first->c_index]+cal_y]*u[first->u_index];
              tmp_out << "y[" << index_vara[first->c_index]+cal_y << "]" << "(" << y[index_vara[first->c_index]+cal_y] << ")*u[" << first->u_index << "](" << u[first->u_index] << ")+";
              first = first->NZE_R_N;
            }
          yy = -(yy+y[eq]+u[b[pos]]);
          direction[eq] = yy;
          y[eq] += slowc_l*yy;
        }
    }
  return res1;
}

double
SparseMatrix::simple_bksub(int it_, int Size, double slowc_l)
{
  int i, k;
  double yy;
  NonZeroElem *first;
  res1 = res2 = max_res = 0;
  for (i = 0; i < y_size; i++)
    y[i+it_*y_size] = ya[i+it_*y_size];
  for (i = Size-1; i >= 0; i--)
    {
      int pos = pivot[i];
      int nb_var = At_Row(pos, &first);
      first = first->NZE_R_N;
      nb_var--;
      int eq = index_vara[i];
      yy = 0;
      ostringstream tmp_out;
      tmp_out.clear();
      tmp_out << "|";
      for (k = 0; k < nb_var; k++)