void deriveEquiFreqAndGtrParamsForReversibleRM(matrix_t const & Q, vector_t & equiFreq, vector_t & gtrParams)
  {
    equiFreq = deriveEquiFreqForReversibleRM(Q);
    unsigned n = equiFreq.size();
   
    // check
    for (unsigned i = 0; i < n; i++)
      assert(equiFreq[i] > 0);
    
    matrix_t pm = Q;  // parameter matrix
    for (unsigned i = 0; i < n; i++)
      for (unsigned j = 0; j < n; j++)
	pm(i, j) = Q(i, j) / equiFreq[j];

    // check grtParams size
    unsigned paramCount = (n * (n - 1) / 2);
    if (gtrParams.size() != paramCount)
      gtrParams.resize(paramCount);

    // reverse of gtrRateParametersToMatrix
    unsigned idx = 0;
    for (unsigned i = 0; i < n; i++)
      for (unsigned j = i + 1; j < n; j++) {
	gtrParams[idx] = pm(i, j);
	idx++;
      }
  }

void lfit(vector_t &x, vector_t &y, vector_t &sig, vector_t &a,
 	  vector<bool> &ia, matrix_t &covar, double &chisq, matrix_t & X)
{
  int i,j,k,l,m,mfit=0;
  double ym,wt,sum,sig2i;
  
  int ndat=x.size();
  int ma=a.size();
  vector_t afunc(ma);
  matrix_t beta;
  sizeMatrix(beta,ma,1);
  for (j=0;j<ma;j++)
    if (ia[j]) mfit++;
  if (mfit == 0) error("lfit: no parameters to be fitted");
  for (j=0;j<mfit;j++) {
    for (k=0;k<mfit;k++) covar[j][k]=0.0;
    beta[j][0]=0.0;
  }
  for (i=0;i<ndat;i++) {
    afunc = X[i];

    ym=y[i];
    if (mfit < ma) {
      for (j=0;j<ma;j++)
	if (!ia[j]) ym -= a[j]*afunc[j];
    }
    sig2i=1.0/SQR(sig[i]);
    for (j=0,l=0;l<ma;l++) {
      if (ia[l]) {
	wt=afunc[l]*sig2i;
	for (k=0,m=0;m<=l;m++)
	  if (ia[m]) covar[j][k++] += wt*afunc[m];
	beta[j++][0] += ym*wt;
      }
    }
  }
  for (j=1;j<mfit;j++)
    for (k=0;k<j;k++)
      covar[k][j]=covar[j][k];
  vector<vector<double> >  temp;
  sizeMatrix(temp,mfit,mfit);
  for (j=0;j<mfit;j++)
    for (k=0;k<mfit;k++)
      temp[j][k]=covar[j][k];
  gaussj(temp,beta);
  for (j=0;j<mfit;j++)
    for (k=0;k<mfit;k++)
      covar[j][k]=temp[j][k];
  for (j=0,l=0;l<ma;l++)
    if (ia[l]) a[l]=beta[j++][0];
  chisq=0.0;
  for (i=0;i<ndat;i++) {
    afunc = X[i];
    sum=0.0;
    for (j=0;j<ma;j++) sum += a[j]*afunc[j];
    chisq += SQR((y[i]-sum)/sig[i]);
  }
  covsrt(covar,ia,mfit);
}

vector_t next_vec( const vector_t &current, const vector_t &grad,
                   const vector_t &lapl ) {
    vector_t next( current.size() );
//    std::cout << "next_vec" << std::endl;
//    std::cout << current.size() << " " << grad.size() << " "
//        << lapl.size() << std::endl;
    for ( unsigned int i = 0; i < current.size(); ++i ) {
        next[i] = current[i] - grad[i] / lapl[i]; }
    return next; }

static void CheckVector( const vector_t& cv, size_t expected_size, size_t old_size ) {
    ASSERT( cv.capacity()>=expected_size, NULL );
    ASSERT( cv.size()==expected_size, NULL );
    ASSERT( cv.empty()==(expected_size==0), NULL );
    for( int j=0; j<int(expected_size); ++j ) {
        if( cv[j].bar()!=~j )
            REPORT("ERROR on line %d for old_size=%ld expected_size=%ld j=%d\n",__LINE__,long(old_size),long(expected_size),j);
    }
}

