// NOTE: does not pass tests
SparseMatrix BSplineBasis::evalBasisJacobian(DenseVector &x) const
{
    // Jacobian basis matrix
    SparseMatrix J(getNumBasisFunctions(), numVariables);
    //J.setZero(numBasisFunctions(), numInputs);

    // Calculate partial derivatives
    for (unsigned int i = 0; i < numVariables; ++i)
    {
        // One column in basis jacobian
        std::vector<SparseVector> values(numVariables);

        for (unsigned int j = 0; j < numVariables; ++j)
        {
            if (j == i)
            {
                // Differentiated basis
                values.at(j) = bases.at(j).evalDerivative(x(j), 1);
            }
            else
            {
                // Normal basis
                values.at(j) = bases.at(j).eval(x(j));
            }
        }

        SparseVector Ji = kroneckerProductVectors(values);

        // Fill out column
        for (int k = 0; k < Ji.outerSize(); ++k)
        for (SparseMatrix::InnerIterator it(Ji,k); it; ++it)
        {
            if (it.value() != 0)
                J.insert(it.row(),i) = it.value();
        }
        //J.block(0,i,Ji.rows(),1) = bi.block(0,0,Ji.rows(),1);
    }

    J.makeCompressed();

    return J;
}

void TestGradient::add02() {
	Variable x,y;
	Function f(x,y,ExprVector::new_(sqr(x)-y,x*y,false));

	IntervalVector box(2);
	box[0]=Interval(1,2);
	box[1]=Interval(3,4);

	IntervalMatrix J(2,2);
	f.jacobian(box,J);
	check(J[0][0],Interval(2,4));
	check(J[0][1],Interval(-1,-1));
	check(J[1][0],Interval(3,4));
	check(J[1][1],Interval(1,2));
	CPPUNIT_ASSERT(J[0][0].is_superset(Interval(2,4)));
	CPPUNIT_ASSERT(J[0][1].is_superset(Interval(-1,-1)));
	CPPUNIT_ASSERT(J[1][0].is_superset(Interval(3,4)));
	CPPUNIT_ASSERT(J[1][1].is_superset(Interval(1,2)));

}

void PCM::setExtraForces(IonicGradient* forces, const ScalarFieldTilde& A_nCavityTilde) const
{   if(forces)
    {   forces->init(e.iInfo);
        //VDW contribution:
        switch(fsp.pcmVariant)
        {
        case PCM_SaLSA:
        case PCM_CANDLE:
        case PCM_SGA13:
        {   const auto& solvent = fsp.solvents[0];
            const ScalarFieldTilde sTilde = J(fsp.pcmVariant==PCM_SaLSA ? shape : shapeVdw);
            ScalarFieldTildeArray Ntilde(Sf.size());
            for(unsigned i=0; i<Sf.size(); i++)
                Ntilde[i] = solvent->Nbulk * (Sf[i] * sTilde);
            const double vdwScaleEff = (fsp.pcmVariant==PCM_CANDLE) ? fsp.sqrtC6eff : fsp.vdwScale;
            e.vanDerWaals->energyAndGrad(atpos, Ntilde, atomicNumbers, vdwScaleEff, 0, forces);
            break;
        }
        default:
            break; //no VDW forces
        }
        //Full core contribution:
        switch(fsp.pcmVariant)
        {
        case PCM_SaLSA:
        case PCM_CANDLE:
        {   VectorFieldTilde gradAtpos;
            nullToZero(gradAtpos, gInfo);
            for(unsigned iSp=0; iSp<atpos.size(); iSp++)
                for(unsigned iAtom=0; iAtom<atpos[iSp].size(); iAtom++)
                {   callPref(gradSGtoAtpos)(gInfo.S, atpos[iSp][iAtom], A_nCavityTilde->dataPref(), gradAtpos.dataPref());
                    for(int k=0; k<3; k++)
                        (*forces)[iSp][iAtom][k] -= e.iInfo.species[iSp]->ZfullCore * sum(gradAtpos[k]); //negative gradient on ith atom type
                }
            break;
        }
        default:
            break; //no full-core forces
        }
    }
}

