//============================================================================
//
// This file is part of the Shapes Generative Model Library (SGML) project.
//
// Copyright (c) 2010-2014 - Evangelos Kalogerakis, Siddhartha Chaudhuri (authors of the code) / Stanford University, UMass-Amherst
//
// SGML 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.

// SGML 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 SGML.  If not, see <http://www.gnu.org/licenses/>.
//
//============================================================================

#include "S12_ShapeBNetNode.hpp"

namespace SNL {
namespace S12 {

//////////////////////////////////////////////////////////
///////////////////// Gaussian ////////////////////////////
//////////////////////////////////////////////////////////
void Gaussian::initialize(size_t _d)
{
  b.resize(0, 0);
  b_0.clear();

  num_data_points = 0;
  rank = _d;
  determinant_covariance = 0;
  mean.resize(_d, 0.0);
  covariance.resize(_d, _d, 0.0);
  inverse_covariance.resize(_d, _d, 0.0);
}


double Gaussian::evaluateGaussian(SNL_VECTOR(double) x)
{
  assert(!mean.empty());
  assert(b_0.empty());
  double xsx = 0.0;
  selfsubtract(x, mean);
  SNL_VECTOR(double) xs(x.size(), 0.0);

  for (size_t i = 0; i < x.size(); i++)
  {
    for (size_t j = 0; j < x.size(); j++)
      xs[i] += x[j] * inverse_covariance[j][i];
  }

  for (size_t i = 0; i < x.size(); i++)
    xsx += xs[i] * x[i];

  xsx /= -2.0;

  double d = double(x.size());
  //return max( min( weight * exp( (double)xsx - log( max(sqrt( determinant_covariance ), SUPEREPSILON) ) - (d/2.0)*log(2.0*M_PI) ), 1.0/EPSILON), EPSILON);
  return max(min(exp((double)xsx - log(max(sqrt(determinant_covariance), SUPEREPSILON)) - (d / 2.0) * log(2.0 * M_PI)), 1.0 / EPSILON), EPSILON);
}


double Gaussian::evaluateCondGaussian(SNL_VECTOR(double) y, SNL_VECTOR(double) x)
{
  assert(mean.empty());
  assert(!b_0.empty());
  assert(!b.isEmpty());
  SNL_VECTOR(double) newmean = add(b_0, mult(transpose(b), x));
  double ysy = 0.0;
  selfsubtract(y, newmean);
  SNL_VECTOR(double) ys(y.size(), 0.0);

  for (size_t i = 0; i < y.size(); i++)
  {
    for (size_t j = 0; j < y.size(); j++)
      ys[i] += y[j] * inverse_covariance[j][i];
  }

  for (size_t i = 0; i < y.size(); i++)
    ysy += ys[i] * y[i];

  ysy /= -2.0;
  double d = double(y.size());

  //std::cout << ysy << std::endl;
  //std::cout << weight << std::endl;
  //std::cout << determinant_covariance << std::endl;
  //std::cout << log( sqrt( determinant_covariance ) ) << std::endl;
  //std::cout << max( min( weight * exp( (double)ysy - log( max(sqrt( determinant_covariance ), 1e-300)  ) - (d/2.0)*log(2.0*M_PI) ), 1.0/EPSILON), EPSILON) << std::endl;
  //system("pause");

  //return max( min( weight * exp( (double)ysy - log( max(sqrt( determinant_covariance ), SUPEREPSILON) ) - (d/2.0)*log(2.0*M_PI) ), 1.0/EPSILON), EPSILON);
  return max(min(exp((double)ysy - log(max(sqrt(determinant_covariance), SUPEREPSILON)) - (d / 2.0) * log(2.0 * M_PI)), 1.0 / EPSILON), EPSILON);
}


double Gaussian::getRoughProbability(double density, double dim, size_t intervals_to_divide_cov)
{
	double std_dev = max( sqrt( determinant_covariance ), SUPEREPSILON );
	double interval_size_1d = 5.0 * pow(std_dev, 1.0/dim ) / intervals_to_divide_cov; // get inside 10sigma, 5 bins each dimensions
	return pow(interval_size_1d, dim ) *density;
}


static inline std::ostream & operator << (std::ostream & os, const Gaussian & g)
{
  os << "Gaussian parms: " << std::endl;
  os <<  "determinant of cov.: " << g.determinant_covariance << ", rank: " << g.rank << ", num data points: " << g.num_data_points << std::endl;
  os << "mean: " << g.mean;
  os << "covariance: \n" << g.covariance;
  os << "inv covariance: \n" << g.inverse_covariance;
  os << "b: \n" << g.b;
  os << "b0: \n" << g.b_0;
  return os << std::endl;
}


static inline std::ostream &operator << (std::ostream &os, const Gaussian::Canonical &c)
{
  os << "K: " << c.K << std::endl;
  os << "h: " << c.h << std::endl;
  os << "g: " << c.g << std::endl;
  return os;
}


//////////////////////////////////////////////////////////
/////////////////////// Node ////////////////////////////
//////////////////////////////////////////////////////////

void Node::initialize()
{
  size_t cpt_size = domain_size;

  for (size_t p = 0; p < parents.size(); p++)
    cpt_size *= ((Node*)parents[p])->domain_size;

  cpt.resize(cpt_size);
  MS.resize(cpt_size);

  for (size_t d = 0; d < cpt_size; d++)
  {
    cpt[d] = 0.0;
    MS[d] = 0.0;
  }
}


void Node::clearMS()
{
  for (size_t d = 0; d < cpt.size(); d++)
  {
    MS[d] = 0.0;
  }
}


size_t Node::ac2(size_t d, size_t p)  // column major access
{
  assert(parents.size() == 1);
  return d + p * domain_size; //d*parents[0]->domain_size + p;
}


size_t Node::acn(size_t d, SNL_VECTOR(size_t) x)  // column major access
{
  size_t offset = d;

  for (size_t k = 0; k < x.size(); k++)
  {
    size_t prod = domain_size;

    for (size_t l = k + 1; l < x.size(); l++)
      prod *= ((Node*)parents[l])->domain_size;

    offset += prod * x[k];
  }

  return (size_t)offset;
}


size_t Node::getValues(size_t offset, SNL_VECTOR(size_t) & x)
{
  x.resize(parents.size());

  for (size_t k = 0; k < x.size(); k++)
  {
    int prod = domain_size;

    for (size_t l = k + 1; l < x.size(); l++)
      prod *= ((Node*)parents[l])->domain_size;

    x[k] = offset / prod;
    offset = offset % prod;
  }

  return offset;
}


void RootNode::initialize() { Node::initialize(); }
void RootNode::clearMS() { Node::clearMS(); }
size_t RootNode::getValuePos(size_t & value) const { return Node::getValuePos(value); }

void LayoutNode::initialize() { Node::initialize(); }
void LayoutNode::clearMS() { Node::clearMS(); }
size_t LayoutNode::getValuePos(size_t & value) const { return Node::getValuePos(value); }

void ExistsNode::initialize() { Node::initialize(); }
void ExistsNode::clearMS() { Node::clearMS(); }
size_t ExistsNode::getValuePos(size_t & value) const { return Node::getValuePos(value); }

void CardinalityNode::initialize() { Node::initialize(); }
void CardinalityNode::clearMS() { Node::clearMS(); }
size_t CardinalityNode::getValuePos(size_t & value) const { return Node::getValuePos(value); }

void AdjacencyNumPartsNode::initialize() { Node::initialize(); }
void AdjacencyNumPartsNode::clearMS() { Node::clearMS(); }
size_t AdjacencyNumPartsNode::getValuePos(size_t & value) const { return Node::getValuePos(value); }


//////////////////////////////////////////////////////////
/////////////////////// Learning nodes ////////////////////////////
//////////////////////////////////////////////////////////


void RootNode::learn(double MAP_PRIOR_PARAM)
{
  for (size_t m = 0; m < Pr.numRows(); m++)
    for (size_t d = 0; d < domain_size; d++)
      cpt[d] += Pr[m][d];

  for (size_t d = 0; d < domain_size; d++)
    cpt[d] = (cpt[d] + MAP_PRIOR_PARAM) / (double(Pr.numRows()) + MAP_PRIOR_PARAM * double(domain_size));
}


void LayoutNode::learn(double MAP_PRIOR_PARAM)
{
  assert(parent != NULL);
  assert(parents.size() == 1);

  SNL_MATRIX(double) cpt_nom(domain_size, parent->domain_size, 0.0);
  SNL_VECTOR(double) cpt_denom(parent->domain_size, 0.0);

  for (size_t m = 0; m < Plr.size(); m++)
  {
    for (size_t d = 0; d < parent->domain_size; d++)
    {
      double ckj = 0;

      for (size_t d2 = 0; d2 < domain_size; d2++)
      {
        cpt_nom[ d2 ][ d ] += Plr[m][d2][d];
        ckj += Plr[m][d2][d];
      }

      cpt_denom[ d ] += parent->Pr[m][d];
    }
  }

  for (size_t d = 0; d < parent->domain_size; d++)
    for (size_t d2 = 0; d2 < domain_size; d2++)
      cpt[ ac2(d2, d) ] = (cpt_nom[ d2 ][ d ] + MAP_PRIOR_PARAM) / (cpt_denom[ d ] + MAP_PRIOR_PARAM * double(domain_size));
}


// has deterministic CPT
void ExistsNode::learn()
{
  for (size_t d = 0; d < parent->domain_size; d++)
  {
    cpt[ ac2(0, d) ] = d == 0;
    cpt[ ac2(1, d) ] = d > 0;
  }
}


void CardinalityNode::learn(const SNL_VECTOR( SNL_VECTOR(size_t) ) & training_data, double MAP_PRIOR_PARAM, bool use_cpt)
{
  assert(parent != NULL);
  assert(parents.size() >= 2);
  assert(!values.empty());
  e->initialize();
  e->learn();

  SNL_VECTOR( size_t ) Y(training_data.size());
  SNL_VECTOR( SNL_VECTOR(size_t) ) X(training_data.size());
  SNL_VECTOR( SNL_VECTOR(size_t) ) X_values(1 + parents_CardinalityNode.size());
  X_values[0].push_back(0);
  X_values[0].push_back(1);

  for (size_t p = 0; p < parents_CardinalityNode.size(); p++)
  {
    X_values[p + 1] = parents_CardinalityNode[p]->values;
  }

  for (size_t m = 0; m < training_data.size(); m++)
  {
    Y[m] = training_data[m][ label->id ];
    X[m].push_back(training_data[m][ label->id ] > 0);

    for (size_t p = 0; p < parents_CardinalityNode.size(); p++)
    {
      X[m].push_back(training_data[m][ parents_CardinalityNode[p]->label->id ]);
    }
  }


  if (!use_cpt)
  {
    if (sigmoids.empty())
    {
      sigmoids.resize(parent->domain_size);

      for (size_t d = 0; d < parent->domain_size; d++)
      {
        sigmoids[d] = SigmoidCPD(values.size());
        sigmoids[d].learn(Y, values, X, X_values);
      }
    }
  }


  SNL_VECTOR(double) denom(cpt.size(), 0.0);

  for (size_t i = 0; i < cpt.size(); i++)
  {
    SNL_VECTOR(size_t) v;
    size_t d = getValues(i, v);

    if ((v[1] == 0 && values[d] > 0) || (v[1] == 1 && values[d] == 0))
    {
      cpt[i]  = 0.0;
    }
    else
    {
      cpt[ i ] = MAP_PRIOR_PARAM;
      denom[ acn(0, v) ] += MAP_PRIOR_PARAM;
    }

    for (size_t m = 0; m < training_data.size(); m++)
    {
      if (equals(X[m], replaceElementsOfYAccordingToX(subvector(v, 1), X_values)) && values[d] == Y[m] )
      {
        if (cpt[i] == 0.0)
          continue;

        if (!use_cpt)
        {
          cpt[ i ] += parent->Pr[m][ v[0] ] * sigmoids[ v[0] ].R[m][d];
          denom[ acn(0, v) ] += parent->Pr[m][ v[0] ] * sigmoids[ v[0] ].R[m][d];
        }
        else
        {
          cpt[ i ] += parent->Pr[m][ v[0] ];
          denom[ acn(0, v) ] += parent->Pr[m][ v[0] ];
        }
      }
    }
  }

  for (size_t i = 0; i < cpt.size(); i++)
  {
    SNL_VECTOR(size_t) v;
    size_t d = getValues(i, v);

    // no data with these parent values were found
    if (denom[ acn(0, v) ] <= double(domain_size-1)*MAP_PRIOR_PARAM)
    {
      // re-inforce 0 value in this case.
      if (values[d] == 0)
      {
        cpt[ i ] += 1.0;
        denom[ acn(0, v) ] += 1.0;
      }
    }

    if (denom[ acn(0, v) ] < SUPEREPSILON)
      denom[ acn(0, v) ] = SUPEREPSILON;
  }

  for (size_t i = 0; i < cpt.size(); i++)
  {
    SNL_VECTOR(size_t) v;
    getValues(i, v);
    cpt[i] /= denom[ acn(0, v) ];
  }

//   if (this->label->name == "fixedwing")
//   {
//     printcpt();
//     for (size_t i = 0; i < cpt.size(); i++)
//     {
//       SNL_VECTOR(size_t) v;
//       size_t d = getValues(i, v);
//       std::cout << denom[ acn(0, v) ] << std::endl;
//       std::cout << double(domain_size)*MAP_PRIOR_PARAM << std::endl;
//     }
//
//     getch();
//   }
//   for (size_t i = 0; i < cpt.size(); i++)
//   {
// //    if (isnan(cpt[i]) || !finite(cpt[i]) || cpt[i] > 1.01)
// //   {
//       SNL_VECTOR(size_t) v;
//       size_t d = getValues(i, v);
//       std::cout << "val[d]: " << values[d] << std::endl;
//       std::cout << "v: " << v;
//       std::cout << "cpt: " << cpt[i] << std::endl;
//       std::cout << "denom: " << denom[ acn(0, v) ] << std::endl;
//     }
//  }
}


void AdjacencyNumPartsNode::learn(const SNL_VECTOR( SNL_VECTOR( SNL_VECTOR(size_t) ) ) & training_data, const SNL_VECTOR( SNL_VECTOR(size_t) ) & cardinality_training_data, double MAP_PRIOR_PARAM, bool use_cpt)
{
  assert(parent != NULL);
  assert(parents.size() >= 1);
  assert(!values.empty());

  SNL_VECTOR( size_t ) Y(training_data.size());
  SNL_VECTOR( SNL_VECTOR(size_t) ) X(training_data.size());
  SNL_VECTOR( SNL_VECTOR(size_t) ) X_values(parents_CardinalityNode.size());

  for (size_t p = 0; p < parents_CardinalityNode.size(); p++)
  {
    X_values[p] = parents_CardinalityNode[p]->values;
  }

  for (size_t m = 0; m < training_data.size(); m++)
  {
    Y[m] = training_data[m][ label->id ][ adj_label_pos ];

    for (size_t p = 0; p < parents_CardinalityNode.size(); p++)
    {
      X[m].push_back(cardinality_training_data[m][ parents_CardinalityNode[p]->label->id ]);
    }
  }

  if (!use_cpt)
  {
    if (sigmoids.empty())
    {
      sigmoids.resize(parent->domain_size);

      for (size_t d2 = 0; d2 < parent->domain_size; d2++)
      {
        sigmoids[d2] = SigmoidCPD(values.size());
        sigmoids[d2].learn(Y, values, X, X_values);
      }
    }
  }

  SNL_VECTOR(double) denom(cpt.size(), SUPEREPSILON);

  for (size_t i = 0; i < cpt.size(); i++)
  {
    SNL_VECTOR(size_t) v;
    size_t d = getValues(i, v);
    cpt[ i ] = MAP_PRIOR_PARAM;

    for (size_t vi = 1; vi < v.size(); vi++)
      if (X_values[vi - 1][ v[vi] ] == 0 && values[d] > 0)
        cpt[ i ] = 0.0;

    if (v[0] == 0 && values[d] > 0)
      cpt[ i ] = 0.0;

    if (cpt[ i ] == MAP_PRIOR_PARAM)
      denom[ acn(0, v) ] += MAP_PRIOR_PARAM;

    for (size_t m = 0; m < training_data.size(); m++)
    {
      if (equals(X[m], replaceElementsOfYAccordingToX(subvector(v, 1), X_values)) && values[d] == Y[m] )
      {
        if (cpt[ i ] == 0.0)
          continue;

        if (!use_cpt)
        {
          cpt[ i ] += parent->Pl[m][ v[0] ] * sigmoids[ v[0] ].R[m][d];
          denom[ acn(0, v) ] += parent->Pl[m][ v[0] ] * sigmoids[ v[0] ].R[m][d];
        }
        else
        {
          cpt[ i ] += parent->Pl[m][ v[0] ];
          denom[ acn(0, v) ] += parent->Pl[m][ v[0] ];
        }
      }
    }
  }

  for (size_t i = 0; i < cpt.size(); i++)
  {
    SNL_VECTOR(size_t) v;
    getValues(i, v);
    cpt[i] /= denom[ acn(0, v) ];
  }

//   for (size_t i = 0; i < cpt.size(); i++)
//   {
//     if (isnan(cpt[i]) || !finite(cpt[i]) || cpt[i] > 1.01)
//     {
//       SNL_VECTOR(size_t) v;
//       size_t d = getValues(i, v);
//       std::cout << "fatal error: cpt during learning acquired illegal values" << std::endl;
//       std::cout << "d: " << d << std::endl;
//       std::cout << "v: " << v << std::endl;
//       std::cout << "denom: " << denom[ acn(0, v) ] << std::endl;
//       std::cout << "cpt: " << cpt << std::endl;
//       exit(-1);
//     }
//   }

}


bool GaussianNode::learn(const SNL_VECTOR( SNL_VECTOR( SNL_VECTOR(double) ) ) & training_data, double T_0, double NU)
{
  bool problem = false;
  assert(layout_node != NULL);

  this->gaussians.clear();

  for (size_t d2 = 0; d2 < layout_node->domain_size; d2++)
  {
    this->gaussians.push_back(Gaussian());
  }

  size_t l = layout_node->label->id;

  SNL_VECTOR(double) class_mean;
  double ck = 0;

  for (size_t m = 0; m < training_data.size(); m++)
  {
    if (!training_data[m][l].empty())
    {
      if (class_mean.empty())
        class_mean.resize(training_data[m][l].size(), 0.0);

      selfadd(class_mean, training_data[m][l]);
      ck++;
    }
  }

  selfdiv(class_mean, ck);


  if (this->parents_gaussians.empty())
  {
    for (size_t d2 = 1; d2 < layout_node->domain_size; d2++)
    {
      double weight = 0.0;
      double k = 0.0; // used to re-normalize the gaussian.weight

      for (size_t m = 0; m < training_data.size(); m++)
      {
        if (!training_data[m][l].empty())
        {
          if (this->gaussians[d2].mean.empty())
          {
            this->gaussians[d2].initialize(training_data[m][l].size());
          }

          selfadd(this->gaussians[d2].mean, mult(training_data[m][l], layout_node->Pl[m][d2]));
          k++;
          weight += layout_node->Pl[m][d2];
        }
      }

      //    if ( value_to_layout_id[l][d2] != -1 && layout_nodes[l]->gaussians[d2].mean.size() > 0 && layout_nodes[l]->gaussians[d2].weight < EPSILON )
      //    {
      //      layout_nodes[l]->gaussians[d2].weight = EPSILON;
      //      std::cout << "Warning: two or more gaussians converged - log lik will now become -inf" << std::endl;
      //      this->loglik = -FLT_MAX;
      //    }

      if (this->gaussians[d2].mean.size() > 0)
      {
        selfadd(this->gaussians[d2].mean, mult(class_mean, NU));
        selfdiv(this->gaussians[d2].mean, (weight + NU));
        this->gaussians[d2].bak_mean = this->gaussians[d2].mean;
        this->gaussians[d2].num_data_points = (size_t)k;
      }

      for (size_t m = 0; m < training_data.size(); m++)
      {
        if (!training_data[m][l].empty())
        {
          SNL_VECTOR(double)  Fi = training_data[m][l];
          selfsubtract(Fi, this->gaussians[d2].mean);
          selfadd(this->gaussians[d2].covariance, mult(covfromvector(Fi, T_0), layout_node->Pl[m][d2]));      // covfromvector has addition of T
        }
      }

      if (this->gaussians[d2].mean.size() > 0)
      {
        SNL_VECTOR(double) bias = subtract(class_mean, this->gaussians[d2].mean);
        selfadd(this->gaussians[d2].covariance, mult(covfromvector(bias, 0.0), NU));
        selfdiv(this->gaussians[d2].covariance, (weight + NU));

        // DIAGONALIZE IF NECESSARY
        if ( this->gaussians[d2].covariance.numRows()*this->gaussians[d2].covariance.numRows() + this->gaussians[d2].covariance.numRows() >= k )
        {
          selfDiagonalize(this->gaussians[d2].covariance, 6);
        }

        this->gaussians[d2].bak_covariance = this->gaussians[d2].covariance;
        this->gaussians[d2].inverse_covariance = invert(this->gaussians[d2].covariance, &problem);
        this->gaussians[d2].determinant_covariance = determinant(this->gaussians[d2].covariance, this->gaussians[d2].rank);
      }
    } // end of loop over parent layout values
  } // end of gaussian node learning without cont. parents
  else
  {
    for (size_t d2 = 1; d2 < layout_node->domain_size; d2++)
    {
      SNL_MATRIX(double)     covariance_xy;   // X x Y
      SNL_MATRIX(double)     covariance_xx;
      SNL_MATRIX(double)     covariance_yy;
      SNL_VECTOR( double )   mean_y;
      SNL_VECTOR( double )   mean_x;
      SNL_VECTOR( double )   class_mean_x;
      double                 weight = 0.0;

      double k = 0.0; // used to re-normalize the gaussian.weight

      for (size_t m = 0; m < training_data.size(); m++)
      {
        if (!training_data[m][l].empty())
        {
          SNL_VECTOR( double ) Fy = training_data[m][l];
          SNL_VECTOR( double ) Fx;

          for (size_t p = 0; p < this->parents_gaussians.size(); p++)
          {
            size_t lx = this->parents_gaussians[p]->layout_node->label->id;
            Fx = concatenateVectors(Fx, training_data[m][lx]);
          }

          if (mean_y.empty())
          {
            mean_y.resize(Fy.size(), 0.0);
            mean_x.resize(Fx.size(), 0.0);
            class_mean_x.resize(Fx.size(), 0.0);
            covariance_xx.resize(Fx.size(), Fx.size(), 0.0);
            covariance_xy.resize(Fx.size(), Fy.size(), 0.0);
            covariance_yy.resize(Fy.size(), Fy.size(), 0.0);
          }

          selfadd(mean_y, mult(Fy, layout_node->Pl[m][d2]));
          selfadd(mean_x, mult(Fx, layout_node->Pl[m][d2]));
          selfadd(class_mean_x, Fx);
          k++;
          weight += layout_node->Pl[m][d2];
        }
      }

      if (mean_y.size() > 0)
      {
        selfadd(mean_y, mult(class_mean, NU));
        selfdiv(mean_y, (weight + NU));
        this->gaussians[d2].bak_mean = mean_y;

        selfdiv(class_mean_x, k);
        selfadd(mean_x, mult(class_mean_x, NU));
        selfdiv(mean_x, (weight + NU));
        this->gaussians[d2].num_data_points = (size_t)k;
      }

      for (size_t m = 0; m < training_data.size(); m++)
      {
        if (!training_data[m][l].empty())
        {
          SNL_VECTOR( double ) Fy = training_data[m][l];
          SNL_VECTOR( double ) Fx;

          for (size_t p = 0; p < this->parents_gaussians.size(); p++)
          {
            size_t lx = this->parents_gaussians[p]->layout_node->label->id;
            Fx = concatenateVectors(Fx, training_data[m][lx]);
          }

          selfsubtract(Fy, mean_y);
          selfsubtract(Fx, mean_x);
          selfadd(covariance_yy, mult(covfromvector(Fy, T_0), layout_node->Pl[m][d2]));
          selfadd(covariance_xx, mult(covfromvector(Fx, T_0), layout_node->Pl[m][d2]));
          selfadd(covariance_xy, mult(covfromvector(Fx, Fy), layout_node->Pl[m][d2]));
        }
      }

      if (mean_y.size() > 0)
      {
        SNL_VECTOR(double) bias_y = subtract(class_mean, mean_y);
        SNL_VECTOR(double) bias_x = subtract(class_mean_x, mean_x);
        selfadd(covariance_yy, mult(covfromvector(bias_y, 0.0), NU));
        selfadd(covariance_xx, mult(covfromvector(bias_x, 0.0), NU));
        selfdiv(covariance_yy, (weight + NU));
        selfdiv(covariance_xx, (weight + NU));
        selfdiv(covariance_xy, weight);

        SNL_MATRIX(double) covariance_yx = transpose(covariance_xy);
        this->gaussians[d2].b_0 = subtract(mean_y, mult(mult(covariance_yx, invert(covariance_xx, &problem)), mean_x));         // Y x 1
        this->gaussians[d2].b   = mult(invert(covariance_xx, &problem), covariance_xy);     // X x Y
        this->gaussians[d2].covariance = subtract(covariance_yy, mult(mult(covariance_yx, invert(covariance_xx, &problem)), covariance_xy));       // Y x Y
                
        if ( this->gaussians[d2].covariance.numRows()*this->gaussians[d2].covariance.numRows() + this->gaussians[d2].covariance.numRows() >= k )
        {
          selfDiagonalize(this->gaussians[d2].covariance);
        }
				this->gaussians[d2].inverse_covariance = invert(this->gaussians[d2].covariance, &problem);
        this->gaussians[d2].determinant_covariance = determinant(this->gaussians[d2].covariance, this->gaussians[d2].rank);

        if ( covariance_xx.numRows()*covariance_xx.numRows() + covariance_xx.numRows() >= k )
        {
          selfDiagonalize(covariance_xx);
        }
				this->gaussians[d2].covariance_xx = covariance_xx;

        if ( covariance_yy.numRows()*covariance_yy.numRows() + covariance_yy.numRows() >= k )
        {
          selfDiagonalize(covariance_yy);
        }
				this->gaussians[d2].bak_covariance = covariance_yy;


        //this->gaussians[d2].weight = weight / k;
      }
    } // end of loop over parent layout values
  } // end of gaussian node learning with  cont. parents

  return problem;
}

} // namespace S12
} // namespace SNL
