text: /* Siconos is a program dedicated to modeling, simulation and control
   * of non smooth dynamical systems.
   *
   * Copyright 2021 INRIA.
   *
   * Licensed under the Apache License, Version 2.0 (the "License");
   * you may not use this file except in compliance with the License.
   * You may obtain a copy of the License at
   *
   * http://www.apache.org/licenses/LICENSE-2.0
   *
   * Unless required by applicable law or agreed to in writing, software
   * distributed under the License is distributed on an "AS IS" BASIS,
   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   * See the License for the specific language governing permissions and
   * limitations under the License.
  */
  #include "Relay.hpp"
  #include <iostream>
  #include <assert.h>
  #include "Tools.hpp"
  #include "Simulation.hpp"
  #include "RelayNSL.hpp"
  #include "OSNSMatrix.hpp"
  
  // --- Numerics headers ---
  #include "NonSmoothDrivers.h"
  #include <Relay_Solvers.h>
  
  #include <limits>
  
  using namespace RELATION;
  
  
  Relay::Relay(int numericsSolverId):
    Relay(SP::SolverOptions(solver_options_create(numericsSolverId),
                            solver_options_delete))
  {}
  
  Relay::Relay(SP::SolverOptions options):
    LinearOSNS(options), _numerics_problem(new RelayProblem)
  {}
  
  
  /* nslaw dispatch on bounds */
  
  struct Relay::_BoundsNSLEffect : public SiconosVisitor
  {
  
    using SiconosVisitor::visit;
  
    Relay* _parent;
    SP::Interaction _inter;
    unsigned int _pos;
  
  
    _BoundsNSLEffect(Relay *p, SP::Interaction inter, unsigned int pos) :
      _parent(p), _inter(inter), _pos(pos) {};
  
    void visit(const RelayNSL& nslaw)
    {
  
      for(unsigned i = 0; i <  _inter->nonSmoothLaw()->size(); ++i)
      {
        (*(_parent->lb()))(_pos + i) = nslaw.lb();
        (*(_parent->ub()))(_pos + i) = nslaw.ub();
      }
    }
  
    void visit(const ComplementarityConditionNSL& nslaw)
    {
      for(unsigned i = 0; i <  _inter->nonSmoothLaw()->size(); ++i)
      {
        (*(_parent->lb()))(_pos + i) = 0.0;
        (*(_parent->ub()))(_pos + i) = std::numeric_limits<double>::infinity();
      }
    }
  
  };
  
  
  void Relay::initialize(SP::Simulation sim)
  {
    LinearOSNS::initialize(sim);
    //cout << "Relay::initialize" <<std::endl;
  
  
    // initialize memory for _lb and _ub
    if(! _lb)
      _lb.reset(new SiconosVector(maxSize()));
    else
    {
      if(_lb->size() != maxSize())
        _lb->resize(maxSize());
    }
    if(! _ub)
      _ub.reset(new SiconosVector(maxSize()));
    else
    {
      if(_ub->size() != maxSize())
        _ub->resize(maxSize());
    }
  }
  bool Relay::checkCompatibleNSLaw(NonSmoothLaw& nslaw)
  {
    float type_number= (float) (Type::value(nslaw));
    _nslawtype.insert(type_number);
  
    if (not (Type::value(nslaw) == Type::ComplementarityConditionNSL ||
             Type::value(nslaw) == Type::RelayNSL))
    {
      THROW_EXCEPTION("\nRelay::checkCompatibleNSLaw -  \n\
                        The chosen nonsmooth law is not compatible with Relay one step nonsmooth problem. \n \
                        Compatible NonSmoothLaw are: ComplementarityConditionNSL or RelayNSL\n");
      return false;
    }
  
    return true;
  }
  
  
  int Relay::compute(double time)
  {
    int info = 0;
    // --- Prepare data for Relay computing ---
    bool cont = preCompute(time);
    if(!cont)
      return info;
  
    // fill _lb and _ub wiht the value of the NonSmooth Law
  
    InteractionsGraph& indexSet = *simulation()->indexSet(indexSetLevel());
  
    //cout << " _sizeOutput =" <<_sizeOutput <<std::endl;
    if(_lb->size() != _sizeOutput)
    {
      _lb->resize(_sizeOutput, false);
      _lb->zero();
    }
    if(_ub->size() != _sizeOutput)
    {
      _ub->resize(_sizeOutput, false);
      _ub->zero();
    }
  
    InteractionsGraph::VIterator ui, uiend;
    for(std::tie(ui, uiend) = indexSet.vertices(); ui != uiend; ++ui)
    {
      SP::Interaction inter = indexSet.bundle(*ui);
  
      // Compute q, this depends on the type of non smooth problem, on
      // the relation type and on the non smooth law
      unsigned int pos = indexSet.properties(*ui).absolute_position;
      SP::SiconosVisitor NSLEffect(new _BoundsNSLEffect(this, inter, pos));
      inter->nonSmoothLaw()->accept(*NSLEffect);
    }
  
    // --- Call Numerics driver ---
    // Inputs:
    // - the problem (M,q ...)
    // - the unknowns (z,w)
    // - the options for the solver (name, max iteration number ...)
    // - the global options for Numerics (verbose mode ...)
  
    if(_sizeOutput != 0)
    {
      // The Relay in Numerics format
      RelayProblem numerics_problem;
      numerics_problem.M = &*_M->numericsMatrix();
      numerics_problem.q = _q->getArray();
      numerics_problem.lb = _lb->getArray();
      numerics_problem.ub = _ub->getArray();
      numerics_problem.size = _sizeOutput;
  
      //int nbSolvers = 1;
      // Call Relay Driver
  
      //      Relay_display(&numerics_problem);
  
      info = relay_driver(&numerics_problem, _z->getArray(), _w->getArray(),
                          &*_numerics_solver_options);
  
      if(info != 0)
      {
        std::cout << "Warning : Problem in Relay resolution" <<std::endl;
      }
  
      // --- Recovering of the desired variables from Relay output ---
      postCompute();
    }
  
    return info;
  }
  
  void Relay::display() const
  {
    std::cout << "======= Relay of size " << _sizeOutput << " with: " <<std::endl;
    LinearOSNS::display();
    std::cout<< "lower bound : (_lb)"<<std::endl;
    _lb->display();
    std::cout<< "upper bound : (_ub)"<<std::endl;
    _ub->display();
  
  }
  
  