int contactConstraints(RigidBodyManipulator *r, int nc, std::vector<SupportStateElement,Eigen::aligned_allocator<SupportStateElement>>& supp, void *map_ptr, MatrixXd &n, MatrixXd &D, MatrixXd &Jp, MatrixXd &Jpdot,double terrain_height)
{
  int j, k=0, nq = r->num_positions;

  n.resize(nc,nq);
  D.resize(nq,nc*2*m_surface_tangents);
  Jp.resize(3*nc,nq);
  Jpdot.resize(3*nc,nq);
  
  Vector3d contact_pos,pos,normal;
  MatrixXd J(3,nq);
  Matrix<double,3,m_surface_tangents> d;
  
  for (std::vector<SupportStateElement,Eigen::aligned_allocator<SupportStateElement>>::iterator iter = supp.begin(); iter!=supp.end(); iter++) {
    if (nc>0) {
      for (std::vector<Vector4d,aligned_allocator<Vector4d>>::iterator pt_iter=iter->contact_pts.begin(); pt_iter!=iter->contact_pts.end(); pt_iter++) {
        r->forwardKin(iter->body_idx,*pt_iter,0,contact_pos);
        r->forwardJac(iter->body_idx,*pt_iter,0,J);

        collisionDetect(map_ptr,contact_pos,pos,&normal,terrain_height);
        surfaceTangents(normal,d);

        n.row(k) = normal.transpose()*J;
        for (j=0; j<m_surface_tangents; j++) {
          D.col(2*k*m_surface_tangents+j) = J.transpose()*d.col(j);
          D.col((2*k+1)*m_surface_tangents+j) = -D.col(2*k*m_surface_tangents+j);
        }

        // store away kin sols into Jp and Jpdot
        // NOTE: I'm cheating and using a slightly different ordering of J and Jdot here
        Jp.block(3*k,0,3,nq) = J;
        r->forwardJacDot(iter->body_idx,*pt_iter,0,J);
        Jpdot.block(3*k,0,3,nq) = J;
        
        k++;
      }
    }
  }
  
  return k;
}

void BlockMatrixTest::testZero()
{
  std::cout << "--> Test: zero." <<std::endl;
  SP::SiconosMatrix A(new SimpleMatrix(2, 2));
  A->eye();
  SP::SiconosMatrix H(new SimpleMatrix(2, 4));
  H->eye();
  SP::SiconosMatrix I(new SimpleMatrix(5, 2));
  I->eye();
  SP::SiconosMatrix J(new SimpleMatrix(5, 4));
  J->eye();

  std::vector<SP::SiconosMatrix> v(4);
  v[0] = A ;
  v[1] = H ;
  v[2] = I ;
  v[3] = J ;
  SP::SiconosMatrix test(new BlockMatrix(v, 2, 2));
  test->zero();
  unsigned int n1 = test->size(0);
  unsigned int n2 = test->size(1);
  for (unsigned int i = 0; i < n1; ++i)
    for (unsigned int j = 0; j < n2; ++j)
      CPPUNIT_ASSERT_EQUAL_MESSAGE("testZero : ", (*test)(i, j) == 0, true);
  for (unsigned int i = 0; i < 2; ++i)
  {
    for (unsigned int j = 0; j < 2; ++j)
      CPPUNIT_ASSERT_EQUAL_MESSAGE("testZero : ", (*A)(i, j) == 0, true);
    for (unsigned int j = 0; j < 4; ++j)
      CPPUNIT_ASSERT_EQUAL_MESSAGE("testZero : ", (*H)(i, j) == 0, true);
  }
  for (unsigned int i = 0; i < 5; ++i)
  {
    for (unsigned int j = 0; j < 2; ++j)
      CPPUNIT_ASSERT_EQUAL_MESSAGE("testZero : ", (*I)(i, j) == 0, true);
    for (unsigned int j = 0; j < 4; ++j)
      CPPUNIT_ASSERT_EQUAL_MESSAGE("testZero : ", (*J)(i, j) == 0, true);
  }

  std::cout << "--> zero test ended with success." <<std::endl;
}

