// This chunk of code adds
// z_t = \sum_{k=1}^{2 |V_g|} \lambda_k^g, \Prod_{j \in J_t} \chi_j^{g,k}
//   \forall J_t \subseteq V_g
void
MultilinearTermsHandler::handleZDefConstraints_(RelaxationPtr relaxation, HandleCallingFunction wherefrom, ModVector &mods)
{
  for(ConstTermIterator it = termsR_.begin(); it != termsR_.end(); ++it) {
    ConstVariablePtr zt = it->first;
    SetOfVars const &jt = it->second;

    for (UInt gix = 0; gix < groups_.size(); ++gix) {
      SetOfVars &vg = groups_[gix];
       
      if (std::includes(vg.begin(), vg.end(), jt.begin(), jt.end())) { 
        // jt is a subset of vg, add constraint
        LinearFunctionPtr lf = (LinearFunctionPtr) new LinearFunction();
        lf->addTerm(zt, -1.0);
        int pix = 0;

        for (std::set<SetOfVars>::iterator it2 = points_[gix].begin(); 
             it2 != points_[gix].end(); ++it2) {
          double prodval = 1.0;
          VariablePtr lam = lambdavars_[gix][pix];

          for(SetOfVars::const_iterator jt_it = jt.begin(); jt_it != jt.end(); ++jt_it) {
            ConstVariablePtr xvar = *jt_it;
            double tmp = varIsAtLowerBoundAtPoint_(xvar, *it2) ? xvar->getLb() : xvar->getUb();
            prodval *= tmp;
          }

          lf->addTerm(lam, prodval);
          ++pix;
        }        
        FunctionPtr f = (FunctionPtr) new Function(lf);

        if (wherefrom == relaxInit_Call) {
          ConstraintPtr c = relaxation->newConstraint(f, 0.0, 0.0);
          zConMap_.insert(std::make_pair(IntVarPtrPair(gix, zt), c));
        }
        else { 
          IntVarPtrPairConstraintMap::iterator pos;
          pos = zConMap_.find(IntVarPtrPair(gix, zt));
          if (pos == zConMap_.end()) {
            assert(0);
          }
          ConstraintPtr c = pos->second;
          //XXX Here you should just check if the constraint really was going to
          //change and do nothing if it doesn't...  (will be faster).
          if (wherefrom == relaxNodeInc_Call) {
            relaxation->changeConstraint(c, lf, 0.0, 0.0);
          }
          LinConModPtr lcmod = (LinConModPtr) new LinConMod(c, lf, 0.0, 0.0); 
          mods.push_back(lcmod);
        }
      }
      
    }
  } 


}
void
MultilinearTermsHandler::handleXDefConstraints_(RelaxationPtr relaxation, HandleCallingFunction wherefrom, ModVector &mods)
{
  for (UInt gix = 0; gix < groups_.size(); ++gix) {
    for(SetOfVars::const_iterator it = groups_[gix].begin(); it != groups_[gix].end(); ++it) {
      ConstVariablePtr xvar = *it;
      LinearFunctionPtr lf = (LinearFunctionPtr) new LinearFunction();
      lf->addTerm(xvar, -1.0);
      
      int pix = 0;
      for (std::set<SetOfVars>::iterator it2 = points_[gix].begin(); it2 != points_[gix].end(); ++it2) {
        VariablePtr lam = lambdavars_[gix][pix];
        double val = varIsAtLowerBoundAtPoint_(xvar, *it2) ? xvar->getLb() : xvar->getUb();
        lf->addTerm(lam, val);
#if defined(DEBUG_MULTILINEARTERMS_HANDLER)
        std::cout << xvar->getName() << ", lam: " << gix << "," << pix << " value is: " 
                  << val << std::endl;
#endif        
        ++pix;
      }      
      FunctionPtr f = (FunctionPtr) new Function(lf);

      if (wherefrom == relaxInit_Call) {
        ConstraintPtr c = relaxation->newConstraint(f, 0.0, 0.0);
        xConMap_.insert(std::make_pair(IntVarPtrPair(gix, xvar), c));
      }
      else { 
        IntVarPtrPairConstraintMap::iterator pos;
        pos = xConMap_.find(IntVarPtrPair(gix, xvar));
        if (pos == xConMap_.end()) {
          assert(0);
        }
        ConstraintPtr c = pos->second;
        //XXX Here you should just check if the constraint really was going to
        //change and do nothing if it doesn't...  (will be faster).
        if (wherefrom == relaxNodeInc_Call) {
          relaxation->changeConstraint(c, lf, 0.0, 0.0);
        }
        else {
          assert(0);
        }
        LinConModPtr lcmod = (LinConModPtr) new LinConMod(c, lf, 0.0, 0.0); 
        mods.push_back(lcmod);
      }      
    }
  }  
  
}
void CxUnivarConstraintData::addSecant(RelaxationPtr rel,
                                       ConstVariablePtr riv,
                                       ConstVariablePtr rov,
                                       FunctionPtr fn, DoubleVector& tmpX,
                                       bool init, ModVector &mods) 
{

  int error;
  double xlb, xub, fxlb, fxub, m, intercept;
  LinearFunctionPtr lf; 
  FunctionPtr f;

  // First add the secant inequalities based on variable bounds
  xlb = riv->getLb();
  xub = riv->getUb();
	
#if defined(DEBUG_CXUNIVARHANDLER)
  std::cout << "Adding secant on variable rix index: " << riv->getIndex() 
	    << " rov index: " << rov->getIndex()
	    << " xlb: " << xlb << " xub: " << xub << std::endl;
#endif 
  // no secant if unbounded either way
  if (xlb <= -0.9*INFINITY || xub >= 0.9*INFINITY) {
    std::cout << "Cannot add secant -- bound is infinite" << std::endl;
    return;
  }

  // TODO: Check the error value!
  tmpX[riv->getIndex()] = xlb;
  fxlb =  fn->eval(tmpX, &error);
  tmpX[riv->getIndex()] = xub;
  fxub =  fn->eval(tmpX, &error);
  tmpX[riv->getIndex()] = 0.0;

  // TODO: check/remedy numerical issues in this division
  if (xub - xlb > 10e-7) {
    m = (fxub - fxlb)/(xub - xlb);
  }
  else {
    m = 0.0;
  }

  intercept = fxlb - m*xlb;
  lf = (LinearFunctionPtr) new LinearFunction();
  lf->addTerm(rov, 1.0);
  lf->addTerm(riv, -m);

  // rovar <= m*rivar + intercept 
  if (init) {
     f = (FunctionPtr) new Function(lf);
     secCon_ = rel->newConstraint(f, -INFINITY, intercept);
  }
  else {
    rel->changeConstraint(secCon_, lf, -INFINITY, intercept);
    LinConModPtr lcmod = (LinConModPtr) new LinConMod(secCon_, lf, -INFINITY,
                                                      intercept);
    mods.push_back(lcmod);
  }

  
}
void CxUnivarConstraintData::addLin(RelaxationPtr rel, ConstVariablePtr riv,
				    ConstVariablePtr rov, FunctionPtr fn,
                                    DoubleVector& tmpX, DoubleVector& grad,
                                    bool init, ModVector &mods)
{
 
  int error;
  ConstraintPtr cons; 
  double xlb = riv->getLb();
  double xub = riv->getUb();
  double fxlbval=0, fxubval=0, dfxlbval=0, dfxubval=0;
  double tmpxval, fxval, dfxval; 
  LinearFunctionPtr lf; 
  FunctionPtr f;

  // More sophisticated strategies hopefully could be obtained by simply
  // changing this array 
  int npts = 3;
  double xvals[] = {xlb, xub, (xub-xlb)/2.0};

#if defined(DEBUG_CXUNIVARHANDLER)
  std::cout << "Adding linearizations.  rix id: " << riv->getId() 
	    << " rix index: " << riv->getIndex() << " rov id: " << rov->getId() 
	    << " rov index: " << rov->getIndex()
	    << " xlb: " << xlb << " xub: " << xub << std::endl;
#endif
  
  for (int i = 0; i < npts; i++) {

    // Zero out tmpX and grad each time, or else bad things happen
    for (UInt j = 0; j < tmpX.size(); ++j) {
      tmpX[j] = 0.0;
      grad[j] = 0.0;
    }
    
    if (i == 2) {
      // Third linearization point taken to be where first two intersect:
      // x3 = (f'(xub)*xub - f'(xlb)*xlb + f(xlb) - f(xub))/(f'(xub) - f'(xlb))
      // Unless this would put it too close to one of the end points
      if (dfxubval - dfxlbval > 0.0001 || dfxubval - dfxlbval < -0.0001) {
        tmpxval = (dfxubval*xub - dfxlbval*xlb + fxlbval - fxubval)/
                  (dfxubval - dfxlbval);
        if (tmpxval < xlb + (xub-xlb)*0.05) {
          xvals[2] = xlb + (xub-xlb)*0.05;
        }
        else if (tmpxval > xub - (xub-xlb)*0.05) {
          xvals[2] = xub - (xub-xlb)*0.05;
        }
        else {
          xvals[2] = tmpxval;
        }
      }
    }
    tmpX[riv->getIndex()] = xvals[i];
    error = 0;
    fxval =  fn->eval(tmpX, &error);
    fn->evalGradient(&tmpX[0], &grad[0], &error);
#if defined(DEBUG_CXUNIVARHANDLER2)
    for (UInt j = 0; j < tmpX.size(); ++j) {
      std::cout << "x[" << j << "] = " << tmpX[j] << " dfdx[" << j << "] = "
                << grad[j] << std::endl;
    }
#endif
    dfxval = grad[riv->getIndex()];
    if (i == 0) {
       fxlbval = fxval;
       dfxlbval = dfxval; 
    }
    else if (i == 1) {
       fxubval = fxval;
       dfxubval = dfxval; 
    }
    // linearization:  rov >= f(xval) + f'(xval)(riv - xval) 
    //                 rov - f'(xval)*riv >= f(xval) - f'(xval)*xval
    lf = (LinearFunctionPtr) new LinearFunction();
    lf->addTerm(rov, 1.0);
    lf->addTerm(riv, -dfxval);
    if (init) {
        f = (FunctionPtr) new Function(lf);
    	cons = rel->newConstraint(f, fxval - dfxval*xvals[i], INFINITY);
	linCons_.push_back(cons);
    }
    else {
#if defined(DEBUG_CXUNIVARHANDLER)
       std::cout << "Will change 'linearization  ' constraint to have "
                 << "linear function: ";
       lf->write(std::cout);  
       std::cout << std::endl;
#endif

       rel->changeConstraint(linCons_[i], lf, fxval - dfxval*xvals[i], INFINITY); 
       LinConModPtr lcmod = (LinConModPtr) new LinConMod(linCons_[i], lf, 
                                                         fxval -
                                                         dfxval*xvals[i],
                                                         INFINITY); 
       mods.push_back(lcmod);
    }
  }
  tmpX[riv->getIndex()] = 0.0;
  grad[riv->getIndex()] = 0.0;

}