bool ModelUtil::EarlyStop(vector_t val_losses, size_t patience, float delta) {
  // Check for edge cases
  PELOTON_ASSERT(patience > 1);
  PELOTON_ASSERT(delta > 0);
  if (val_losses.size() < patience) return false;
  float cur_loss = val_losses[val_losses.size() - 1];
  float pat_loss = val_losses[val_losses.size() - patience];
  // Loss should have at least dropped by delta at this point
  return (pat_loss - cur_loss) < delta;
}

quat_t MeCtQuickDraw::rotation_to_target( vector_t l_forward_dir, vector_t w_target_pos, joint_state_t* state_p )	{

	vector_t l_target_dir_n = ( -state_p->parent_rot ) * ( w_target_pos - state_p->world_pos ).normal();

	gwiz::float_t rad = safe_acos( l_forward_dir.dot( l_target_dir_n ) );
	gwiz::float_t angle = deg( rad );
	vector_t axis = l_forward_dir.cross( l_target_dir_n ).normal();

	return( quat_t( angle, axis ) );
}

void rl_nac::solve_for_grad( vector_t& guess ) {

  matrix_t copyA;
  //vector_t guess;

  LapackInfo info(0);

  copyA = A;
  guess = b;
  
  // Regularize this: A = A + REGULARIZER*eye(N)
  int cnt = A.GetM();
  for ( int i=0; i<cnt; i++ ) {
    copyA(i,i) = copyA(i,i) + REGULARIZER;
  }

#ifdef DEBUG
  printf("=====================================\n");
  printf("Solving for gradient:\n");
  copyA.Print();
  guess.Print();
  printf("=====================================\n");
#endif

  vector_t tau;
  GetQR( copyA, tau, info );

  if ( info.GetInfo() != 0 ) {
    fprintf( stderr, "Error in QR decomposition!\n" );
    for ( int a=0; a<act_cnt; a++ ) {
      (*acts)[a]->natural_grad.Fill(0);
    }
    guess.Fill( 0 );
    return;
  }

  SolveQR( copyA, tau, guess, info );

  if ( info.GetInfo() != 0 ) {
    fprintf( stderr, "Error solving linear system!\n" );
    for ( int a=0; a<act_cnt; a++ ) {
      (*acts)[a]->natural_grad.Fill(0);
    }
    guess.Fill( 0 );
    return;
  }

  // results are stored in guess
#ifdef DEBUG
  printf("Final answer:\n");
  guess.Print();
#endif

  return;
}

 scalar_t vgrad(const vector_t& x, vector_t* gx) const override
 {
         m_idata = map_tensor(x.data(), m_idata.dims());
         m_op.output(m_idata, m_wdata, m_bdata, m_odata);
         if (gx)
         {
                 gx->resize(x.size());
                 auto idata = map_tensor(gx->data(), m_idata.dims());
                 m_op.ginput(idata, m_wdata, m_bdata, m_odata);
         }
         return m_odata.array().square().sum() / 2;
 }

        void quaternion_t::compute_rotation(const vector_t& v1, const vector_t& v2)
        {
            PRX_ASSERT(v1.get_dim() == v2.get_dim());

            vector_t v(3);
            double w;

            v.cross_product(v1,v2);
            w = sqrt(v1.squared_norm() * v2.squared_norm()) + v1.dot_product(v2);
            set(v[0],v[1],v[2],w);

            normalize();
        }

		static std::uint32_t to(state_t &state, const vector_t &val)
		{
			::lua_createtable(state, (int)val.size(), (int)val.size());

			std::uint32_t i = 1;
			std::for_each(val.cbegin(), val.cend(), 
				[&state, &i](const T &t)
			{
				convertion_t<T>::to(state, t);
				::lua_rawseti(state, -2, i++);
			});

			return 1;
		}

void pvt_base::check_gas_common (const vector_t &pressure, const vector_t &fvf, const vector_t &visc)
{
    for (t_long i = 1, cnt = (t_long)pressure.size (); i < cnt; ++i)
    {
        if (pressure[i] - pressure[i - 1] < EPS_DIFF)
        {
            throw bs_exception ("", "pressure curve should be monotonically increasing function");
        }
        if (fvf[i] - fvf[i - 1] >= 0)
        {
            BOSOUT (section::pvt, level::critical) << "gas: fvf" << bs_end;
            for (t_long j = 0; j < cnt; ++j)
            {
                BOSOUT (section::pvt, level::critical) << fvf[j] << bs_end;
            }

            throw bs_exception ("", "FVF curve should be monotonically decreasing function");
        }
        if (visc[i] - visc[i - 1] <= 0)
        {
            BOSOUT (section::pvt, level::critical) << "gas: visc" << bs_end;
            for (t_long j = 0; j < cnt; ++j)
            {
                BOSOUT (section::pvt, level::critical) << visc[j] << bs_end;
            }

            throw bs_exception ("", "Viscosity curve should be monotonically increasing function");
        }
    }
}