void EquivalentInclusionConductivity<EvalT, Traits>::
evaluateFields(typename Traits::EvalData workset)
{
  int numCells = workset.numCells;

  if (is_constant) {
    for (int cell=0; cell < numCells; ++cell) {
      for (int qp=0; qp < numQPs; ++qp) {
    	  effectiveK(cell,qp) = constant_value;
      }
    }
  }
#ifdef ALBANY_STOKHOS
  else {
    for (int cell=0; cell < numCells; ++cell) {
      for (int qp=0; qp < numQPs; ++qp) {
	Teuchos::Array<MeshScalarT> point(numDims);
	for (int i=0; i<numDims; i++)
	  point[i] = Sacado::ScalarValue<MeshScalarT>::eval(coordVec(cell,qp,i));
	effectiveK(cell,qp) = exp_rf_kl->evaluate(point, rv);
      }
    }
  }
#endif


  if (isPoroElastic) {
      for (int cell=0; cell < numCells; ++cell) {
        for (int qp=0; qp < numQPs; ++qp) {
        	effectiveK(cell,qp) = porosity(cell,qp)*condKf +
        			              (J(cell,qp)-porosity(cell,qp))*condKs;
       // 	effectiveK(cell,qp) = condKf
       //  	                + ( J(cell,qp) - porosity(cell,qp))*
        //			              (condKs - condKf)*condKf/
        //			              ((condKs - condKf)*porosity(cell,qp)
        //			            	+ J(cell,qp)*condKf);
        }
      }
    }

}

/* Does the string at (i, j) have any more liberty than the one at
 * (libi, libj)?
 */
static int
has_additional_liberty(int i, int j, int libi, int libj)
{
  int pos = POS(i, j);
  do {
    int ai = I(pos);
    int aj = J(pos);
    int k;
    for (k = 0; k < 4; k++) {
      int bi = ai + deltai[k];
      int bj = aj + deltaj[k];
      if (on_board(bi, bj) && get_board(bi, bj) == EMPTY
	  && (bi != libi || bj != libj))
	return 1;
    }

    pos = next_stone[pos];
  } while (pos != POS(i, j));

  return 0;
}

int IKCom::calc_jacobian()
{
	if(n_const == 0) return 0;
	int n_dof = ik->NumDOF();
	fMat Jtemp;
	J.resize(n_const, n_dof);
	Jtemp.resize(3, n_dof);
	Jtemp.zero();
	ik->ComJacobian(Jtemp, cur_com, charname);
	int i, j, count = 0;
	for(i=0; i<3; i++)
	{
		if(const_index[i] == IK::HAVE_CONSTRAINT)
		{
			for(j=0; j<n_dof; j++)
				J(count, j) = Jtemp(i, j);
			count++;
		}
	}
	return 0;
}

void Function::hansen_matrix(const IntervalVector& box, IntervalMatrix& H, const VarSet& set) const {
	int n=set.nb_var;
	int m=image_dim();

	assert(H.nb_cols()==n);
	assert(box.size()==nb_var());
	assert(H.nb_rows()==m);

	IntervalVector var_box=set.var_box(box);
	IntervalVector param_box=set.param_box(box);

	IntervalVector x=var_box.mid();
	IntervalMatrix J(m,n);

	for (int var=0; var<n; var++) {
		//var=tab[i];
		x[var]=var_box[var];
		jacobian(set.full_box(x,param_box),J,set);
		H.set_col(var,J.col(var));
	}
}

