<commit_before>/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2016 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 "MoreauJeanGOSI.hpp"
#include "Simulation.hpp"
#include "Model.hpp"
#include "NonSmoothDynamicalSystem.hpp"
#include "NewtonEulerDS.hpp"
#include "LagrangianLinearTIDS.hpp"
#include "NewtonEulerR.hpp"
#include "LagrangianRheonomousR.hpp"
#include "NewtonImpactNSL.hpp"
#include "MultipleImpactNSL.hpp"
#include "NewtonImpactFrictionNSL.hpp"
#include "CxxStd.hpp"

#include "TypeName.hpp"

#include "OneStepNSProblem.hpp"
#include "BlockVector.hpp"


//#define DEBUG_STDOUT
//#define DEBUG_NOCOLOR
//#define DEBUG_MESSAGES
//#define DEBUG_WHERE_MESSAGES
#include <debug.h>


using namespace RELATION;

// --- constructor from a set of data ---
MoreauJeanGOSI::MoreauJeanGOSI(double theta, double gamma):
  OneStepIntegrator(OSI::MOREAUJEANGOSI), _useGammaForRelation(false), _explicitNewtonEulerDSOperators(false)
{
  _levelMinForOutput= 0;
  _levelMaxForOutput =1;
  _levelMinForInput =0;
  _levelMaxForInput =1;
  _steps=1;
  _theta = theta;
  if(!isnan(gamma))
  {
    _gamma = gamma;
    _useGamma = true;
  }
  else
  {
    _gamma = 1.0;
    _useGamma = false;
  }
}
void MoreauJeanGOSI::initializeDynamicalSystem(Model& m, double t, SP::DynamicalSystem ds)
{
  // Get work buffers from the graph
  VectorOfVectors& workVectors = *_initializeDSWorkVectors(ds);

  // Check dynamical system type
  Type::Siconos dsType = Type::value(*ds);

  // Compute W (iteration matrix)
  initializeIterationMatrixW(t, ds);
  if(dsType == Type::LagrangianLinearTIDS || dsType == Type::LagrangianDS)
    {
      SP::LagrangianDS lds = std11::static_pointer_cast<LagrangianDS> (ds);

      workVectors.resize(OneStepIntegrator::work_vector_of_vector_size);
      workVectors[OneStepIntegrator::residu_free].reset(new SiconosVector(lds->dimension()));
      workVectors[OneStepIntegrator::free].reset(new SiconosVector(lds->dimension()));
      workVectors[OneStepIntegrator::local_buffer].reset(new SiconosVector(lds->dimension()));

      lds->computeForces(m.t0(), lds->q(), lds->velocity());
      lds->swapInMemory();
    }
    else if(dsType == Type::NewtonEulerDS)
    {
      SP::NewtonEulerDS neds = std11::static_pointer_cast<NewtonEulerDS> (ds);
      workVectors.resize(OneStepIntegrator::work_vector_of_vector_size);
      workVectors[OneStepIntegrator::residu_free].reset(new SiconosVector(neds->dimension()));
      workVectors[OneStepIntegrator::free].reset(new SiconosVector(neds->dimension()));
      //Compute a first value of the dotq  to store it in  _dotqMemory
      SP::SiconosMatrix T = neds->T();
      SP::SiconosVector dotq = neds->dotq();
      SP::SiconosVector v = neds->twist();
      prod(*T, *v, *dotq, true);

      //Compute a first value of the forces to store it in _forcesMemory
      neds->computeForces(m.t0(), neds->q(), v);
      neds->swapInMemory();
    }
}

void MoreauJeanGOSI::fillDSLinks(Interaction &inter,
				   InteractionProperties& interProp,
				   DynamicalSystemsGraph & DSG)
{
  SP::DynamicalSystem ds1= interProp.source;
  SP::DynamicalSystem ds2= interProp.target;
  assert(ds1);
  assert(ds2);
  VectorOfVectors& workV = *interProp.workVectors;
  workV.resize(MoreauJeanGOSI::WORK_INTERACTION_LENGTH);
  workV[MoreauJeanGOSI::OSNSP_RHS].reset(new SiconosVector(inter.getSizeOfY()));
  VectorOfBlockVectors& DSlink = *interProp.DSlink;

  Relation &relation =  *inter.relation();  
  RELATION::TYPES relationType = relation.getType();

  // Check if interations levels (i.e. y and lambda sizes) are compliant with the current osi.
  _check_and_update_interaction_levels(inter);
  // Initialize/allocate memory buffers in interaction.
  bool computeResidu = relation.requireResidu();
  inter.initializeMemory(computeResidu,_steps);

  if (!(checkOSI(DSG.descriptor(ds1)) && checkOSI(DSG.descriptor(ds2))))
  {
    RuntimeException::selfThrow("MoreauJeanGOSI::fillDSLinks. The implementation is not correct for two different OSI for one interaction");
  }

  /* allocate and set work vectors for the osi */
  VectorOfVectors &workVds1 = *DSG.properties(DSG.descriptor(ds1)).workVectors;
  if (relationType == Lagrangian)
  {
    DSlink[LagrangianR::xfree].reset(new BlockVector());
    DSlink[LagrangianR::xfree]->insertPtr(workVds1[OneStepIntegrator::free]);
  }
  else if (relationType == NewtonEuler)
  {
    DSlink[NewtonEulerR::xfree].reset(new BlockVector());
    DSlink[NewtonEulerR::xfree]->insertPtr(workVds1[OneStepIntegrator::free]);
  }

  if (ds1 != ds2)
  {
    VectorOfVectors &workVds2 = *DSG.properties(DSG.descriptor(ds2)).workVectors;
    if (relationType == Lagrangian)
    {
      DSlink[LagrangianR::xfree]->insertPtr(workVds2[OneStepIntegrator::free]);
    }
    else if (relationType == NewtonEuler)
    {
      DSlink[NewtonEulerR::xfree]->insertPtr(workVds2[OneStepIntegrator::free]);
    }
  }
}