void
pvt_base::check_oil_common (const vector_t &pressure, const vector_t &fvf, const vector_t &visc)
{
    for (t_long i = 0, cnt = (t_long)pressure.size (); i < cnt; ++i)
    {
        if (pressure[i] < 0)
        {
            // TODO: LOG
            BS_ASSERT (false) (pressure[i]);
            throw bs_exception ("", "pressure should be greater than 0");
        }
        if (fvf[i] < 0)
        {
            // TODO:LOG
            BS_ASSERT (false) (fvf[i]);
            throw bs_exception ("", "fvf should be greater than 0");
        }
        if (visc[i] < 0)
        {
            // TODO: LOG
            BS_ASSERT (false) (visc[i]);
            throw bs_exception ("", "viscosity should be greater than 0");
        }
    }
}

// QR Factorization of a MxN General Matrix A.
//    a       (IN/OUT - matrix(M,N)) On entry, the coefficient matrix A. On exit , the upper triangle and diagonal is the min(M,N) by N upper triangular matrix R.  The lower triangle, together with the tau vector, is the orthogonal matrix Q as a product of min(M,N) elementary reflectors.
//    tau     (OUT - vector (min(M,N))) Vector of the same numerical type as A. The scalar factors of the elementary reflectors.
//    info    (OUT - int)
//   0   : function completed normally
//   < 0 : The ith argument, where i = abs(return value) had an illegal value.
int geqrf (matrix_t& a, vector_t& tau)
{
	int              _m = int(a.size1());
	int              _n = int(a.size2());
	int              _lda = int(a.size1());
	int              _info;

	// make_sure tau's size is greater than or equal to min(m,n)
	if (int(tau.size()) < (_n<_m ? _n : _m) )
		return -104;

	int ldwork = _n*_n;
	vector_t dwork(ldwork);
	rawLAPACK::geqrf (_m, _n, a.data().begin(), _lda, tau.data().begin(), dwork.data().begin(), ldwork, _info);

	return _info;
}

 bool geometry_t::contains_point(const vector_t& v, double buffer)
 {
     double squared_rad;
     vector_t n;
     switch(type)
     {
         case PRX_SPHERE://sphere
             squared_rad = info[0]*info[0];
             if(v.squared_norm() < squared_rad+buffer)
                 return true;
             break;
         case PRX_BOX://box
             if(v[0]>-.5*info[0]-buffer && v[0]<.5*info[0]+buffer &&
                v[1]>-.5*info[1]-buffer && v[1]<.5*info[1]+buffer &&
                v[2]>-.5*info[2]-buffer && v[2]<.5*info[2]+buffer)
             {
                 return true;
             }
             break;
         case PRX_CONE:
             PRX_WARN_S("Cannot check point containment in cone");
             break;
         case PRX_CYLINDER://cylinder
         case PRX_OPEN_CYLINDER:
             n = v;
             n[2] = 0;
             squared_rad = info[0]*info[0]+buffer;
             if(v.squared_norm() < squared_rad &&
                v[2]>-.5*info[1]-buffer && v[2]<.5*info[1]+buffer )
                 return true;
             break;
         case PRX_CAPSULE:
             PRX_WARN_S("Cannot check point containment in capsule");
             break;
         case PRX_MESH:
             // PRX_WARN_S(trimesh.min_x<<" "<<trimesh.max_x<<" "<<trimesh.min_y<<" "<<trimesh.max_y<<" "<<trimesh.min_z<<" "<<trimesh.max_z);
             if(  trimesh.min_x - buffer < v[0] && trimesh.max_x + buffer > v[0] 
                 && trimesh.min_y - buffer < v[1] && trimesh.max_y + buffer > v[1] 
                 && trimesh.min_z - buffer < v[2] && trimesh.max_z + buffer > v[2])
                 return true;
             break;
         default:
             PRX_WARN_S("Cannot check point containment in geometry");
     }
     return false;
 }

 void quaternion_t::set( const vector_t& v)
 {
     PRX_ASSERT(v.get_dim() == 4);
     q[0] = v[0];
     q[1] = v[1];
     q[2] = v[2];
     q[3] = v[3];
 }

 void quaternion_t::get( vector_t& v) const
 {
     PRX_ASSERT(v.get_dim() == 4);
     v[0] = q[0];
     v[1] = q[1];
     v[2] = q[2];
     v[3] = q[3];
 }

  unsigned countNonZeroEntries(vector_t const & v)
  {

    unsigned n = 0;
    for (unsigned i = 0; i < v.size(); i++)
      if ( v(i) )
	n++;
    return n;
  }

  boost::shared_ptr<BaseTRRateMatrix> TRRateMatrixDispatcher(string const & substModel, vector_t const & rateParameters, vector_t const & equiFreq, StateMap const & alphabet)
  {
    boost::shared_ptr<BaseTRRateMatrix> rmPtr;

    if (substModel == "GTR")
      rmPtr = boost::shared_ptr<GTRRateMatrix>( new GTRRateMatrix(alphabet) );
    // define other substitution models here ...
    else
      errorAbort("substitution model '" + substModel + "' not defined. If newly implemented, add to TRRateMatrixDispatcher.");


    if (equiFreq.size() > 0) // not defined, stick with default
      rmPtr->resetEquiFreqs(equiFreq);
    if (rateParameters.size() > 0) // not defined, stick with default
      rmPtr->resetRateParameters(rateParameters);

    return rmPtr;
  }