int main()
{
Array<float,2> test(8,8), test2(5,5) ;

test = 5;

Range I(2,6) ;
Range J(3,7) ;

// Koenig lookup hack
#if defined(__GNUC__) && (__GNUC__ < 3)
test2 = where(blitz::operator> (test(I,J), test(I-1,J)), 0, test(I,J));
#else
test2 = where(test(I,J) > test(I-1,J), 0, test(I,J));
#endif

BZTEST(test2(3,3) == 5);

cout << test2 << endl ;

}

void DfFockMatrix::setCoulomb(const METHOD_TYPE nMethodType,
                              TlDenseSymmetricMatrix_Lapack& F) {
  TlDenseSymmetricMatrix_Lapack J(this->m_nNumOfAOs);
  if (this->J_engine_ == J_ENGINE_RI_J) {
    this->setCoulomb<TlDenseSymmetricMatrix_Lapack, TlDenseVector_Lapack,
                     DfEriX>(nMethodType, J);
    // if (this->isUseNewEngine_ == true) {
    //     this->logger(" use new engine\n");
    //     this->setCoulomb<TlDenseSymmetricMatrix_Lapack, TlDenseVector_Lapack,
    //     DfEriX>(nMethodType,
    //     J);
    // } else {
    //     this->setCoulomb<TlDenseSymmetricMatrix_Lapack, TlDenseVector_Lapack,
    //     DfEri>(nMethodType, J);
    // }
    F += J;

    // update method
    if (this->m_nIteration > 1) {
      const TlDenseSymmetricMatrix_Lapack prevJ =
          DfObject::getJMatrix<TlDenseSymmetricMatrix_Lapack>(
              this->m_nIteration - 1);
      J += prevJ;
    }

    this->saveJMatrix(this->m_nIteration, J);
  } else {
    J = this->getJMatrix<TlDenseSymmetricMatrix_Lapack>(this->m_nIteration);

    // update method
    if (this->m_nIteration > 1) {
      const TlDenseSymmetricMatrix_Lapack prevJ =
          DfObject::getJMatrix<TlDenseSymmetricMatrix_Lapack>(
              this->m_nIteration - 1);
      J -= prevJ;
    }

    F += J;
  }
}

    std::unique_ptr<flattened_tensor> M::compute(const index_literal_list& indices)
      {
        if(indices.size() != M_TENSOR_INDICES) throw tensor_exception("M indices");

        auto result = std::make_unique<flattened_tensor>(this->fl.get_flattened_size<field_index>(M_TENSOR_INDICES));
    
        const field_index max_i = this->shared.get_max_field_index(indices[0]->get_variance());
        const field_index max_j = this->shared.get_max_field_index(indices[1]->get_variance());

        // set up a TensorJanitor to manage use of cache
        TensorJanitor J(*this, indices);

        for(field_index i = field_index(0, indices[0]->get_variance()); i < max_i; ++i)
          {
            for(field_index j = field_index(0, indices[1]->get_variance()); j < max_j; ++j)
              {
                (*result)[this->fl.flatten(i, j)] = this->compute_component(i, j);
              }
          }

        return(result);
      }

/**
 * General implementation of the method for all peaks. Calculates derivatives
 * only
 * for a range of x values limited to a certain number of FWHM around the peak
 * centre.
 * For the points outside the range all derivatives are set to 0.
 * Calls functionDerivLocal() to compute the actual values
 * @param out :: Derivatives
 * @param xValues :: X values for data points
 * @param nData :: Number of data points
 */
void IPeakFunction::functionDeriv1D(Jacobian *out, const double *xValues,
                                    const size_t nData) {
  double c = this->centre();
  double dx = fabs(s_peakRadius * this->fwhm());
  int i0 = -1;
  int n = 0;
  for (size_t i = 0; i < nData; ++i) {
    if (fabs(xValues[i] - c) < dx) {
      if (i0 < 0)
        i0 = static_cast<int>(i);
      ++n;
    } else {
      for (size_t ip = 0; ip < this->nParams(); ++ip) {
        out->set(i, ip, 0.0);
      }
    }
  }
  if (i0 < 0 || n == 0)
    return;
  PartialJacobian1 J(out, i0);
  this->functionDerivLocal(&J, xValues + i0, n);
}