void MoreauJeanGOSI::initialize_nonsmooth_problems()
{
  SP::OneStepNSProblems  allOSNS  = _simulation->oneStepNSProblems();
  ((*allOSNS)[SICONOS_OSNSP_TS_VELOCITY])->setIndexSetLevel(1);
  ((*allOSNS)[SICONOS_OSNSP_TS_VELOCITY])->setInputOutputLevel(1);
  //  ((*allOSNS)[SICONOS_OSNSP_TS_VELOCITY])->initialize(_simulation);
}

void MoreauJeanGOSI::initializeIterationMatrixW(double t, SP::DynamicalSystem ds)
{
  DEBUG_PRINT("MoreauJeanGOSI::initializeIterationMatrixW starts\n");
  // This function:
  // - allocate memory for the matrix W
  // - update its content for the current (initial) state of the dynamical system, depending on its type.<commit_msg>[kernel] correct implementation for two OSI<commit_after>/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2016 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 "MoreauJeanGOSI.hpp"
#include "Simulation.hpp"
#include "Model.hpp"
#include "NonSmoothDynamicalSystem.hpp"
#include "NewtonEulerDS.hpp"
#include "LagrangianLinearTIDS.hpp"
#include "NewtonEulerR.hpp"
#include "LagrangianRheonomousR.hpp"
#include "NewtonImpactNSL.hpp"
#include "MultipleImpactNSL.hpp"
#include "NewtonImpactFrictionNSL.hpp"
#include "CxxStd.hpp"

#include "TypeName.hpp"

#include "OneStepNSProblem.hpp"
#include "BlockVector.hpp"


//#define DEBUG_STDOUT
//#define DEBUG_NOCOLOR
//#define DEBUG_MESSAGES
//#define DEBUG_WHERE_MESSAGES
#include <debug.h>


using namespace RELATION;

// --- constructor from a set of data ---
MoreauJeanGOSI::MoreauJeanGOSI(double theta, double gamma):
  OneStepIntegrator(OSI::MOREAUJEANGOSI), _useGammaForRelation(false), _explicitNewtonEulerDSOperators(false)
{
  _levelMinForOutput= 0;
  _levelMaxForOutput =1;
  _levelMinForInput =0;
  _levelMaxForInput =1;
  _steps=1;
  _theta = theta;
  if(!isnan(gamma))
  {
    _gamma = gamma;
    _useGamma = true;
  }
  else
  {
    _gamma = 1.0;
    _useGamma = false;
  }
}
void MoreauJeanGOSI::initializeDynamicalSystem(Model& m, double t, SP::DynamicalSystem ds)
{
  // Get work buffers from the graph
  VectorOfVectors& workVectors = *_initializeDSWorkVectors(ds);

  // Check dynamical system type
  Type::Siconos dsType = Type::value(*ds);

  // Compute W (iteration matrix)
  initializeIterationMatrixW(t, ds);
  if(dsType == Type::LagrangianLinearTIDS || dsType == Type::LagrangianDS)
    {
      SP::LagrangianDS lds = std11::static_pointer_cast<LagrangianDS> (ds);

      workVectors.resize(OneStepIntegrator::work_vector_of_vector_size);
      workVectors[OneStepIntegrator::residu_free].reset(new SiconosVector(lds->dimension()));
      workVectors[OneStepIntegrator::free].reset(new SiconosVector(lds->dimension()));
      workVectors[OneStepIntegrator::local_buffer].reset(new SiconosVector(lds->dimension()));

      lds->computeForces(m.t0(), lds->q(), lds->velocity());
      lds->swapInMemory();
    }
    else if(dsType == Type::NewtonEulerDS)
    {
      SP::NewtonEulerDS neds = std11::static_pointer_cast<NewtonEulerDS> (ds);
      workVectors.resize(OneStepIntegrator::work_vector_of_vector_size);
      workVectors[OneStepIntegrator::residu_free].reset(new SiconosVector(neds->dimension()));
      workVectors[OneStepIntegrator::free].reset(new SiconosVector(neds->dimension()));
      //Compute a first value of the dotq  to store it in  _dotqMemory
      SP::SiconosMatrix T = neds->T();
      SP::SiconosVector dotq = neds->dotq();
      SP::SiconosVector v = neds->twist();
      prod(*T, *v, *dotq, true);

      //Compute a first value of the forces to store it in _forcesMemory
      neds->computeForces(m.t0(), neds->q(), v);
      neds->swapInMemory();
    }
}

void MoreauJeanGOSI::fillDSLinks(Interaction &inter,
				   InteractionProperties& interProp,
				   DynamicalSystemsGraph & DSG)
{
  SP::DynamicalSystem ds1= interProp.source;
  SP::DynamicalSystem ds2= interProp.target;
  assert(ds1);
  assert(ds2);
  VectorOfVectors& workV = *interProp.workVectors;
  workV.resize(MoreauJeanGOSI::WORK_INTERACTION_LENGTH);
  workV[MoreauJeanGOSI::OSNSP_RHS].reset(new SiconosVector(inter.getSizeOfY()));
  VectorOfBlockVectors& DSlink = *interProp.DSlink;

  Relation &relation =  *inter.relation();  
  RELATION::TYPES relationType = relation.getType();

  // Check if interations levels (i.e. y and lambda sizes) are compliant with the current osi.
  _check_and_update_interaction_levels(inter);
  // Initialize/allocate memory buffers in interaction.
  bool computeResidu = relation.requireResidu();
  inter.initializeMemory(computeResidu,_steps);

  // if (!(checkOSI(DSG.descriptor(ds1)) && checkOSI(DSG.descriptor(ds2))))
  // {
  //   RuntimeException::selfThrow("MoreauJeanGOSI::fillDSLinks. The implementation is not correct for two different OSI for one interaction");
  // }



  
  /* allocate and set work vectors for the osi */
  unsigned int xfree =0;
  if (relationType == Lagrangian)
  {
    xfree = LagrangianR::xfree;
  }
  else if (relationType == NewtonEuler)
  {
    xfree = NewtonEulerR::xfree;
  }
  if (ds1 != ds2)
  {
    DEBUG_PRINT("ds1 != ds2\n");
    if ((!DSlink[xfree]) || (DSlink[xfree]->numberOfBlocks() !=2 ))
      DSlink[xfree].reset(new BlockVector(2));
  }
  else
  {
    if ((!DSlink[xfree]) || (DSlink[xfree]->numberOfBlocks() !=1 ))
      DSlink[xfree].reset(new BlockVector(1));
  }

  if(checkOSI(DSG.descriptor(ds1)))
  {
    DEBUG_PRINTF("ds1->number() %i is taken into account\n", ds1->number());
    assert(DSG.properties(DSG.descriptor(ds1)).workVectors);
    VectorOfVectors &workVds1 = *DSG.properties(DSG.descriptor(ds1)).workVectors;
    DSlink[xfree]->setVectorPtr(0,workVds1[OneStepIntegrator::free]);
  }
  DEBUG_PRINTF("ds1->number() %i\n",ds1->number());
  DEBUG_PRINTF("ds2->number() %i\n",ds2->number());


  if (ds1 != ds2)
  {
    DEBUG_PRINT("ds1 != ds2\n");
    if(checkOSI(DSG.descriptor(ds2)))
    {
      DEBUG_PRINTF("ds2->number() %i is taken into account\n",ds2->number());
      assert(DSG.properties(DSG.descriptor(ds2)).workVectors);
      VectorOfVectors &workVds2 = *DSG.properties(DSG.descriptor(ds2)).workVectors;
      DSlink[xfree]->setVectorPtr(1,workVds2[OneStepIntegrator::free]);
    }
  }
  
}

void MoreauJeanGOSI::initialize_nonsmooth_problems()
{
  SP::OneStepNSProblems  allOSNS  = _simulation->oneStepNSProblems();
  ((*allOSNS)[SICONOS_OSNSP_TS_VELOCITY])->setIndexSetLevel(1);
  ((*allOSNS)[SICONOS_OSNSP_TS_VELOCITY])->setInputOutputLevel(1);
  //  ((*allOSNS)[SICONOS_OSNSP_TS_VELOCITY])->initialize(_simulation);
}

void MoreauJeanGOSI::initializeIterationMatrixW(double t, SP::DynamicalSystem ds)
{
  DEBUG_PRINT("MoreauJeanGOSI::initializeIterationMatrixW starts\n");
  // This function:
  // - allocate memory for the matrix W
  // - update its content for the current (initial) state of the dynamical system, depending on its type.
{'fraction_non_alphanumeric': 0.12829484487986936, 'fraction_numerical': 0.007853199595676853, 'mean_word_length': 4.806772009029346, 'pattern_counts': {'":': 0, '<': 9, '<?xml version=': 0, '>': 63, 'https://': 0, 'lorem ipsum': 0, 'www.': 2, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '4814418', 'n_tokens_mistral': 4532, 'n_tokens_neox': 4353, 'n_words': 888}