void build_sah(vector_t<PrimRef>& prims, isa::PrimInfo& pinfo)
{
  size_t N = pinfo.size();

  /* fast allocator that supports thread local operation */
  FastAllocator allocator;

  for (size_t i=0; i<2; i++)
  {
    std::cout << "iteration " << i << ": building BVH over " << N << " primitives, " << std::flush;
    double t0 = getSeconds();
    
    allocator.reset();

    Node* root;
    isa::BVHBuilderBinnedSAH::build<Node*>(
      root,
      /* thread local allocator for fast allocations */
      [&] () -> FastAllocator::ThreadLocal* { 
        return allocator.threadLocal(); 
      },

      /* lambda function that creates BVH nodes */
      [&](const isa::BVHBuilderBinnedSAH::BuildRecord& current, isa::BVHBuilderBinnedSAH::BuildRecord* children, const size_t N, FastAllocator::ThreadLocal* alloc) -> int
      {
        assert(N <= 2);
        InnerNode* node = new (alloc->malloc(sizeof(InnerNode))) InnerNode;
        for (size_t i=0; i<N; i++) {
          node->bounds[i] = children[i].pinfo.geomBounds;
          children[i].parent = (size_t*) &node->children[i];
        }
        *current.parent = (size_t) node;
	return 0;
      },

      /* lambda function that creates BVH leaves */
      [&](const isa::BVHBuilderBinnedSAH::BuildRecord& current, FastAllocator::ThreadLocal* alloc) -> int
      {
        assert(current.prims.size() == 1);
        Node* node = new (alloc->malloc(sizeof(LeafNode))) LeafNode(prims[current.prims.begin()].ID(),prims[current.prims.begin()].bounds());
        *current.parent = (size_t) node;
	return 0;
      },

      /* progress monitor function */
      [&] (size_t dn) { 
        // throw an exception here to cancel the build operation
      },

      prims.data(),pinfo,2,1024,1,1,1,1.0f,1.0f);
    
    double t1 = getSeconds();

    std::cout << 1000.0f*(t1-t0) << "ms, " << 1E-6*double(N)/(t1-t0) << " Mprims/s, sah = " << root->sah() << " [DONE]" << std::endl;
  }
}

UG_API bool
ContainsPoint(const EdgeVertices* e, const vector_t& p, TAAPos aaPos)
{
	number center = (aaPos[e->vertex(0)].x() + aaPos[e->vertex(1)].x()) / 2.;
	number rad = fabs(aaPos[e->vertex(1)].x() - aaPos[e->vertex(0)].x()) / 2.;

	if(fabs(p.x() - center) <= rad)
		return true;
	return false;
}