/* Based on the entries in the reading cache and their nodes field,
 * compute where the relatively most expensive tactical reading is
 * going on.
 */
void
reading_hotspots(float values[BOARDMAX])
{
  int m, n, k;
  int sum_nodes = 0;

  for (m = 0; m < board_size; m++)
    for (n = 0; n < board_size; n++)
      values[POS(m, n)] = 0.0;
  
  /* Compute the total number of nodes for the cached entries. */
  for (k = 0; k < persistent_reading_cache_size; k++)
    sum_nodes += persistent_reading_cache[k].nodes;

  if (sum_nodes <= 100)
    return;

  /* Loop over all entries and increase the value of vertices adjacent
   * to dragons involving expensive tactical reading.
   */
  for (k = 0; k < persistent_reading_cache_size; k++) {
    struct reading_cache *entry = &(persistent_reading_cache[k]);
    float contribution = entry->nodes / (float) sum_nodes;
    if (0) {
      gprintf("Reading hotspots: %d %1m %f\n", entry->routine, entry->str,
	      contribution);
    }
    switch (entry->routine) {
    case ATTACK:
    case FIND_DEFENSE:
      mark_string_hotspot_values(values, I(entry->str), J(entry->str),
				 contribution);
      break;
    default:
      gg_assert(0); /* Shouldn't happen. */
      break;
    }
  }
}

/** \brief Calculate the Jacobian using numerical differentiation by column
 */
std::vector<std::vector<double> > FuncWrapperND::Jacobian(const std::vector<double> &x)
{
    double epsilon;
    std::size_t N = x.size();
    std::vector<double> r, xp;
    std::vector<std::vector<double> > J(N, std::vector<double>(N, 0));
    std::vector<double> r0 = call(x);
    // Build the Jacobian by column
    for (std::size_t i = 0; i < N; ++i)
    {
        xp = x;
        epsilon = 0.001*x[i];
        xp[i] += epsilon;
        r = call(xp);
        
        for(std::size_t j = 0; j < N; ++j)
        {
            J[j][i] = (r[j]-r0[j])/epsilon;
        }
    }
    return J;
}

/* Based on the entries in the reading cache and their nodes field,
 * compute where the relatively most expensive tactical reading is
 * going on.
 */
void
reading_hotspots(float values[BOARDMAX])
{
  int pos;
  int k;
  int sum_nodes = 0;

  for (pos = BOARDMIN; pos < BOARDMAX; pos++)
    values[pos] = 0.0;
  
  /* Compute the total number of nodes for the cached entries. */
  for (k = 0; k < reading_cache.current_size; k++)
    sum_nodes += reading_cache.table[k].cost;

  if (sum_nodes <= 100)
    return;

  /* Loop over all entries and increase the value of vertices adjacent
   * to dragons involving expensive tactical reading.
   */
  for (k = 0; k < reading_cache.current_size; k++) {
    struct persistent_cache_entry *entry = &(reading_cache.table[k]);
    float contribution = entry->cost / (float) sum_nodes;
    if (0) {
      gprintf("Reading hotspots: %d %1m %f\n", entry->routine, entry->apos,
	      contribution);
    }
    switch (entry->routine) {
    case ATTACK:
    case FIND_DEFENSE:
      mark_string_hotspot_values(values, I(entry->apos), J(entry->apos),
				 contribution);
      break;
    default:
      gg_assert(0); /* Shouldn't happen. */
      break;
    }
  }
}

void SaLSA::set_internal(const ScalarFieldTilde& rhoExplicitTilde, const ScalarFieldTilde& nCavityTilde)
{
	this->rhoExplicitTilde = rhoExplicitTilde; zeroNyquist(this->rhoExplicitTilde);
	
	//Compute cavity shape function (0 to 1)
	nCavity = I(nFluid * (nCavityTilde + getFullCore()));
	updateCavity();

	//Compute site shape functions with the spherical ansatz:
	const auto& solvent = fsp.solvents[0];
	for(unsigned iSite=0; iSite<solvent->molecule.sites.size(); iSite++)
		siteShape[iSite] = I(Sf[iSite] * J(shape));
	
	logPrintf("\tSaLSA fluid occupying %lf of unit cell:", integral(shape)/gInfo.detR);
	logFlush();

	//Update the inhomogeneity factor of the preconditioner
	epsInv = inv(1. + (epsBulk-1.)*shape);
	
	//Initialize the state if it hasn't been loaded:
	if(!state) nullToZero(state, gInfo);
}

//assumes missing component is the third component - could generalise at some
//point
double ThreePointPlane::getMissingComponent(ThreeVector& vec)
{
// call J_ our new origin
  if(isCollinear_ij()) return getMissingComponentCollinear(vec);
  ThreeVector J( J_.i_-I_.i_,J_.j_-I_.j_,J_.k_-I_.k_);
  ThreeVector K( K_.i_-I_.i_,K_.j_-I_.j_,K_.k_-I_.k_);
  ThreeVector vecP(vec.i_-I_.i_,vec.j_-I_.j_,vec.k_-I_.k_);


  // create new basis in which missing component is 0
  ThreeVector A = J*K;
  ThreeVector B = J*A;

  // need unit vectors
  B.normalise(); J.normalise(); A.normalise();

  double num = J.k_*(J.i_*vecP.i_ + J.j_*vecP.j_) +
               B.k_*(vecP.i_*B.i_ + vecP.j_*B.j_);
  double denom = 1. - J.k_*J.k_ - B.k_*B.k_;

  return ((num/denom) + I_.k_);
}

void DP(int X0, int X1, int prd)
{
	//TODO:	Given the current period prd, the state variables X0 and X1,
	//	this function iterates the value function for one step.
	//VAR:	Y0U		Upper Bound of Y0 = min(X1, X0+K_0)
	//	Y1U		Upper Bound of Y1 = X1+K_1

	int  Y0U, Y1U, Y[2];
	double tmpJ;
	Y0U = MIN(X1, X0+K[0]);
	Y1U = X1 + K[1];
	for (Y[0] = X0; Y[0] <= Y0U; Y[0] ++) {
		for (Y[1] = X1; Y[1] <= Y1U; Y[1] ++) {
			tmpJ = J(Y, prd);
			if (Retrieve(V, prd, X0, X1) > tmpJ) {
				Retrieve(V, prd, X0, X1) = tmpJ;
				Retrieve(Plc, prd, X0, X1)[0] = Y[0];
				Retrieve(Plc, prd, X0, X1)[1] = Y[1];
			}
		}
	}
}

void ScalarL2ProjectionResidual<EvalT, Traits>::
evaluateFields(typename Traits::EvalData workset)
{

  ScalarT J(1);

  for (int cell=0; cell < workset.numCells; ++cell)
  {
	  for (int qp=0; qp < numQPs; ++qp)
	  {
		  Intrepid::Tensor<ScalarT> F(numDims, DefGrad,cell, qp,0,0);
		  J = Intrepid::det(F);
		  tauH(cell,qp) = 0.0;
		  for (int i=0; i<numDims; i++){
			  tauH(cell,qp) += J*Pstress(cell, qp, i,i)/numDims;
		  }
	  }
  }

  for (int cell=0; cell < workset.numCells; ++cell)
  {
	  for (int node=0; node < numNodes; ++node)
	  {
		  TResidual(cell,node)=0.0;
	  }
  }

  for (int cell=0; cell < workset.numCells; ++cell) {
	  for (int node=0; node < numNodes; ++node) {
		  for (int qp=0; qp < numQPs; ++qp) {
				  	  TResidual(cell,node) += ( projectedStress(cell,qp)-
	                		          tauH(cell, qp))*wBF(cell,node,qp);
		  }
	  }
  }


}